sr_muscle_robot_lib.hpp

Go to the documentation of this file.

#ifndef _SR_MUSCLE_ROBOT_LIB_HPP_
#define _SR_MUSCLE_ROBOT_LIB_HPP_

#include "sr_robot_lib/sr_robot_lib.hpp"

#include "sr_robot_lib/muscle_updater.hpp"
#include <sr_utilities/calibration.hpp>
#include <string>
#include <map>
#include <queue>
#include <utility>
#include <list>
#include <vector>

#define NUM_MUSCLE_DRIVERS      4

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

  void update(StatusType *status_data);

  void build_command(CommandType *command);

  void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
                       diagnostic_updater::DiagnosticStatusWrapper &d);

  void reinitialize_motors();


  // Current update state of the motor (initialization, operation..)
  operation_mode::device_update_state::DeviceUpdateState muscle_current_state;


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

  void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);

  shadow_joints::CalibrationMap pressure_calibration_map_;
  boost::shared_ptr<shadow_robot::JointCalibration> pressure_calibration_tmp_;


  virtual shadow_joints::CalibrationMap read_pressure_calibration();


  void read_additional_muscle_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);

  void process_position_sensor_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data,
                                    double timestamp);

  void read_muscle_driver_data(std::vector<shadow_joints::MuscleDriver>::iterator muscle_driver_tmp,
                               StatusType *status_data);

  std::vector<std::pair<std::string, bool> > humanize_flags(int flag);

  sr_actuator::SrMuscleActuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp)
  {
    return static_cast<sr_actuator::SrMuscleActuator *>(joint_tmp->actuator_wrapper->actuator);
  }

  unsigned int get_muscle_pressure(int muscle_driver_id, int muscle_id, StatusType *status_data);

  inline void set_muscle_driver_data_received_flags(unsigned int msg_type, int muscle_driver_id);

  inline bool check_muscle_driver_data_received_flags();

  inline void set_valve_demand(uint8_t *muscle_data_byte_to_set, int8_t valve_value, uint8_t shifting_index);

  void init_timer_callback(const ros::TimerEvent &event);

  std::vector<shadow_joints::MuscleDriver> muscle_drivers_vector_;


  boost::shared_ptr<generic_updater::MuscleUpdater<CommandType> > muscle_updater_;


  // contains a queue of muscle driver indexes to reset
  std::queue<int16_t, std::list<int16_t> > reset_muscle_driver_queue;


  // The update rate for each muscle information
  std::vector<generic_updater::UpdateConfig> muscle_update_rate_configs_vector;

  std::map<unsigned int, unsigned int> from_muscle_driver_data_received_flags_;


  ros::Timer check_init_timeout_timer;
  static const double timeout;
  ros::Duration init_max_duration;

  // A mutual exclusion object to ensure that the initialization timeout event does work without threading issues
  boost::shared_ptr<boost::mutex> lock_init_timeout_;
};  // end class
}  // namespace shadow_robot

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

#endif