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"

#include <string>
#include <queue>
#include <utility>
#include <list>
#include <vector>

namespace shadow_robot
{
template<class StatusType, class CommandType>
class SrMotorRobotLib :
        public SrRobotLib<StatusType, CommandType>
{
public:
  SrMotorRobotLib(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 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) = 0;

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

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

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

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

  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;
  // this index is used to iterate over the config we're sending.
  int config_index;

  // contains a queue of motor indexes to reset
  std::queue<int16_t, std::list<int16_t> > reset_motors_queue;


  // The index of the motor in all the 20 motors
  int motor_index_full;
  // The index of the motor in the current message (from 0 to 9)
  int index_motor_in_msg;

  int8u crc_byte;
  int16u crc_result;
  int8u crc_i;


  // The update rate for each motor information
  std::vector<generic_updater::UpdateConfig> motor_update_rate_configs_vector;

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


  // The current type of control (FORCE demand or PWM demand sent to the motors)
  sr_robot_msgs::ControlType control_type_;
  bool control_type_changed_flag_;
  // A service server used to change the control type on the fly.
  ros::ServiceServer change_control_type_;
  // A mutual exclusion object to ensure that no command will be sent to the robot while a change
  // in the control type (PWM or torque) is ongoing
  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);

  // The Flag which will be sent to change the motor controls
  std::queue<std::vector<sr_robot_msgs::MotorSystemControls>,
          std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
  // A service server used to call the different motor system controls "buttons"
  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
}  // namespace shadow_robot

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

#endif