47 namespace joint_trajectory_action
50 const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0;
54 action_server_(node_,
"joint_trajectory_action",
55 boost::bind(&JointTrajectoryAction::goalCB, this, _1),
56 boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false)
60 pn.param(
"constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_);
62 std::map<int, RobotGroup> robot_groups;
65 for (
size_t i = 0; i < robot_groups.size(); i++)
67 std::string joint_path_action_name = robot_groups[i].get_ns() +
"/" + robot_groups[i].get_name();
68 std::vector<std::string> rg_joint_names = robot_groups[i].get_joint_names();
69 int group_number_int = robot_groups[i].get_group_id();
71 all_joint_names_.insert(all_joint_names_.end(), rg_joint_names.begin(), rg_joint_names.end());
74 node_, joint_path_action_name +
"/joint_trajectory_action" ,
false);
76 boost::bind(&JointTrajectoryAction::goalCB,
77 this, _1, group_number_int));
78 actionServer_->registerCancelCallback(
79 boost::bind(&JointTrajectoryAction::cancelCB,
80 this, _1, group_number_int));
82 pub_trajectory_command_ = node_.advertise<motoman_msgs::DynamicJointTrajectory>(
83 joint_path_action_name +
"/joint_path_command", 1);
84 sub_trajectory_state_ = this->node_.subscribe<control_msgs::FollowJointTrajectoryFeedback>(
85 joint_path_action_name +
"/feedback_states", 1,
86 boost::bind(&JointTrajectoryAction::controllerStateCB,
87 this, _1, group_number_int));
88 sub_robot_status_ = node_.subscribe(
89 "robot_status", 1, &JointTrajectoryAction::robotStatusCB,
this);
91 pub_trajectories_[group_number_int] = pub_trajectory_command_;
92 sub_trajectories_[group_number_int] = (sub_trajectory_state_);
93 sub_status_[group_number_int] = (sub_robot_status_);
95 this->act_servers_[group_number_int] = actionServer_;
97 this->act_servers_[group_number_int]->
start();
99 this->watchdog_timer_map_[group_number_int] = node_.createTimer(
101 &JointTrajectoryAction::watchdog,
this, _1, group_number_int));
104 pub_trajectory_command_ = node_.advertise<motoman_msgs::DynamicJointTrajectory>(
105 "joint_path_command", 1);
107 this->robot_groups_ = robot_groups;
109 action_server_.
start();
112 JointTrajectoryAction::~JointTrajectoryAction()
116 void JointTrajectoryAction::robotStatusCB(
117 const industrial_msgs::RobotStatusConstPtr &msg)
119 last_robot_status_ = msg;
125 if (!last_trajectory_state_)
127 ROS_DEBUG(
"Waiting for subscription to joint trajectory state");
129 if (!trajectory_state_recvd_)
131 ROS_DEBUG(
"Trajectory state not received since last watchdog");
135 if (has_active_goal_)
137 if (!trajectory_state_recvd_)
140 if (!last_trajectory_state_)
142 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
147 "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ <<
" seconds");
154 trajectory_state_recvd_ =
false;
157 void JointTrajectoryAction::watchdog(
const ros::TimerEvent &e,
int group_number)
160 if (!last_trajectory_state_map_[group_number])
162 ROS_DEBUG(
"Waiting for subscription to joint trajectory state");
164 if (!trajectory_state_recvd_map_[group_number])
166 ROS_DEBUG(
"Trajectory state not received since last watchdog");
170 if (has_active_goal_map_[group_number])
172 if (!trajectory_state_recvd_map_[group_number])
175 if (!last_trajectory_state_map_[group_number])
177 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
182 "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ <<
" seconds");
184 abortGoal(group_number);
188 trajectory_state_recvd_ =
false;
191 void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
201 motoman_msgs::DynamicJointTrajectory dyn_traj;
203 for (
size_t i = 0; i < gh.getGoal()->trajectory.points.size(); i++)
205 motoman_msgs::DynamicJointPoint dpoint;
207 for (
size_t rbt_idx = 0; rbt_idx < robot_groups_.size(); rbt_idx++)
209 size_t ros_idx = std::find(
210 gh.getGoal()->trajectory.joint_names.begin(),
211 gh.getGoal()->trajectory.joint_names.end(),
212 robot_groups_[rbt_idx].get_joint_names()[0])
213 - gh.getGoal()->trajectory.joint_names.begin();
215 bool is_found = ros_idx < gh.getGoal()->trajectory.joint_names.size();
217 group_number = rbt_idx;
218 motoman_msgs::DynamicJointsGroup dyn_group;
220 int num_joints = robot_groups_[group_number].get_joint_names().size();
224 if (gh.getGoal()->trajectory.points[i].positions.empty())
226 std::vector<double> positions(num_joints, 0.0);
227 dyn_group.positions = positions;
230 dyn_group.positions.insert(
231 dyn_group.positions.begin(),
232 gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx,
233 gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx
234 + robot_groups_[rbt_idx].get_joint_names().size());
236 if (gh.getGoal()->trajectory.points[i].velocities.empty())
238 std::vector<double> velocities(num_joints, 0.0);
239 dyn_group.velocities = velocities;
242 dyn_group.velocities.insert(
243 dyn_group.velocities.begin(),
244 gh.getGoal()->trajectory.points[i].velocities.begin()
245 + ros_idx, gh.getGoal()->trajectory.points[i].velocities.begin()
246 + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
248 if (gh.getGoal()->trajectory.points[i].accelerations.empty())
250 std::vector<double> accelerations(num_joints, 0.0);
251 dyn_group.accelerations = accelerations;
254 dyn_group.accelerations.insert(
255 dyn_group.accelerations.begin(),
256 gh.getGoal()->trajectory.points[i].accelerations.begin()
257 + ros_idx, gh.getGoal()->trajectory.points[i].accelerations.begin()
258 + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
259 if (gh.getGoal()->trajectory.points[i].effort.empty())
261 std::vector<double> effort(num_joints, 0.0);
262 dyn_group.effort = effort;
265 dyn_group.effort.insert(
266 dyn_group.effort.begin(),
267 gh.getGoal()->trajectory.points[i].effort.begin()
268 + ros_idx, gh.getGoal()->trajectory.points[i].effort.begin()
269 + ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
270 dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
271 dyn_group.group_number = group_number;
272 dyn_group.num_joints = dyn_group.positions.size();
278 std::vector<double> positions(num_joints, 0.0);
279 std::vector<double> velocities(num_joints, 0.0);
280 std::vector<double> accelerations(num_joints, 0.0);
281 std::vector<double> effort(num_joints, 0.0);
283 dyn_group.positions = positions;
284 dyn_group.velocities = velocities;
285 dyn_group.accelerations = accelerations;
286 dyn_group.effort = effort;
288 dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
289 dyn_group.group_number = group_number;
290 dyn_group.num_joints = num_joints;
293 dpoint.groups.push_back(dyn_group);
295 dpoint.num_groups = dpoint.groups.size();
296 dyn_traj.points.push_back(dpoint);
298 dyn_traj.header = gh.getGoal()->trajectory.header;
301 dyn_traj.joint_names = all_joint_names_;
303 this->pub_trajectory_command_.publish(dyn_traj);
306 void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle gh)
311 ROS_DEBUG(
"Received action cancel request");
314 void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh,
int group_number)
316 if (!gh.getGoal()->trajectory.points.empty())
319 this->robot_groups_[group_number].get_joint_names(),
320 gh.getGoal()->trajectory.joint_names))
323 if (has_active_goal_map_[group_number])
325 ROS_WARN(
"Received new goal, canceling current goal");
326 abortGoal(group_number);
329 if (withinGoalConstraints(last_trajectory_state_map_[group_number],
330 gh.getGoal()->trajectory, group_number))
332 ROS_INFO_STREAM(
"Already within goal constraints, setting goal succeeded");
335 has_active_goal_map_[group_number] =
false;
340 active_goal_map_[group_number] = gh;
341 has_active_goal_map_[group_number] =
true;
345 current_traj_map_[group_number] = active_goal_map_[group_number].getGoal()->trajectory;
347 int num_joints = robot_groups_[group_number].get_joint_names().size();
349 motoman_msgs::DynamicJointTrajectory dyn_traj;
351 for (
size_t i = 0; i < current_traj_map_[group_number].points.size(); ++i)
353 motoman_msgs::DynamicJointsGroup dyn_group;
354 motoman_msgs::DynamicJointPoint dyn_point;
356 if (gh.getGoal()->trajectory.points[i].positions.empty())
358 std::vector<double> positions(num_joints, 0.0);
359 dyn_group.positions = positions;
362 dyn_group.positions = gh.getGoal()->trajectory.points[i].positions;
364 if (gh.getGoal()->trajectory.points[i].velocities.empty())
366 std::vector<double> velocities(num_joints, 0.0);
367 dyn_group.velocities = velocities;
370 dyn_group.velocities = gh.getGoal()->trajectory.points[i].velocities;
371 if (gh.getGoal()->trajectory.points[i].accelerations.empty())
373 std::vector<double> accelerations(num_joints, 0.0);
374 dyn_group.accelerations = accelerations;
377 dyn_group.accelerations = gh.getGoal()->trajectory.points[i].accelerations;
378 if (gh.getGoal()->trajectory.points[i].effort.empty())
380 std::vector<double> effort(num_joints, 0.0);
381 dyn_group.effort = effort;
384 dyn_group.effort = gh.getGoal()->trajectory.points[i].effort;
385 dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
386 dyn_group.group_number = group_number;
387 dyn_group.num_joints = robot_groups_[group_number].get_joint_names().size();
388 dyn_point.groups.push_back(dyn_group);
390 dyn_point.num_groups = 1;
391 dyn_traj.points.push_back(dyn_point);
393 dyn_traj.header = gh.getGoal()->trajectory.header;
394 dyn_traj.joint_names = gh.getGoal()->trajectory.joint_names;
395 this->pub_trajectories_[group_number].publish(dyn_traj);
400 ROS_ERROR(
"Joint trajectory action failing on invalid joints");
401 control_msgs::FollowJointTrajectoryResult rslt;
402 rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
403 gh.setRejected(rslt,
"Joint names do not match");
408 ROS_ERROR(
"Joint trajectory action failed on empty trajectory");
409 control_msgs::FollowJointTrajectoryResult rslt;
410 rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
411 gh.setRejected(rslt,
"Empty trajectory");
415 if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
417 ROS_WARN_STREAM(
"Ignoring goal time tolerance in action goal, may be supported in the future");
419 if (!gh.getGoal()->goal_tolerance.empty())
422 "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ <<
" instead");
424 if (!gh.getGoal()->path_tolerance.empty())
426 ROS_WARN_STREAM(
"Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
430 void JointTrajectoryAction::cancelCB(
431 JointTractoryActionServer::GoalHandle gh,
int group_number)
433 ROS_DEBUG(
"Received action cancel request");
434 if (active_goal_map_[group_number] == gh)
437 motoman_msgs::DynamicJointTrajectory empty;
438 empty.joint_names = robot_groups_[group_number].get_joint_names();
439 this->pub_trajectories_[group_number].publish(empty);
442 active_goal_map_[group_number].setCanceled();
443 has_active_goal_map_[group_number] =
false;
447 ROS_WARN(
"Active goal and goal cancel do not match, ignoring cancel request");
451 void JointTrajectoryAction::controllerStateCB(
452 const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
int robot_id)
454 ROS_DEBUG(
"Checking controller state feedback");
455 last_trajectory_state_map_[robot_id] = msg;
456 trajectory_state_recvd_map_[robot_id] =
true;
458 if (!has_active_goal_map_[robot_id])
460 ROS_DEBUG(
"No active goal, ignoring feedback");
464 if (current_traj_map_[robot_id].points.empty())
466 ROS_DEBUG(
"Current trajectory is empty, ignoring feedback");
472 ROS_ERROR(
"Joint names from the controller don't match our joint names.");
480 if (withinGoalConstraints(last_trajectory_state_map_[robot_id], current_traj_map_[robot_id], robot_id))
482 if (last_robot_status_)
490 ROS_INFO(
"Inside goal constraints, stopped moving, return success for action");
491 active_goal_map_[robot_id].setSucceeded();
492 has_active_goal_map_[robot_id] =
false;
494 else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
496 ROS_INFO(
"Inside goal constraints, return success for action");
497 ROS_WARN(
"Robot status in motion unknown, the robot driver node and controller code should be updated");
498 active_goal_map_[robot_id].setSucceeded();
499 has_active_goal_map_[robot_id] =
false;
503 ROS_DEBUG(
"Within goal constraints but robot is still moving");
508 ROS_INFO(
"Inside goal constraints, return success for action");
509 ROS_WARN(
"Robot status is not being published the robot driver node and controller code should be updated");
510 active_goal_map_[robot_id].setSucceeded();
511 has_active_goal_map_[robot_id] =
false;
516 void JointTrajectoryAction::controllerStateCB(
517 const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
519 ROS_DEBUG(
"Checking controller state feedback");
520 last_trajectory_state_ = msg;
521 trajectory_state_recvd_ =
true;
523 if (!has_active_goal_)
525 ROS_DEBUG(
"No active goal, ignoring feedback");
528 if (current_traj_.points.empty())
530 ROS_DEBUG(
"Current trajectory is empty, ignoring feedback");
536 ROS_ERROR(
"Joint names from the controller don't match our joint names.");
544 if (withinGoalConstraints(last_trajectory_state_, current_traj_))
546 if (last_robot_status_)
554 ROS_INFO(
"Inside goal constraints, stopped moving, return success for action");
555 active_goal_.setSucceeded();
556 has_active_goal_ =
false;
558 else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
560 ROS_INFO(
"Inside goal constraints, return success for action");
561 ROS_WARN(
"Robot status in motion unknown, the robot driver node and controller code should be updated");
562 active_goal_.setSucceeded();
563 has_active_goal_ =
false;
567 ROS_DEBUG(
"Within goal constraints but robot is still moving");
572 ROS_INFO(
"Inside goal constraints, return success for action");
573 ROS_WARN(
"Robot status is not being published the robot driver node and controller code should be updated");
574 active_goal_.setSucceeded();
575 has_active_goal_ =
false;
580 void JointTrajectoryAction::abortGoal()
583 trajectory_msgs::JointTrajectory empty;
584 pub_trajectory_command_.publish(empty);
587 active_goal_.setAborted();
588 has_active_goal_ =
false;
591 void JointTrajectoryAction::abortGoal(
int robot_id)
594 motoman_msgs::DynamicJointTrajectory empty;
595 pub_trajectories_[robot_id].publish(empty);
598 active_goal_map_[robot_id].setAborted();
599 has_active_goal_map_[robot_id] =
false;
602 bool JointTrajectoryAction::withinGoalConstraints(
603 const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
604 const trajectory_msgs::JointTrajectory & traj)
607 if (traj.points.empty())
609 ROS_WARN(
"Empty joint trajectory passed to check goal constraints, return false");
614 int last_point = traj.points.size() - 1;
617 last_trajectory_state_->joint_names,
618 last_trajectory_state_->actual.positions, traj.joint_names,
619 traj.points[last_point].positions, goal_threshold_))
631 bool JointTrajectoryAction::withinGoalConstraints(
632 const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
633 const trajectory_msgs::JointTrajectory & traj,
int robot_id)
636 if (traj.points.empty())
638 ROS_WARN(
"Empty joint trajectory passed to check goal constraints, return false");
643 int last_point = traj.points.size() - 1;
645 int group_number = robot_id;
648 robot_groups_[group_number].get_joint_names(),
649 last_trajectory_state_map_[group_number]->actual.positions, traj.joint_names,
650 traj.points[last_point].positions, goal_threshold_))
bool getJointGroups(const std::string topic_param, std::map< int, RobotGroup > &robot_groups)
Parses multi-group joints from topic_param.
void registerGoalCallback(boost::function< void(GoalHandle)> cb)
static const double DEFAULT_GOAL_THRESHOLD_
The default goal joint threshold see(goal_threshold). Unit are joint specific (i.e. radians or meters).
bool isSimilar(std::vector< std::string > lhs, std::vector< std::string > rhs)
#define ROS_WARN_STREAM(args)
#define ROS_INFO_STREAM(args)
bool isWithinRange(const std::vector< double > &lhs, const std::vector< double > &rhs, double full_range)