sr_articulated_robot.h
Go to the documentation of this file.
1 
30 #ifndef SR_HAND_HAND_SR_ARTICULATED_ROBOT_H
31 #define SR_HAND_HAND_SR_ARTICULATED_ROBOT_H
32 
33 #include <ros/ros.h>
34 #include <string>
35 #include <vector>
36 #include <map>
37 #include <algorithm>
38 
39 // self_test
40 #include "diagnostic_msgs/SelfTest.h"
41 #include "self_test/self_test.h"
42 
43 #include <boost/smart_ptr.hpp>
44 #include <boost/thread/mutex.hpp>
45 #include <boost/noncopyable.hpp>
46 #include <boost/assign.hpp>
47 
48 #ifdef GAZEBO
49 #include <sensor_msgs/JointState.h>
50 #endif
51 
52 
53 namespace debug_values
54 {
56  static const std::map<const std::string, const unsigned int> names_and_offsets
57  = boost::assign::map_list_of("Sensor PID last in", 0) \
58 ("Sensor PID iState", 1) \
59 ("Sensor PID last D", 2) \
60 ("Sensor PID last out", 3) \
61 ("PID last in", 4) \
62 ("PID iState", 5) \
63 ("PID last D", 6) \
64 ("PID last out", 7) \
65 ("Strain Gauge Offset 0", 8) \
66 ("Strain Gauge Offset 1", 9) \
67 ("Num setup Msgs received", 10) \
68 ("Num sensor Msgs received", 11) \
69 ("Sensor Val (motor set P)", 12) \
70 ("Sensor Val (motor sensor)", 13) \
71 ("H-Bridge Duty", 14) \
72 ("Duty Temp", 15);
73 } // namespace debug_values
74 
75 
76 namespace shadowrobot
77 {
81 struct JointData
82 {
83  double position;
84  double target;
85  double temperature;
86  double current;
87  double force;
88  std::string flags;
90  double min;
91  double max;
92  int16_t isJointZero;
93 
102  double last_pos;
103  double velocity;
104 
106  position(0.0), target(0.0), temperature(0.0), current(0.0), force(0.0), flags(""), jointIndex(0), min(0.0),
107  max(90.0), isJointZero(0), publisher_index(0), last_pos_time(0.0), last_pos(0.0), velocity(0.0)
108  {
109  }
110 
112  position(jd.position), target(jd.target), temperature(jd.temperature), current(jd.current), force(jd.force),
113  flags(jd.flags), jointIndex(jd.jointIndex), min(jd.min), max(jd.max), isJointZero(jd.isJointZero),
114  publisher_index(jd.publisher_index), last_pos_time(jd.last_pos_time), last_pos(jd.last_pos),
115  velocity(jd.velocity)
116  {
117  }
118 
119  JointData(const JointData &jd) :
120  position(jd.position), target(jd.target), temperature(jd.temperature), current(jd.current), force(jd.force),
121  flags(jd.flags), jointIndex(jd.jointIndex), min(jd.min), max(jd.max), isJointZero(jd.isJointZero),
122  publisher_index(jd.publisher_index), last_pos_time(jd.last_pos_time), last_pos(jd.last_pos),
123  velocity(jd.velocity)
124  {
125  }
126 };
127 
132 {
133  PARAM_sensor, // select the sensor that is being controller
134  PARAM_target, // select the sensor to read the setpoint from
135  PARAM_valve, // select which valve is being controlled, if appropriate
136  PARAM_deadband, // select the deadband value, if appropriate (actual deadband used is \f$2^{(deadband-16)}\f$)
137  PARAM_p, // change the proportional gain
138  PARAM_i, // change the integral gain
139  PARAM_d, // change the derivative gain
140  PARAM_imax, // PARAM_imax
141  PARAM_output_offset, // PARAM_output_offset
142  PARAM_shift, // PARAM_shift
143  PARAM_output_max, // PARAM_output_max
144 
145  PARAM_motor_maxforce, // PARAM_motor_maxforce
146  PARAM_motor_safeforce, // PARAM_motor_safeforce
147  PARAM_force_p, // PARAM_force_p
148  PARAM_force_i, // PARAM_force_i
149  PARAM_force_d, // PARAM_force_d
150  PARAM_force_imax, // PARAM_force_imax
151  PARAM_force_out_shift, // PARAM_force_out_shift
152  PARAM_force_deadband, // PARAM_force_deadband
153  PARAM_force_offset, // PARAM_force_offset
154  PARAM_sensor_imax, // PARAM_sensor_imax
155  PARAM_sensor_out_shift, // PARAM_sensor_out_shift
156  PARAM_sensor_deadband, // PARAM_sensor_deadband
157  PARAM_sensor_offset, // PARAM_sensor_offset
158  PARAM_max_temperature, // PARAM_max_temperature
159  PARAM_max_current, // PARAM_max_current
160  PARAM_type_of_sensor, // PARAM_type_of_sensor
161  PARAM_type_of_setpoint // PARAM_type_of_setpoint
162 };
163 
168 {
170  std::string name;
172  std::string value;
173 
175  name(""), value("")
176  {
177  }
178 
180  name(param.name), value(param.value)
181  {
182  }
183 
184  Parameters(const Parameters &param) :
185  name(param.name), value(param.value)
186  {
187  }
188 };
189 
194 {
195  std::vector<Parameters> data;
196 
198  data()
199  {
200  }
201 
203  data(jcd.data)
204  {
205  }
206 
208  data(jcd.data)
209  {
210  }
211 };
212 
213 
218 {
220  std::string joint_name;
222  int16_t level;
223 
225  std::string flags;
226 
232  double target;
233 
235  std::map<std::string, int> debug_values;
237  double position;
238 
240  double temperature;
242  double current;
244  double force;
245 
248 };
249 
258  boost::noncopyable
259 {
260 public:
265  {
266  };
267 
272  {
273  };
274 
276  typedef std::map<std::string, JointData> JointsMap;
278  typedef std::map<std::string, enum controller_parameters> ParametersMap;
279 
286  virtual int16_t sendupdate(std::string joint_name, double target) = 0;
287 
293  virtual JointData getJointData(std::string joint_name) = 0;
294 
299  virtual JointsMap getAllJointsData() = 0;
300 
307  virtual int16_t setContrl(std::string contrlr_name, JointControllerData ctrlr_data) = 0;
308 
314  virtual JointControllerData getContrl(std::string contrlr_name) = 0;
315 
323  virtual int16_t setConfig(std::vector<std::string> myConfig) = 0;
324 
331  virtual void getConfig(std::string joint_name) = 0;
332 
337  virtual std::vector<DiagnosticData> getDiagnostics() = 0;
338 
340  JointsMap joints_map;
341 
343  ParametersMap parameters_map;
344 
345  boost::mutex joints_map_mutex;
346  boost::mutex parameters_map_mutex;
347  boost::mutex controllers_map_mutex;
348 
349 protected:
352 #ifdef GAZEBO
353  std::vector<ros::Publisher> gazebo_publishers;
354  ros::Subscriber gazebo_subscriber;
355 #endif
356 }; // end class
357 
358 } // namespace shadowrobot
359 
360 /* For the emacs weenies in the crowd.
361 Local Variables:
362  c-basic-offset: 2
363 End:
364 */
365 
366 #endif // SR_HAND_HAND_SR_ARTICULATED_ROBOT_H
std::string name
name of the parameter
JointControllerData(const JointControllerData &jcd)
uint64_t num_sensor_msgs_received
values read from the debug node.
Parameters(const Parameters &param)
std::string flags
a string containing flags: FORCE, TEMPERATURE, ... indicating the different cutouts / warning sent by...
std::string joint_name
the name of the joint
std::map< std::string, JointData > JointsMap
static const std::map< const std::string, const unsigned int > names_and_offsets
a map containing the names and offsets of the smart motor node
int target_sensor_num
the channel number of the target sensor
boost::shared_ptr< self_test::TestRunner > self_test
this is the handle for the self tests.
int position_sensor_num
the channel number of the position sensor
std::map< std::string, int > debug_values
the debug values
JointControllerData(JointControllerData &jcd)
JointsMap joints_map
A mapping between the joint names and the information regarding those joints.
JointData(const JointData &jd)
ParametersMap parameters_map
A mapping between the parameter names and their values.
This is a parent class for the different types of Shadowhand we can have. It makes it possible to swa...
std::string value
value of the parameter
double temperature
temperature value
std::map< std::string, enum controller_parameters > ParametersMap
int16_t level
the level of alert: 0 = OK, 1 = WARNING, 2 = ERROR
double target
the actual value of the target
Parameters(Parameters &param)
double position
the actual value of the position


sr_hand
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:53