Пример #1
0
void JointTrajectoryHandler::trajectoryStop()
{
  JointMessage jMsg;
  SimpleMessage msg;
  SimpleMessage reply;

  ROS_INFO("Joint trajectory handler: entering stopping state");
  jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
  jMsg.toRequest(msg);
  ROS_DEBUG("Sending stop command");
  this->robot_->sendAndReceiveMsg(msg, reply);
  ROS_DEBUG("Stop command sent, entring idle mode");
  this->state_ = JointTrajectoryStates::IDLE;
}
Пример #2
0
void JointTrajectoryHandler::trajectoryHandler()
{
  JointMessage jMsg;
  SimpleMessage msg;
  SimpleMessage reply;
  trajectory_msgs::JointTrajectoryPoint pt;
  ROS_INFO("Starting joint trajectory handler state");
  while (ros::ok())
  {
    this->mutex_.lock();

    if (this->robot_->isConnected())
    {
      
      //TODO: These variables should be moved outside of this loop
      //so that we aren't contantly reinitializing them.
      JointMessage jMsg;
      SimpleMessage msg;
      SimpleMessage reply;
      trajectory_msgs::JointTrajectoryPoint pt;
      switch (this->state_)
      {
        case JointTrajectoryStates::IDLE:
          ros::Duration(0.250).sleep();
          break;


        case JointTrajectoryStates::STREAMING:
          if (this->currentPoint < this->current_traj_.points.size())
          {
            // unsigned int lastCurrentPoint = currentPoint;
            // currentPoint = getNextTrajectoryPoint(current_traj_,
            //                                       streaming_start_,
            //                                       ros::Time::now());
            //ROS_INFO("Skipping from point[%d] to point[%d]", lastCurrentPoint, currentPoint);
            pt = this->current_traj_.points[this->currentPoint];
            
            jMsg.setSequence(this->currentPoint);

            for (int i = 0; i < this->current_traj_.joint_names.size(); i++)
            {
              jMsg.getJoints().setJoint(i, pt.positions[i]);
            }
            
            ROS_DEBUG("Sending joint point");
            jMsg.toRequest(msg);
            if (this->robot_->sendAndReceiveMsg(msg, reply, false))
            //jMsg.toTopic(msg);
            //if (this->robot_->sendMsg(msg))
            {
              ROS_INFO("Point[%d of %d] sent to controller", 
                       this->currentPoint, this->current_traj_.points.size());
              this->currentPoint++;
            }
            else
            {
              ROS_WARN("Failed sent joint point, will try again");
            }
            //ROS_INFO_STREAM("Time taken to stream single point is " << (ros::WallTime::now()-start));
          }
          else
          {
            ROS_INFO("Trajectory streaming complete, setting state to IDLE");
            this->state_ = JointTrajectoryStates::IDLE;
          }
          break;

        default:
          ROS_ERROR("Joint trajectory handler: unknown state");
          this->state_ = JointTrajectoryStates::IDLE;
          break;
      }

    }
    else
    {
      ROS_INFO("Connecting to robot motion server");
      this->robot_->makeConnect();
    }

    this->mutex_.unlock();
    ros::Duration(0.005).sleep();
  }

  ROS_WARN("Exiting trajectory handler thread");
}