Go to the documentation of this file.
37 #ifndef JOINT_TRAJECTORY_CONTROLLER_H__
38 #define JOINT_TRAJECTORY_CONTROLLER_H__
42 #include <boost/scoped_ptr.hpp>
43 #include <boost/shared_ptr.hpp>
50 #include "trajectory_msgs/JointTrajectory.h"
52 #include "pr2_controllers_msgs/QueryTrajectoryState.h"
53 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
73 std::vector<pr2_mechanism_model::JointState*>
joints_;
74 std::vector<control_toolbox::Pid>
pids_;
78 void commandCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg);
82 pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
123 double& position,
double& velocity,
double& acceleration);
void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
std::vector< Segment > SpecifiedTrajectory
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
std::vector< double > qdd
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
ros::ServiceServer serve_query_state_
~JointSplineTrajectoryController()
std::vector< double > coef
std::vector< control_toolbox::Pid > pids_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
ros::Subscriber sub_command_
std::vector< Spline > splines
static void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
JointSplineTrajectoryController()
std::vector< pr2_mechanism_model::JointState * > joints_
bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
pr2_mechanism_model::RobotState * robot_