00001 00027 #ifndef _MOVEMENT_PUBLISHER_HPP_ 00028 #define _MOVEMENT_PUBLISHER_HPP_ 00029 00030 #include <ros/ros.h> 00031 #include <boost/thread.hpp> 00032 00033 #include <std_msgs/Float64.h> 00034 #include "sr_movements/partial_movement.hpp" 00035 #include <pr2_controllers_msgs/JointControllerState.h> 00036 #include <sr_robot_msgs/JointControllerState.h> 00037 #include <math.h> 00038 00039 namespace shadowrobot 00040 { 00041 class MovementPublisher 00042 { 00043 public: 00044 MovementPublisher( double min_value = 0.0, double max_value = 1.5, 00045 double rate=100.0, unsigned int repetition = 1, unsigned int nb_mvt_step = 1000 , std::string controller_type = ""); 00046 virtual ~MovementPublisher(); 00047 00048 void start(); 00049 void stop(); 00050 00057 void calculateErrorCallback(const sr_robot_msgs::JointControllerState::ConstPtr& msg); 00058 00065 void pr2_calculateErrorCallback(const pr2_controllers_msgs::JointControllerState::ConstPtr& msg); 00066 00071 void execute_step(int index_mvt_step, int index_partial_movement); 00072 00073 void add_movement(PartialMovement mvt); 00074 00075 void set_publisher(ros::Publisher publisher); 00076 00077 protected: 00078 std::vector<PartialMovement> partial_movements; 00079 ros::NodeHandle nh_tilde; 00080 ros::Publisher pub; 00081 ros::Publisher pub_2; 00082 ros::Subscriber sub_; 00083 00084 ros::Rate publishing_rate; 00085 unsigned int repetition; 00086 double min, max; 00087 00088 std_msgs::Float64 msg; 00089 double last_target_; 00090 00091 unsigned int nb_mvt_step; 00092 double SError_; 00093 double MSError_; 00094 unsigned int n_samples_; 00095 std::string controller_type; 00096 }; 00097 } 00098 00099 /* For the emacs weenies in the crowd. 00100 Local Variables: 00101 c-basic-offset: 2 00102 End: 00103 */ 00104 00105 #endif