joint_trajectory_interface.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include <algorithm>
36 
37 using namespace industrial_utils::param;
42 typedef trajectory_msgs::JointTrajectoryPoint ros_JointTrajPt;
43 
45 {
46 namespace joint_trajectory_interface
47 {
48 
49 #define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0)
50 
51 bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
52 {
53  std::string ip;
54  int port;
55 
56  // override IP/port with ROS params, if available
57  ros::param::param<std::string>("robot_ip_address", ip, default_ip);
58  ros::param::param<int>("~port", port, default_port);
59 
60  // check for valid parameter values
61  if (ip.empty())
62  {
63  ROS_ERROR("No valid robot IP address found. Please set ROS 'robot_ip_address' param");
64  return false;
65  }
66  if (port <= 0)
67  {
68  ROS_ERROR("No valid robot IP port found. Please set ROS '~port' param");
69  return false;
70  }
71 
72  char* ip_addr = strdup(ip.c_str()); // connection.init() requires "char*", not "const char*"
73  ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
74  default_tcp_connection_.init(ip_addr, port);
75  free(ip_addr);
76 
77  return init(&default_tcp_connection_);
78 }
79 
80 bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
81 {
82  std::vector<std::string> joint_names;
83  if (!getJointNames("controller_joint_names", "robot_description", joint_names))
84  {
85  ROS_ERROR("Failed to initialize joint_names. Aborting");
86  return false;
87  }
88 
89  return init(connection, joint_names);
90 }
91 
92 bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
93  const std::map<std::string, double> &velocity_limits)
94 {
95  this->connection_ = connection;
96  this->all_joint_names_ = joint_names;
97  this->joint_vel_limits_ = velocity_limits;
98  connection_->makeConnect();
99 
100  // try to read velocity limits from URDF, if none specified
101  if (joint_vel_limits_.empty() && !industrial_utils::param::getJointVelocityLimits("robot_description", joint_vel_limits_))
102  ROS_WARN("Unable to read velocity limits from 'robot_description' param. Velocity validation disabled.");
103 
104  this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this);
105  this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
106  this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
107  this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);
108 
109  return true;
110 }
111 
112 JointTrajectoryInterface::~JointTrajectoryInterface()
113 {
114  trajectoryStop();
115  this->sub_joint_trajectory_.shutdown();
116 }
117 
118 bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
119  industrial_msgs::CmdJointTrajectory::Response &res)
120 {
121  trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
122  *traj_ptr = req.trajectory; // copy message data
123  this->jointTrajectoryCB(traj_ptr);
124 
125  // no success/fail result from jointTrajectoryCB. Assume success.
126  res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
127 
128  return true; // always return true. To distinguish between call-failed and service-unavailable.
129 }
130 
131 void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
132 {
133  ROS_INFO("Receiving joint trajectory message");
134 
135  // check for STOP command
136  if (msg->points.empty())
137  {
138  ROS_INFO("Empty trajectory received, canceling current trajectory");
139  trajectoryStop();
140  return;
141  }
142 
143  // convert trajectory into robot-format
144  std::vector<JointTrajPtMessage> robot_msgs;
145  if (!trajectory_to_msgs(msg, &robot_msgs))
146  return;
147 
148  // send command messages to robot
149  send_to_robot(robot_msgs);
150 }
151 
152 bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<JointTrajPtMessage>* msgs)
153 {
154  msgs->clear();
155 
156  // check for valid trajectory
157  if (!is_valid(*traj))
158  return false;
159 
160  for (size_t i=0; i<traj->points.size(); ++i)
161  {
162  ros_JointTrajPt rbt_pt, xform_pt;
163  double vel, duration;
164 
165  // select / reorder joints for sending to robot
166  if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt))
167  return false;
168 
169  // transform point data (e.g. for joint-coupling)
170  if (!transform(rbt_pt, &xform_pt))
171  return false;
172 
173  // reduce velocity to a single scalar, for robot command
174  if (!calc_speed(xform_pt, &vel, &duration))
175  return false;
176 
177  JointTrajPtMessage msg = create_message(i, xform_pt.positions, vel, duration);
178  msgs->push_back(msg);
179  }
180 
181  return true;
182 }
183 
184 bool JointTrajectoryInterface::select(const std::vector<std::string>& ros_joint_names, const ros_JointTrajPt& ros_pt,
185  const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
186 {
187  ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
188 
189  // initialize rbt_pt
190  *rbt_pt = ros_pt;
191  rbt_pt->positions.clear(); rbt_pt->velocities.clear(); rbt_pt->accelerations.clear();
192 
193  for (size_t rbt_idx=0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
194  {
195  bool is_empty = rbt_joint_names[rbt_idx].empty();
196 
197  // find matching ROS element
198  size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(), rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
199  bool is_found = ros_idx < ros_joint_names.size();
200 
201  // error-chk: required robot joint not found in ROS joint-list
202  if (!is_empty && !is_found)
203  {
204  ROS_ERROR("Expected joint (%s) not found in JointTrajectory. Aborting command.", rbt_joint_names[rbt_idx].c_str());
205  return false;
206  }
207 
208  if (is_empty)
209  {
210  if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(default_joint_pos_);
211  if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
212  if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
213  }
214  else
215  {
216  if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
217  if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
218  if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
219  }
220  }
221  return true;
222 }
223 
224 bool JointTrajectoryInterface::calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration)
225 {
226  return calc_velocity(pt, rbt_velocity) && calc_duration(pt, rbt_duration);
227 }
228 
229 // default velocity calculation computes the %-of-max-velocity for the "critical joint" (closest to velocity-limit)
230 // such that 0.2 = 20% of maximum joint speed.
231 //
232 // NOTE: this calculation uses the maximum joint speeds from the URDF file, which may differ from those defined on
233 // the physical robot. These differences could lead to different actual movement velocities than intended.
234 // Behavior should be verified on a physical robot if movement velocity is critical.
235 bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity)
236 {
237  std::vector<double> vel_ratios;
238 
239  ROS_ASSERT(all_joint_names_.size() == pt.positions.size());
240 
241  // check for empty velocities in ROS topic
242  if (pt.velocities.empty())
243  {
244  ROS_WARN("Joint velocities unspecified. Using default/safe speed.");
245  *rbt_velocity = default_vel_ratio_;
246  return true;
247  }
248 
249  for (size_t i=0; i<all_joint_names_.size(); ++i)
250  {
251  const std::string &jnt_name = all_joint_names_[i];
252 
253  // update vel_ratios
254  if (jnt_name.empty()) // ignore "dummy joints" in velocity calcs
255  vel_ratios.push_back(-1);
256  else if (joint_vel_limits_.count(jnt_name) == 0) // no velocity limit specified for this joint
257  vel_ratios.push_back(-1);
258  else
259  vel_ratios.push_back( fabs(pt.velocities[i] / joint_vel_limits_[jnt_name]) ); // calculate expected duration for this joint
260  }
261 
262  // find largest velocity-ratio (closest to max joint-speed)
263  int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
264 
265  if (vel_ratios[max_idx] > 0)
266  *rbt_velocity = vel_ratios[max_idx];
267  else
268  {
269  ROS_WARN_ONCE("Joint velocity-limits unspecified. Using default velocity-ratio.");
270  *rbt_velocity = default_vel_ratio_;
271  }
272 
273  if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) )
274  {
275  ROS_WARN("computed velocity (%.1f %%) is out-of-range. Clipping to [0-100%%]", *rbt_velocity * 100);
276  *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity)); // clip to [0,1]
277  }
278 
279  return true;
280 }
281 
282 bool JointTrajectoryInterface::calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration)
283 {
284  std::vector<double> durations;
285  double this_time = pt.time_from_start.toSec();
286  static double last_time = 0;
287 
288  if (this_time <= last_time) // earlier time => new trajectory. Move slowly to first point.
289  *rbt_duration = default_duration_;
290  else
291  *rbt_duration = this_time - last_time;
292 
293  last_time = this_time;
294 
295  return true;
296 }
297 
298 JointTrajPtMessage JointTrajectoryInterface::create_message(int seq, std::vector<double> joint_pos, double velocity, double duration)
299 {
301  ROS_ASSERT(joint_pos.size() <= (unsigned int)pos.getMaxNumJoints());
302 
303  for (size_t i=0; i<joint_pos.size(); ++i)
304  pos.setJoint(i, joint_pos[i]);
305 
306  rbt_JointTrajPt pt;
307  pt.init(seq, pos, velocity, duration);
308 
309  JointTrajPtMessage msg;
310  msg.init(pt);
311 
312  return msg;
313 }
314 
315 void JointTrajectoryInterface::trajectoryStop()
316 {
317  JointTrajPtMessage jMsg;
318  SimpleMessage msg, reply;
319 
320  ROS_INFO("Joint trajectory handler: entering stopping state");
321  jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
322  jMsg.toRequest(msg);
323  ROS_DEBUG("Sending stop command");
324  this->connection_->sendAndReceiveMsg(msg, reply);
325 }
326 
327 bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
328  industrial_msgs::StopMotion::Response &res)
329 {
330  trajectoryStop();
331 
332  // no success/fail result from trajectoryStop. Assume success.
333  res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
334 
335  return true; // always return true. To distinguish between call-failed and service-unavailable.
336 }
337 
338 bool JointTrajectoryInterface::is_valid(const trajectory_msgs::JointTrajectory &traj)
339 {
340  for (int i=0; i<traj.points.size(); ++i)
341  {
342  const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
343 
344  // check for non-empty positions
345  if (pt.positions.empty())
346  ROS_ERROR_RETURN(false, "Validation failed: Missing position data for trajectory pt %d", i);
347 
348  // check for joint velocity limits
349  for (int j=0; j<pt.velocities.size(); ++j)
350  {
351  std::map<std::string, double>::iterator max_vel = joint_vel_limits_.find(traj.joint_names[j]);
352  if (max_vel == joint_vel_limits_.end()) continue; // no velocity-checking if limit not defined
353 
354  if (std::abs(pt.velocities[j]) > max_vel->second)
355  ROS_ERROR_RETURN(false, "Validation failed: Max velocity exceeded for trajectory pt %d, joint '%s'", i, traj.joint_names[j].c_str());
356  }
357 
358  // check for valid timestamp
359  if ((i > 0) && (pt.time_from_start.toSec() == 0))
360  ROS_ERROR_RETURN(false, "Validation failed: Missing valid timestamp data for trajectory pt %d", i);
361  }
362 
363  return true;
364 }
365 
366 // copy robot JointState into local cache
367 void JointTrajectoryInterface::jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
368 {
369  this->cur_joint_pos_ = *msg;
370 }
371 
372 } //joint_trajectory_interface
373 } //industrial_robot_client
374 
industrial::joint_data::JointData
industrial::joint_traj_pt::JointTrajPt
industrial::joint_traj_pt_message::JointTrajPtMessage::setSequence
void setSequence(industrial::shared_types::shared_int sequence)
ROS_DEBUG
#define ROS_DEBUG(...)
industrial_utils::param::getJointNames
bool getJointNames(const std::string joint_list_param, const std::string urdf_param, std::vector< std::string > &joint_names)
industrial::joint_traj_pt_message::JointTrajPtMessage::init
void init()
ROS_WARN_ONCE
#define ROS_WARN_ONCE(...)
ros_JointTrajPt
trajectory_msgs::JointTrajectoryPoint ros_JointTrajPt
Definition: joint_trajectory_interface.cpp:42
param_utils.h
joint_traj_pt.h
rbt_JointTrajPt
industrial::joint_traj_pt::JointTrajPt rbt_JointTrajPt
Definition: joint_trajectory_interface.cpp:41
industrial::joint_traj_pt::SpecialSeqValues
industrial::simple_message::SimpleMessage
industrial::typed_message::TypedMessage::toRequest
virtual bool toRequest(industrial::simple_message::SimpleMessage &msg)
industrial_utils::param
industrial::simple_socket::StandardSocketPorts
ROS_ERROR
#define ROS_ERROR(...)
ROS_WARN
#define ROS_WARN(...)
joint_trajectory_interface.h
industrial::joint_traj_pt::JointTrajPt::init
void init()
industrial::smpl_msg_connection::SmplMsgConnection
industrial::smpl_msg_connection::SmplMsgConnection::makeConnect
virtual bool makeConnect()=0
industrial_robot_client
Definition: joint_relay_handler.h:46
industrial::joint_traj_pt_message::JointTrajPtMessage
ROS_ERROR_RETURN
#define ROS_ERROR_RETURN(rtn,...)
Definition: joint_trajectory_interface.cpp:49
industrial::joint_data::JointData::getMaxNumJoints
int getMaxNumJoints() const
industrial_utils::param::getJointVelocityLimits
bool getJointVelocityLimits(const std::string urdf_param_name, std::map< std::string, double > &velocity_limits)
init
void init(const M_string &remappings)
ROS_INFO
#define ROS_INFO(...)
ROS_ASSERT
#define ROS_ASSERT(cond)
industrial::joint_data::JointData::setJoint
bool setJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real value)


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Wed Mar 2 2022 00:24:59