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
00030
00031
00032
00033
00034
00035 #ifndef JOINT_VELOCITY_CONTROLLER_H
00036 #define JOINT_VELOCITY_CONTROLLER_H
00037
00062 #include <ros/node_handle.h>
00063 #include <boost/scoped_ptr.hpp>
00064 #include <boost/thread/condition.hpp>
00065
00066 #include "pr2_controller_interface/controller.h"
00067 #include "control_toolbox/pid.h"
00068 #include "control_toolbox/pid_gains_setter.h"
00069
00070
00071 #include <std_msgs/Float64.h>
00072
00073
00074 #include <pr2_controllers_msgs/JointControllerState.h>
00075 #include <realtime_tools/realtime_publisher.h>
00076
00077 namespace controller
00078 {
00079
00080 class JointVelocityController : public pr2_controller_interface::Controller
00081 {
00082 public:
00083
00084 JointVelocityController();
00085 ~JointVelocityController();
00086
00087 bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid);
00088 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00089
00095 void setCommand(double cmd);
00096
00100 void getCommand(double & cmd);
00101
00106 virtual void starting() {
00107 command_ = 0.0;
00108 pid_controller_.reset();
00109 }
00110 virtual void update();
00111
00112 void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
00113 void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
00114
00115 std::string getJointName();
00116 pr2_mechanism_model::JointState *joint_state_;
00117 ros::Duration dt_;
00118
00119 double command_;
00120 private:
00121 ros::NodeHandle node_;
00122 pr2_mechanism_model::RobotState *robot_;
00123 control_toolbox::Pid pid_controller_;
00124 ros::Time last_time_;
00125 int loop_count_;
00126
00127 friend class JointVelocityControllerNode;
00128
00129 boost::scoped_ptr<
00130 realtime_tools::RealtimePublisher<
00131 pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ;
00132
00133 ros::Subscriber sub_command_;
00134 void setCommandCB(const std_msgs::Float64ConstPtr& msg);
00135 };
00136
00137 }
00138
00139 #endif