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 <string>
#include <vector>

#include <std_msgs/Float64.h>
#include "sr_movements/partial_movement.hpp"
#include <control_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 sr_calculateErrorCallback(const sr_robot_msgs::JointControllerState::ConstPtr& msg);

  void calculateErrorCallback(const control_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;
};
}  // namespace shadowrobot

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

#endif