51 namespace joint_trajectory_interface
54 #define ROS_ERROR_RETURN(rtn, ...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while (0) // NOLINT(whitespace/braces) 62 ros::param::param<std::string>(
"robot_ip_address", ip, default_ip);
63 ros::param::param<int>(
"~port", port, default_port);
68 ROS_ERROR(
"No valid robot IP address found. Please set ROS 'robot_ip_address' param");
73 ROS_ERROR(
"No valid robot TCP port found. Please set ROS '~port' param");
77 char* ip_addr = strdup(ip.c_str());
78 ROS_INFO(
"Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
87 std::map<int, RobotGroup> robot_groups;
90 this->version_0_ =
false;
91 return init(connection, robot_groups);
95 this->version_0_ =
true;
96 std::vector<std::string> joint_names;
97 if (!
getJointNames(
"controller_joint_names",
"robot_description", joint_names))
99 ROS_WARN(
"Unable to read 'controller_joint_names' param. Using standard 6-DOF joint names.");
101 return init(connection, joint_names);
107 const std::map<std::string, double> &velocity_limits)
117 ROS_WARN(
"Unable to read velocity limits from 'robot_description' param. Velocity validation disabled.");
133 SmplMsgConnection* connection,
const std::map<int, RobotGroup> &robot_groups,
134 const std::map<std::string, double> &velocity_limits)
137 this->robot_groups_ = robot_groups;
145 ROS_WARN(
"Unable to read velocity limits from 'robot_description' param. Velocity validation disabled.");
149 "joint_path_command", &JointTrajectoryInterface::jointTrajectoryExCB,
this);
151 "joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryExCB,
this);
155 for (it_type iterator = this->robot_groups_.begin(); iterator != this->robot_groups_.end(); iterator++)
157 std::string name_str, ns_str;
158 int robot_id = iterator->first;
159 name_str = iterator->second.get_name();
160 ns_str = iterator->second.get_ns();
167 ns_str +
"/" + name_str +
"/stop_motion",
170 ns_str +
"/" + name_str +
"/joint_path_command",
171 &JointTrajectoryInterface::jointTrajectoryExCB,
this);
173 ns_str +
"/" + name_str +
"/joint_path_command", 0,
174 &JointTrajectoryInterface::jointTrajectoryExCB,
this);
176 this->srv_stops_[robot_id] = srv_stop_motion;
177 this->srv_joints_[robot_id] = srv_joint_trajectory;
178 this->sub_joint_trajectories_[robot_id] = sub_joint_trajectory;
181 ns_str +
"/" + name_str +
"/joint_states", 1,
184 this->sub_cur_positions_[robot_id] = this->
sub_cur_pos_;
197 bool JointTrajectoryInterface::jointTrajectoryExCB(
198 motoman_msgs::CmdJointTrajectoryEx::Request &req,
199 motoman_msgs::CmdJointTrajectoryEx::Response &res)
201 motoman_msgs::DynamicJointTrajectoryPtr traj_ptr(
202 new motoman_msgs::DynamicJointTrajectory);
203 *traj_ptr = req.trajectory;
204 this->jointTrajectoryExCB(traj_ptr);
207 res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
214 industrial_msgs::CmdJointTrajectory::Request &req,
215 industrial_msgs::CmdJointTrajectory::Response &res)
217 trajectory_msgs::JointTrajectoryPtr traj_ptr(
218 new trajectory_msgs::JointTrajectory);
219 *traj_ptr = req.trajectory;
222 res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
227 void JointTrajectoryInterface::jointTrajectoryExCB(
228 const motoman_msgs::DynamicJointTrajectoryConstPtr &msg)
230 ROS_INFO(
"Receiving joint trajectory message Dynamic");
233 if (msg->points.empty())
235 ROS_INFO(
"Empty trajectory received, canceling current trajectory");
241 std::vector<SimpleMessage> robot_msgs;
251 const trajectory_msgs::JointTrajectoryConstPtr &msg)
253 ROS_INFO(
"Receiving joint trajectory message");
256 if (msg->points.empty())
258 ROS_INFO(
"Empty trajectory received, canceling current trajectory");
264 std::vector<SimpleMessage> robot_msgs;
273 const motoman_msgs::DynamicJointTrajectoryConstPtr& traj,
274 std::vector<SimpleMessage>* msgs)
278 std::vector<double>::iterator it;
280 if (traj->points[0].num_groups == 1)
288 for (
size_t i = 0; i < traj->points.size(); ++i)
294 if (!
select(traj->joint_names, traj->points[i].groups[0],
295 robot_groups_[traj->points[i].groups[0].group_number].get_joint_names(),
307 msgs->push_back(msg);
311 else if (traj->points[0].num_groups <= 4)
313 for (
size_t i = 0; i < traj->points.size(); ++i)
316 create_message_ex(i, traj->points[i], &msg);
317 msgs->push_back(msg);
324 const trajectory_msgs::JointTrajectoryConstPtr& traj,
325 std::vector<SimpleMessage>* msgs)
333 for (
size_t i = 0; i < traj->points.size(); ++i)
339 if (!
select(traj->joint_names, traj->points[i],
340 this->all_joint_names_, &rbt_pt))
351 msgs->push_back(msg);
358 const std::vector<std::string>& ros_joint_names,
362 ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
365 rbt_pt->positions.clear();
366 rbt_pt->velocities.clear();
367 rbt_pt->accelerations.clear();
369 for (
size_t rbt_idx = 0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
371 bool is_empty = rbt_joint_names[rbt_idx].empty();
374 size_t ros_idx = std::find(ros_joint_names.begin(),
375 ros_joint_names.end(),
376 rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
377 bool is_found = ros_idx < ros_joint_names.size();
381 if (!is_empty && !is_found)
383 ROS_ERROR(
"Expected joint (%s) not found in JointTrajectory.Aborting command.",
384 rbt_joint_names[rbt_idx].c_str());
391 if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
392 if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
396 if (!ros_pt.positions.empty())
397 rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
398 if (!ros_pt.velocities.empty())
399 rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
400 if (!ros_pt.accelerations.empty())
401 rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
409 const std::vector<std::string>& ros_joint_names,
410 const ros_JointTrajPt& ros_pt,
const std::vector<std::string>& rbt_joint_names,
413 ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
416 rbt_pt->positions.clear();
417 rbt_pt->velocities.clear();
418 rbt_pt->accelerations.clear();
420 for (
size_t rbt_idx = 0; rbt_idx < rbt_joint_names.size(); ++rbt_idx)
422 bool is_empty = rbt_joint_names[rbt_idx].empty();
425 size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(),
426 rbt_joint_names[rbt_idx]) - ros_joint_names.begin();
427 bool is_found = ros_idx < ros_joint_names.size();
430 if (!is_empty && !is_found)
432 ROS_ERROR(
"Expected joint (%s) not found in JointTrajectory. Aborting command.",
433 rbt_joint_names[rbt_idx].c_str());
440 if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1);
441 if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1);
445 if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]);
446 if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]);
447 if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]);
460 const trajectory_msgs::JointTrajectoryPoint& pt,
double* rbt_velocity)
462 std::vector<double> vel_ratios;
467 if (pt.velocities.empty())
469 ROS_WARN(
"Joint velocities unspecified. Using default/safe speed.");
479 if (jnt_name.empty())
480 vel_ratios.push_back(-1);
482 vel_ratios.push_back(-1);
484 vel_ratios.push_back(
489 int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
491 if (vel_ratios[max_idx] > 0)
492 *rbt_velocity = vel_ratios[max_idx];
495 ROS_WARN_ONCE(
"Joint velocity-limits unspecified. Using default velocity-ratio.");
499 if ((*rbt_velocity < 0) || (*rbt_velocity > 1))
501 ROS_WARN(
"computed velocity (%.1f %%) is out-of-range. Clipping to [0-100%%]", *rbt_velocity * 100);
502 *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));
510 std::vector<double> vel_ratios;
513 if (pt.velocities.empty())
515 ROS_WARN(
"Joint velocities unspecified. Using default/safe speed.");
520 robot_groups_[pt.group_number].get_joint_names();
521 for (
size_t i = 0; i < robot_groups_[pt.group_number].get_joint_names().size(); ++i)
523 const std::string &jnt_name = robot_groups_[pt.group_number].get_joint_names()[i];
526 if (jnt_name.empty())
527 vel_ratios.push_back(-1);
529 vel_ratios.push_back(-1);
531 vel_ratios.push_back(
536 int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
538 if (vel_ratios[max_idx] > 0)
539 *rbt_velocity = vel_ratios[max_idx];
542 ROS_WARN_ONCE(
"Joint velocity-limits unspecified. Using default velocity-ratio.");
546 if ((*rbt_velocity < 0) || (*rbt_velocity > 1))
548 ROS_WARN(
"computed velocity (%.1f %%) is out-of-range. Clipping to [0-100%%]", *rbt_velocity * 100);
549 *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));
556 (
const trajectory_msgs::JointTrajectoryPoint& pt,
double* rbt_duration)
558 std::vector<double> durations;
559 double this_time = pt.time_from_start.toSec();
560 static double last_time = 0;
562 if (this_time <= last_time)
565 *rbt_duration = this_time - last_time;
567 last_time = this_time;
573 const motoman_msgs::DynamicJointsGroup& pt,
double* rbt_duration)
575 std::vector<double> durations;
576 double this_time = pt.time_from_start.toSec();
577 static double last_time = 0;
579 if (this_time <= last_time)
582 *rbt_duration = this_time - last_time;
584 last_time = this_time;
590 int seq,
const motoman_msgs::DynamicJointsGroup &pt,
SimpleMessage *msg)
594 for (
size_t i = 0; i < pt.positions.size(); ++i)
600 double velocity, duration;
605 msg_data.
init(seq, pos, velocity, duration);
607 JointTrajPtMessage jtp_msg;
608 jtp_msg.init(msg_data);
610 return jtp_msg.toTopic(*msg);
613 bool JointTrajectoryInterface::create_message_ex(
614 int seq,
const motoman_msgs::DynamicJointPoint &pt,
SimpleMessage *msg)
620 int seq,
const trajectory_msgs::JointTrajectoryPoint &pt,
SimpleMessage *msg)
625 for (
size_t i = 0; i < pt.positions.size(); ++i)
629 double velocity, duration;
634 msg_data.
init(seq, pos, velocity, duration);
636 JointTrajPtMessage jtp_msg;
637 jtp_msg.init(msg_data);
639 return jtp_msg.toTopic(*msg);
644 JointTrajPtMessage jMsg;
647 ROS_INFO(
"Joint trajectory handler: entering stopping state");
655 industrial_msgs::StopMotion::Request &req,
656 industrial_msgs::StopMotion::Response &res)
661 res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;
668 for (
size_t i = 0; i < traj.points.size(); ++i)
670 const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
673 if (pt.positions.empty())
674 ROS_ERROR_RETURN(
false,
"Validation failed: Missing position data for trajectory pt %lu", i);
677 for (
size_t j = 0; j < pt.velocities.size(); ++j)
679 std::map<std::string, double>::iterator max_vel =
joint_vel_limits_.find(traj.joint_names[j]);
682 if (std::abs(pt.velocities[j]) > max_vel->second)
683 ROS_ERROR_RETURN(
false,
"Validation failed: Max velocity exceeded for trajectory pt %lu, joint '%s'", i,
684 traj.joint_names[j].c_str());
688 if ((i > 0) && (pt.time_from_start.toSec() == 0))
689 ROS_ERROR_RETURN(
false,
"Validation failed: Missing valid timestamp data for trajectory pt %lu", i);
697 for (
size_t i = 0; i < traj.points.size(); ++i)
699 for (
int gr = 0; gr < traj.points[i].num_groups; gr++)
701 const motoman_msgs::DynamicJointsGroup &pt = traj.points[i].groups[gr];
704 if (pt.positions.empty())
705 ROS_ERROR_RETURN(
false,
"Validation failed: Missing position data for trajectory pt %lu", i);
707 for (
size_t j = 0; j < pt.velocities.size(); ++j)
709 std::map<std::string, double>::iterator max_vel =
joint_vel_limits_.find(traj.joint_names[j]);
712 if (std::abs(pt.velocities[j]) > max_vel->second)
713 ROS_ERROR_RETURN(
false,
"Validation failed: Max velocity exceeded for trajectory pt %lu, joint '%s'", i,
714 traj.joint_names[j].c_str());
718 if ((i > 0) && (pt.time_from_start.toSec() == 0))
719 ROS_ERROR_RETURN(
false,
"Validation failed: Missing valid timestamp data for trajectory pt %lu", i);
727 const sensor_msgs::JointStateConstPtr &msg)
733 const sensor_msgs::JointStateConstPtr &msg,
int robot_id)
735 this->cur_joint_pos_map_[robot_id] = *msg;
ros::Subscriber sub_cur_pos_
bool init(char *buff, int port_num)
bool getJointNames(const std::string joint_list_param, const std::string urdf_param, std::vector< std::string > &joint_names)
#define ROS_ERROR_RETURN(rtn,...)
virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity)
bool getJointGroups(const std::string topic_param, std::map< int, RobotGroup > &robot_groups)
Parses multi-group joints from topic_param.
ros::ServiceServer srv_joint_trajectory_
std::vector< std::string > all_joint_names_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual bool transform(const trajectory_msgs::JointTrajectoryPoint &pt_in, trajectory_msgs::JointTrajectoryPoint *pt_out)
virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_duration)
SmplMsgConnection * connection_
sensor_msgs::JointState cur_joint_pos_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer srv_stop_motion_
virtual bool send_to_robot(const std::vector< JointTrajPtMessage > &messages)=0
bool sendAndReceiveMsg(industrial::simple_message::SimpleMessage &send, industrial::simple_message::SimpleMessage &recv, bool verbose=false)
double default_joint_pos_
static JointTrajPtMessage create_message(int seq, std::vector< double > joint_pos, double velocity, double duration)
double default_vel_ratio_
#define ROS_WARN_ONCE(...)
bool setJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real value)
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
virtual bool select(const std::vector< std::string > &ros_joint_names, const trajectory_msgs::JointTrajectoryPoint &ros_pt, const std::vector< std::string > &rbt_joint_names, trajectory_msgs::JointTrajectoryPoint *rbt_pt)
virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
std::map< std::string, double > joint_vel_limits_
virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj)
virtual void trajectoryStop()
bool getJointVelocityLimits(const std::string urdf_param_name, std::map< std::string, double > &velocity_limits)
motoman_msgs::DynamicJointsGroup ros_dynamicPoint
ros::Subscriber sub_joint_trajectory_
trajectory_msgs::JointTrajectoryPoint ros_JointTrajPt
int getMaxNumJoints() const
virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res)
virtual ~JointTrajectoryInterface()
TcpClient default_tcp_connection_
industrial::joint_traj_pt::JointTrajPt rbt_JointTrajPt
virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)
virtual bool makeConnect()=0