sr_articulated_robot.h

Go to the documentation of this file.

#ifndef    SR_HAND_HAND_SR_ARTICULATED_ROBOT_H
#define    SR_HAND_HAND_SR_ARTICULATED_ROBOT_H

#include <ros/ros.h>
#include <string>
#include <vector>
#include <map>
#include <algorithm>

// self_test
#include "diagnostic_msgs/SelfTest.h"
#include "self_test/self_test.h"

#include <boost/smart_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/noncopyable.hpp>
#include <boost/assign.hpp>

#ifdef GAZEBO
#include <sensor_msgs/JointState.h>
#endif


namespace debug_values
{
  static const std::map<const std::string, const unsigned int> names_and_offsets
          = boost::assign::map_list_of("Sensor PID last in", 0) \
("Sensor PID iState", 1) \
("Sensor PID last D", 2) \
("Sensor PID last out", 3) \
("PID last in", 4) \
("PID iState", 5) \
("PID last D", 6) \
("PID last out", 7) \
("Strain Gauge Offset 0", 8) \
("Strain Gauge Offset 1", 9) \
("Num setup Msgs received", 10) \
("Num sensor Msgs received", 11) \
("Sensor Val (motor set P)", 12) \
("Sensor Val (motor sensor)", 13) \
("H-Bridge Duty", 14) \
("Duty Temp", 15);
}  // namespace debug_values


namespace shadowrobot
{
struct JointData
{
  double position;
  double target;
  double temperature;
  double current;
  double force;
  std::string flags;
  int jointIndex;
  double min;
  double max;
  int16_t isJointZero;

  int publisher_index;
  ros::Time last_pos_time;
  double last_pos;
  double velocity;

  JointData() :
          position(0.0), target(0.0), temperature(0.0), current(0.0), force(0.0), flags(""), jointIndex(0), min(0.0),
          max(90.0), isJointZero(0), publisher_index(0), last_pos_time(0.0), last_pos(0.0), velocity(0.0)
  {
  }

  JointData(JointData &jd) :
          position(jd.position), target(jd.target), temperature(jd.temperature), current(jd.current), force(jd.force),
          flags(jd.flags), jointIndex(jd.jointIndex), min(jd.min), max(jd.max), isJointZero(jd.isJointZero),
          publisher_index(jd.publisher_index), last_pos_time(jd.last_pos_time), last_pos(jd.last_pos),
          velocity(jd.velocity)
  {
  }

  JointData(const JointData &jd) :
          position(jd.position), target(jd.target), temperature(jd.temperature), current(jd.current), force(jd.force),
          flags(jd.flags), jointIndex(jd.jointIndex), min(jd.min), max(jd.max), isJointZero(jd.isJointZero),
          publisher_index(jd.publisher_index), last_pos_time(jd.last_pos_time), last_pos(jd.last_pos),
          velocity(jd.velocity)
  {
  }
};

enum controller_parameters
{
  PARAM_sensor,  // select the sensor that is being controller
  PARAM_target,  //  select the sensor to read the setpoint from
  PARAM_valve,  //  select which valve is being controlled, if appropriate
  PARAM_deadband,  //  select the deadband value, if appropriate (actual deadband used is \f$2^{(deadband-16)}\f$)
  PARAM_p,  //  change the proportional gain
  PARAM_i,  //  change the integral gain
  PARAM_d,  //  change the derivative gain
  PARAM_imax,  //  PARAM_imax
  PARAM_output_offset,  //  PARAM_output_offset
  PARAM_shift,  //  PARAM_shift
  PARAM_output_max,  //  PARAM_output_max

  PARAM_motor_maxforce,  //  PARAM_motor_maxforce
  PARAM_motor_safeforce,  //  PARAM_motor_safeforce
  PARAM_force_p,  //  PARAM_force_p
  PARAM_force_i,  //  PARAM_force_i
  PARAM_force_d,  //  PARAM_force_d
  PARAM_force_imax,  //  PARAM_force_imax
  PARAM_force_out_shift,  //  PARAM_force_out_shift
  PARAM_force_deadband,  //  PARAM_force_deadband
  PARAM_force_offset,  //  PARAM_force_offset
  PARAM_sensor_imax,  //  PARAM_sensor_imax
  PARAM_sensor_out_shift,  //  PARAM_sensor_out_shift
  PARAM_sensor_deadband,  //  PARAM_sensor_deadband
  PARAM_sensor_offset,  //  PARAM_sensor_offset
  PARAM_max_temperature,  //  PARAM_max_temperature
  PARAM_max_current,  //  PARAM_max_current
  PARAM_type_of_sensor,  //  PARAM_type_of_sensor
  PARAM_type_of_setpoint  //  PARAM_type_of_setpoint
};

struct Parameters
{
  std::string name;
  std::string value;

  Parameters() :
          name(""), value("")
  {
  }

  Parameters(Parameters &param) :
          name(param.name), value(param.value)
  {
  }

  Parameters(const Parameters &param) :
          name(param.name), value(param.value)
  {
  }
};

struct JointControllerData
{
  std::vector<Parameters> data;

  JointControllerData() :
          data()
  {
  }

  JointControllerData(JointControllerData &jcd) :
          data(jcd.data)
  {
  }

  JointControllerData(const JointControllerData &jcd) :
          data(jcd.data)
  {
  }
};


struct DiagnosticData
{
  std::string joint_name;
  int16_t level;

  std::string flags;

  int target_sensor_num;
  int position_sensor_num;
  double target;

  std::map<std::string, int> debug_values;
  double position;

  double temperature;
  double current;
  double force;

  uint64_t num_sensor_msgs_received;
};

class SRArticulatedRobot :
        boost::noncopyable
{
public:
  SRArticulatedRobot()
  {
  };

  virtual ~SRArticulatedRobot()
  {
  };

  typedef std::map<std::string, JointData> JointsMap;
  typedef std::map<std::string, enum controller_parameters> ParametersMap;

  virtual int16_t sendupdate(std::string joint_name, double target) = 0;

  virtual JointData getJointData(std::string joint_name) = 0;

  virtual JointsMap getAllJointsData() = 0;

  virtual int16_t setContrl(std::string contrlr_name, JointControllerData ctrlr_data) = 0;

  virtual JointControllerData getContrl(std::string contrlr_name) = 0;

  virtual int16_t setConfig(std::vector<std::string> myConfig) = 0;

  virtual void getConfig(std::string joint_name) = 0;

  virtual std::vector<DiagnosticData> getDiagnostics() = 0;

  JointsMap joints_map;

  ParametersMap parameters_map;

  boost::mutex joints_map_mutex;
  boost::mutex parameters_map_mutex;
  boost::mutex controllers_map_mutex;

protected:
  boost::shared_ptr<self_test::TestRunner> self_test;
#ifdef GAZEBO
  std::vector<ros::Publisher> gazebo_publishers;
  ros::Subscriber gazebo_subscriber;
#endif
};  // end class

}  // namespace shadowrobot

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

#endif  // SR_HAND_HAND_SR_ARTICULATED_ROBOT_H