00001 00030 #ifndef _SRH_MUSCLE_VALVE_CONTROLLER_HPP_ 00031 #define _SRH_MUSCLE_VALVE_CONTROLLER_HPP_ 00032 00033 #include <sr_mechanism_controllers/sr_controller.hpp> 00034 #include <sr_robot_msgs/JointMuscleValveControllerState.h> 00035 #include <sr_robot_msgs/JointMuscleValveControllerCommand.h> 00036 00037 namespace controller 00038 { 00039 class SrhJointMuscleValveController : public SrController 00040 { 00041 public: 00042 SrhJointMuscleValveController(); 00043 ~SrhJointMuscleValveController(); 00044 00045 bool init( pr2_mechanism_model::RobotState *robot, const std::string &joint_name); 00046 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00047 00048 virtual void starting(); 00049 00053 virtual void update(); 00054 00055 virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00056 virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); 00057 //bool setGains(sr_robot_msgs::SetEffortControllerGains::Request &req, sr_robot_msgs::SetEffortControllerGains::Response &resp); 00058 00059 int8_t cmd_valve_muscle_[2]; 00060 unsigned int cmd_duration_ms_[2]; 00061 unsigned int current_duration_ms_[2]; // These are the actual counters used to maintain the commanded value for the valve during the commanded time 00062 00063 int8_t cmd_valve_muscle_min_; 00064 int8_t cmd_valve_muscle_max_; 00065 00066 private: 00067 //publish our joint controller state 00068 boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::JointMuscleValveControllerState> > controller_state_publisher_; 00069 00070 ros::Subscriber sub_command_; 00071 void setCommandCB(const sr_robot_msgs::JointMuscleValveControllerCommandConstPtr& msg); 00072 00074 void read_parameters(); 00075 00077 int8_t clamp_command( int8_t cmd ); 00078 }; 00079 } // namespace 00080 00081 /* For the emacs weenies in the crowd. 00082 Local Variables: 00083 c-basic-offset: 2 00084 End: 00085 */ 00086 00087 00088 #endif