movement_publisher.hpp

Go to the documentation of this file.

#ifndef _MOVEMENT_PUBLISHER_HPP_
#define _MOVEMENT_PUBLISHER_HPP_

#include <ros/ros.h>
#include <boost/thread.hpp>

#include <std_msgs/Float64.h>
#include "sr_movements/partial_movement.hpp"
#include <pr2_controllers_msgs/JointControllerState.h>
#include <sr_robot_msgs/JointControllerState.h>
#include <math.h>
#include <sr_hand/hand_commander.hpp>

namespace shadowrobot
{
  class MovementPublisher
  {
  public:
    MovementPublisher( std::string joint_name, double rate=100.0,
                       unsigned int repetition = 1, unsigned int nb_mvt_step = 1000 ,
                       std::string controller_type = "", bool testing = false,
                       HandCommander* hand_commander = NULL);

    MovementPublisher( double min_value = 0.0, double max_value = 1.5,
                       double rate=100.0, unsigned int repetition = 1,
                       unsigned int nb_mvt_step = 1000 , std::string controller_type = "");

    virtual ~MovementPublisher();

    void start();
    void stop();

    void calculateErrorCallback(const sr_robot_msgs::JointControllerState::ConstPtr& msg);

    void pr2_calculateErrorCallback(const pr2_controllers_msgs::JointControllerState::ConstPtr& msg);

    void execute_step(int index_mvt_step, int index_partial_movement);

    void add_movement(PartialMovement mvt);

    void set_publisher(ros::Publisher publisher);
    void set_subscriber(ros::Subscriber subscriber);

    std::string get_subscriber_topic();

  protected:
    void subscribe_and_default_pub_(std::string input);

    void publish_();

    boost::shared_ptr<HandCommander> hand_commander_;
    std::string joint_name_;

    std::vector<PartialMovement> partial_movements;
    ros::NodeHandle nh_tilde;
    ros::Publisher pub;
    ros::Publisher pub_mse_;
    ros::Subscriber sub_;

    ros::Rate publishing_rate;
    unsigned int repetition;
    double min, max;

    std_msgs::Float64 msg;
    std::vector<sr_robot_msgs::joint> joint_vector_;
    double last_target_;

    unsigned int nb_mvt_step;
    double SError_;
    double MSError_;
    unsigned int n_samples_;
    std::string controller_type;
  };
}

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/

#endif