joint_trajectory_action.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Stuart Glaser
31 
32 #include <ros/ros.h>
34 
35 #include <trajectory_msgs/JointTrajectory.h>
36 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
37 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
38 
39 const double DEFAULT_GOAL_THRESHOLD = 0.1;
40 
42 {
43 private:
46 public:
48  node_(n),
49  action_server_(node_, "joint_trajectory_action",
50  boost::bind(&JointTrajectoryExecuter::goalCB, this, _1),
51  boost::bind(&JointTrajectoryExecuter::cancelCB, this, _1),
52  false),
53  has_active_goal_(false)
54  {
55  using namespace XmlRpc;
56  ros::NodeHandle pn("~");
57 
58  // Gets all of the joints
59  XmlRpc::XmlRpcValue joint_names;
60  if (!pn.getParam("joints", joint_names))
61  {
62  ROS_FATAL("No joints given. (namespace: %s)", pn.getNamespace().c_str());
63  exit(1);
64  }
65  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
66  {
67  ROS_FATAL("Malformed joint specification. (namespace: %s)", pn.getNamespace().c_str());
68  exit(1);
69  }
70  for (int i = 0; i < joint_names.size(); ++i)
71  {
72  XmlRpcValue &name_value = joint_names[i];
73  if (name_value.getType() != XmlRpcValue::TypeString)
74  {
75  ROS_FATAL("Array of joint names should contain all strings. (namespace: %s)",
76  pn.getNamespace().c_str());
77  exit(1);
78  }
79 
80  joint_names_.push_back((std::string)name_value);
81  }
82 
83  pn.param("constraints/goal_time", goal_time_constraint_, 0.0);
84 
85  // Gets the constraints for each joint.
86  for (size_t i = 0; i < joint_names_.size(); ++i)
87  {
88  std::string ns = std::string("constraints/") + joint_names_[i];
89  double g, t;
90  pn.param(ns + "/goal", g, -1.0);
91  pn.param(ns + "/trajectory", t, -1.0);
93  trajectory_constraints_[joint_names_[i]] = t;
94  }
95  pn.param("constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01);
96 
97 
99  node_.advertise<trajectory_msgs::JointTrajectory>("command", 1);
102 
104 
105  ros::Time started_waiting_for_controller = ros::Time::now();
106  while (ros::ok() && !last_controller_state_)
107  {
108  ros::spinOnce();
109  if (started_waiting_for_controller != ros::Time(0) &&
110  ros::Time::now() > started_waiting_for_controller + ros::Duration(30.0))
111  {
112  ROS_WARN("Waited for the controller for 30 seconds, but it never showed up.");
113  started_waiting_for_controller = ros::Time(0);
114  }
115  ros::Duration(0.1).sleep();
116  }
117 
119  }
120 
122  {
126  }
127 
128 private:
129 
130  static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
131  {
132  if (a.size() != b.size())
133  return false;
134 
135  for (size_t i = 0; i < a.size(); ++i)
136  {
137  if (count(b.begin(), b.end(), a[i]) != 1)
138  return false;
139  }
140  for (size_t i = 0; i < b.size(); ++i)
141  {
142  if (count(a.begin(), a.end(), b[i]) != 1)
143  return false;
144  }
145 
146  return true;
147  }
148 
149  void watchdog(const ros::TimerEvent &e)
150  {
151  ros::Time now = ros::Time::now();
152 
153  // Aborts the active goal if the controller does not appear to be active.
154  if (has_active_goal_)
155  {
156  bool should_abort = false;
158  {
159  should_abort = true;
160  ROS_WARN("Aborting goal because we have never heard a controller state message.");
161  }
162  else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
163  {
164  should_abort = true;
165  ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
166  (now - last_controller_state_->header.stamp).toSec());
167  }
168 
169  if (should_abort)
170  {
171  // Stops the controller.
172  trajectory_msgs::JointTrajectory empty;
173  empty.joint_names = joint_names_;
175 
176  // Marks the current goal as aborted.
178  has_active_goal_ = false;
179  }
180  }
181  }
182 
183  void goalCB(GoalHandle gh)
184  {
185  // Ensures that the joints in the goal match the joints we are commanding.
186  if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names))
187  {
188  ROS_ERROR("Joints on incoming goal don't match our joints");
189  gh.setRejected();
190  return;
191  }
192 
193  // Cancels the currently active goal.
194  if (has_active_goal_)
195  {
196  // Stops the controller.
197  trajectory_msgs::JointTrajectory empty;
198  empty.joint_names = joint_names_;
200 
201  // Marks the current goal as canceled.
203  has_active_goal_ = false;
204  }
205 
206  gh.setAccepted();
207  active_goal_ = gh;
208  has_active_goal_ = true;
209 
210  // Sends the trajectory along to the controller
211  current_traj_ = active_goal_.getGoal()->trajectory;
213  }
214 
215  void cancelCB(GoalHandle gh)
216  {
217  if (active_goal_ == gh)
218  {
219  // Stops the controller.
220  trajectory_msgs::JointTrajectory empty;
221  empty.joint_names = joint_names_;
223 
224  // Marks the current goal as canceled.
226  has_active_goal_ = false;
227  }
228  }
229 
230 
236 
238  GoalHandle active_goal_;
239  trajectory_msgs::JointTrajectory current_traj_;
240 
241 
242  std::vector<std::string> joint_names_;
243  std::map<std::string,double> goal_constraints_;
244  std::map<std::string,double> trajectory_constraints_;
247 
248  pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
249  void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
250  {
251  last_controller_state_ = msg;
252  ros::Time now = ros::Time::now();
253 
254  if (!has_active_goal_)
255  return;
256  if (current_traj_.points.empty())
257  return;
258  if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start)
259  return;
260 
261  if (!setsEqual(joint_names_, msg->joint_names))
262  {
263  ROS_ERROR("Joint names from the controller don't match our joint names.");
264  return;
265  }
266 
267  int last = current_traj_.points.size() - 1;
268  ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start;
269 
270  // Verifies that the controller has stayed within the trajectory constraints.
271 
272  if (now < end_time)
273  {
274  // Checks that the controller is inside the trajectory constraints.
275  for (size_t i = 0; i < msg->joint_names.size(); ++i)
276  {
277  double abs_error = fabs(msg->error.positions[i]);
278  double constraint = trajectory_constraints_[msg->joint_names[i]];
279  if (constraint >= 0 && abs_error > constraint)
280  {
281  // Stops the controller.
282  trajectory_msgs::JointTrajectory empty;
283  empty.joint_names = joint_names_;
284  pub_controller_command_.publish(empty);
285 
286  active_goal_.setAborted();
287  has_active_goal_ = false;
288  ROS_WARN("Aborting because we would up outside the trajectory constraints");
289  return;
290  }
291  }
292  }
293  else
294  {
295  // Checks that we have ended inside the goal constraints
296  bool inside_goal_constraints = true;
297  for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
298  {
299  double abs_error = fabs(msg->error.positions[i]);
300  double goal_constraint = goal_constraints_[msg->joint_names[i]];
301  if (goal_constraint >= 0 && abs_error > goal_constraint)
302  inside_goal_constraints = false;
303 
304  // It's important to be stopped if that's desired.
305  if (fabs(msg->desired.velocities[i]) < 1e-6)
306  {
307  if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
308  inside_goal_constraints = false;
309  }
310  }
311 
312  if (inside_goal_constraints)
313  {
314  active_goal_.setSucceeded();
315  has_active_goal_ = false;
316  }
317  else if (now < end_time + ros::Duration(goal_time_constraint_))
318  {
319  // Still have some time left to make it.
320  }
321  else
322  {
323  ROS_WARN("Aborting because we wound up outside the goal constraints");
324  active_goal_.setAborted();
325  has_active_goal_ = false;
326  }
327 
328  }
329  }
330 };
331 
332 
333 int main(int argc, char** argv)
334 {
335  ros::init(argc, argv, "joint_trajectory_action_node");
336  ros::NodeHandle node;//("~");
337  JointTrajectoryExecuter jte(node);
338 
339  ros::spin();
340 
341  return 0;
342 }
#define ROS_FATAL(...)
void watchdog(const ros::TimerEvent &e)
void publish(const boost::shared_ptr< M > &message) const
void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
bool sleep() const
void stop()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< const Goal > getGoal() const
std::map< std::string, double > goal_constraints_
Type const & getType() const
#define ROS_WARN(...)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
void setAccepted(const std::string &text=std::string(""))
const double DEFAULT_GOAL_THRESHOLD
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
JointTrajectoryExecuter(ros::NodeHandle &n)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
trajectory_msgs::JointTrajectory current_traj_
ROSCPP_DECL bool ok()
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
const std::string & getNamespace() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_
static bool setsEqual(const std::vector< std::string > &a, const std::vector< std::string > &b)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< std::string > joint_names_
int main(int argc, char **argv)
bool getParam(const std::string &key, std::string &s) const
static Time now()
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
actionlib::ActionServer< pr2_controllers_msgs::JointTrajectoryAction > JTAS
std::map< std::string, double > trajectory_constraints_


joint_trajectory_action
Author(s): Stuart Glaser
autogenerated on Mon Jun 10 2019 14:26:21