message_helpers.cpp
Go to the documentation of this file.
1 #include "message_helpers.hpp"
2 
3 // Statically initialize the metadata for each message type.
4 static HebiCommandMetadata command_metadata;
5 static HebiFeedbackMetadata feedback_metadata;
6 static HebiInfoMetadata info_metadata;
7 
8 struct Init {
9  Init() {
10  hebiCommandGetMetadata(&command_metadata);
11  hebiFeedbackGetMetadata(&feedback_metadata);
12  hebiInfoGetMetadata(&info_metadata);
13  }
14 };
15 
16 static Init initter;
17 
18 namespace hebi {
19 
20 template<typename RefT, typename MetadataT>
21 HebiStatusCode floatGetter(const RefT& ref, MetadataT& metadata, int field, float* value) {
22  auto index = static_cast<int32_t>(field);
23  if (index > metadata.float_field_count_ || index < 0) {
24  return HebiStatusInvalidArgument;
25  }
26  auto has_offset = static_cast<size_t>(index + metadata.float_field_bitfield_offset_);
27  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
28  if (!has_bits.get(has_offset)) {
29  return HebiStatusValueNotSet;
30  }
31  if (value != nullptr) {
32  *value = ref.float_fields_[index];
33  }
34  return HebiStatusSuccess;
35 }
36 
37 HebiStatusCode floatGetter(const HebiCommandRef& ref, int field, float* value) {
38  return floatGetter(ref, command_metadata, field, value);
39 }
40 
41 HebiStatusCode floatGetter(const HebiFeedbackRef& ref, int field, float* value) {
42  return floatGetter(ref, feedback_metadata, field, value);
43 }
44 
45 HebiStatusCode floatGetter(const HebiInfoRef& ref, int field, float* value) {
46  return floatGetter(ref, info_metadata, field, value);
47 }
48 
49 template<typename RefT, typename MetadataT>
50 HebiStatusCode highResAngleGetter(const RefT& ref, MetadataT& metadata, int field, int64_t* revs, float* offset) {
51  auto index = static_cast<int32_t>(field);
52  if (index > metadata.high_res_angle_field_count_ || index < 0) {
53  return HebiStatusInvalidArgument;
54  }
55  auto has_offset = static_cast<size_t>(index + metadata.high_res_angle_field_bitfield_offset_);
56  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
57  if (!has_bits.get(has_offset)) {
58  return HebiStatusValueNotSet;
59  }
60  if (revs != nullptr && offset != nullptr) {
61  const auto& val = ref.high_res_angle_fields_[index];
62  *revs = val.revolutions_;
63  *offset = val.offset_;
64  }
65  return HebiStatusSuccess;
66 }
67 
68 HebiStatusCode highResAngleGetter(const HebiCommandRef& ref, int field, int64_t* revs, float* offset) {
69  return highResAngleGetter(ref, command_metadata, field, revs, offset);
70 }
71 
72 HebiStatusCode highResAngleGetter(const HebiFeedbackRef& ref, int field, int64_t* revs, float* offset) {
73  return highResAngleGetter(ref, feedback_metadata, field, revs, offset);
74 }
75 
76 HebiStatusCode highResAngleGetter(const HebiInfoRef& ref, int field, int64_t* revs, float* offset) {
77  return highResAngleGetter(ref, info_metadata, field, revs, offset);
78 }
79 
80 template<typename RefT, typename MetadataT>
81 HebiStatusCode numberedFloatGetter(const RefT& ref, MetadataT& metadata, int field, size_t number, float* value) {
82  auto index = static_cast<int32_t>(field);
83  if (index > metadata.numbered_float_field_count_ || index < 0) {
84  return HebiStatusInvalidArgument;
85  }
86  if (number == 0 || number > metadata.numbered_float_field_sizes_[index] ) {
87  return HebiStatusValueNotSet;
88  }
89  const auto relativeOffset = static_cast<size_t>(metadata.numbered_float_relative_offsets_[index] + number - 1);
90  const auto has_offset = static_cast<size_t>(metadata.numbered_float_field_bitfield_offset_ + relativeOffset);
91  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
92  if (!has_bits.get(has_offset)) {
93  return HebiStatusValueNotSet;
94  }
95  if (value != nullptr) {
96  *value = ref.numbered_float_fields_[relativeOffset];
97  }
98  return HebiStatusSuccess;
99 }
100 
101 HebiStatusCode numberedFloatGetter(const HebiCommandRef& ref, int field, size_t number, float* value) {
102  return numberedFloatGetter(ref, command_metadata, field, number, value);
103 }
104 
105 HebiStatusCode numberedFloatGetter(const HebiFeedbackRef& ref, int field, size_t number, float* value) {
106  return numberedFloatGetter(ref, feedback_metadata, field, number, value);
107 }
108 
109 HebiStatusCode numberedFloatGetter(const HebiInfoRef& ref, int field, size_t number, float* value) {
110  return numberedFloatGetter(ref, info_metadata, field, number, value);
111 }
112 
113 template<typename RefT, typename MetadataT>
114 HebiStatusCode vector3fGetter(const RefT& ref, MetadataT& metadata, int field, HebiVector3f* value) {
115  auto index = static_cast<int32_t>(field);
116  if (index > metadata.vector3f_field_count_ || index < 0) {
117  return HebiStatusInvalidArgument;
118  }
119  auto has_offset = static_cast<size_t>(index + metadata.vector3f_field_bitfield_offset_);
120  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
121  if (!has_bits.get(has_offset)) {
122  return HebiStatusValueNotSet;
123  }
124  if (value != nullptr) {
125  *value = ref.vector3f_fields_[index];
126  }
127  return HebiStatusSuccess;
128 }
129 
130 HebiStatusCode vector3fGetter(const HebiCommandRef& ref, int field, HebiVector3f* value) {
131  return vector3fGetter(ref, command_metadata, field, value);
132 }
133 
134 HebiStatusCode vector3fGetter(const HebiFeedbackRef& ref, int field, HebiVector3f* value) {
135  return vector3fGetter(ref, feedback_metadata, field, value);
136 }
137 
138 HebiStatusCode vector3fGetter(const HebiInfoRef& ref, int field, HebiVector3f* value) {
139  return vector3fGetter(ref, info_metadata, field, value);
140 }
141 
142 template<typename RefT, typename MetadataT>
143 HebiStatusCode quaternionfGetter(const RefT& ref, MetadataT& metadata, int field, HebiQuaternionf* value) {
144  auto index = static_cast<int32_t>(field);
145  if (index > metadata.quaternionf_field_count_ || index < 0) {
146  return HebiStatusInvalidArgument;
147  }
148  auto has_offset = static_cast<size_t>(index + metadata.quaternionf_field_bitfield_offset_);
149  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
150  if (!has_bits.get(has_offset)) {
151  return HebiStatusValueNotSet;
152  }
153  if (value != nullptr) {
154  *value = ref.quaternionf_fields_[index];
155  }
156  return HebiStatusSuccess;
157 }
158 
159 HebiStatusCode quaternionfGetter(const HebiCommandRef& ref, int field, HebiQuaternionf* value) {
160  return quaternionfGetter(ref, command_metadata, field, value);
161 }
162 
163 HebiStatusCode quaternionfGetter(const HebiFeedbackRef& ref, int field, HebiQuaternionf* value) {
164  return quaternionfGetter(ref, feedback_metadata, field, value);
165 }
166 
167 HebiStatusCode quaternionfGetter(const HebiInfoRef& ref, int field, HebiQuaternionf* value) {
168  return quaternionfGetter(ref, info_metadata, field, value);
169 }
170 
171 template<typename RefT, typename MetadataT>
172 HebiStatusCode floatIoPinGetter(const RefT& ref, MetadataT& metadata, int index, size_t pin_number, float* value) {
173  if (index > metadata.io_field_count_ || index < 0) {
174  return HebiStatusInvalidArgument;
175  }
176  if (pin_number == 0 || pin_number > metadata.io_field_sizes_[index] ) {
177  return HebiStatusInvalidArgument;
178  }
179  const auto relativeOffset = static_cast<size_t>(metadata.io_relative_offsets_[index] + pin_number - 1);
180  const auto has_offset = static_cast<size_t>(metadata.io_field_bitfield_offset_ + relativeOffset);
181  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
182  if (!has_bits.get(has_offset)) {
183  return HebiStatusValueNotSet;
184  }
185  // Additionally, need to check the value of the field (since it holds the variant state)
186  const auto& field = ref.io_fields_[relativeOffset];
187  if (field.stored_type_ != HebiIoBankPinResidentTypeFloat) {
188  return HebiStatusValueNotSet;
189  }
190  if (value != nullptr) {
191  *value = field.float_value_;
192  }
193  return HebiStatusSuccess;
194 }
195 
196 HebiStatusCode floatIoPinGetter(const HebiCommandRef& ref, int index, size_t pin_number, float* value) {
197  return floatIoPinGetter(ref, command_metadata, index, pin_number, value);
198 }
199 
200 HebiStatusCode floatIoPinGetter(const HebiFeedbackRef& ref, int index, size_t pin_number, float* value) {
201  return floatIoPinGetter(ref, feedback_metadata, index, pin_number, value);
202 }
203 
204 HebiStatusCode floatIoPinGetter(const HebiInfoRef& ref, int index, size_t pin_number, float* value) {
205  return floatIoPinGetter(ref, info_metadata, index, pin_number, value);
206 }
207 
208 template<typename RefT, typename MetadataT>
209 HebiStatusCode intIoPinGetter(const RefT& ref, MetadataT& metadata, int index, size_t pin_number, int64_t* value) {
210  if (index > metadata.io_field_count_ || index < 0) {
211  return HebiStatusInvalidArgument;
212  }
213  if (pin_number == 0 || pin_number > metadata.io_field_sizes_[index] ) {
214  return HebiStatusInvalidArgument;
215  }
216  const auto relativeOffset = static_cast<size_t>(metadata.io_relative_offsets_[index] + pin_number - 1);
217  const auto has_offset = static_cast<size_t>(metadata.io_field_bitfield_offset_ + relativeOffset);
218  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
219  if (!has_bits.get(has_offset)) {
220  return HebiStatusValueNotSet;
221  }
222  // Additionally, need to check the value of the field (since it holds the variant state)
223  const auto& field = ref.io_fields_[relativeOffset];
224  if (field.stored_type_ != HebiIoBankPinResidentTypeInteger) {
225  return HebiStatusValueNotSet;
226  }
227  if (value != nullptr) {
228  *value = field.int_value_;
229  }
230  return HebiStatusSuccess;
231 }
232 
233 HebiStatusCode intIoPinGetter(const HebiCommandRef& ref, int index, size_t pin_number, int64_t* value) {
234  return intIoPinGetter(ref, command_metadata, index, pin_number, value);
235 }
236 
237 HebiStatusCode intIoPinGetter(const HebiFeedbackRef& ref, int index, size_t pin_number, int64_t* value) {
238  return intIoPinGetter(ref, feedback_metadata, index, pin_number, value);
239 }
240 
241 HebiStatusCode intIoPinGetter(const HebiInfoRef& ref, int index, size_t pin_number, int64_t* value) {
242  return intIoPinGetter(ref, info_metadata, index, pin_number, value);
243 }
244 
245 template<typename RefT, typename MetadataT>
246 bool flagGetter(const RefT& ref, MetadataT& metadata, int field) {
247  auto index = static_cast<size_t>(field);
248  if (index > metadata.flag_field_count_ || index < 0) {
249  return 0;
250  }
251  auto has_offset = static_cast<size_t>(index + metadata.flag_field_bitfield_offset_);
252  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
253  return has_bits.get(has_offset);
254 }
255 
256 bool flagGetter(const HebiCommandRef& ref, int field) {
257  return flagGetter(ref, command_metadata, field);
258 }
259 
260 bool flagGetter(const HebiFeedbackRef& ref, int field) {
261  return flagGetter(ref, feedback_metadata, field);
262 }
263 
264 bool flagGetter(const HebiInfoRef& ref, int field) {
265  return flagGetter(ref, info_metadata, field);
266 }
267 
268 template<typename RefT, typename MetadataT>
269 HebiStatusCode boolGetter(const RefT& ref, MetadataT& metadata, int field, bool* value) {
270  auto index = static_cast<int32_t>(field);
271  if (index > metadata.bool_field_count_ || index < 0) {
272  return HebiStatusInvalidArgument;
273  }
274  auto has_offset = static_cast<size_t>(index + metadata.bool_field_bitfield_offset_);
275  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
276  if (!has_bits.get(has_offset)) {
277  return HebiStatusValueNotSet;
278  }
279  if (value != nullptr) {
280  *value = ref.bool_fields_[index];
281  }
282  return HebiStatusSuccess;
283 }
284 
285 HebiStatusCode boolGetter(const HebiCommandRef& ref, int field, bool* value) {
286  return boolGetter(ref, command_metadata, field, value);
287 }
288 
289 HebiStatusCode boolGetter(const HebiFeedbackRef& ref, int field, bool* value) {
290  return boolGetter(ref, feedback_metadata, field, value);
291 }
292 
293 HebiStatusCode boolGetter(const HebiInfoRef& ref, int field, bool* value) {
294  return boolGetter(ref, info_metadata, field, value);
295 }
296 
297 template<typename RefT, typename MetadataT>
298 HebiStatusCode enumGetter(const RefT& ref, MetadataT& metadata, int field, int32_t* value) {
299  auto index = static_cast<int32_t>(field);
300  if (index > metadata.enum_field_count_ || index < 0) {
301  return HebiStatusInvalidArgument;
302  }
303  auto has_offset = static_cast<size_t>(index + metadata.enum_field_bitfield_offset_);
304  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
305  if (!has_bits.get(has_offset)) {
306  return HebiStatusValueNotSet;
307  }
308  if (value != nullptr) {
309  *value = ref.enum_fields_[index];
310  }
311  return HebiStatusSuccess;
312 }
313 
314 HebiStatusCode enumGetter(const HebiCommandRef& ref, int field, int32_t* value) {
315  return enumGetter(ref, command_metadata, field, value);
316 }
317 
318 HebiStatusCode enumGetter(const HebiFeedbackRef& ref, int field, int32_t* value) {
319  return enumGetter(ref, feedback_metadata, field, value);
320 }
321 
322 HebiStatusCode enumGetter(const HebiInfoRef& ref, int field, int32_t* value) {
323  return enumGetter(ref, info_metadata, field, value);
324 }
325 
326 template<typename RefT, typename MetadataT>
327 HebiStatusCode uint64Getter(const RefT& ref, MetadataT& metadata, int field, uint64_t* value) {
328  auto index = static_cast<int32_t>(field);
329  if (index > metadata.uint64_field_count_ || index < 0) {
330  return HebiStatusInvalidArgument;
331  }
332  auto has_offset = static_cast<size_t>(index + metadata.uint64_field_bitfield_offset_);
333  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
334  if (!has_bits.get(has_offset)) {
335  return HebiStatusValueNotSet;
336  }
337  if (value != nullptr) {
338  *value = ref.uint64_fields_[index];
339  }
340  return HebiStatusSuccess;
341 }
342 
343 HebiStatusCode uint64Getter(const HebiCommandRef& ref, int field, uint64_t* value) {
344  return uint64Getter(ref, command_metadata, field, value);
345 }
346 
347 HebiStatusCode uint64Getter(const HebiFeedbackRef& ref, int field, uint64_t* value) {
348  return uint64Getter(ref, feedback_metadata, field, value);
349 }
350 
351 HebiStatusCode uint64Getter(const HebiInfoRef& ref, int field, uint64_t* value) {
352  return uint64Getter(ref, info_metadata, field, value);
353 }
354 
355 template<typename RefT, typename MetadataT>
356 HebiStatusCode ledGetter(const RefT& ref, MetadataT& metadata, int field, uint8_t* r, uint8_t* g, uint8_t* b, uint8_t* a) {
357  auto index = static_cast<int32_t>(field);
358  if (index > metadata.led_field_count_ || index < 0) {
359  return HebiStatusInvalidArgument;
360  }
361  auto has_offset = static_cast<size_t>(index + metadata.led_field_bitfield_offset_);
362  hebi::ProxyBitSet has_bits(ref.message_bitfield_, metadata.message_bitfield_count_);
363  if (!has_bits.get(has_offset)) {
364  return HebiStatusValueNotSet;
365  }
366  if (r != nullptr && g != nullptr && b != nullptr && a != nullptr) {
367  uint32_t color = ref.led_fields_[index];
368  *r = (color & (0xFF << 24)) >> 24;
369  *g = (color & (0xFF << 16)) >> 16;
370  *b = (color & (0xFF << 8)) >> 8;
371  *a = (color & 0xFF);
372  }
373  return HebiStatusSuccess;
374 }
375 
376 HebiStatusCode ledGetter(const HebiCommandRef ref, int field, uint8_t* r, uint8_t* g, uint8_t* b, uint8_t* a) {
377  return ledGetter(ref, command_metadata, field, r, g, b, a);
378 }
379 
380 HebiStatusCode ledGetter(const HebiFeedbackRef ref, int field, uint8_t* r, uint8_t* g, uint8_t* b, uint8_t* a) {
381  return ledGetter(ref, feedback_metadata, field, r, g, b, a);
382 }
383 
384 HebiStatusCode ledGetter(const HebiInfoRef ref, int field, uint8_t* r, uint8_t* g, uint8_t* b, uint8_t* a) {
385  return ledGetter(ref, info_metadata, field, r, g, b, a);
386 }
387 
388 void hebiCommandSetEnum(HebiCommandRef& command, HebiCommandEnumField field, const int32_t* value) {
389  auto index = static_cast<size_t>(field);
390  if (index > command_metadata.enum_field_count_ || index < 0) {
391  return;
392  }
393  auto hasOffset = static_cast<size_t>(index + command_metadata.enum_field_bitfield_offset_);
394  hebi::MutableProxyBitSet hasBits(command.message_bitfield_, command_metadata.message_bitfield_count_);
395  if (value == nullptr) {
396  hasBits.reset(hasOffset);
397  } else {
398  hasBits.set(hasOffset);
399  command.enum_fields_[index] = *value;
400  }
401 }
402 
403 void hebiCommandSetFloat(HebiCommandRef& command, HebiCommandFloatField field, const float* value) {
404  auto index = static_cast<size_t>(field);
405  if (index > command_metadata.float_field_count_ || index < 0) {
406  return;
407  }
408  auto has_offset = static_cast<size_t>(index + command_metadata.float_field_bitfield_offset_);
409  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
410  if (value == nullptr) {
411  has_bits.reset(has_offset);
412  } else {
413  has_bits.set(has_offset);
414  command.float_fields_[index] = *value;
415  }
416 }
417 
418 void hebiCommandSetHighResAngle(HebiCommandRef& command, HebiCommandHighResAngleField field, const int64_t* int_part,
419  const float* dec_part) {
420  auto index = static_cast<size_t>(field);
421  if (index > command_metadata.high_res_angle_field_count_ || index < 0) {
422  return;
423  }
424  auto has_offset = static_cast<size_t>(index + command_metadata.high_res_angle_field_bitfield_offset_);
425  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
426  if (int_part != nullptr && dec_part != nullptr) {
427  has_bits.set(has_offset);
428  auto& val = command.high_res_angle_fields_[index];
429  val.revolutions_ = *int_part;
430  val.offset_ = *dec_part;
431  } else {
432  has_bits.reset(has_offset);
433  }
434 }
435 
436 void hebiCommandSetNumberedFloat(HebiCommandRef& command, HebiCommandNumberedFloatField field, size_t number,
437  const float* value) {
438  auto index = static_cast<size_t>(field);
439  if (index > command_metadata.numbered_float_field_count_ || index < 0) {
440  return;
441  }
442  if (number > command_metadata.numbered_float_field_sizes_[index] || number < 1) {
443  return;
444  }
445  // Special cased: also account for `numbered_float_relative_offsets_`, along with the actual number within the field
446  const auto relativeOffset = static_cast<size_t>(command_metadata.numbered_float_relative_offsets_[index] + number - 1);
447  const auto has_offset = static_cast<size_t>(command_metadata.numbered_float_field_bitfield_offset_ + relativeOffset);
448  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
449  if (value != nullptr) {
450  has_bits.set(has_offset);
451  command.numbered_float_fields_[relativeOffset] = *value;
452  } else {
453  has_bits.reset(has_offset);
454  }
455 }
456 
457 void hebiCommandSetBool(HebiCommandRef& command, HebiCommandBoolField field, const int32_t* value) {
458  auto index = static_cast<size_t>(field);
459  if (index > command_metadata.bool_field_count_ || index < 0) {
460  return;
461  }
462  auto has_offset = static_cast<size_t>(index + command_metadata.bool_field_bitfield_offset_);
463  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
464  if (value == nullptr) {
465  has_bits.reset(has_offset);
466  } else {
467  has_bits.set(has_offset);
468  command.bool_fields_[index] = static_cast<bool>(*value);
469  }
470 }
471 
472 void hebiCommandSetFlag(HebiCommandRef& command, HebiCommandFlagField field, int32_t value) {
473  auto index = static_cast<size_t>(field);
474  if (index > command_metadata.flag_field_count_ || index < 0) {
475  return;
476  }
477  auto has_offset = static_cast<size_t>(index + command_metadata.flag_field_bitfield_offset_);
478  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
479  if (value != 0) {
480  has_bits.set(has_offset);
481  } else {
482  has_bits.reset(has_offset);
483  }
484 }
485 
486 void hebiCommandSetIoPinInt(HebiCommandRef& command, HebiCommandIoPinBank bank, size_t pin_number, const int64_t* value) {
487  auto index = static_cast<size_t>(bank);
488  if (index > command_metadata.io_field_count_ || index < 0) {
489  return;
490  }
491  if (pin_number > 8 || pin_number < 1) {
492  return;
493  }
494  const auto relativeOffset = static_cast<size_t>(command_metadata.io_relative_offsets_[index] + pin_number - 1);
495  const auto has_offset = static_cast<size_t>(command_metadata.io_field_bitfield_offset_ + relativeOffset);
496  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
497  auto& field = command.io_fields_[relativeOffset];
498  if (value == nullptr) {
499  has_bits.reset(has_offset);
500  field.stored_type_ = HebiIoBankPinResidentTypeNone;
501  } else {
502  has_bits.set(has_offset);
503  field.stored_type_ = HebiIoBankPinResidentTypeInteger;
504  field.int_value_ = *value;
505  }
506 }
507 
508 void hebiCommandSetIoPinFloat(HebiCommandRef& command, HebiCommandIoPinBank bank, size_t pin_number, const float* value) {
509  auto index = static_cast<size_t>(bank);
510  if (index > command_metadata.io_field_count_ || index < 0) {
511  return;
512  }
513  if (pin_number > 8 || pin_number < 1) {
514  return;
515  }
516  const auto relativeOffset = static_cast<size_t>(command_metadata.io_relative_offsets_[index] + pin_number - 1);
517  const auto has_offset = static_cast<size_t>(command_metadata.io_field_bitfield_offset_ + relativeOffset);
518  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
519  auto& field = command.io_fields_[relativeOffset];
520  if (value == nullptr) {
521  has_bits.reset(has_offset);
522  field.stored_type_ = HebiIoBankPinResidentTypeNone;
523  } else {
524  has_bits.set(has_offset);
525  field.stored_type_ = HebiIoBankPinResidentTypeFloat;
526  field.float_value_ = *value;
527  }
528 }
529 
530 void hebiCommandSetLed(HebiCommandRef& command, HebiCommandLedField field, const Color* color) {
531  auto index = static_cast<size_t>(field);
532  if (index > command_metadata.led_field_count_ || index < 0) {
533  return;
534  }
535  auto has_offset = static_cast<size_t>(index + command_metadata.led_field_bitfield_offset_);
536  hebi::MutableProxyBitSet has_bits(command.message_bitfield_, command_metadata.message_bitfield_count_);
537 
538  if (color == nullptr) {
539  // Clear LED
540  has_bits.reset(has_offset);
541  command.led_fields_[index] = 0;
542  } else {
543  // Note -- 255 alpha is "override", 0 alpha is "module control"
544  has_bits.set(has_offset);
545  auto& val = command.led_fields_[index];
546  val = (static_cast<int>(color->getRed()) << 24) |
547  (static_cast<int>(color->getGreen()) << 16) |
548  (static_cast<int>(color->getBlue()) << 8) |
549  (static_cast<int>(color->getAlpha()));
550  }
551 }
552 
553 
554 } // namespace hebi
hebi::floatGetter
HebiStatusCode floatGetter(const RefT &ref, MetadataT &metadata, int field, float *value)
Definition: message_helpers.cpp:21
hebi::Color::getRed
uint8_t getRed() const
Returns the red channel; value is between 0 and 255.
Definition: color.hpp:27
hebi::hebiCommandSetBool
void hebiCommandSetBool(HebiCommandRef &command, HebiCommandBoolField field, const int32_t *value)
Definition: message_helpers.cpp:457
HebiCommandLedField
HebiCommandLedField
Definition: hebi.h:153
hebi::Color
Structure to describe an RGB color.
Definition: color.hpp:8
HebiCommandMetadata_::io_field_bitfield_offset_
uint32_t io_field_bitfield_offset_
Definition: hebi.h:673
hebi::hebiCommandSetHighResAngle
void hebiCommandSetHighResAngle(HebiCommandRef &command, HebiCommandHighResAngleField field, const int64_t *int_part, const float *dec_part)
Definition: message_helpers.cpp:418
hebi::hebiCommandSetNumberedFloat
void hebiCommandSetNumberedFloat(HebiCommandRef &command, HebiCommandNumberedFloatField field, size_t number, const float *value)
Definition: message_helpers.cpp:436
HebiStatusSuccess
@ HebiStatusSuccess
Definition: hebi.h:24
HebiStatusCode
HebiStatusCode
Enum Types.
Definition: hebi.h:23
HebiQuaternionf_
Definition: hebi.h:570
HebiCommandFloatField
HebiCommandFloatField
Command Enums.
Definition: hebi.h:55
HebiIoBankPinResidentTypeNone
@ HebiIoBankPinResidentTypeNone
Definition: hebi.h:46
hebi::vector3fGetter
HebiStatusCode vector3fGetter(const RefT &ref, MetadataT &metadata, int field, HebiVector3f *value)
Definition: message_helpers.cpp:114
HebiCommandMetadata_
Definition: hebi.h:650
HebiCommandRef_::led_fields_
uint32_t * led_fields_
Definition: hebi.h:643
HebiFeedbackMetadata_
Definition: hebi.h:708
hebi::Color::getGreen
uint8_t getGreen() const
Returns the green channel; value is between 0 and 255.
Definition: color.hpp:29
HebiCommandMetadata_::led_field_count_
uint32_t led_field_count_
Definition: hebi.h:661
hebi::ledGetter
HebiStatusCode ledGetter(const RefT &ref, MetadataT &metadata, int field, uint8_t *r, uint8_t *g, uint8_t *b, uint8_t *a)
Definition: message_helpers.cpp:356
hebi::hebiCommandSetIoPinFloat
void hebiCommandSetIoPinFloat(HebiCommandRef &command, HebiCommandIoPinBank bank, size_t pin_number, const float *value)
Definition: message_helpers.cpp:508
initter
static Init initter
Definition: message_helpers.cpp:16
HebiCommandRef_::message_bitfield_
int32_t * message_bitfield_
Definition: hebi.h:633
hebiCommandGetMetadata
void hebiCommandGetMetadata(HebiCommandMetadata *metadata)
HebiCommandRef_::io_fields_
HebiIoBankPinStruct * io_fields_
Definition: hebi.h:642
HebiIoBankPinResidentTypeFloat
@ HebiIoBankPinResidentTypeFloat
Definition: hebi.h:48
HebiCommandRef_::high_res_angle_fields_
HebiHighResAngleStruct * high_res_angle_fields_
Definition: hebi.h:635
HebiFeedbackRef_
Definition: hebi.h:690
HebiCommandNumberedFloatField
HebiCommandNumberedFloatField
Definition: hebi.h:112
HebiHighResAngleStruct_::revolutions_
int64_t revolutions_
Definition: hebi.h:617
hebi::Color::getAlpha
uint8_t getAlpha() const
Definition: color.hpp:36
HebiCommandMetadata_::enum_field_count_
uint32_t enum_field_count_
Definition: hebi.h:657
hebi::flagGetter
bool flagGetter(const RefT &ref, MetadataT &metadata, int field)
Definition: message_helpers.cpp:246
hebi::hebiCommandSetEnum
void hebiCommandSetEnum(HebiCommandRef &command, HebiCommandEnumField field, const int32_t *value)
Definition: message_helpers.cpp:388
info_metadata
static HebiInfoMetadata info_metadata
Definition: message_helpers.cpp:6
HebiStatusValueNotSet
@ HebiStatusValueNotSet
A buffer supplied to the routine was too small (normally determined by a size parameter)
Definition: hebi.h:27
HebiStatusInvalidArgument
@ HebiStatusInvalidArgument
Success; no failures occurred.
Definition: hebi.h:25
hebi::hebiCommandSetLed
void hebiCommandSetLed(HebiCommandRef &command, HebiCommandLedField field, const Color *color)
Definition: message_helpers.cpp:530
HebiCommandFlagField
HebiCommandFlagField
Definition: hebi.h:129
HebiCommandMetadata_::numbered_float_field_sizes_
const uint32_t * numbered_float_field_sizes_
Definition: hebi.h:679
HebiIoBankPinResidentTypeInteger
@ HebiIoBankPinResidentTypeInteger
Definition: hebi.h:47
HebiCommandMetadata_::numbered_float_relative_offsets_
const uint32_t * numbered_float_relative_offsets_
Definition: hebi.h:678
HebiCommandMetadata_::bool_field_bitfield_offset_
uint32_t bool_field_bitfield_offset_
Definition: hebi.h:671
Init::Init
Init()
Definition: message_helpers.cpp:9
hebi::highResAngleGetter
HebiStatusCode highResAngleGetter(const RefT &ref, MetadataT &metadata, int field, int64_t *revs, float *offset)
Definition: message_helpers.cpp:50
HebiCommandMetadata_::message_bitfield_count_
uint32_t message_bitfield_count_
Definition: hebi.h:684
hebi::hebiCommandSetIoPinInt
void hebiCommandSetIoPinInt(HebiCommandRef &command, HebiCommandIoPinBank bank, size_t pin_number, const int64_t *value)
Definition: message_helpers.cpp:486
HebiCommandEnumField
HebiCommandEnumField
Definition: hebi.h:137
hebi::MutableProxyBitSet::reset
void reset(size_t index)
Definition: bit_set.hpp:42
HebiCommandMetadata_::numbered_float_field_bitfield_offset_
uint32_t numbered_float_field_bitfield_offset_
Definition: hebi.h:672
hebi::Color::getBlue
uint8_t getBlue() const
Returns the blue channel; value is between 0 and 255.
Definition: color.hpp:31
HebiCommandRef_::bool_fields_
bool * bool_fields_
Definition: hebi.h:640
HebiCommandRef_::numbered_float_fields_
float * numbered_float_fields_
Definition: hebi.h:641
HebiCommandMetadata_::flag_field_count_
uint32_t flag_field_count_
Definition: hebi.h:663
hebi
Definition: arm.cpp:5
HebiCommandRef_::float_fields_
float * float_fields_
Definition: hebi.h:634
hebi::ProxyBitSet::get
bool get(size_t index) const
Definition: bit_set.hpp:74
hebi::boolGetter
HebiStatusCode boolGetter(const RefT &ref, MetadataT &metadata, int field, bool *value)
Definition: message_helpers.cpp:269
HebiCommandHighResAngleField
HebiCommandHighResAngleField
Definition: hebi.h:106
HebiCommandBoolField
HebiCommandBoolField
Definition: hebi.h:116
HebiCommandMetadata_::enum_field_bitfield_offset_
uint32_t enum_field_bitfield_offset_
Definition: hebi.h:670
hebiInfoGetMetadata
void hebiInfoGetMetadata(HebiInfoMetadata *metadata)
HebiCommandIoPinBank
HebiCommandIoPinBank
Definition: hebi.h:144
HebiInfoRef_
Definition: hebi.h:748
HebiCommandMetadata_::high_res_angle_field_bitfield_offset_
uint32_t high_res_angle_field_bitfield_offset_
Definition: hebi.h:666
HebiCommandMetadata_::float_field_bitfield_offset_
uint32_t float_field_bitfield_offset_
Definition: hebi.h:665
hebiFeedbackGetMetadata
void hebiFeedbackGetMetadata(HebiFeedbackMetadata *metadata)
HebiCommandMetadata_::io_field_count_
uint32_t io_field_count_
Definition: hebi.h:660
HebiCommandMetadata_::bool_field_count_
uint32_t bool_field_count_
Definition: hebi.h:658
hebi::hebiCommandSetFlag
void hebiCommandSetFlag(HebiCommandRef &command, HebiCommandFlagField field, int32_t value)
Definition: message_helpers.cpp:472
message_helpers.hpp
HebiCommandMetadata_::numbered_float_field_count_
uint32_t numbered_float_field_count_
Definition: hebi.h:659
hebi::enumGetter
HebiStatusCode enumGetter(const RefT &ref, MetadataT &metadata, int field, int32_t *value)
Definition: message_helpers.cpp:298
Init
Definition: message_helpers.cpp:8
command_metadata
static HebiCommandMetadata command_metadata
Definition: message_helpers.cpp:4
hebi::quaternionfGetter
HebiStatusCode quaternionfGetter(const RefT &ref, MetadataT &metadata, int field, HebiQuaternionf *value)
Definition: message_helpers.cpp:143
HebiCommandMetadata_::flag_field_bitfield_offset_
uint32_t flag_field_bitfield_offset_
Definition: hebi.h:676
hebi::intIoPinGetter
HebiStatusCode intIoPinGetter(const RefT &ref, MetadataT &metadata, int index, size_t pin_number, int64_t *value)
Definition: message_helpers.cpp:209
hebi::MutableProxyBitSet
Definition: bit_set.hpp:23
HebiCommandMetadata_::led_field_bitfield_offset_
uint32_t led_field_bitfield_offset_
Definition: hebi.h:674
hebi::uint64Getter
HebiStatusCode uint64Getter(const RefT &ref, MetadataT &metadata, int field, uint64_t *value)
Definition: message_helpers.cpp:327
HebiCommandRef_
Definition: hebi.h:632
hebi::numberedFloatGetter
HebiStatusCode numberedFloatGetter(const RefT &ref, MetadataT &metadata, int field, size_t number, float *value)
Definition: message_helpers.cpp:81
HebiCommandRef_::enum_fields_
int32_t * enum_fields_
Definition: hebi.h:639
HebiCommandMetadata_::io_relative_offsets_
const uint32_t * io_relative_offsets_
Definition: hebi.h:681
hebi::MutableProxyBitSet::set
void set(size_t index)
Definition: bit_set.hpp:35
HebiCommandMetadata_::float_field_count_
uint32_t float_field_count_
Definition: hebi.h:652
hebi::hebiCommandSetFloat
void hebiCommandSetFloat(HebiCommandRef &command, HebiCommandFloatField field, const float *value)
Definition: message_helpers.cpp:403
hebi::floatIoPinGetter
HebiStatusCode floatIoPinGetter(const RefT &ref, MetadataT &metadata, int index, size_t pin_number, float *value)
Definition: message_helpers.cpp:172
HebiVector3f_
Definition: hebi.h:564
HebiCommandMetadata_::high_res_angle_field_count_
uint32_t high_res_angle_field_count_
Definition: hebi.h:653
HebiInfoMetadata_
Definition: hebi.h:766
hebi::ProxyBitSet
Definition: bit_set.hpp:69
feedback_metadata
static HebiFeedbackMetadata feedback_metadata
Definition: message_helpers.cpp:5

hebi_cpp_api_ros
Author(s): Chris Bollinger , Matthew Tesch
autogenerated on Fri Aug 2 2024 08:35:18