sr_articulated_robot.h

Go to the documentation of this file.

#ifndef         SHADOWHAND_H_
# define        SHADOWHAND_H_

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

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

#include <boost/smart_ptr.hpp>
#include <boost/thread.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 shadowrobot
{
  struct JointData
  {
    double position;
    double target;
    double temperature;
    double current;
    double force;
    std::string flags;
    int jointIndex;
    double min;
    double max;
    short 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, 
    PARAM_target, 
    PARAM_valve, 
    PARAM_deadband, 
    PARAM_p, 
    PARAM_i, 
    PARAM_d, 
    PARAM_imax, 
    PARAM_output_offset, 
    PARAM_shift, 
    PARAM_output_max, 

    PARAM_motor_maxforce, 
    PARAM_motor_safeforce, 
    PARAM_force_p, 
    PARAM_force_i, 
    PARAM_force_d, 
    PARAM_force_imax, 
    PARAM_force_out_shift, 
    PARAM_force_deadband, 
    PARAM_force_offset, 
    PARAM_sensor_imax, 
    PARAM_sensor_out_shift,
    PARAM_sensor_deadband, 
    PARAM_sensor_offset, 
    PARAM_max_temperature, 
    PARAM_max_current, 
    PARAM_type_of_sensor, 
    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;
    short 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;

    //values read from the debug node.
    uint64_t num_sensor_msgs_received;

  };

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

    ~SRArticulatedRobot()
    {
    }
    ;

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

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

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

    virtual JointsMap getAllJointsData() = 0;

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

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

    virtual short 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

}//end namespace


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

#endif      /* !SHADOWHAND_H_ */