sr_hand_lib.hpp

Go to the documentation of this file.

#ifndef _SR_HAND_LIB_HPP_
#define _SR_HAND_LIB_HPP_

#include "sr_robot_lib/sr_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
{
  class SrHandLib : public SrRobotLib
  {
  public:
    SrHandLib(pr2_hardware_interface::HardwareInterface *hw);
    ~SrHandLib();

    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> motor_ids,
                            std::vector<shadow_joints::JointToSensor> joint_to_sensors,
                            std::vector<sr_actuator::SrActuator*> 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:
    ros::NodeHandle nodehandle_;

    std::vector<shadow_joints::JointToSensor> read_joint_to_sensor_mapping();

    shadow_joints::CalibrationMap read_joint_calibration();

    std::vector<int> read_joint_to_motor_mapping();

    std::vector<generic_updater::UpdateConfig> read_update_rate_configs(std::string base_param, int nb_data_defined, const char* human_readable_data_types[], const int32u data_types[]);


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

    static const int nb_sensor_data;
    static const char* human_readable_sensor_data_types[];
    static const int32u sensor_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