Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <dx100/joint_trajectory_handler.h>
00033 #include "simple_message/messages/joint_message.h"
00034 #include "simple_message/smpl_msg_connection.h"
00035
00036 using namespace industrial::smpl_msg_connection;
00037 using namespace industrial::joint_message;
00038 using namespace industrial::simple_message;
00039
00040 namespace motoman
00041 {
00042 namespace joint_trajectory_handler
00043 {
00044 JointTrajectoryHandler::JointTrajectoryHandler()
00045 {
00046 }
00047
00048 JointTrajectoryHandler::JointTrajectoryHandler(ros::NodeHandle &n, SmplMsgConnection* robotConnecton) :
00049 node_(n)
00050 {
00051 ROS_INFO("Constructor joint trajectory handler node");
00052
00053 this->mutex_.lock();
00054 this->sub_joint_tranectory_ = this->node_.subscribe("command", 0, &JointTrajectoryHandler::jointTrajectoryCB,
00055 this);
00056 this->robot_ = robotConnecton;
00057 this->currentPoint = 0;
00058 this->state_ = JointTrajectoryStates::IDLE;
00059 this->trajectoryHandler_ =
00060 new boost::thread(boost::bind(&JointTrajectoryHandler::trajectoryHandler, this));
00061 ROS_INFO("Unlocking mutex");
00062 this->mutex_.unlock();
00063 }
00064
00065 JointTrajectoryHandler::~JointTrajectoryHandler()
00066 {
00067 trajectoryStop();
00068 this->sub_joint_tranectory_.shutdown();
00069 delete this->trajectoryHandler_;
00070 }
00071
00072 void JointTrajectoryHandler::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00073 {
00074 ROS_INFO("Receiving joint trajctory message");
00075 this->mutex_.lock();
00076 ROS_INFO("Processing joint trajctory message (mutex acquired)");
00077 ROS_DEBUG("Current state is: %d", this->state_);
00078 if (JointTrajectoryStates::IDLE != this->state_)
00079 {
00080 if (msg->points.empty())
00081 {
00082 ROS_INFO("Empty trajectory received, canceling current trajectory");
00083 }
00084 else
00085 {
00086 ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
00087 }
00088 trajectoryStop();
00089 }
00090 if (msg->points.empty())
00091 {
00092 ROS_INFO("Empty trajectory received while in IDLE state, nothing is done");
00093 }
00094 else
00095 {
00096 ROS_INFO("Loading trajectory, setting state to streaming");
00097 this->current_traj_ = *msg;
00098
00099
00100
00101
00102 while (current_traj_.points.size() <= 6)
00103 {
00104 ROS_DEBUG("Padding trajectory, current size: %d", current_traj_.points.size());
00105 current_traj_.points.push_back(current_traj_.points[current_traj_.points.size()-1]);
00106 };
00107
00108 ROS_INFO("Executing trajectory of size: %d", this->current_traj_.points.size());
00109 this->currentPoint = 0;
00110 this->state_ = JointTrajectoryStates::STREAMING;
00111 streaming_start_ = ros::Time::now();
00112 }
00113 this->mutex_.unlock();
00114 }
00115
00116 void JointTrajectoryHandler::trajectoryHandler()
00117 {
00118 JointMessage jMsg;
00119 SimpleMessage msg;
00120 SimpleMessage reply;
00121 trajectory_msgs::JointTrajectoryPoint pt;
00122 ROS_INFO("Starting joint trajectory handler state");
00123 while (ros::ok())
00124 {
00125 this->mutex_.lock();
00126
00127 if (this->robot_->isConnected())
00128 {
00129
00130
00131
00132 JointMessage jMsg;
00133 SimpleMessage msg;
00134 SimpleMessage reply;
00135 trajectory_msgs::JointTrajectoryPoint pt;
00136 switch (this->state_)
00137 {
00138 case JointTrajectoryStates::IDLE:
00139 ros::Duration(0.250).sleep();
00140 break;
00141
00142
00143 case JointTrajectoryStates::STREAMING:
00144 if (this->currentPoint < this->current_traj_.points.size())
00145 {
00146
00147
00148
00149
00150
00151 pt = this->current_traj_.points[this->currentPoint];
00152
00153 jMsg.setSequence(this->currentPoint);
00154
00155 for (int i = 0; i < this->current_traj_.joint_names.size(); i++)
00156 {
00157 jMsg.getJoints().setJoint(i, pt.positions[i]);
00158 }
00159
00160 ROS_DEBUG("Sending joint point");
00161 jMsg.toRequest(msg);
00162 if (this->robot_->sendAndReceiveMsg(msg, reply, false))
00163
00164
00165 {
00166 ROS_INFO("Point[%d of %d] sent to controller",
00167 this->currentPoint, this->current_traj_.points.size());
00168 this->currentPoint++;
00169 }
00170 else
00171 {
00172 ROS_WARN("Failed sent joint point, will try again");
00173 }
00174
00175 }
00176 else
00177 {
00178 ROS_INFO("Trajectory streaming complete, setting state to IDLE");
00179 this->state_ = JointTrajectoryStates::IDLE;
00180 }
00181 break;
00182
00183 default:
00184 ROS_ERROR("Joint trajectory handler: unknown state");
00185 this->state_ = JointTrajectoryStates::IDLE;
00186 break;
00187 }
00188
00189 }
00190 else
00191 {
00192 ROS_INFO("Connecting to robot motion server");
00193 this->robot_->makeConnect();
00194 }
00195
00196 this->mutex_.unlock();
00197 ros::Duration(0.005).sleep();
00198 }
00199
00200 ROS_WARN("Exiting trajectory handler thread");
00201 }
00202
00203 unsigned int JointTrajectoryHandler::getNextTrajectoryPoint(const trajectory_msgs::JointTrajectory& traj,
00204 const ros::Time& start,
00205 const ros::Time& cur)
00206 {
00207 for(unsigned int i = 0; i < traj.points.size(); i++) {
00208 ros::Duration dur(cur-start);
00209 if(dur < traj.points[i].time_from_start) {
00210 return i;
00211 }
00212 }
00213 return traj.points.size()-1;
00214 }
00215
00216
00217 void JointTrajectoryHandler::trajectoryStop()
00218 {
00219 JointMessage jMsg;
00220 SimpleMessage msg;
00221 SimpleMessage reply;
00222
00223 ROS_INFO("Joint trajectory handler: entering stopping state");
00224 jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
00225 jMsg.toRequest(msg);
00226 ROS_DEBUG("Sending stop command");
00227 this->robot_->sendAndReceiveMsg(msg, reply);
00228 ROS_DEBUG("Stop command sent, entring idle mode");
00229 this->state_ = JointTrajectoryStates::IDLE;
00230 }
00231
00232
00233
00234
00235
00236 }
00237 }
00238