Go to the documentation of this file.
35 #ifndef JOINT_GROUP_VELOCITY_CONTROLLER_H
36 #define JOINT_GROUP_VELOCITY_CONTROLLER_H
63 #include <boost/scoped_ptr.hpp>
64 #include <boost/thread/condition.hpp>
71 #include <std_msgs/Float64.h>
72 #include <std_msgs/Float64MultiArray.h>
75 #include <pr2_controllers_msgs/JointControllerState.h>
76 #include <pr2_controllers_msgs/JointControllerStateArray.h>
121 std::vector<pr2_mechanism_model::JointState*>
joints_;
134 void setCommandCB(
const std_msgs::Float64MultiArrayConstPtr& msg);
std::vector< pr2_mechanism_model::JointState * > joints_
std::vector< control_toolbox::Pid > pid_controllers_
bool init(pr2_mechanism_model::RobotState *robot, const std::vector< std::string > &joint_names, const control_toolbox::Pid &pid)
ros::Subscriber sub_command_
virtual void starting()
Issues commands to the joint. Should be called at regular intervals.
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerStateArray > > controller_state_publisher_
void setCommand(std::vector< double > cmd)
Give set position of the joint for next update: revolute (angle) and prismatic (position)
realtime_tools::RealtimeBuffer< std::vector< double > > commands_buffer_
std::vector< std::string > getJointName()
JointGroupVelocityController()
pr2_mechanism_model::RobotState * robot_
~JointGroupVelocityController()
void getGains(control_toolbox::Pid &pid, double &p, double &i, double &d, double &i_max, double &i_min)
void getCommand(std::vector< double > &cmd)
Get latest position command to the joint: revolute (angle) and prismatic (position).
void setCommandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
friend class JointGroupVelocityControllerNode