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>

namespace shadow_robot
{
  template <class StatusType, class CommandType>
  class SrMotorHandLib : public SrMotorRobotLib<StatusType, CommandType>
  {
  public:
    SrMotorHandLib(pr2_hardware_interface::HardwareInterface *hw);
    ~SrMotorHandLib();

    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,
                            std::vector<sr_actuator::SrGenericActuator*> actuators);

    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[];

    ros::ServiceServer debug_service;

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

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

}

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

#endif