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>

#define NUM_MUSCLE_DRIVERS      4

namespace shadow_robot
{
  template <class StatusType, class CommandType>
  class SrMuscleRobotLib : public SrRobotLib<StatusType, CommandType>
  {
  public:
    SrMuscleRobotLib(pr2_hardware_interface::HardwareInterface *hw);
    virtual ~SrMuscleRobotLib() {};

    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();


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

     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(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data);

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

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

    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);

    boost::ptr_vector<shadow_joints::MuscleDriver> muscle_drivers_vector_;


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


    std::queue<short, std::list<short> > reset_muscle_driver_queue;



    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;

    boost::shared_ptr<boost::mutex> lock_init_timeout_;
  };//end class
}//end namespace

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

#endif