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
00049 #include <geometry_msgs/Twist.h>
00050 #include <nav_msgs/Odometry.h>
00051 #include <trajectory_msgs/JointTrajectory.h>
00052 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00053
00054 class BaseTrajectoryActionController
00055 {
00056 typedef actionlib::ActionServer<pr2_controllers_msgs::JointTrajectoryAction> JTAS;
00057 typedef JTAS::GoalHandle GoalHandle;
00058
00059
00060 class JointState {
00061 public:
00062 std::string name;
00063 double position_, velocity_;
00064 double max_abs_velocity_;
00065 double commanded_velocity_;
00066 double commanded_effort_;
00067 };
00068
00069 public:
00070
00071 BaseTrajectoryActionController(const ros::NodeHandle& n);
00072 ~BaseTrajectoryActionController();
00073
00074 bool init();
00075
00076 void starting();
00077 void update();
00078
00079 private:
00080 bool use_pid;
00081 JointState x_joint_,y_joint_,z_joint_;
00082 ros::Time last_time_, robot_time_;
00083 std::vector<JointState*> joints_;
00084 double goal_time_constraint_;
00085 double goal_threshold_;
00086 double stopped_velocity_tolerance_;
00087 std::vector<double> goal_constraints_;
00088 std::vector<double> trajectory_constraints_;
00089
00090 ros::NodeHandle node_;
00091
00092 void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg);
00093 ros::Publisher pub_command_;
00094
00095 void odomCB(const nav_msgs::Odometry::ConstPtr& msg);
00096 ros::Subscriber sub_odom_;
00097
00098 boost::scoped_ptr<JTAS> action_server_;
00099 void goalCB(GoalHandle gh);
00100 void cancelCB(GoalHandle gh);
00101 ros::Timer goal_handle_timer_;
00102
00103 boost::shared_ptr<GoalHandle> active_goal_;
00104
00105 void commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &traj, boost::shared_ptr<GoalHandle> gh = boost::shared_ptr<GoalHandle>((GoalHandle*)NULL));
00106
00107
00108 struct Spline
00109 {
00110 std::vector<double> coef;
00111
00112 Spline() : coef(6, 0.0) {}
00113 };
00114
00115 struct Segment
00116 {
00117 double start_time;
00118 double duration;
00119 std::vector<Spline> splines;
00120
00121 boost::shared_ptr<GoalHandle> gh;
00122 };
00123 typedef std::vector<Segment> SpecifiedTrajectory;
00124
00125 boost::shared_ptr<const SpecifiedTrajectory> current_trajectory_;
00126
00127
00128
00129
00130
00131
00132
00133 std::vector<double> q, qd, qdd;
00134 std::vector<double> e, ed, edd;
00135
00136
00137
00138
00139 static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
00140 double& position, double& velocity, double& acceleration);
00141 };
00142
00143
00144 #endif