Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00037 #ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
00038 #define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
00039
00040 #include <vector>
00041
00042 #include <boost/scoped_ptr.hpp>
00043 #include <boost/thread/recursive_mutex.hpp>
00044 #include <boost/thread/condition.hpp>
00045 #include <ros/node_handle.h>
00046
00047 #include <actionlib/server/action_server.h>
00048 #include <control_toolbox/limited_proxy.h>
00049 #include <control_toolbox/pid.h>
00050 #include <filters/filter_chain.h>
00051 #include <pr2_controller_interface/controller.h>
00052 #include <realtime_tools/realtime_publisher.h>
00053 #include <realtime_tools/realtime_box.h>
00054
00055
00056 #include <control_msgs/FollowJointTrajectoryAction.h>
00057 #include <trajectory_msgs/JointTrajectory.h>
00058 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00059 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00060 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00061
00062
00063 namespace controller {
00064
00065 template <class Action>
00066 class RTServerGoalHandle
00067 {
00068 private:
00069 ACTION_DEFINITION(Action);
00070
00071
00072 typedef actionlib::ServerGoalHandle<Action> GoalHandle;
00073 typedef boost::shared_ptr<Result> ResultPtr;
00074 typedef boost::shared_ptr<Feedback> FeedbackPtr;
00075
00076 uint8_t state_;
00077
00078 bool req_abort_;
00079 bool req_succeed_;
00080 ResultConstPtr req_result_;
00081
00082 public:
00083 GoalHandle gh_;
00084 ResultPtr preallocated_result_;
00085 FeedbackPtr preallocated_feedback_;
00086
00087 RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL))
00088 : req_abort_(false), req_succeed_(false), gh_(gh), preallocated_result_(preallocated_result)
00089 {
00090 if (!preallocated_result_)
00091 preallocated_result_.reset(new Result);
00092 if (!preallocated_feedback_)
00093 preallocated_feedback_.reset(new Feedback);
00094 }
00095
00096 void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
00097 {
00098 if (!req_succeed_ && !req_abort_)
00099 {
00100 req_result_ = result;
00101 req_abort_ = true;
00102 }
00103 }
00104
00105 void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
00106 {
00107 if (!req_succeed_ && !req_abort_)
00108 {
00109 req_result_ = result;
00110 req_succeed_ = true;
00111 }
00112 }
00113
00114 bool valid()
00115 {
00116 return gh_.getGoal() != NULL;
00117 }
00118
00119 void runNonRT(const ros::TimerEvent &te)
00120 {
00121 using namespace actionlib_msgs;
00122 if (valid())
00123 {
00124 actionlib_msgs::GoalStatus gs = gh_.getGoalStatus();
00125 if (req_abort_ && gs.status == GoalStatus::ACTIVE)
00126 {
00127 if (req_result_)
00128 gh_.setAborted(*req_result_);
00129 else
00130 gh_.setAborted();
00131 }
00132 else if (req_succeed_ && gs.status == GoalStatus::ACTIVE)
00133 {
00134 if (req_result_)
00135 gh_.setSucceeded(*req_result_);
00136 else
00137 gh_.setSucceeded();
00138 }
00139 }
00140 }
00141 };
00142
00143 class JointTolerance
00144 {
00145 public:
00146 JointTolerance(double _position = 0, double _velocity = 0, double _acceleration = 0) :
00147 position(_position), velocity(_velocity), acceleration(_acceleration)
00148 {
00149 }
00150
00151 bool violated(double p_err, double v_err = 0, double a_err = 0) const
00152 {
00153 return
00154 (position > 0 && fabs(p_err) > position) ||
00155 (velocity > 0 && fabs(v_err) > velocity) ||
00156 (acceleration > 0 && fabs(a_err) > acceleration);
00157 }
00158
00159 double position;
00160 double velocity;
00161 double acceleration;
00162 };
00163
00164
00165 class JointTrajectoryActionController : public pr2_controller_interface::Controller
00166 {
00167
00168 typedef actionlib::ActionServer<pr2_controllers_msgs::JointTrajectoryAction> JTAS;
00169 typedef JTAS::GoalHandle GoalHandle;
00170 typedef RTServerGoalHandle<pr2_controllers_msgs::JointTrajectoryAction> RTGoalHandle;
00171
00172
00173 typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> FJTAS;
00174 typedef FJTAS::GoalHandle GoalHandleFollow;
00175 typedef RTServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RTGoalHandleFollow;
00176
00177 public:
00178
00179 JointTrajectoryActionController();
00180 ~JointTrajectoryActionController();
00181
00182 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00183
00184 void starting();
00185 void update();
00186
00187 private:
00188 int loop_count_;
00189 pr2_mechanism_model::RobotState *robot_;
00190 ros::Time last_time_;
00191 std::vector<pr2_mechanism_model::JointState*> joints_;
00192 std::vector<double> masses_;
00193 std::vector<control_toolbox::Pid> pids_;
00194 std::vector<bool> proxies_enabled_;
00195 std::vector<control_toolbox::LimitedProxy> proxies_;
00196
00197 std::vector<JointTolerance> default_trajectory_tolerance_;
00198 std::vector<JointTolerance> default_goal_tolerance_;
00199 double default_goal_time_constraint_;
00200
00201
00202
00203
00204
00205
00206
00207 std::vector<boost::shared_ptr<filters::FilterChain<double> > > output_filters_;
00208
00209 ros::NodeHandle node_;
00210
00211 void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg);
00212 ros::Subscriber sub_command_;
00213
00214 bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00215 pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
00216 ros::ServiceServer serve_query_state_;
00217
00218 boost::scoped_ptr<
00219 realtime_tools::RealtimePublisher<
00220 pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
00221
00222 boost::scoped_ptr<JTAS> action_server_;
00223 boost::scoped_ptr<FJTAS> action_server_follow_;
00224 void goalCB(GoalHandle gh);
00225 void cancelCB(GoalHandle gh);
00226 void goalCBFollow(GoalHandleFollow gh);
00227 void cancelCBFollow(GoalHandleFollow gh);
00228 ros::Timer goal_handle_timer_;
00229
00230 boost::shared_ptr<RTGoalHandle> rt_active_goal_;
00231 boost::shared_ptr<RTGoalHandleFollow> rt_active_goal_follow_;
00232
00233
00234
00235 void commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &traj,
00236 boost::shared_ptr<RTGoalHandle> gh = boost::shared_ptr<RTGoalHandle>((RTGoalHandle*)NULL),
00237 boost::shared_ptr<RTGoalHandleFollow> gh_follow = boost::shared_ptr<RTGoalHandleFollow>((RTGoalHandleFollow*)NULL));
00238
00239 void preemptActiveGoal();
00240
00241
00242 struct Spline
00243 {
00244 std::vector<double> coef;
00245
00246 Spline() : coef(6, 0.0) {}
00247 };
00248
00249 struct Segment
00250 {
00251 double start_time;
00252 double duration;
00253 std::vector<Spline> splines;
00254
00255 std::vector<JointTolerance> trajectory_tolerance;
00256 std::vector<JointTolerance> goal_tolerance;
00257 double goal_time_tolerance;
00258
00259 boost::shared_ptr<RTGoalHandle> gh;
00260 boost::shared_ptr<RTGoalHandleFollow> gh_follow;
00261 };
00262 typedef std::vector<Segment> SpecifiedTrajectory;
00263
00264 realtime_tools::RealtimeBox<
00265 boost::shared_ptr<const SpecifiedTrajectory> > current_trajectory_box_;
00266
00267
00268
00269
00270
00271
00272
00273 std::vector<double> q, qd, qdd;
00274
00275
00276
00277
00278 static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
00279 double& position, double& velocity, double& acceleration);
00280 };
00281
00282 }
00283
00284 #endif