joint_trajectory_interface.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *      * Redistributions of source code must retain the above copyright
00011  *      notice, this list of conditions and the following disclaimer.
00012  *      * Redistributions in binary form must reproduce the above copyright
00013  *      notice, this list of conditions and the following disclaimer in the
00014  *      documentation and/or other materials provided with the distribution.
00015  *      * Neither the name of the Southwest Research Institute, nor the names
00016  *      of its contributors may be used to endorse or promote products derived
00017  *      from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
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 #define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0)
00050 
00051 bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
00052 {
00053   std::string ip;
00054   int port;
00055 
00056   // override IP/port with ROS params, if available
00057   ros::param::param<std::string>("robot_ip_address", ip, default_ip);
00058   ros::param::param<int>("~port", port, default_port);
00059 
00060   // check for valid parameter values
00061   if (ip.empty())
00062   {
00063     ROS_ERROR("No valid robot IP address found.  Please set ROS 'robot_ip_address' param");
00064     return false;
00065   }
00066   if (port <= 0)
00067   {
00068     ROS_ERROR("No valid robot IP port found.  Please set ROS '~port' param");
00069     return false;
00070   }
00071 
00072   char* ip_addr = strdup(ip.c_str());  // connection.init() requires "char*", not "const char*"
00073   ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
00074   default_tcp_connection_.init(ip_addr, port);
00075   free(ip_addr);
00076 
00077   return init(&default_tcp_connection_);
00078 }
00079 
00080 bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
00081 {
00082   std::vector<std::string> joint_names;
00083   if (!getJointNames("controller_joint_names", "robot_description", joint_names))
00084   {
00085     ROS_ERROR("Failed to initialize joint_names.  Aborting");
00086     return false;
00087   }
00088 
00089   return init(connection, joint_names);
00090 }
00091 
00092 bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00093                                     const std::map<std::string, double> &velocity_limits)
00094 {
00095   this->connection_ = connection;
00096   this->all_joint_names_ = joint_names;
00097   this->joint_vel_limits_ = velocity_limits;
00098   connection_->makeConnect();
00099 
00100   // try to read velocity limits from URDF, if none specified
00101   if (joint_vel_limits_.empty() && !industrial_utils::param::getJointVelocityLimits("robot_description", joint_vel_limits_))
00102     ROS_WARN("Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.");
00103 
00104   this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this);
00105   this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
00106   this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
00107   this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);
00108 
00109   return true;
00110 }
00111 
00112 JointTrajectoryInterface::~JointTrajectoryInterface()
00113 {  
00114   trajectoryStop();
00115   this->sub_joint_trajectory_.shutdown();
00116 }
00117 
00118 bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
00119                                                  industrial_msgs::CmdJointTrajectory::Response &res)
00120 {
00121   trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
00122   *traj_ptr = req.trajectory;  // copy message data
00123   this->jointTrajectoryCB(traj_ptr);
00124 
00125   // no success/fail result from jointTrajectoryCB.  Assume success.
00126   res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00127 
00128   return true;  // always return true.  To distinguish between call-failed and service-unavailable.
00129 }
00130 
00131 void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00132 {
00133   ROS_INFO("Receiving joint trajectory message");
00134 
00135   // check for STOP command
00136   if (msg->points.empty())
00137   {
00138     ROS_INFO("Empty trajectory received, canceling current trajectory");
00139     trajectoryStop();
00140     return;
00141   }
00142 
00143   // convert trajectory into robot-format
00144   std::vector<JointTrajPtMessage> robot_msgs;
00145   if (!trajectory_to_msgs(msg, &robot_msgs))
00146     return;
00147 
00148   // send command messages to robot
00149   send_to_robot(robot_msgs);
00150 }
00151 
00152 bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<JointTrajPtMessage>* msgs)
00153 {
00154   msgs->clear();
00155 
00156   // check for valid trajectory
00157   if (!is_valid(*traj))
00158     return false;
00159 
00160   for (size_t i=0; i<traj->points.size(); ++i)
00161   {
00162     ros_JointTrajPt rbt_pt, xform_pt;
00163     double vel, duration;
00164 
00165     // select / reorder joints for sending to robot
00166     if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt))
00167       return false;
00168 
00169     // transform point data (e.g. for joint-coupling)
00170     if (!transform(rbt_pt, &xform_pt))
00171       return false;
00172 
00173     // reduce velocity to a single scalar, for robot command
00174     if (!calc_speed(xform_pt, &vel, &duration))
00175       return false;
00176 
00177     JointTrajPtMessage msg = create_message(i, xform_pt.positions, vel, duration);
00178     msgs->push_back(msg);
00179   }
00180 
00181   return true;
00182 }
00183 
00184 bool JointTrajectoryInterface::select(const std::vector<std::string>& ros_joint_names, const ros_JointTrajPt& ros_pt,
00185                       const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
00186 {
00187   ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
00188 
00189   // initialize rbt_pt
00190   *rbt_pt = ros_pt;
00191   rbt_pt->positions.clear(); rbt_pt->velocities.clear(); rbt_pt->accelerations.clear();
00192 
00193   for (size_t rbt_idx=0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
00194   {
00195     bool is_empty = rbt_joint_names[rbt_idx].empty();
00196 
00197     // find matching ROS element
00198     size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(), rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
00199     bool is_found = ros_idx < ros_joint_names.size();
00200 
00201     // error-chk: required robot joint not found in ROS joint-list
00202     if (!is_empty && !is_found)
00203     {
00204       ROS_ERROR("Expected joint (%s) not found in JointTrajectory.  Aborting command.", rbt_joint_names[rbt_idx].c_str());
00205       return false;
00206     }
00207 
00208     if (is_empty)
00209     {
00210       if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(default_joint_pos_);
00211       if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
00212       if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
00213     }
00214     else
00215     {
00216       if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
00217       if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
00218       if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
00219     }
00220   }
00221   return true;
00222 }
00223 
00224 bool JointTrajectoryInterface::calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration)
00225 {
00226         return calc_velocity(pt, rbt_velocity) && calc_duration(pt, rbt_duration);
00227 }
00228 
00229 // default velocity calculation computes the %-of-max-velocity for the "critical joint" (closest to velocity-limit)
00230 // such that 0.2 = 20% of maximum joint speed.
00231 //
00232 // NOTE: this calculation uses the maximum joint speeds from the URDF file, which may differ from those defined on
00233 // the physical robot.  These differences could lead to different actual movement velocities than intended.
00234 // Behavior should be verified on a physical robot if movement velocity is critical.
00235 bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity)
00236 {
00237   std::vector<double> vel_ratios;
00238 
00239   ROS_ASSERT(all_joint_names_.size() == pt.positions.size());
00240 
00241   // check for empty velocities in ROS topic
00242   if (pt.velocities.empty())
00243   {
00244     ROS_WARN("Joint velocities unspecified.  Using default/safe speed.");
00245     *rbt_velocity = default_vel_ratio_;
00246     return true;
00247   }
00248 
00249   for (size_t i=0; i<all_joint_names_.size(); ++i)
00250   {
00251     const std::string &jnt_name = all_joint_names_[i];
00252 
00253     // update vel_ratios
00254     if (jnt_name.empty())                             // ignore "dummy joints" in velocity calcs
00255       vel_ratios.push_back(-1);
00256     else if (joint_vel_limits_.count(jnt_name) == 0)  // no velocity limit specified for this joint
00257       vel_ratios.push_back(-1);
00258     else
00259       vel_ratios.push_back( fabs(pt.velocities[i] / joint_vel_limits_[jnt_name]) );  // calculate expected duration for this joint
00260   }
00261 
00262   // find largest velocity-ratio (closest to max joint-speed)
00263   int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
00264   
00265   if (vel_ratios[max_idx] > 0)
00266     *rbt_velocity = vel_ratios[max_idx];
00267   else
00268   {
00269     ROS_WARN_ONCE("Joint velocity-limits unspecified.  Using default velocity-ratio.");
00270     *rbt_velocity = default_vel_ratio_;
00271   }
00272 
00273   if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) )
00274   {
00275     ROS_WARN("computed velocity (%.1f %%) is out-of-range.  Clipping to [0-100%%]", *rbt_velocity * 100);
00276     *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));  // clip to [0,1]
00277   }
00278   
00279   return true;
00280 }
00281 
00282 bool JointTrajectoryInterface::calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration)
00283 {
00284   std::vector<double> durations;
00285   double this_time = pt.time_from_start.toSec();
00286   static double last_time = 0;
00287 
00288   if (this_time <= last_time)  // earlier time => new trajectory.  Move slowly to first point.
00289     *rbt_duration = default_duration_;
00290   else
00291     *rbt_duration = this_time - last_time;
00292 
00293   last_time = this_time;
00294 
00295   return true;
00296 }
00297 
00298 JointTrajPtMessage JointTrajectoryInterface::create_message(int seq, std::vector<double> joint_pos, double velocity, double duration)
00299 {
00300   industrial::joint_data::JointData pos;
00301   ROS_ASSERT(joint_pos.size() <= (unsigned int)pos.getMaxNumJoints());
00302 
00303   for (size_t i=0; i<joint_pos.size(); ++i)
00304     pos.setJoint(i, joint_pos[i]);
00305 
00306   rbt_JointTrajPt pt;
00307   pt.init(seq, pos, velocity, duration);
00308 
00309   JointTrajPtMessage msg;
00310   msg.init(pt);
00311 
00312   return msg;
00313 }
00314 
00315 void JointTrajectoryInterface::trajectoryStop()
00316 {
00317   JointTrajPtMessage jMsg;
00318   SimpleMessage msg, reply;
00319 
00320   ROS_INFO("Joint trajectory handler: entering stopping state");
00321   jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
00322   jMsg.toRequest(msg);
00323   ROS_DEBUG("Sending stop command");
00324   this->connection_->sendAndReceiveMsg(msg, reply);
00325 }
00326 
00327 bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
00328                                                     industrial_msgs::StopMotion::Response &res)
00329 {
00330   trajectoryStop();
00331 
00332   // no success/fail result from trajectoryStop.  Assume success.
00333   res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00334 
00335   return true;  // always return true.  To distinguish between call-failed and service-unavailable.
00336 }
00337 
00338 bool JointTrajectoryInterface::is_valid(const trajectory_msgs::JointTrajectory &traj)
00339 {
00340   for (int i=0; i<traj.points.size(); ++i)
00341   {
00342     const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
00343 
00344     // check for non-empty positions
00345     if (pt.positions.empty())
00346       ROS_ERROR_RETURN(false, "Validation failed: Missing position data for trajectory pt %d", i);
00347 
00348     // check for joint velocity limits
00349     for (int j=0; j<pt.velocities.size(); ++j)
00350     {
00351       std::map<std::string, double>::iterator max_vel = joint_vel_limits_.find(traj.joint_names[j]);
00352       if (max_vel == joint_vel_limits_.end()) continue;  // no velocity-checking if limit not defined
00353 
00354       if (std::abs(pt.velocities[j]) > max_vel->second)
00355         ROS_ERROR_RETURN(false, "Validation failed: Max velocity exceeded for trajectory pt %d, joint '%s'", i, traj.joint_names[j].c_str());
00356     }
00357 
00358     // check for valid timestamp
00359     if ((i > 0) && (pt.time_from_start.toSec() == 0))
00360       ROS_ERROR_RETURN(false, "Validation failed: Missing valid timestamp data for trajectory pt %d", i);
00361   }
00362 
00363   return true;
00364 }
00365 
00366 // copy robot JointState into local cache
00367 void JointTrajectoryInterface::jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
00368 {
00369   this->cur_joint_pos_ = *msg;
00370 }
00371 
00372 } //joint_trajectory_interface
00373 } //industrial_robot_client
00374 


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Sat Jun 8 2019 20:43:29