joint_trajectory_action.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 
35 #include <industrial_utils/utils.h>
36 
38 {
39 namespace joint_trajectory_action
40 {
41 
44 
46  action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1),
47  boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false),
48  controller_alive_(false), has_moved_once_(false), name_("joint_trajectory_action")
49 {
50  ros::NodeHandle pn("~");
51 
52  pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_);
53 
54  if (!industrial_utils::param::getJointNames("controller_joint_names", "robot_description", joint_names_))
55  ROS_ERROR_NAMED(name_, "Failed to initialize joint_names.");
56 
57  // The controller joint names parameter includes empty joint names for those joints not supported
58  // by the controller. These are removed since the trajectory action should ignore these.
59  std::remove(joint_names_.begin(), joint_names_.end(), std::string());
60  ROS_INFO_STREAM_NAMED(name_, "Filtered joint names to " << joint_names_.size() << " joints");
61 
62  pub_trajectory_command_ = node_.advertise<trajectory_msgs::JointTrajectory>("joint_path_command", 1);
65 
68 }
69 
71 {
72 }
73 
74 void JointTrajectoryAction::robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg)
75 {
76  last_robot_status_ = msg; //caching robot status for later use.
77  has_moved_once_ = has_moved_once_ ? true : (last_robot_status_->in_motion.val == industrial_msgs::TriState::TRUE);
78 }
79 
81 {
82  // Some debug logging
84  {
85  ROS_DEBUG_NAMED(name_, "Waiting for subscription to joint trajectory state");
86  }
87 
88  ROS_WARN_NAMED(name_, "Trajectory state not received for %f seconds", WATCHDOG_PERIOD_);
89  controller_alive_ = false;
90 
91 
92  // Aborts the active goal if the controller does not appear to be active.
93  if (has_active_goal_)
94  {
95  // last_trajectory_state_ is null if the subscriber never makes a connection
97  {
98  ROS_WARN_NAMED(name_, "Aborting goal because we have never heard a controller state message.");
99  }
100  else
101  {
103  "Aborting goal because we haven't heard from the controller in " << WATCHDOG_PERIOD_ << " seconds");
104  }
105 
106  abortGoal();
107  }
108 }
109 
111 {
112  ROS_INFO_STREAM_NAMED(name_, "Received new goal");
113 
114  // reject all goals as long as we haven't heard from the remote controller
115  if (!controller_alive_)
116  {
117  ROS_ERROR_NAMED(name_, "Joint trajectory action rejected: waiting for (initial) feedback from controller");
118  control_msgs::FollowJointTrajectoryResult rslt;
119  rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
120  gh.setRejected(rslt, "Waiting for (initial) feedback from controller");
121 
122  // no point in continuing: already rejected
123  return;
124  }
125 
126  if (!gh.getGoal()->trajectory.points.empty())
127  {
128  if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))
129  {
130 
131  // Cancels the currently active goal.
132  if (has_active_goal_)
133  {
134  ROS_WARN_NAMED(name_, "Received new goal, canceling current goal");
135  abortGoal();
136  }
137 
138  gh.setAccepted();
139  active_goal_ = gh;
140  has_active_goal_ = true;
142  ros::Duration(active_goal_.getGoal()->trajectory.points.back().time_from_start.toSec() / 2.0);
143  has_moved_once_ = false;
144 
145  ROS_INFO_STREAM_NAMED(name_, "Publishing trajectory");
146 
147  current_traj_ = active_goal_.getGoal()->trajectory;
149 
150  }
151  else
152  {
153  ROS_ERROR_NAMED(name_, "Joint trajectory action failing on invalid joints");
154  control_msgs::FollowJointTrajectoryResult rslt;
155  rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
156  gh.setRejected(rslt, "Joint names do not match");
157  }
158  }
159  else
160  {
161  ROS_ERROR_NAMED(name_, "Joint trajectory action failed on empty trajectory");
162  control_msgs::FollowJointTrajectoryResult rslt;
163  rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
164  gh.setRejected(rslt, "Empty trajectory");
165  }
166 
167  // Adding some informational log messages to indicate unsupported goal constraints
168  if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
169  {
170  ROS_WARN_STREAM_NAMED(name_, "Ignoring goal time tolerance in action goal, may be supported in the future");
171  }
172  if (!gh.getGoal()->goal_tolerance.empty())
173  {
175  "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead");
176  }
177  if (!gh.getGoal()->path_tolerance.empty())
178  {
179  ROS_WARN_STREAM_NAMED(name_, "Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
180  }
181 }
182 
184 {
185  ROS_DEBUG_NAMED(name_, "Received action cancel request");
186  if (active_goal_ == gh)
187  {
188  // Stops the controller.
189  trajectory_msgs::JointTrajectory empty;
190  empty.joint_names = joint_names_;
192 
193  // Marks the current goal as canceled.
195  has_active_goal_ = false;
196  }
197  else
198  {
199  ROS_WARN_NAMED(name_, "Active goal and goal cancel do not match, ignoring cancel request");
200  }
201 }
202 
203 void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
204 {
205  ROS_DEBUG_STREAM_NAMED(name_, "Checking controller state feedback");
206 
208  controller_alive_ = true;
209 
212 
213  if (!has_active_goal_)
214  {
215  //ROS_DEBUG_NAMED(name_, "No active goal, ignoring feedback");
216  return;
217  }
218  if (current_traj_.points.empty())
219  {
220  ROS_INFO_NAMED(name_, "Current trajectory is empty, ignoring feedback");
221  return;
222  }
223 
224  if (!industrial_utils::isSimilar(joint_names_, msg->joint_names))
225  {
226  ROS_ERROR_NAMED(name_, "Joint names from the controller don't match our joint names.");
227  return;
228  }
229 
231  {
232  ROS_INFO_NAMED(name_, "Waiting to check for goal completion until halfway through trajectory");
233  return;
234  }
235 
236  // Checking for goal constraints
237  // Checks that we have ended inside the goal constraints and has motion stopped
238 
239  ROS_DEBUG_STREAM_NAMED(name_, "Checking goal constraints");
241  {
242  if (last_robot_status_)
243  {
244  // Additional check for motion stoppage since the controller goal may still
245  // be moving. The current robot driver calls a motion stop if it receives
246  // a new trajectory while it is still moving. If the driver is not publishing
247  // the motion state (i.e. old driver), this will still work, but it warns you.
248  if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE)
249  {
250  ROS_INFO_NAMED("joint_trajectory_action.controllerStateCB", "Inside goal constraints - stopped moving- return success for action");
252  has_active_goal_ = false;
253  }
254  else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
255  {
256  ROS_INFO_NAMED(name_, "Inside goal constraints, return success for action");
257  ROS_WARN_NAMED(name_, "Robot status in motion unknown, the robot driver node and controller code should be updated");
259  has_active_goal_ = false;
260  }
261  else
262  {
263  ROS_DEBUG_NAMED(name_, "Within goal constraints but robot is still moving");
264  }
265  }
266  else
267  {
268  ROS_INFO_NAMED(name_, "Inside goal constraints, return success for action");
269  ROS_WARN_NAMED(name_, "Robot status is not being published the robot driver node and controller code should be updated");
271  has_active_goal_ = false;
272  }
273  }
274 }
275 
277 {
278  // Stops the controller.
279  trajectory_msgs::JointTrajectory empty;
281 
282  // Marks the current goal as aborted.
284  has_active_goal_ = false;
285 }
286 
287 bool JointTrajectoryAction::withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
288  const trajectory_msgs::JointTrajectory & traj)
289 {
290  bool rtn = false;
291  if (traj.points.empty())
292  {
293  ROS_WARN_NAMED(name_, "Empty joint trajectory passed to check goal constraints, return false");
294  rtn = false;
295  }
296  else
297  {
298  int last_point = traj.points.size() - 1;
299 
301  last_trajectory_state_->actual.positions, traj.joint_names,
302  traj.points[last_point].positions, goal_threshold_))
303  {
304  rtn = true;
305  }
306  else
307  {
308  rtn = false;
309  }
310  }
311  return rtn;
312 }
313 
314 } //joint_trajectory_action
315 } //industrial_robot_client
void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg)
Controller status callback (executed when robot status message received)
void watchdog(const ros::TimerEvent &e)
Watch dog callback, used to detect robot driver failures.
#define ROS_INFO_NAMED(name,...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
bool getJointNames(const std::string joint_list_param, const std::string urdf_param, std::vector< std::string > &joint_names)
boost::shared_ptr< const Goal > getGoal() const
#define ROS_WARN_NAMED(name,...)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void stop()
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
void start()
#define ROS_INFO_STREAM_NAMED(name, args)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
ros::Publisher pub_trajectory_command_
Publishes desired trajectory (typically to the robot driver)
void setAccepted(const std::string &text=std::string(""))
#define ROS_DEBUG_NAMED(name,...)
control_msgs::FollowJointTrajectoryFeedbackConstPtr last_trajectory_state_
Cache of the last subscribed feedback message.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
industrial_msgs::RobotStatusConstPtr last_robot_status_
Cache of the last subscribed status message.
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
std::string name_
Name of this class, for logging namespacing.
double goal_threshold_
The goal joint threshold used for determining if a robot is near it final destination. A single value is used for all joints.
std::vector< std::string > joint_names_
The joint names associated with the robot the action is interfacing with. The joint names must be the...
static const double DEFAULT_GOAL_THRESHOLD_
The default goal joint threshold see(goal_threshold). Unit are joint specific (i.e. radians or meters).
bool controller_alive_
Controller was alive during the last watchdog interval.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool isSimilar(std::vector< std::string > lhs, std::vector< std::string > rhs)
ros::Subscriber sub_robot_status_
Subscribes to the robot status (typically published by the robot driver).
ros::Timer watchdog_timer_
Watchdog time used to fail the action request if the robot driver is not responding.
ros::Subscriber sub_trajectory_state_
Subscribes to trajectory feedback (typically published by the robot driver).
void abortGoal()
Aborts the current action goal and sends a stop command (empty message) to the robot driver...
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
bool has_moved_once_
Indicates that the robot has been in a moving state at least once since starting the current active t...
void cancelCB(JointTractoryActionServer::GoalHandle gh)
Action server cancel callback method.
bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, const trajectory_msgs::JointTrajectory &traj)
Controller status callback (executed when robot status message received)
JointTractoryActionServer::GoalHandle active_goal_
Cache of the current active goal.
bool isWithinRange(const std::vector< double > &lhs, const std::vector< double > &rhs, double full_range)
Checks to see if members are within the same range. Specifically, Each item in abs(lhs[i] - rhs[i]) <...
Definition: utils.cpp:45
#define ROS_ERROR_NAMED(name,...)
static Time now()
trajectory_msgs::JointTrajectory current_traj_
Cache of the current active trajectory.
void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
Controller state callback (executed when feedback message received)
void goalCB(JointTractoryActionServer::GoalHandle gh)
Action server goal callback method.
#define ROS_WARN_STREAM_NAMED(name, args)
ros::Time time_to_check_
Time at which to start checking for completion of current goal, if one is active. ...


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Mon Feb 28 2022 22:34:42