UBI0.cpp
Go to the documentation of this file.
1 
27 #include "sr_robot_lib/UBI0.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_v2
33 
34 namespace tactiles
35 {
36  template<class StatusType, class CommandType>
38  std::vector<generic_updater::UpdateConfig> update_configs_vector,
40  : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
41  {
42  init(update_configs_vector, update_state);
43  }
44 
45  template<class StatusType, class CommandType>
47  std::vector<generic_updater::UpdateConfig> update_configs_vector,
49  boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector)
50  : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
51  {
52  init(update_configs_vector, update_state);
53  tactiles_vector->clear();
54  for (unsigned int i = 0; i < this->nb_tactiles; i++)
55  {
56  UBI0Data tmp_pst(init_tactiles_vector->at(i));
57  tactiles_vector->push_back(tmp_pst);
58  }
59  }
60 
61  template<class StatusType, class CommandType>
62  void UBI0<StatusType, CommandType>::init(std::vector<generic_updater::UpdateConfig> update_configs_vector,
64  {
65  // Auxiliar Spi data (sometimes it is a palm tactile sensor) real time publisher
68 
69  // initialize the vector of tactiles
70  tactiles_vector = boost::shared_ptr<std::vector<UBI0Data> >(new std::vector<UBI0Data>(this->nb_tactiles));
72  new std::vector<AllTactileData>(this->nb_tactiles));
73 
74  for (size_t i = 0; i < this->all_tactile_data->size(); ++i)
75  {
76  this->all_tactile_data->at(i).type = "ubi";
77  }
78 
79  // initialize the palm sensors
80  palm_tactiles = boost::shared_ptr<UBI0PalmData>(new UBI0PalmData());
81  }
82 
83  template<class StatusType, class CommandType>
84  void UBI0<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  {
90  // the rest of the data is sampled at different rates
91  switch (static_cast<int32u>(status_data->tactile_data_type))
92  {
93  // TACTILE DATA
95  for (unsigned int i = 0; i < tactiles_vector->at(id_sensor).distal.size(); ++i)
96  {
97  tactiles_vector->at(id_sensor).distal[i] =
98  static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[i]));
99  }
100  for (unsigned int i = 0; i < tactiles_vector->at(id_sensor).middle.size(); ++i)
101  {
102  tactiles_vector->at(id_sensor).middle[i] =
103  static_cast<int>(static_cast<int16u>(status_data->tactile_mid_prox[id_sensor].named.middle[i]));
104  }
105  for (unsigned int i = 0; i < tactiles_vector->at(id_sensor).proximal.size(); ++i)
106  {
107  tactiles_vector->at(id_sensor).proximal[i] =
108  static_cast<int>(static_cast<int16u>(status_data->tactile_mid_prox[id_sensor].named.proximal[i]));
109  }
110  break;
111 
112  // COMMON DATA
114  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
115  {
116  tactiles_vector->at(id_sensor).sample_frequency =
117  static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
118  }
119  break;
120 
122  {
123  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
124  {
125  tactiles_vector->at(id_sensor).manufacturer = this->sanitise_string(status_data->tactile[id_sensor].string,
127  }
128  }
129  break;
130 
132  {
133  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
134  {
135  tactiles_vector->at(id_sensor).serial_number = this->sanitise_string(status_data->tactile[id_sensor].string,
137  }
138  }
139  break;
140 
142  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
143  {
144  tactiles_vector->at(id_sensor).set_software_version(status_data->tactile[id_sensor].string);
145  }
146  break;
147 
149  if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
150  {
151  tactiles_vector->at(id_sensor).pcb_version = this->sanitise_string(status_data->tactile[id_sensor].string,
153  }
154  break;
155 
156  default:
157  break;
158  } // end switch
159  } // end for tactile
160 
161  for (unsigned int i = 0; i < palm_tactiles->palm.size(); ++i)
162  {
163  palm_tactiles->palm[i] = static_cast<int>(static_cast<int16u>(status_data->aux_spi_sensor.sensor[i]));
164  }
165 
167  {
168  this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
169  if (this->sensor_updater->initialization_configs_vector.size() == 0)
170  {
172  }
173  }
174  }
175 
176  template<class StatusType, class CommandType>
178  {
179  // @TODO move this to the controller publisher (issue #38)
180  // this is not easily accessible from the tactile controller publisher yet
181  if (aux_spi_publisher->trylock())
182  {
183  sr_robot_msgs::AuxSpiData tactiles;
184  tactiles.header.stamp = ros::Time::now();
185 
186  tactiles.sensors = palm_tactiles->palm;
187 
188  aux_spi_publisher->msg_ = tactiles;
189  aux_spi_publisher->unlockAndPublish();
190  }
191  } // end publish
192 
193  template<class StatusType, class CommandType>
194  void UBI0<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
196  {
197  for (unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
198  {
199  std::stringstream ss;
200  std::string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
201 
202  ss << prefix << "Tactile " << id_tact + 1;
203 
204  d.name = ss.str().c_str();
205  d.summary(d.OK, "OK");
206  d.clear();
207 
208  d.addf("Sample Frequency", "%d", tactiles_vector->at(id_tact).sample_frequency);
209  d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
210  d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
211 
212  d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
213  d.addf("PCB Version", "%s", tactiles_vector->at(id_tact).pcb_version.c_str());
214 
215  vec.push_back(d);
216  }
217  }
218 
219  template<class StatusType, class CommandType>
220  std::vector<AllTactileData> *UBI0<StatusType, CommandType>::get_tactile_data()
221  {
222  for (unsigned int i = 0; i < tactiles_vector->size(); ++i)
223  {
224  this->all_tactile_data->at(i).ubi0 = tactiles_vector->at(i);
225  }
226 
227  return this->all_tactile_data.get();
228  }
229 
230  // Only to ensure that the template class is compiled for the types we are interested in
231  template
233 
234  template
236 
237  template
239 
240  template
242 
243 } // namespace tactiles
244 
245 /* For the emacs weenies in the crowd.
246  Local Variables:
247  c-basic-offset: 2
248  End:
249 */
unsigned short int16u
TACTILE_SENSOR_TYPE_SOFTWARE_VERSION
void summary(unsigned char lvl, const std::string s)
TACTILE_SENSOR_TYPE_MANUFACTURER
UBI0(ros::NodeHandle nh, std::string device_id, std::vector< generic_updater::UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
Definition: UBI0.cpp:37
virtual std::vector< AllTactileData > * get_tactile_data()
Definition: UBI0.cpp:220
virtual void update(StatusType *status_data)
Definition: UBI0.cpp:84
static const unsigned int nb_tactiles
Number of tactile sensors (TODO: should probably be defined in the protocol)
#define TACTILE_DATA_LENGTH_BYTES
Definition: UBI0.cpp:32
TACTILE_SENSOR_TYPE_UBI0_TACTILE
void addf(const std::string &key, const char *format,...)
boost::shared_ptr< generic_updater::SensorUpdater< CommandType > > sensor_updater
void init(std::vector< generic_updater::UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
Definition: UBI0.cpp:62
boost::shared_ptr< UBI0PalmData > palm_tactiles
the object containing the data from the palm sensors
Definition: UBI0.hpp:92
TACTILE_SENSOR_TYPE_SERIAL_NUMBER
boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::AuxSpiData > > aux_spi_publisher
Definition: UBI0.hpp:95
boost::shared_ptr< std::vector< UBI0Data > > tactiles_vector
the vector containing the data for the tactiles.
Definition: UBI0.hpp:89
TACTILE_SENSOR_TYPE_PCB_VERSION
void process_received_data_type(int32u data)
virtual void publish()
Definition: UBI0.cpp:177
TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ
static Time now()
boost::shared_ptr< std::vector< AllTactileData > > all_tactile_data
std::string sanitise_string(const char *raw_string, const unsigned int str_size)
virtual void add_diagnostics(std::vector< diagnostic_msgs::DiagnosticStatus > &vec, diagnostic_updater::DiagnosticStatusWrapper &d)
Definition: UBI0.cpp:194


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