joint_trajectory_interface.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2013, 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 "fs100/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 SpecialSeqValues = industrial::joint_traj_pt::SpecialSeqValues;
00040 typedef industrial::joint_traj_pt::JointTrajPt rbt_JointTrajPt;
00041 typedef trajectory_msgs::JointTrajectoryPoint  ros_JointTrajPt;
00042 
00043 namespace industrial_robot_client
00044 {
00045 namespace joint_trajectory_interface
00046 {
00047 
00048 #define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0)
00049 
00050 bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
00051 {
00052   std::string ip;
00053   int port;
00054 
00055   // override IP/port with ROS params, if available
00056   ros::param::param<std::string>("robot_ip_address", ip, default_ip);
00057   ros::param::param<int>("~port", port, default_port);
00058 
00059   // check for valid parameter values
00060   if (ip.empty())
00061   {
00062     ROS_ERROR("No valid robot IP address found.  Please set ROS 'robot_ip_address' param");
00063     return false;
00064   }
00065   if (port <= 0)
00066   {
00067     ROS_ERROR("No valid robot IP port found.  Please set ROS '~port' param");
00068     return false;
00069   }
00070 
00071   char* ip_addr = strdup(ip.c_str()); // connection.init() requires "char*", not "const char*"
00072   ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
00073   default_tcp_connection_.init(ip_addr, port);
00074   free(ip_addr);
00075 
00076   return init(&default_tcp_connection_);
00077 }
00078 
00079 bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
00080 {
00081   std::vector<std::string> joint_names;
00082   if (!getJointNames("controller_joint_names", "robot_description", joint_names))
00083   {
00084     ROS_ERROR("Failed to initialize joint_names.  Aborting");
00085     return false;
00086   }
00087 
00088   return init(connection, joint_names);
00089 }
00090 
00091 bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00092                                     const std::map<std::string, double> &velocity_limits)
00093 {
00094   this->connection_ = connection;
00095   this->all_joint_names_ = joint_names;
00096   this->joint_vel_limits_ = velocity_limits;
00097   connection_->makeConnect();
00098 
00099   // try to read velocity limits from URDF, if none specified
00100   if (joint_vel_limits_.empty() && !industrial_utils::param::getJointVelocityLimits("robot_description", joint_vel_limits_))
00101     ROS_WARN("Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.");
00102 
00103   this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this);
00104   this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
00105   this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
00106   this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);
00107 
00108   return true;
00109 }
00110 
00111 JointTrajectoryInterface::~JointTrajectoryInterface()
00112 {  
00113   trajectoryStop();
00114   this->sub_joint_trajectory_.shutdown();
00115 }
00116 
00117 bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
00118                                                  industrial_msgs::CmdJointTrajectory::Response &res)
00119 {
00120   trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
00121   *traj_ptr = req.trajectory;  // copy message data
00122   this->jointTrajectoryCB(traj_ptr);
00123 
00124   // no success/fail result from jointTrajectoryCB.  Assume success.
00125   res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00126 
00127   return true;  // always return true.  To distinguish between call-failed and service-unavailable.
00128 }
00129 
00130 void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00131 {
00132   ROS_INFO("Receiving joint trajectory message");
00133 
00134   // check for STOP command
00135   if (msg->points.empty())
00136   {
00137     ROS_INFO("Empty trajectory received, canceling current trajectory");
00138     trajectoryStop();
00139     return;
00140   }
00141 
00142   // convert trajectory into robot-format
00143   std::vector<SimpleMessage> robot_msgs;
00144   if (!trajectory_to_msgs(msg, &robot_msgs))
00145     return;
00146 
00147   // send command messages to robot
00148   send_to_robot(robot_msgs);
00149 }
00150 
00151 bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<SimpleMessage>* msgs)
00152 {
00153   msgs->clear();
00154 
00155   // check for valid trajectory
00156   if (!is_valid(*traj))
00157     return false;
00158 
00159   for (size_t i=0; i<traj->points.size(); ++i)
00160   {
00161     SimpleMessage msg;
00162     ros_JointTrajPt rbt_pt, xform_pt;
00163 
00164     // select / reorder joints for sending to robot
00165     if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt))
00166       return false;
00167 
00168     // transform point data (e.g. for joint-coupling)
00169     if (!transform(rbt_pt, &xform_pt))
00170       return false;
00171 
00172     // convert trajectory point to ROS message
00173     if (!create_message(i, xform_pt, &msg))
00174       return false;
00175 
00176     msgs->push_back(msg);
00177   }
00178 
00179   return true;
00180 }
00181 
00182 bool JointTrajectoryInterface::select(const std::vector<std::string>& ros_joint_names, const ros_JointTrajPt& ros_pt,
00183                       const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
00184 {
00185   ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
00186 
00187   // initialize rbt_pt
00188   *rbt_pt = ros_pt;
00189   rbt_pt->positions.clear(); rbt_pt->velocities.clear(); rbt_pt->accelerations.clear();
00190 
00191   for (size_t rbt_idx=0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
00192   {
00193     bool is_empty = rbt_joint_names[rbt_idx].empty();
00194 
00195     // find matching ROS element
00196     size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(), rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
00197     bool is_found = ros_idx < ros_joint_names.size();
00198 
00199     // error-chk: required robot joint not found in ROS joint-list
00200     if (!is_empty && !is_found)
00201     {
00202       ROS_ERROR("Expected joint (%s) not found in JointTrajectory.  Aborting command.", rbt_joint_names[rbt_idx].c_str());
00203       return false;
00204     }
00205 
00206     if (is_empty)
00207     {
00208       if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(default_joint_pos_);
00209       if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
00210       if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
00211     }
00212     else
00213     {
00214       if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
00215       if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
00216       if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
00217     }
00218   }
00219   return true;
00220 }
00221 
00222 // default velocity calculation computes the %-of-max-velocity for the "critical joint" (closest to velocity-limit)
00223 // such that 0.2 = 20% of maximum joint speed.
00224 //
00225 // NOTE: this calculation uses the maximum joint speeds from the URDF file, which may differ from those defined on
00226 // the physical robot.  These differences could lead to different actual movement velocities than intended.
00227 // Behavior should be verified on a physical robot if movement velocity is critical.
00228 bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity)
00229 {
00230   std::vector<double> vel_ratios;
00231 
00232   ROS_ASSERT(all_joint_names_.size() == pt.positions.size());
00233 
00234   // check for empty velocities in ROS topic
00235   if (pt.velocities.empty())
00236   {
00237     ROS_WARN("Joint velocities unspecified.  Using default/safe speed.");
00238     *rbt_velocity = default_vel_ratio_;
00239     return true;
00240   }
00241 
00242   for (size_t i=0; i<all_joint_names_.size(); ++i)
00243   {
00244     const std::string &jnt_name = all_joint_names_[i];
00245 
00246     // update vel_ratios
00247     if (jnt_name.empty())                             // ignore "dummy joints" in velocity calcs
00248       vel_ratios.push_back(-1);
00249     else if (joint_vel_limits_.count(jnt_name) == 0)  // no velocity limit specified for this joint
00250       vel_ratios.push_back(-1);
00251     else
00252       vel_ratios.push_back( fabs(pt.velocities[i] / joint_vel_limits_[jnt_name]) );  // calculate expected duration for this joint
00253   }
00254 
00255   // find largest velocity-ratio (closest to max joint-speed)
00256   int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
00257   
00258   if (vel_ratios[max_idx] > 0)
00259     *rbt_velocity = vel_ratios[max_idx];
00260   else
00261   {
00262     ROS_WARN_ONCE("Joint velocity-limits unspecified.  Using default velocity-ratio.");
00263     *rbt_velocity = default_vel_ratio_;
00264   }
00265 
00266   if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) )
00267   {
00268     ROS_WARN("computed velocity (%.1f %%) is out-of-range.  Clipping to [0-100%%]", *rbt_velocity * 100);
00269     *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));  // clip to [0,1]
00270   }
00271   
00272   return true;
00273 }
00274 
00275 bool JointTrajectoryInterface::calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration)
00276 {
00277   std::vector<double> durations;
00278   double this_time = pt.time_from_start.toSec();
00279   static double last_time = 0;
00280 
00281   if (this_time <= last_time)  // earlier time => new trajectory.  Move slowly to first point.
00282     *rbt_duration = default_duration_;
00283   else
00284     *rbt_duration = this_time - last_time;
00285 
00286   last_time = this_time;
00287 
00288   return true;
00289 }
00290 
00291 bool JointTrajectoryInterface::create_message(int seq, const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage *msg)
00292 {
00293   industrial::joint_data::JointData pos;
00294   ROS_ASSERT(pt.positions.size() <= (unsigned int)pos.getMaxNumJoints());
00295 
00296   for (size_t i=0; i<pt.positions.size(); ++i)
00297     pos.setJoint(i, pt.positions[i]);
00298 
00299   // calculate velocity & duration
00300   double velocity, duration;
00301   if (!calc_velocity(pt, &velocity) || !calc_duration(pt, &duration))
00302     return false;
00303 
00304   rbt_JointTrajPt msg_data;
00305   msg_data.init(seq, pos, velocity, duration);
00306 
00307   JointTrajPtMessage jtp_msg;
00308   jtp_msg.init(msg_data);
00309 
00310   return jtp_msg.toTopic(*msg);  // assume "topic" COMM_TYPE for now
00311 }
00312 
00313 void JointTrajectoryInterface::trajectoryStop()
00314 {
00315   JointTrajPtMessage jMsg;
00316   SimpleMessage msg, reply;
00317 
00318   ROS_INFO("Joint trajectory handler: entering stopping state");
00319   jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
00320   jMsg.toRequest(msg);
00321   ROS_DEBUG("Sending stop command");
00322   this->connection_->sendAndReceiveMsg(msg, reply);
00323 }
00324 
00325 bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
00326                                             industrial_msgs::StopMotion::Response &res)
00327 {
00328   trajectoryStop();
00329 
00330   // no success/fail result from trajectoryStop.  Assume success.
00331   res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
00332 
00333   return true;  // always return true.  To distinguish between call-failed and service-unavailable.
00334 }
00335 
00336 bool JointTrajectoryInterface::is_valid(const trajectory_msgs::JointTrajectory &traj)
00337 {
00338   for (int i=0; i<traj.points.size(); ++i)
00339   {
00340     const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
00341 
00342     // check for non-empty positions
00343     if (pt.positions.empty())
00344       ROS_ERROR_RETURN(false, "Validation failed: Missing position data for trajectory pt %d", i);
00345 
00346     // check for joint velocity limits
00347     for (int j=0; j<pt.velocities.size(); ++j)
00348     {
00349       std::map<std::string, double>::iterator max_vel = joint_vel_limits_.find(traj.joint_names[j]);
00350       if (max_vel == joint_vel_limits_.end()) continue;  // no velocity-checking if limit not defined
00351 
00352       if (std::abs(pt.velocities[j]) > max_vel->second)
00353         ROS_ERROR_RETURN(false, "Validation failed: Max velocity exceeded for trajectory pt %d, joint '%s'", i, traj.joint_names[j].c_str());
00354     }
00355 
00356     // check for valid timestamp
00357     if ((i > 0) && (pt.time_from_start.toSec() == 0))
00358       ROS_ERROR_RETURN(false, "Validation failed: Missing valid timestamp data for trajectory pt %d", i);
00359   }
00360 
00361   return true;
00362 }
00363 
00364 // copy robot JointState into local cache
00365 void JointTrajectoryInterface::jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
00366 {
00367   this->cur_joint_pos_ = *msg;
00368 }
00369 
00370 } //joint_trajectory_interface
00371 } //industrial_robot_client
00372 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


fs100
Author(s): Jeremy Zoss (Southwest Research Institute)
autogenerated on Mon May 27 2013 19:43:14