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 <algorithm>
00033 #include "industrial_robot_client/joint_trajectory_interface.h"
00034 #include "simple_message/joint_traj_pt.h"
00035 #include "industrial_utils/param_utils.h"
00036
00037 using namespace industrial_utils::param;
00038 using industrial::simple_message::SimpleMessage;
00039 namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts;
00040 namespace SpecialSeqValues = industrial::joint_traj_pt::SpecialSeqValues;
00041 typedef industrial::joint_traj_pt::JointTrajPt rbt_JointTrajPt;
00042 typedef trajectory_msgs::JointTrajectoryPoint ros_JointTrajPt;
00043
00044 namespace industrial_robot_client
00045 {
00046 namespace joint_trajectory_interface
00047 {
00048
00049 bool JointTrajectoryInterface::init()
00050 {
00051 std::string s;
00052
00053
00054 if (!node_.getParam("robot_ip_address", s))
00055 {
00056 ROS_ERROR("Robot State failed to get param 'robot_ip_address'");
00057 return false;
00058 }
00059
00060 char* ip_addr = strdup(s.c_str());
00061 ROS_INFO("Joint Trajectory Interface connecting to IP address: %s", ip_addr);
00062 default_tcp_connection_.init(ip_addr, StandardSocketPorts::MOTION);
00063 free(ip_addr);
00064
00065 return init(&default_tcp_connection_);
00066 }
00067
00068 bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
00069 {
00070 std::vector<std::string> joint_names;
00071 if (!getJointNames("controller_joint_names", joint_names))
00072 ROS_WARN("Unable to read 'controller_joint_names' param. Using standard 6-DOF joint names.");
00073
00074 return init(connection, joint_names);
00075 }
00076
00077 bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00078 const std::map<std::string, double> &velocity_limits)
00079 {
00080 this->connection_ = connection;
00081 this->all_joint_names_ = joint_names;
00082 this->joint_vel_limits_ = velocity_limits;
00083 connection_->makeConnect();
00084
00085 this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this);
00086 this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
00087 this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
00088
00089 return true;
00090 }
00091
00092 JointTrajectoryInterface::~JointTrajectoryInterface()
00093 {
00094 trajectoryStop();
00095 this->sub_joint_trajectory_.shutdown();
00096 }
00097
00098 bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
00099 industrial_msgs::CmdJointTrajectory::Response &res)
00100 {
00101 trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
00102 *traj_ptr = req.trajectory;
00103 this->jointTrajectoryCB(traj_ptr);
00104
00105
00106 res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00107
00108 return true;
00109 }
00110
00111 void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00112 {
00113 ROS_INFO("Receiving joint trajectory message");
00114
00115
00116 if (msg->points.empty())
00117 {
00118 ROS_INFO("Empty trajectory received, canceling current trajectory");
00119 trajectoryStop();
00120 return;
00121 }
00122
00123
00124 std::vector<JointTrajPtMessage> robot_msgs;
00125 if (!trajectory_to_msgs(msg, &robot_msgs))
00126 return;
00127
00128
00129 send_to_robot(robot_msgs);
00130 }
00131
00132 bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<JointTrajPtMessage>* msgs)
00133 {
00134 msgs->clear();
00135
00136 for (size_t i=0; i<traj->points.size(); ++i)
00137 {
00138 ros_JointTrajPt rbt_pt, xform_pt;
00139 double vel, duration;
00140
00141
00142 if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt))
00143 return false;
00144
00145
00146 if (!transform(rbt_pt, &xform_pt))
00147 return false;
00148
00149
00150 if (!calc_speed(xform_pt, &vel, &duration))
00151 return false;
00152
00153 JointTrajPtMessage msg = create_message(i, xform_pt.positions, vel, duration);
00154 msgs->push_back(msg);
00155 }
00156
00157 return true;
00158 }
00159
00160 bool JointTrajectoryInterface::select(const std::vector<std::string>& ros_joint_names, const ros_JointTrajPt& ros_pt,
00161 const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
00162 {
00163 ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
00164
00165
00166 *rbt_pt = ros_pt;
00167 rbt_pt->positions.clear(); rbt_pt->velocities.clear(); rbt_pt->accelerations.clear();
00168
00169 for (size_t rbt_idx=0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
00170 {
00171 bool is_empty = rbt_joint_names[rbt_idx].empty();
00172
00173
00174 size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(), rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
00175 bool is_found = ros_idx < ros_joint_names.size();
00176
00177
00178 if (!is_empty && !is_found)
00179 {
00180 ROS_ERROR("Expected joint (%s) not found in JointTrajectory. Aborting command.", rbt_joint_names[rbt_idx].c_str());
00181 return false;
00182 }
00183
00184 if (is_empty)
00185 {
00186 if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(default_joint_pos_);
00187 if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
00188 if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
00189 }
00190 else
00191 {
00192 if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
00193 if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
00194 if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
00195 }
00196 }
00197 return true;
00198 }
00199
00200 bool JointTrajectoryInterface::calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration)
00201 {
00202 return calc_velocity(pt, rbt_velocity) && calc_duration(pt, rbt_duration);
00203 }
00204
00205
00206
00207
00208
00209
00210
00211 bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity)
00212 {
00213 std::vector<double> vel_ratios;
00214 static bool limits_initialized = false;
00215
00216 ROS_ASSERT(all_joint_names_.size() == pt.positions.size());
00217
00218
00219 if (pt.velocities.empty())
00220 {
00221 ROS_WARN("Joint velocities unspecified. Using default/safe speed.");
00222 *rbt_velocity = default_vel_ratio_;
00223 return true;
00224 }
00225
00226
00227 if (joint_vel_limits_.empty() && !limits_initialized)
00228 {
00229 limits_initialized = true;
00230 if (!getJointVelocityLimits("robot_description", joint_vel_limits_))
00231 ROS_WARN("Unable to read velocity limits from 'robot_description' param. Will use default/safe speed.");
00232 }
00233
00234 for (size_t i=0; i<all_joint_names_.size(); ++i)
00235 {
00236 const std::string &jnt_name = all_joint_names_[i];
00237
00238
00239 if (jnt_name.empty())
00240 vel_ratios.push_back(-1);
00241 else if (joint_vel_limits_.count(jnt_name) == 0)
00242 vel_ratios.push_back(-1);
00243 else
00244 vel_ratios.push_back( fabs(pt.velocities[i] / joint_vel_limits_[jnt_name]) );
00245 }
00246
00247
00248 int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
00249
00250 if (vel_ratios[max_idx] > 0)
00251 *rbt_velocity = vel_ratios[max_idx];
00252 else
00253 {
00254 ROS_WARN("Joint velocity-limits unspecified. Using default velocity-ratio.");
00255 *rbt_velocity = default_vel_ratio_;
00256 }
00257
00258 if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) )
00259 {
00260 ROS_WARN("computed velocity (%.1f %%) is out-of-range. Clipping to [0-100%%]", *rbt_velocity * 100);
00261 *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));
00262 }
00263
00264 return true;
00265 }
00266
00267 bool JointTrajectoryInterface::calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration)
00268 {
00269 std::vector<double> durations;
00270 double this_time = pt.time_from_start.toSec();
00271 static double last_time = 0;
00272
00273 if (this_time <= last_time)
00274 *rbt_duration = default_duration_;
00275 else
00276 *rbt_duration = this_time - last_time;
00277
00278 last_time = this_time;
00279
00280 return true;
00281 }
00282
00283 JointTrajPtMessage JointTrajectoryInterface::create_message(int seq, std::vector<double> joint_pos, double velocity, double duration)
00284 {
00285 industrial::joint_data::JointData pos;
00286 ROS_ASSERT(joint_pos.size() <= (unsigned int)pos.getMaxNumJoints());
00287
00288 for (size_t i=0; i<joint_pos.size(); ++i)
00289 pos.setJoint(i, joint_pos[i]);
00290
00291 rbt_JointTrajPt pt;
00292 pt.init(seq, pos, velocity, duration);
00293
00294 JointTrajPtMessage msg;
00295 msg.init(pt);
00296
00297 return msg;
00298 }
00299
00300 void JointTrajectoryInterface::trajectoryStop()
00301 {
00302 JointTrajPtMessage jMsg;
00303 SimpleMessage msg, reply;
00304
00305 ROS_INFO("Joint trajectory handler: entering stopping state");
00306 jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
00307 jMsg.toRequest(msg);
00308 ROS_DEBUG("Sending stop command");
00309 this->connection_->sendAndReceiveMsg(msg, reply);
00310 }
00311
00312 bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
00313 industrial_msgs::StopMotion::Response &res)
00314 {
00315 trajectoryStop();
00316
00317
00318 res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00319
00320 return true;
00321 }
00322
00323 }
00324 }
00325