joint_trajectory_action_controller.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 
37 #ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
38 #define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
39 
40 #include <vector>
41 
42 #include <boost/scoped_ptr.hpp>
43 #include <boost/thread/recursive_mutex.hpp>
44 #include <boost/thread/condition.hpp>
45 #include <ros/node_handle.h>
46 
49 #include <control_toolbox/pid.h>
50 #include <filters/filter_chain.h>
54 
55 
56 #include <control_msgs/FollowJointTrajectoryAction.h>
57 #include <trajectory_msgs/JointTrajectory.h>
58 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
59 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
60 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
61 
62 
63 namespace controller {
64 
65 template <class Action>
67 {
68 private:
69  ACTION_DEFINITION(Action);
70 
71  //typedef actionlib::ActionServer<Action>::GoalHandle GoalHandle;
75 
76  uint8_t state_;
77 
78  bool req_abort_;
80  ResultConstPtr req_result_;
81 
82 public:
83  GoalHandle gh_;
84  ResultPtr preallocated_result_; // Preallocated so it can be used in realtime
86 
87  RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL))
88  : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result)
89  {
90  if (!preallocated_result_)
91  preallocated_result_.reset(new Result);
92  if (!preallocated_feedback_)
93  preallocated_feedback_.reset(new Feedback);
94  }
95 
96  void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
97  {
98  if (!req_succeed_ && !req_abort_)
99  {
100  req_result_ = result;
101  req_abort_ = true;
102  }
103  }
104 
105  void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
106  {
107  if (!req_succeed_ && !req_abort_)
108  {
109  req_result_ = result;
110  req_succeed_ = true;
111  }
112  }
113 
114  bool valid()
115  {
116  return gh_.getGoal() != NULL;
117  }
118 
119  void runNonRT(const ros::TimerEvent &te)
120  {
121  using namespace actionlib_msgs;
122  if (valid())
123  {
124  actionlib_msgs::GoalStatus gs = gh_.getGoalStatus();
125  if (req_abort_ && gs.status == GoalStatus::ACTIVE)
126  {
127  if (req_result_)
128  gh_.setAborted(*req_result_);
129  else
130  gh_.setAborted();
131  }
132  else if (req_succeed_ && gs.status == GoalStatus::ACTIVE)
133  {
134  if (req_result_)
135  gh_.setSucceeded(*req_result_);
136  else
137  gh_.setSucceeded();
138  }
139  }
140  }
141 };
142 
144 {
145 public:
146  JointTolerance(double _position = 0, double _velocity = 0, double _acceleration = 0) :
147  position(_position), velocity(_velocity), acceleration(_acceleration)
148  {
149  }
150 
151  bool violated(double p_err, double v_err = 0, double a_err = 0) const
152  {
153  return
154  (position > 0 && fabs(p_err) > position) ||
155  (velocity > 0 && fabs(v_err) > velocity) ||
156  (acceleration > 0 && fabs(a_err) > acceleration);
157  }
158 
159  double position;
160  double velocity;
161  double acceleration;
162 };
163 
164 
166 {
167  // Action typedefs for the original PR2 specific joint trajectory action
171 
172  // Action typedefs for the new follow joint trajectory action
176 
177 public:
178 
181 
182  bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
183 
184  void starting();
185  void update();
186 
187 private:
191  std::vector<pr2_mechanism_model::JointState*> joints_;
192  std::vector<double> masses_; // Rough estimate of joint mass, used for feedforward control
193  std::vector<control_toolbox::Pid> pids_;
194  std::vector<bool> proxies_enabled_;
195  std::vector<control_toolbox::LimitedProxy> proxies_;
196 
197  std::vector<JointTolerance> default_trajectory_tolerance_;
198  std::vector<JointTolerance> default_goal_tolerance_;
200 
201  /*
202  double goal_time_constraint_;
203  double stopped_velocity_tolerance_;
204  std::vector<double> goal_constraints_;
205  std::vector<double> trajectory_constraints_;
206  */
207  std::vector<boost::shared_ptr<filters::FilterChain<double> > > output_filters_;
208 
210 
211  void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg);
213 
214  bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
215  pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
217 
218  boost::scoped_ptr<
220  pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
221 
222  boost::scoped_ptr<JTAS> action_server_;
223  boost::scoped_ptr<FJTAS> action_server_follow_;
224  void goalCB(GoalHandle gh);
225  void cancelCB(GoalHandle gh);
226  void goalCBFollow(GoalHandleFollow gh);
227  void cancelCBFollow(GoalHandleFollow gh);
229 
232 
233  // ------ Mechanisms for passing the trajectory into realtime
234 
235  void commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &traj,
238 
239  void preemptActiveGoal();
240 
241  // coef[0] + coef[1]*t + ... + coef[5]*t^5
242  struct Spline
243  {
244  std::vector<double> coef;
245 
246  Spline() : coef(6, 0.0) {}
247  };
248 
249  struct Segment
250  {
251  double start_time;
252  double duration;
253  std::vector<Spline> splines;
254 
255  std::vector<JointTolerance> trajectory_tolerance;
256  std::vector<JointTolerance> goal_tolerance;
258 
260  boost::shared_ptr<RTGoalHandleFollow> gh_follow; // Goal handle for the newer FollowJointTrajectory action
261  };
262  typedef std::vector<Segment> SpecifiedTrajectory;
263 
266 
267  // Holds the trajectory that we are currently following. The mutex
268  // guarding current_trajectory_ is locked from within realtime, so
269  // it may only be locked for a bounded duration.
270  //boost::shared_ptr<const SpecifiedTrajectory> current_trajectory_;
271  //boost::recursive_mutex current_trajectory_lock_RT_;
272 
273  std::vector<double> q, qd, qdd; // Preallocated in init
274 
275  // Samples, but handling time bounds. When the time is past the end
276  // of the spline duration, the position is the last valid position,
277  // and the derivatives are all 0.
278  static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
279  double& position, double& velocity, double& acceleration);
280 };
281 
282 }
283 
284 #endif
actionlib::ServerGoalHandle< Action > GoalHandle
RTServerGoalHandle< pr2_controllers_msgs::JointTrajectoryAction > RTGoalHandle
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result=ResultPtr((Result *) NULL))
void setAborted(ResultConstPtr result=ResultConstPtr((Result *) NULL))
boost::shared_ptr< const Goal > getGoal() const
JointTolerance(double _position=0, double _velocity=0, double _acceleration=0)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setSucceeded(ResultConstPtr result=ResultConstPtr((Result *) NULL))
boost::shared_ptr< RTGoalHandleFollow > rt_active_goal_follow_
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > FJTAS
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
actionlib::ActionServer< pr2_controllers_msgs::JointTrajectoryAction > JTAS
RTServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RTGoalHandleFollow
std::vector< control_toolbox::LimitedProxy > proxies_
std::vector< boost::shared_ptr< filters::FilterChain< double > > > output_filters_
bool violated(double p_err, double v_err=0, double a_err=0) const
actionlib_msgs::GoalStatus getGoalStatus() const
std::vector< pr2_mechanism_model::JointState * > joints_


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Mon Jun 10 2019 14:26:26