sr_motor_hand_lib.hpp

Go to the documentation of this file.

#ifndef _SR_MOTOR_HAND_LIB_HPP_
#define _SR_MOTOR_HAND_LIB_HPP_

#include "sr_robot_lib/sr_motor_robot_lib.hpp"
#include <std_srvs/Empty.h>

// to be able to load the configuration from the
// parameter server
#include <ros/ros.h>
#include <string>
#include <utility>
#include <map>
#include <vector>

namespace shadow_robot
{
template<class StatusType, class CommandType>
class SrMotorHandLib :
        public SrMotorRobotLib<StatusType, CommandType>
{
public:
  SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
                 std::string device_id, std::string joint_prefix);

  bool force_pid_callback(sr_robot_msgs::ForceController::Request &request,
                          sr_robot_msgs::ForceController::Response &response,
                          int motor_index);

  bool reset_motor_callback(std_srvs::Empty::Request &request,
                            std_srvs::Empty::Response &response,
                            std::pair<int, std::string> joint);

#ifdef DEBUG_PUBLISHER

  bool set_debug_data_to_publish(sr_robot_msgs::SetDebugData::Request& request,
                                 sr_robot_msgs::SetDebugData::Response& response);
#endif

protected:
  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
                          std::vector<shadow_joints::JointToSensor> joint_to_sensors);

  void update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f,
                                            int p,
                                            int i, int d, int imax, int deadband, int sign);

  std::string find_joint_name(int motor_index);

private:
  std::vector<int> read_joint_to_motor_mapping();


  static const int nb_motor_data;
  static const char *human_readable_motor_data_types[];
  static const int32u motor_data_types[];

  void resend_pids(std::string joint_name, int motor_index);

  std::map<std::string, ros::Timer> pid_timers;
};
}  // namespace shadow_robot

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

#endif