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_CONTROLLER_H__
00038 #define JOINT_TRAJECTORY_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 #include <control_toolbox/pid.h>
00047 #include <pr2_controller_interface/controller.h>
00048 #include <realtime_tools/realtime_publisher.h>
00049 #include <realtime_tools/realtime_box.h>
00050
00051 #include "trajectory_msgs/JointTrajectory.h"
00052
00053 #include "pr2_controllers_msgs/QueryTrajectoryState.h"
00054 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
00055
00056 namespace controller {
00057
00058 class JointSplineTrajectoryController : public pr2_controller_interface::Controller
00059 {
00060 public:
00061
00062 JointSplineTrajectoryController();
00063 ~JointSplineTrajectoryController();
00064
00065 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00066
00067 void starting();
00068 void update();
00069
00070 private:
00071 int loop_count_;
00072 pr2_mechanism_model::RobotState *robot_;
00073 ros::Time last_time_;
00074 std::vector<pr2_mechanism_model::JointState*> joints_;
00075 std::vector<control_toolbox::Pid> pids_;
00076
00077 ros::NodeHandle node_;
00078
00079 void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
00080 ros::Subscriber sub_command_;
00081
00082 bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00083 pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
00084 ros::ServiceServer serve_query_state_;
00085
00086 boost::scoped_ptr<
00087 realtime_tools::RealtimePublisher<
00088 pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
00089
00090
00091
00092
00093
00094 struct Spline
00095 {
00096 std::vector<double> coef;
00097
00098 Spline() : coef(6, 0.0) {}
00099 };
00100
00101 struct Segment
00102 {
00103 double start_time;
00104 double duration;
00105 std::vector<Spline> splines;
00106 };
00107 typedef std::vector<Segment> SpecifiedTrajectory;
00108
00109 realtime_tools::RealtimeBox<
00110 boost::shared_ptr<const SpecifiedTrajectory> > current_trajectory_box_;
00111
00112
00113
00114
00115
00116
00117
00118 std::vector<double> q, qd, qdd;
00119
00120
00121
00122
00123 static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
00124 double& position, double& velocity, double& acceleration);
00125 };
00126
00127 }
00128
00129 #endif