sr_motor_robot_lib.hpp

Go to the documentation of this file.

#ifndef _SR_MOTOR_ROBOT_LIB_HPP_
#define _SR_MOTOR_ROBOT_LIB_HPP_

#include "sr_robot_lib/sr_robot_lib.hpp"

#include <sr_robot_msgs/ForceController.h>
#include <sr_robot_msgs/ControlType.h>
#include <sr_robot_msgs/ChangeControlType.h>
#include <sr_robot_msgs/MotorSystemControls.h>
#include <sr_robot_msgs/ChangeMotorSystemControls.h>

#include "sr_robot_lib/motor_updater.hpp"
#include "sr_robot_lib/motor_data_checker.hpp"



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

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

    void read_additional_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data);

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

    void generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right,
                                       int f, int p, int i, int d, int imax,
                                       int deadband, int sign);

    boost::shared_ptr<generic_updater::MotorUpdater<CommandType> > motor_updater_;


    typedef std::pair<int, std::vector<crc_unions::union16> > ForceConfig;
    std::queue<ForceConfig, std::list<ForceConfig> > reconfig_queue;
    int config_index;

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


    int motor_index_full;
    int index_motor_in_msg;

    int8u crc_byte;
    int16u crc_result;
    int8u crc_i;


    std::vector<generic_updater::UpdateConfig> motor_update_rate_configs_vector;

    boost::shared_ptr<generic_updater::MotorDataChecker> motor_data_checker;


    sr_robot_msgs::ControlType control_type_;
    bool control_type_changed_flag_;
    ros::ServiceServer change_control_type_;
    boost::shared_ptr<boost::mutex> lock_command_sending_;

    bool change_control_type_callback_( sr_robot_msgs::ChangeControlType::Request& request,
                                        sr_robot_msgs::ChangeControlType::Response& response );

    bool change_control_parameters(int16_t control_type);

    std::queue<std::vector<sr_robot_msgs::MotorSystemControls>, std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
    ros::ServiceServer motor_system_control_server_;

    bool motor_system_controls_callback_( sr_robot_msgs::ChangeMotorSystemControls::Request& request,
                                          sr_robot_msgs::ChangeMotorSystemControls::Response& response );
  };//end class
}//end namespace

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

#endif