generic_tactiles.cpp
Go to the documentation of this file.
1 
28 #include <sr_utilities/sr_math_utils.hpp>
29 #include <cctype>
30 #include <string>
31 #include <vector>
32 
33 // NOTE: The length used in this generic tactile class (that is used to obtain common information to determine
34 // the actual type of tactile sensors)
35 // should be the the minimum length of all the existing types of tactile sensors
36 // (this is used only to sanitise_string for certain data types)
37 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1
38 
39 namespace tactiles
40 {
41  template<class StatusType, class CommandType>
43 
44  template<class StatusType, class CommandType>
45  GenericTactiles<StatusType,
46  CommandType>::GenericTactiles(ros::NodeHandle nh, std::string device_id,
47  std::vector<generic_updater::UpdateConfig> update_configs_vector,
49  : nodehandle_(nh),
50  device_id_(device_id)
51  {
53  new generic_updater::SensorUpdater<CommandType>(update_configs_vector, update_state));
55  {
56  reset_service_client_ = nodehandle_.advertiseService("tactiles/reset", &GenericTactiles::reset, this);
57  }
58 
59  // initialize the vector of tactiles
61  new std::vector<GenericTactileData>(nb_tactiles));
62  all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >(new std::vector<AllTactileData>(nb_tactiles));
63 
64  for (unsigned int i = 0; i < nb_tactiles; i++)
65  {
66  GenericTactileData tmp_pst;
67  tactiles_vector->push_back(tmp_pst);
68  }
69  }
70 
71  template<class StatusType, class CommandType>
73  {
74  int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
75  // @todo use memcopy instead?
76  for (unsigned int id_sensor = 0; id_sensor < nb_tactiles; ++id_sensor)
77  {
78  ROS_DEBUG_STREAM(" received: " << static_cast<int32u>(status_data->tactile_data_type));
79 
80  switch (static_cast<int32u>(status_data->tactile_data_type))
81  {
82  // COMMON DATA
84  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
85  {
86  if (tactiles_vector != NULL)
87  {
88  tactiles_vector->at(id_sensor).which_sensor =
89  static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
90  }
91  ROS_DEBUG_STREAM(" tact[" << id_sensor << "] = " << tactiles_vector->at(id_sensor).which_sensor);
92  }
93  break;
94 
96  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
97  {
98  if (tactiles_vector != NULL)
99  {
100  tactiles_vector->at(id_sensor).sample_frequency =
101  static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
102  }
103  }
104  break;
105 
107  {
108  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
109  {
110  tactiles_vector->at(id_sensor).manufacturer = sanitise_string(status_data->tactile[id_sensor].string,
112  }
113  }
114  break;
115 
117  {
118  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
119  {
120  tactiles_vector->at(id_sensor).serial_number = sanitise_string(status_data->tactile[id_sensor].string,
122  }
123  }
124  break;
125 
127  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
128  {
129  if (tactiles_vector != NULL)
130  {
131  tactiles_vector->at(id_sensor).set_software_version(status_data->tactile[id_sensor].string);
132  }
133  }
134  break;
135 
137  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
138  {
139  if (tactiles_vector != NULL)
140  {
141  tactiles_vector->at(id_sensor).pcb_version = sanitise_string(status_data->tactile[id_sensor].string,
143  }
144  }
145  break;
146 
147  default:
148  break;
149  } // end switch
150  } // end for tactile
151 
152  if (sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
153  {
154  process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
155  if (sensor_updater->initialization_configs_vector.size() == 0)
156  {
157  sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
158  }
159  }
160  }
161 
162  template<class StatusType, class CommandType>
164  {
165  unsigned int i;
166  for (i = 0; i < sensor_updater->initialization_configs_vector.size(); i++)
167  {
168  if (sensor_updater->initialization_configs_vector[i].what_to_update == data)
169  {
170  break;
171  }
172  }
173  if (i < sensor_updater->initialization_configs_vector.size())
174  {
175  sensor_updater->initialization_configs_vector.erase(sensor_updater->initialization_configs_vector.begin() + i);
176  }
177  }
178 
179  template<class StatusType, class CommandType>
181  {
182  // We don't publish anything during the initialization phase
183  } // end publish
184 
185  template<class StatusType, class CommandType>
186  void GenericTactiles<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
188  {
189  // We don't publish diagnostics during the initialization phase
190  }
191 
200  template<class StatusType, class CommandType>
201  bool GenericTactiles<StatusType, CommandType>::reset(std_srvs::Empty::Request &request,
202  std_srvs::Empty::Response &response)
203  {
204  ROS_INFO_STREAM("Resetting tactiles");
205 
206  return sensor_updater->reset();
207  }
208 
209  template<class StatusType, class CommandType>
211  const unsigned int str_size)
212  {
213  std::string sanitised_string = "";
214  for (unsigned int i = 0; i < str_size; ++i)
215  {
216  char tmp = static_cast<char>(raw_string[i]);
217  if (tmp != 0)
218  {
219  if (tmp >= '\x20' && tmp <= '\x7E')
220  {
221  sanitised_string += static_cast<char>(raw_string[i]);
222  }
223  else
224  {
225  sanitised_string += '?';
226  }
227  }
228  else
229  {
230  break;
231  }
232  }
233  return sanitised_string;
234  }
235 
236  template<class StatusType, class CommandType>
238  {
239  return all_tactile_data.get();
240  }
241 
242  // Only to ensure that the template class is compiled for the types we are interested in
243  template
245 
246  template
248 
249  template
251 
252  template
254 } // namespace tactiles
255 
256 /* For the emacs weenies in the crowd.
257  Local Variables:
258  c-basic-offset: 2
259  End:
260 */
unsigned short int16u
TACTILE_SENSOR_TYPE_SOFTWARE_VERSION
TACTILE_SENSOR_TYPE_MANUFACTURER
static const unsigned int nb_tactiles
Number of tactile sensors (TODO: should probably be defined in the protocol)
unsigned int int32u
TACTILE_SENSOR_TYPE_SERIAL_NUMBER
TACTILE_SENSOR_TYPE_PCB_VERSION
#define ROS_DEBUG_STREAM(args)
TACTILE_SENSOR_TYPE_WHICH_SENSORS
TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ
#define ROS_INFO_STREAM(args)
#define TACTILE_DATA_LENGTH_BYTES


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