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 "motoman_driver/industrial_robot_client/joint_trajectory_streamer.h"
00033 #include <map>
00034 #include <vector>
00035 #include <string>
00036
00037 namespace CommTypes = industrial::simple_message::CommTypes;
00038 namespace ReplyTypes = industrial::simple_message::ReplyTypes;
00039
00040 namespace industrial_robot_client
00041 {
00042 namespace joint_trajectory_streamer
00043 {
00044
00045 bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::map<int, RobotGroup> &robot_groups,
00046 const std::map<std::string, double> &velocity_limits)
00047 {
00048 bool rtn = true;
00049
00050 ROS_INFO("JointTrajectoryStreamer: init");
00051
00052 rtn &= JointTrajectoryInterface::init(connection, robot_groups, velocity_limits);
00053
00054 this->mutex_.lock();
00055 this->current_point_ = 0;
00056 this->state_ = TransferStates::IDLE;
00057 this->streaming_thread_ =
00058 new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this));
00059 ROS_INFO("Unlocking mutex");
00060 this->mutex_.unlock();
00061
00062 return rtn;
00063 }
00064
00065 bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00066 const std::map<std::string, double> &velocity_limits)
00067 {
00068 bool rtn = true;
00069
00070 ROS_INFO("JointTrajectoryStreamer: init");
00071
00072 rtn &= JointTrajectoryInterface::init(connection, joint_names, velocity_limits);
00073
00074 this->mutex_.lock();
00075 this->current_point_ = 0;
00076 this->state_ = TransferStates::IDLE;
00077 this->streaming_thread_ =
00078 new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this));
00079 ROS_INFO("Unlocking mutex");
00080 this->mutex_.unlock();
00081
00082 return rtn;
00083 }
00084
00085 JointTrajectoryStreamer::~JointTrajectoryStreamer()
00086 {
00087 delete this->streaming_thread_;
00088 }
00089
00090 void JointTrajectoryStreamer::jointTrajectoryCB(const motoman_msgs::DynamicJointTrajectoryConstPtr &msg)
00091 {
00092 ROS_INFO("Receiving joint trajectory message");
00093
00094
00095 int state = this->state_;
00096
00097 ROS_DEBUG("Current state is: %d", state);
00098 if (TransferStates::IDLE != state)
00099 {
00100 if (msg->points.empty())
00101 ROS_INFO("Empty trajectory received, canceling current trajectory");
00102 else
00103 ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
00104
00105 this->mutex_.lock();
00106 trajectoryStop();
00107 this->mutex_.unlock();
00108 return;
00109 }
00110
00111 if (msg->points.empty())
00112 {
00113 ROS_INFO("Empty trajectory received while in IDLE state, nothing is done");
00114 return;
00115 }
00116
00117
00118 std::vector<SimpleMessage> new_traj_msgs;
00119 if (!trajectory_to_msgs(msg, &new_traj_msgs))
00120 return;
00121
00122
00123 send_to_robot(new_traj_msgs);
00124 }
00125
00126 void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00127 {
00128 ROS_INFO("Receiving joint trajectory message");
00129
00130
00131 int state = this->state_;
00132
00133 ROS_DEBUG("Current state is: %d", state);
00134 if (TransferStates::IDLE != state)
00135 {
00136 if (msg->points.empty())
00137 ROS_INFO("Empty trajectory received, canceling current trajectory");
00138 else
00139 ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
00140
00141 this->mutex_.lock();
00142 trajectoryStop();
00143 this->mutex_.unlock();
00144 return;
00145 }
00146
00147 if (msg->points.empty())
00148 {
00149 ROS_INFO("Empty trajectory received while in IDLE state, nothing is done");
00150 return;
00151 }
00152
00153
00154 std::vector<SimpleMessage> new_traj_msgs;
00155 if (!trajectory_to_msgs(msg, &new_traj_msgs))
00156 return;
00157
00158
00159 send_to_robot(new_traj_msgs);
00160 }
00161
00162 bool JointTrajectoryStreamer::send_to_robot(const std::vector<SimpleMessage>& messages)
00163 {
00164 ROS_INFO("Loading trajectory, setting state to streaming");
00165 this->mutex_.lock();
00166 {
00167 ROS_INFO("Executing trajectory of size: %d", static_cast<int>(messages.size()));
00168 this->current_traj_ = messages;
00169 this->current_point_ = 0;
00170 this->state_ = TransferStates::STREAMING;
00171 this->streaming_start_ = ros::Time::now();
00172 }
00173 this->mutex_.unlock();
00174
00175 return true;
00176 }
00177
00178 bool JointTrajectoryStreamer::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<SimpleMessage>* msgs)
00179 {
00180
00181 if (!JointTrajectoryInterface::trajectory_to_msgs(traj, msgs))
00182 return false;
00183
00184
00185 if (!msgs->empty() && (msgs->size() < (size_t)min_buffer_size_))
00186 {
00187 ROS_DEBUG("Padding trajectory: current(%d) => minimum(%d)", static_cast<int>(msgs->size()), min_buffer_size_);
00188 while (msgs->size() < (size_t)min_buffer_size_)
00189 msgs->push_back(msgs->back());
00190 }
00191
00192 return true;
00193 }
00194
00195 bool JointTrajectoryStreamer::trajectory_to_msgs(const motoman_msgs::DynamicJointTrajectoryConstPtr &traj, std::vector<SimpleMessage>* msgs)
00196 {
00197
00198 if (!JointTrajectoryInterface::trajectory_to_msgs(traj, msgs))
00199 return false;
00200
00201
00202 if (!msgs->empty() && (msgs->size() < (size_t)min_buffer_size_))
00203 {
00204 ROS_DEBUG("Padding trajectory: current(%d) => minimum(%d)", static_cast<int>(msgs->size()), min_buffer_size_);
00205 while (msgs->size() < (size_t)min_buffer_size_)
00206 msgs->push_back(msgs->back());
00207 }
00208
00209 return true;
00210 }
00211
00212
00213 void JointTrajectoryStreamer::streamingThread()
00214 {
00215 int connectRetryCount = 1;
00216
00217 ROS_INFO("Starting joint trajectory streamer thread");
00218 while (ros::ok())
00219 {
00220 ros::Duration(0.005).sleep();
00221
00222
00223 if (connectRetryCount-- > 0)
00224 {
00225 ROS_INFO("Connecting to robot motion server");
00226 this->connection_->makeConnect();
00227 ros::Duration(0.250).sleep();
00228
00229 if (this->connection_->isConnected())
00230 connectRetryCount = 0;
00231 else if (connectRetryCount <= 0)
00232 {
00233 ROS_ERROR("Timeout connecting to robot controller. Send new motion command to retry.");
00234 this->state_ = TransferStates::IDLE;
00235 }
00236 continue;
00237 }
00238
00239 this->mutex_.lock();
00240
00241 SimpleMessage msg, tmpMsg, reply;
00242
00243 switch (this->state_)
00244 {
00245 case TransferStates::IDLE:
00246 ros::Duration(0.250).sleep();
00247 break;
00248
00249 case TransferStates::STREAMING:
00250 if (this->current_point_ >= static_cast<int>(this->current_traj_.size()))
00251 {
00252 ROS_INFO("Trajectory streaming complete, setting state to IDLE");
00253 this->state_ = TransferStates::IDLE;
00254 break;
00255 }
00256
00257 if (!this->connection_->isConnected())
00258 {
00259 ROS_DEBUG("Robot disconnected. Attempting reconnect...");
00260 connectRetryCount = 5;
00261 break;
00262 }
00263
00264 tmpMsg = this->current_traj_[this->current_point_];
00265 msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
00266 ReplyTypes::INVALID, tmpMsg.getData());
00267
00268 ROS_DEBUG("Sending joint trajectory point");
00269 if (this->connection_->sendAndReceiveMsg(msg, reply, false))
00270 {
00271 ROS_INFO("Point[%d of %d] sent to controller",
00272 this->current_point_, static_cast<int>(this->current_traj_.size()));
00273 this->current_point_++;
00274 }
00275 else
00276 ROS_WARN("Failed sent joint point, will try again");
00277
00278 break;
00279 default:
00280 ROS_ERROR("Joint trajectory streamer: unknown state");
00281 this->state_ = TransferStates::IDLE;
00282 break;
00283 }
00284
00285 this->mutex_.unlock();
00286 }
00287
00288 ROS_WARN("Exiting trajectory streamer thread");
00289 }
00290
00291 void JointTrajectoryStreamer::trajectoryStop()
00292 {
00293 JointTrajectoryInterface::trajectoryStop();
00294
00295 ROS_DEBUG("Stop command sent, entering idle mode");
00296 this->state_ = TransferStates::IDLE;
00297 }
00298
00299 }
00300 }
00301