biotac.cpp
Go to the documentation of this file.
1 
27 #include "sr_robot_lib/biotac.hpp"
28 #include <sr_utilities/sr_math_utils.hpp>
29 #include <string>
30 #include <vector>
31 
32 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1
33 
34 namespace tactiles
35 {
36 
37  template <class StatusType, class CommandType>
39 
40  template <class StatusType, class CommandType>
42 
43  template<class StatusType, class CommandType>
45  std::vector<generic_updater::UpdateConfig> update_configs_vector,
47  : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
48  {
49  init(update_configs_vector, update_state);
50  }
51 
52  template<class StatusType, class CommandType>
54  std::vector<generic_updater::UpdateConfig> update_configs_vector,
56  boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector)
57  : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
58  {
59  init(update_configs_vector, update_state);
60  for (unsigned int i = 0; i < this->nb_tactiles; i++)
61  {
62  BiotacData tmp_biotac(init_tactiles_vector->at(i));
63  this->all_tactile_data->at(i).biotac = tmp_biotac;
64  }
65 
67  }
68 
69  template<class StatusType, class CommandType>
70  void Biotac<StatusType, CommandType>::init(std::vector<generic_updater::UpdateConfig> update_configs_vector,
72  {
73  // initialize the vector of tactiles
75  new std::vector<AllTactileData>(this->nb_tactiles));
76 
77  for (size_t i = 0; i < this->all_tactile_data->size(); ++i)
78  {
79  this->all_tactile_data->at(i).type = "biotac";
80  }
81  }
82 
83  template<class StatusType, class CommandType>
84  void Biotac<StatusType, CommandType>::update(StatusType *status_data)
85  {
86  int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
87  // @todo use memcopy instead?
88  for (unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
89  {
91  reinterpret_cast<TACTILE_SENSOR_BIOTAC_DATA_CONTENTS*> (&(status_data->tactile[id_sensor]));
92  // We always receive two latest Pac values
93  this->all_tactile_data->at(id_sensor).biotac.pac0 = static_cast<int>(tactile_data->Pac[0]);
94  this->all_tactile_data->at(id_sensor).biotac.pac1 = static_cast<int>(tactile_data->Pac[1]);
95  this->all_tactile_data->at(id_sensor).biotac.pac_buffer_.push_back(static_cast<int>(tactile_data->Pac[0]));
96  this->all_tactile_data->at(id_sensor).biotac.pac_buffer_.push_back(static_cast<int>(tactile_data->Pac[1]));
97 
98  // the rest of the data is sampled at different rates
99  switch (static_cast<int32u>(status_data->tactile_data_type))
100  {
101  // TACTILE DATA
103  ROS_WARN("received invalid tactile type");
104  break;
105 
107  if (tactile_data->data_valid.other_sensor_0)
108  {
109  this->all_tactile_data->at(id_sensor).biotac.pdc = static_cast<int>(tactile_data->other_sensor_0);
110  }
111  else
112  {
113  // TODO(shadow_team): add some error stats
114  }
115  if (tactile_data->data_valid.other_sensor_1)
116  {
117  this->all_tactile_data->at(id_sensor).biotac.tac = static_cast<int>(tactile_data->other_sensor_1);
118  }
119  break;
120 
121  // case TACTILE_SENSOR_TYPE_BIOTAC_TAC:
122  // this->all_tactile_data->at(id_sensor).biotac.tac =
123  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]));
124  // break;
125 
127  if (tactile_data->data_valid.other_sensor_0)
128  {
129  this->all_tactile_data->at(id_sensor).biotac.tdc = static_cast<int>(tactile_data->other_sensor_0);
130  }
131  if (tactile_data->data_valid.other_sensor_1)
132  {
133  this->all_tactile_data->at(id_sensor).biotac.electrodes[0] = static_cast<int>(tactile_data->other_sensor_1);
134  }
135  break;
136 
137  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1:
138  // this->all_tactile_data->at(id_sensor).biotac.electrodes[0] =
139  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
140  // break;
141 
143  if (tactile_data->data_valid.other_sensor_0)
144  {
145  this->all_tactile_data->at(id_sensor).biotac.electrodes[1] = static_cast<int>(tactile_data->other_sensor_0);
146  }
147  if (tactile_data->data_valid.other_sensor_1)
148  {
149  this->all_tactile_data->at(id_sensor).biotac.electrodes[2] = static_cast<int>(tactile_data->other_sensor_1);
150  }
151  break;
152 
153  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3:
154  // this->all_tactile_data->at(id_sensor).biotac.electrodes[2] =
155  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
156  // break;
157 
159  if (tactile_data->data_valid.other_sensor_0)
160  {
161  this->all_tactile_data->at(id_sensor).biotac.electrodes[3] = static_cast<int>(tactile_data->other_sensor_0);
162  }
163  if (tactile_data->data_valid.other_sensor_1)
164  {
165  this->all_tactile_data->at(id_sensor).biotac.electrodes[4] = static_cast<int>(tactile_data->other_sensor_1);
166  }
167  break;
168 
169  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5:
170  // this->all_tactile_data->at(id_sensor).biotac.electrodes[4] =
171  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
172  // break;
173 
175  if (tactile_data->data_valid.other_sensor_0)
176  {
177  this->all_tactile_data->at(id_sensor).biotac.electrodes[5] = static_cast<int>(tactile_data->other_sensor_0);
178  }
179  if (tactile_data->data_valid.other_sensor_1)
180  {
181  this->all_tactile_data->at(id_sensor).biotac.electrodes[6] = static_cast<int>(tactile_data->other_sensor_1);
182  }
183  break;
184 
185  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7:
186  // this->all_tactile_data->at(id_sensor).biotac.electrodes[6] =
187  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
188  // break;
189 
191  if (tactile_data->data_valid.other_sensor_0)
192  {
193  this->all_tactile_data->at(id_sensor).biotac.electrodes[7] = static_cast<int>(tactile_data->other_sensor_0);
194  }
195  if (tactile_data->data_valid.other_sensor_1)
196  {
197  this->all_tactile_data->at(id_sensor).biotac.electrodes[8] = static_cast<int>(tactile_data->other_sensor_1);
198  }
199  break;
200 
201  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9:
202  // this->all_tactile_data->at(id_sensor).biotac.electrodes[8] =
203  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
204  // break;
205 
207  if (tactile_data->data_valid.other_sensor_0)
208  {
209  this->all_tactile_data->at(id_sensor).biotac.electrodes[9] = static_cast<int>(
210  tactile_data->other_sensor_0);
211  }
212  if (tactile_data->data_valid.other_sensor_1)
213  {
214  this->all_tactile_data->at(id_sensor).biotac.electrodes[10] = static_cast<int>(
215  tactile_data->other_sensor_1);
216  }
217  break;
218 
219  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11:
220  // this->all_tactile_data->at(id_sensor).biotac.electrodes[10] =
221  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
222  // break;
223 
225  if (tactile_data->data_valid.other_sensor_0)
226  {
227  this->all_tactile_data->at(id_sensor).biotac.electrodes[11] = static_cast<int>(
228  tactile_data->other_sensor_0);
229  }
230  if (tactile_data->data_valid.other_sensor_1)
231  {
232  this->all_tactile_data->at(id_sensor).biotac.electrodes[12] = static_cast<int>(
233  tactile_data->other_sensor_1);
234  }
235  break;
236 
237  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13:
238  // this->all_tactile_data->at(id_sensor).biotac.electrodes[12] =
239  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
240  // break;
241 
243  if (tactile_data->data_valid.other_sensor_0)
244  {
245  this->all_tactile_data->at(id_sensor).biotac.electrodes[13] = static_cast<int>(
246  tactile_data->other_sensor_0);
247  }
248  if (tactile_data->data_valid.other_sensor_1)
249  {
250  this->all_tactile_data->at(id_sensor).biotac.electrodes[14] = static_cast<int>(
251  tactile_data->other_sensor_1);
252  }
253  break;
254 
255  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15:
256  // this->all_tactile_data->at(id_sensor).biotac.electrodes[14] =
257  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
258  // break;
259 
261  if (tactile_data->data_valid.other_sensor_0)
262  {
263  this->all_tactile_data->at(id_sensor).biotac.electrodes[15] = static_cast<int>(
264  tactile_data->other_sensor_0);
265  }
266  if (tactile_data->data_valid.other_sensor_1)
267  {
268  this->all_tactile_data->at(id_sensor).biotac.electrodes[16] = static_cast<int>(
269  tactile_data->other_sensor_1);
270  }
271  break;
272 
273  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17:
274  // this->all_tactile_data->at(id_sensor).biotac.electrodes[16] =
275  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
276  // break;
277 
279  if (tactile_data->data_valid.other_sensor_0)
280  {
281  this->all_tactile_data->at(id_sensor).biotac.electrodes[17] = static_cast<int>(
282  tactile_data->other_sensor_0);
283  }
284  if (tactile_data->data_valid.other_sensor_1)
285  {
286  this->all_tactile_data->at(id_sensor).biotac.electrodes[18] = static_cast<int>(
287  tactile_data->other_sensor_1);
288  }
289  break;
290 
291  // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19:
292  // this->all_tactile_data->at(id_sensor).biotac.electrodes[18] =
293  // static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
294  // break;
295 
297  if (tactile_data->data_valid.other_sensor_0)
298  {
299  this->all_tactile_data->at(id_sensor).biotac.electrodes[19] = static_cast<int>(
300  tactile_data->other_sensor_0);
301  }
302  if (tactile_data->data_valid.other_sensor_1)
303  {
304  this->all_tactile_data->at(id_sensor).biotac.electrodes[20] = static_cast<int>(
305  tactile_data->other_sensor_1);
306  }
307  break;
308 
310  if (tactile_data->data_valid.other_sensor_0)
311  {
312  this->all_tactile_data->at(id_sensor).biotac.electrodes[21] = static_cast<int>(
313  tactile_data->other_sensor_0);
314  }
315  if (tactile_data->data_valid.other_sensor_1)
316  {
317  this->all_tactile_data->at(id_sensor).biotac.electrodes[22] = static_cast<int>(
318  tactile_data->other_sensor_1);
319  }
320  break;
321 
323  if (tactile_data->data_valid.other_sensor_0)
324  {
325  this->all_tactile_data->at(id_sensor).biotac.electrodes[23] = static_cast<int>(
326  tactile_data->other_sensor_0);
327  }
328  // if (tactile_data->data_valid.other_sensor_1)
329  // {
330  // this->all_tactile_data->at(id_sensor).biotac.pdc = static_cast<int>(tactile_data->other_sensor_1);
331  // }
332  break;
333 
334  // COMMON DATA
336  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
337  {
338  this->all_tactile_data->at(id_sensor).biotac.sample_frequency =
339  static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
340  }
341  break;
342 
344  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
345  {
346  this->all_tactile_data->at(id_sensor).biotac.manufacturer = this->sanitise_string(
347  status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES);
348  }
349  break;
350 
352  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
353  {
354  this->all_tactile_data->at(id_sensor).biotac.serial_number = this->sanitise_string(
355  status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES);
356  }
357  break;
358 
360  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
361  {
362  this->all_tactile_data->at(id_sensor).biotac.set_software_version(status_data->tactile[id_sensor].string);
363  }
364  break;
365 
367  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
368  {
369  this->all_tactile_data->at(id_sensor).biotac.pcb_version = this->sanitise_string(
370  status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES);
371  }
372  break;
373 
374  default:
375  break;
376  } // end switch
377  } // end for tactile
378 
380  {
381  this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
382  if (this->sensor_updater->initialization_configs_vector.size() == 0)
383  {
385  }
386  }
387  }
388 
389  template<class StatusType, class CommandType>
391  {
392  // left empty, this is published from the controller publisher
393  } // end publish
394 
395  template <class StatusType, class CommandType>
396  void Biotac<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
398  {
399  for (unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
400  {
401  std::stringstream ss;
402  std::string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
403 
404  ss << prefix << "Tactile " << id_tact + 1;
405 
406  d.name = ss.str().c_str();
407  d.summary(d.OK, "OK");
408  d.clear();
409 
410  d.addf("Sample Frequency", "%d", this->all_tactile_data->at(id_tact).biotac.sample_frequency);
411  d.addf("Manufacturer", "%s", this->all_tactile_data->at(id_tact).biotac.manufacturer.c_str());
412  d.addf("Serial Number", "%s", this->all_tactile_data->at(id_tact).biotac.serial_number.c_str());
413 
414  d.addf("Software Version", "%s", this->all_tactile_data->at(id_tact).biotac.get_software_version().c_str());
415  d.addf("PCB Version", "%s", this->all_tactile_data->at(id_tact).biotac.pcb_version.c_str());
416 
417  vec.push_back(d);
418  }
419  }
420 
421  template<class StatusType, class CommandType>
423  {
424  return this->all_tactile_data.get();
425  }
426 
427  template <class StatusType, class CommandType>
429  {
430  nb_electrodes_ = nb_electrodes_v1_; // We consider biotac version one to be the default
431 
432  for (size_t i = 0; i < this->nb_tactiles; ++i)
433  {
434  // At least one of the fingers has a newer serial number, i.e. starting BTSP or BTTB
435  if (this->all_tactile_data->at(i).biotac.serial_number.find("BTSP") != std::string::npos ||
436  this->all_tactile_data->at(i).biotac.serial_number.find("BTTB") != std::string::npos)
437  {
439  break;
440  }
441  }
442 
443  if (nb_electrodes_ == nb_electrodes_v1_) // If biotac version 1 remove polling for non-existing electrodes
444  {
447  {
448  std::vector<generic_updater::UpdateConfig>::iterator it =
449  this->sensor_updater->important_update_configs_vector.begin();
450  while (it != this->sensor_updater->important_update_configs_vector.end())
451  {
452  if (it->what_to_update == data)
453  {
454  it = this->sensor_updater->important_update_configs_vector.erase(it);
455  }
456  else
457  {
458  it++;
459  }
460  }
461  }
462  }
463 
464  for (unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
465  {
466  this->all_tactile_data->at(id_tact).biotac.electrodes.resize(nb_electrodes_);
467  }
468  }
469 
470  // Only to ensure that the template class is compiled for the types we are interested in
471  template
473 
474  template
476 
477  template
479 
480  template
482 } // namespace tactiles
483 
484 /* For the emacs weenies in the crowd.
485  Local Variables:
486  c-basic-offset: 2
487  End:
488 */
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_20
virtual void add_diagnostics(std::vector< diagnostic_msgs::DiagnosticStatus > &vec, diagnostic_updater::DiagnosticStatusWrapper &d)
Definition: biotac.cpp:396
unsigned short int16u
TACTILE_SENSOR_TYPE_SOFTWARE_VERSION
void init(std::vector< generic_updater::UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
Definition: biotac.cpp:70
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4
static const size_t nb_electrodes_v1_
Definition: biotac.hpp:89
TACTILE_SENSOR_TYPE_BIOTAC_TDC
#define TACTILE_DATA_LENGTH_BYTES
Definition: biotac.cpp:32
void summary(unsigned char lvl, const std::string s)
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_22
TACTILE_SENSOR_TYPE_MANUFACTURER
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18
TACTILE_SENSOR_TYPE_BIOTAC_PDC
static const unsigned int nb_tactiles
Number of tactile sensors (TODO: should probably be defined in the protocol)
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10
data
#define ROS_WARN(...)
void addf(const std::string &key, const char *format,...)
boost::shared_ptr< generic_updater::SensorUpdater< CommandType > > sensor_updater
unsigned int int32u
virtual void update(StatusType *status_data)
Definition: biotac.cpp:84
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8
TACTILE_SENSOR_TYPE_SERIAL_NUMBER
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6
TACTILE_SENSOR_TYPE_BIOTAC_INVALID
void set_version_specific_details()
Definition: biotac.cpp:428
static const size_t nb_electrodes_v2_
Definition: biotac.hpp:90
Biotac(ros::NodeHandle nh, std::string device_id, std::vector< generic_updater::UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
Definition: biotac.cpp:44
TACTILE_SENSOR_TYPE_PCB_VERSION
void process_received_data_type(int32u data)
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_24
size_t nb_electrodes_
Definition: biotac.hpp:87
TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ
virtual std::vector< AllTactileData > * get_tactile_data()
Definition: biotac.cpp:422
boost::shared_ptr< std::vector< AllTactileData > > all_tactile_data
std::string sanitise_string(const char *raw_string, const unsigned int str_size)
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12
virtual void publish()
Definition: biotac.cpp:390
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2
struct TACTILE_SENSOR_BIOTAC_DATA_CONTENTS::@5 data_valid
TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16


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