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