sr_robot_lib.hpp
Go to the documentation of this file.
1 /*
2 * @file sr_robot_lib.hpp
3 * @author Ugo Cupcic <ugo@shadowrobot.com>
4 * @date Fri Jun 3 12:12:13 2011
5 *
6 *
7 /* Copyright 2011 Shadow Robot Company Ltd.
8 *
9 * This program is free software: you can redistribute it and/or modify it
10 * under the terms of the GNU General Public License as published by the Free
11 * Software Foundation version 2 of the License.
12 *
13 * This program is distributed in the hope that it will be useful, but WITHOUT
14 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
15 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
16 * more details.
17 *
18 * You should have received a copy of the GNU General Public License along
19 * with this program. If not, see <http://www.gnu.org/licenses/>.
20 *
21 * @brief This is a generic robot library for Shadow Robot's Hardware.
22 *
23 *
24 */
25 
26 #ifndef _SR_ROBOT_LIB_HPP_
27 #define _SR_ROBOT_LIB_HPP_
28 
29 #include <boost/smart_ptr.hpp>
30 #include <boost/ptr_container/ptr_vector.hpp>
31 #include <boost/thread/thread.hpp>
32 #include <boost/thread/mutex.hpp>
33 #include <vector>
34 #include <utility>
35 #include <string>
36 #include <deque>
37 #include <map>
38 
39 // used to publish debug values
40 #include <std_msgs/Int16.h>
41 
42 #include <sr_hardware_interface/sr_actuator.hpp>
43 
44 #include <diagnostic_msgs/DiagnosticStatus.h>
46 
47 #include <ros_ethercat_model/robot_state.hpp>
48 
49 #include <sr_robot_msgs/NullifyDemand.h>
50 #include <sr_robot_msgs/SetDebugData.h>
51 
52 #include <sr_utilities/sr_math_utils.hpp>
53 #include <sr_utilities/calibration.hpp>
54 #include <sr_utilities/thread_safe_map.hpp>
55 
58 
60 #include "sr_interpolation/pwl_interp_2d_scattered.hpp"
61 
62 
63 extern "C"
64 {
70 }
71 
72 namespace operation_mode
73 {
74 namespace robot_state
75 {
77  {
81  };
82 } // namespace robot_state
83 } // namespace operation_mode
84 
85 namespace crc_unions
86 {
87 union CRCUnion
88 {
90  int8u byte[2];
91 };
92 
93 typedef CRCUnion union16;
94 }
95 
96 namespace shadow_robot
97 {
98 
100 {
101  public:
102  CoupledJoint(std::string joint_name, std::string joint_sibling_name,
103  std::vector<double> raw_values_coupled_vector, std::vector<double> calibrated_values_vector);
104  ~CoupledJoint();
105 
106  std::string name_;
107  std::string sibling_name_;
108  std::vector<double> raw_values_coupled_;
109  std::vector<double> calibrated_values_;
110  const int nb_surrounding_points_ = 10;
114  std::vector<int> triangle_;
115  std::vector<int> element_neighbor_;
116 
117  private:
118  void process_calibration_values();
119 };
120 
121 template<class StatusType, class CommandType>
123 {
124 public:
125  SrRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
126  std::string device_id, std::string joint_prefix);
127 
128  virtual ~SrRobotLib()
129  {
130  }
131 
132  /*
133  * This function is called each time a new etherCAT message
134  * is received in the sr06.cpp driver. It updates the joints_vector,
135  * updating the different values, computing the calibrated joint
136  * positions, etc... It also updates the tactile sensors values.
137  *
138  * @param status_data the received etherCAT message
139  */
140  virtual void update(StatusType *status_data) = 0;
141 
142  /*
143  * Builds a command for the robot.
144  *
145  * @param command The command we're building.
146  */
147  virtual void build_command(CommandType *command) = 0;
148 
149  /*
150  * Builds a command to demand information form the tactile sensors.
151  *
152  * @param command The command we're building.
153  */
154  void build_tactile_command(CommandType *command);
155 
156  /*
157  * Reads the tactile information.
158  *
159  * @param status The status information that comes from the robot
160  */
161  void update_tactile_info(StatusType *status);
162 
163  /*
164  * This function adds the diagnostics for the hand to the
165  * multi diagnostic status published in sr06.cpp.
166  */
167  virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
169 
170  /*
171  * Initiates the process to retrieve the initialization information from the sensors
172  */
173  void reinitialize_sensors();
174 
175  /*
176  * This service is used to nullify the demand of the etherCAT
177  * hand. If the nullify_demand parameter is set to True,
178  * the demand sent to the robot will be 0, regardless of the
179  * computed effort demanded by the controller. If set to False,
180  * then the demand computed by the controllers will be sent to the motors.
181  *
182  * @param request contains the nullify_demand parameter
183  * @param response empty
184  *
185  * @return always true as it can't fail
186  */
187  bool nullify_demand_callback(sr_robot_msgs::NullifyDemand::Request &request,
188  sr_robot_msgs::NullifyDemand::Response &response);
189 
190 
191  /*
192  * This is a pointer to the tactile object. This pointer
193  * will be instantiated during the initialization cycle,
194  * depending on the type of sensors attached to the hand.
195  */
197 
198  /*
199  * Contains the idle time of the PIC communicating
200  * via etherCAT with the host.
201  */
203 
204  /*
205  * Contains the minimum idle time of the PIC communicating
206  * via etherCAT with the host, this minimum is reset each
207  * time a diagnostic is being published.
208  */
210 
211  // Current update state of the sensors (initialization, operation..)
213 
215 
216  typedef std::map<std::string, CoupledJoint> CoupledJointMapType;
217 
218 protected:
219  // True if we want to set the demand to 0 (stop the controllers)
221 
223  std::vector<shadow_joints::Joint> joints_vector;
224 
225  /*
226  * Initializes the hand library with the needed values.
227  *
228  * @param joint_names A vector containing all the joint names.
229  * @param actuator_ids A vector containing the corresponding actuator ids.
230  * @param joint_to_sensors A vector mapping the joint to the sensor index we read from the palm.
231  */
232  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
233  std::vector<shadow_joints::JointToSensor> joint_to_sensors) = 0;
234 
235  /*
236  * Compute the calibrated position for the given joint. This method is called
237  * from the update method, each time a new message is received.
238  *
239  * @param joint_tmp The joint we want to calibrate.
240  * @param status_data The status information that comes from the robot
241  */
242  virtual void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data) = 0;
243 
244  /*
245  * Returns a pointer to the actuator state for a certain joint.
246  * It checks the actuator type before accessing the state_ field, to avoid accessing the
247  * base class state_ field which is not what we want
248  *
249  * @param joint_tmp The joint we want to get the actuator state from.
250  *
251  * @return a pointer to the actuator state
252  */
253  virtual ros_ethercat_model::Actuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp) = 0;
254 
255  /*
256  * Reads the mapping between the sensors and the joints from the parameter server.
257  *
258  *
259  * @return a vector (size of the number of joints) containing vectors (containing
260  * the sensors which are combined to form a given joint)
261  */
262  std::vector<shadow_joints::JointToSensor> read_joint_to_sensor_mapping();
263 
264  /*
265  * Reads the calibration from the parameter server.
266  *
267  *
268  * @return a calibration map
269  */
270 
271  shadow_joints::CalibrationMap read_joint_calibration();
272  CoupledJointMapType read_coupled_joint_calibration();
273 
274  /*
275  * Simply reads the config from the parameter server.
276  *
277  * @param base_param string with the base name of the set of parameters to apply (found in the yaml file)
278  * @param nb_data_defined number of data defined in the typedef
279  * @param human_readable_data_types names of the types of messages (must match with those in the yaml file)
280  * @param data_types the command values corresponding to every one of the names
281  * @return A vector of UpdateConfig containing the type of data and the frequency
282  * at which we want to poll this data
283  */
284  std::vector<generic_updater::UpdateConfig> read_update_rate_configs(std::string base_param, int nb_data_defined,
285  const char *human_readable_data_types[],
286  const int32u data_types[]);
287 
288  /*
289  * Calback for the timer that controls the timeout for the tactile initialization period
290  */
291  void tactile_init_timer_callback(const ros::TimerEvent &event);
292 
295 
296 
299 
300  // The ROS service handler for nullifying the demand
302 
303  // boost::shared_ptr<SrSelfTest> self_tests_;
304 
305  // Thread for running the tests in parallel when doing the tests on real hand
307 
310 
312  std::string joint_prefix_;
314  std::string device_id_;
315 
316 
317 #ifdef DEBUG_PUBLISHER
318  // These publishers are useful for debugging
319  static const int nb_debug_publishers_const;
320  std::vector<ros::Publisher> debug_publishers;
321  /*
322  * A vector containing pairs:
323  * - associate a motor index
324  * - to a MOTOR_DATA
325  *
326  * This vector has the same size as the debug_publishers vector.
327  */
328  std::vector<boost::shared_ptr<std::pair<int, int> > > debug_motor_indexes_and_data;
329  // static const int debug_mutex_lock_wait_time;
330  // boost::shared_mutex debug_mutex;
331  ros::NodeHandle node_handle;
332  std_msgs::Int16 msg_debug;
333 #endif
334 
335  // The update rate for each sensor information type
336  std::vector<generic_updater::UpdateConfig> generic_sensor_update_rate_configs_vector;
337  // The update rate for each sensor information type
338  std::vector<generic_updater::UpdateConfig> pst3_sensor_update_rate_configs_vector;
339  // The update rate for each sensor information type
340  std::vector<generic_updater::UpdateConfig> biotac_sensor_update_rate_configs_vector;
341  // The update rate for each sensor information type
342  std::vector<generic_updater::UpdateConfig> ubi0_sensor_update_rate_configs_vector;
343 
344  static const int nb_sensor_data;
345  static const char *human_readable_sensor_data_types[];
346  static const int32u sensor_data_types[];
347 
349  // This avoids the tests blocking the main thread
350  void checkSelfTests();
351 
352 public:
354  static const double tactile_timeout;
357 
360 
362 
365  CoupledJointMapType coupled_calibration_map;
366 }; // end class
367 } // namespace shadow_robot
368 
369 /* For the emacs weenies in the crowd.
370 Local Variables:
371  c-basic-offset: 2
372 End:
373 */
374 
375 #endif
std::vector< int > element_neighbor_
d
unsigned short int16u
std::map< std::string, CoupledJoint > CoupledJointMapType
std::vector< shadow_joints::Joint > joints_vector
The vector containing all the robot joints.
boost::shared_ptr< boost::mutex > lock_tactile_init_timeout_
A mutual exclusion object to ensure that the intitialization timeout event does work without threadin...
ROSCONSOLE_DECL void initialize()
static const double tactile_timeout
Timeout handling variables for UBI sensors.
ros::NodeHandle nodehandle_
a ros nodehandle to be able to access resources out of the node namespace
std::string joint_prefix_
Prefix used to access the joints.
shadow_joints::CalibrationMap calibration_map
The map used to calibrate each joint.
boost::shared_ptr< boost::thread > self_test_thread_
unsigned int int32u
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
boost::shared_ptr< tactiles::GenericTactiles< StatusType, CommandType > > tactiles
operation_mode::device_update_state::DeviceUpdateState tactile_current_state
unsigned char int8u
CoupledJointMapType coupled_calibration_map
boost::shared_ptr< shadow_robot::JointCalibration > calibration_tmp
A temporary calibration for a given joint.
ROSLIB_DECL std::string command(const std::string &cmd)
ros::Duration tactile_init_max_duration
ros::NodeHandle nh_tilde
a ROS nodehandle (private naming, only inside the node namespace) to be able to advertise the Force P...
boost::shared_ptr< tactiles::GenericTactiles< StatusType, CommandType > > tactiles_init
std::vector< generic_updater::UpdateConfig > biotac_sensor_update_rate_configs_vector
static const int nb_sensor_data
std::vector< double > calibrated_values_
ros_ethercat_model::RobotState * hw_
ros::Timer tactile_check_init_timeout_timer
std::string device_id_
Id of the ethercat device (alias)
std::vector< generic_updater::UpdateConfig > generic_sensor_update_rate_configs_vector
threadsafe::Map< boost::shared_ptr< shadow_robot::JointCalibration > > CalibrationMap
std::vector< int > triangle_
std::vector< generic_updater::UpdateConfig > ubi0_sensor_update_rate_configs_vector
ros::ServiceServer nullify_demand_server_
std::vector< double > raw_values_coupled_
const std::string response
std::vector< generic_updater::UpdateConfig > pst3_sensor_update_rate_configs_vector
CRCUnion union16


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:43