Fawkes API  Fawkes Development Version
RobotinoSensorInterface.cpp
1 
2 /***************************************************************************
3  * RobotinoSensorInterface.cpp - Fawkes BlackBoard Interface - RobotinoSensorInterface
4  *
5  * Templated created: Thu Oct 12 10:49:19 2006
6  * Copyright 2012 Tim Niemueller
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <interfaces/RobotinoSensorInterface.h>
25 
26 #include <core/exceptions/software.h>
27 
28 #include <cstring>
29 #include <cstdlib>
30 
31 namespace fawkes {
32 
33 /** @class RobotinoSensorInterface <interfaces/RobotinoSensorInterface.h>
34  * RobotinoSensorInterface Fawkes BlackBoard Interface.
35  * Sensor information of a Robotino robot
36  * @ingroup FawkesInterfaces
37  */
38 
39 
40 
41 /** Constructor */
42 RobotinoSensorInterface::RobotinoSensorInterface() : Interface()
43 {
44  data_size = sizeof(RobotinoSensorInterface_data_t);
45  data_ptr = malloc(data_size);
46  data = (RobotinoSensorInterface_data_t *)data_ptr;
47  data_ts = (interface_data_ts_t *)data_ptr;
48  memset(data_ptr, 0, data_size);
49  add_fieldinfo(IFT_FLOAT, "mot_velocity", 3, &data->mot_velocity);
50  add_fieldinfo(IFT_INT32, "mot_position", 3, &data->mot_position);
51  add_fieldinfo(IFT_FLOAT, "mot_current", 3, &data->mot_current);
52  add_fieldinfo(IFT_BOOL, "bumper", 1, &data->bumper);
53  add_fieldinfo(IFT_FLOAT, "distance", 9, &data->distance);
54  add_fieldinfo(IFT_BOOL, "digital_in", 8, &data->digital_in);
55  add_fieldinfo(IFT_FLOAT, "analog_in", 8, &data->analog_in);
56  add_fieldinfo(IFT_BOOL, "gyro_available", 1, &data->gyro_available);
57  add_fieldinfo(IFT_FLOAT, "gyro_angle", 1, &data->gyro_angle);
58  add_fieldinfo(IFT_FLOAT, "gyro_rate", 1, &data->gyro_rate);
59  unsigned char tmp_hash[] = {0xfe, 0x8a, 0xcf, 0x8f, 0xe8, 0xb9, 0xf, 0x3b, 0x35, 0x3b, 0x1a, 0xea, 0xe7, 0x59, 0xab, 0xc8};
60  set_hash(tmp_hash);
61 }
62 
63 /** Destructor */
64 RobotinoSensorInterface::~RobotinoSensorInterface()
65 {
66  free(data_ptr);
67 }
68 /* Methods */
69 /** Get mot_velocity value.
70  * Velocities of the wheels.
71  * @return mot_velocity value
72  */
73 float *
74 RobotinoSensorInterface::mot_velocity() const
75 {
76  return data->mot_velocity;
77 }
78 
79 /** Get mot_velocity value at given index.
80  * Velocities of the wheels.
81  * @param index index of value
82  * @return mot_velocity value
83  * @exception Exception thrown if index is out of bounds
84  */
85 float
86 RobotinoSensorInterface::mot_velocity(unsigned int index) const
87 {
88  if (index > 3) {
89  throw Exception("Index value %u out of bounds (0..3)", index);
90  }
91  return data->mot_velocity[index];
92 }
93 
94 /** Get maximum length of mot_velocity value.
95  * @return length of mot_velocity value, can be length of the array or number of
96  * maximum number of characters for a string
97  */
98 size_t
99 RobotinoSensorInterface::maxlenof_mot_velocity() const
100 {
101  return 3;
102 }
103 
104 /** Set mot_velocity value.
105  * Velocities of the wheels.
106  * @param new_mot_velocity new mot_velocity value
107  */
108 void
109 RobotinoSensorInterface::set_mot_velocity(const float * new_mot_velocity)
110 {
111  memcpy(data->mot_velocity, new_mot_velocity, sizeof(float) * 3);
112  data_changed = true;
113 }
114 
115 /** Set mot_velocity value at given index.
116  * Velocities of the wheels.
117  * @param new_mot_velocity new mot_velocity value
118  * @param index index for of the value
119  */
120 void
121 RobotinoSensorInterface::set_mot_velocity(unsigned int index, const float new_mot_velocity)
122 {
123  if (index > 3) {
124  throw Exception("Index value %u out of bounds (0..3)", index);
125  }
126  data->mot_velocity[index] = new_mot_velocity;
127  data_changed = true;
128 }
129 /** Get mot_position value.
130  * Positions of the wheels.
131  * @return mot_position value
132  */
133 int32_t *
134 RobotinoSensorInterface::mot_position() const
135 {
136  return data->mot_position;
137 }
138 
139 /** Get mot_position value at given index.
140  * Positions of the wheels.
141  * @param index index of value
142  * @return mot_position value
143  * @exception Exception thrown if index is out of bounds
144  */
145 int32_t
146 RobotinoSensorInterface::mot_position(unsigned int index) const
147 {
148  if (index > 3) {
149  throw Exception("Index value %u out of bounds (0..3)", index);
150  }
151  return data->mot_position[index];
152 }
153 
154 /** Get maximum length of mot_position value.
155  * @return length of mot_position value, can be length of the array or number of
156  * maximum number of characters for a string
157  */
158 size_t
159 RobotinoSensorInterface::maxlenof_mot_position() const
160 {
161  return 3;
162 }
163 
164 /** Set mot_position value.
165  * Positions of the wheels.
166  * @param new_mot_position new mot_position value
167  */
168 void
169 RobotinoSensorInterface::set_mot_position(const int32_t * new_mot_position)
170 {
171  memcpy(data->mot_position, new_mot_position, sizeof(int32_t) * 3);
172  data_changed = true;
173 }
174 
175 /** Set mot_position value at given index.
176  * Positions of the wheels.
177  * @param new_mot_position new mot_position value
178  * @param index index for of the value
179  */
180 void
181 RobotinoSensorInterface::set_mot_position(unsigned int index, const int32_t new_mot_position)
182 {
183  if (index > 3) {
184  throw Exception("Index value %u out of bounds (0..3)", index);
185  }
186  data->mot_position[index] = new_mot_position;
187  data_changed = true;
188 }
189 /** Get mot_current value.
190  * Motor currents.
191  * @return mot_current value
192  */
193 float *
194 RobotinoSensorInterface::mot_current() const
195 {
196  return data->mot_current;
197 }
198 
199 /** Get mot_current value at given index.
200  * Motor currents.
201  * @param index index of value
202  * @return mot_current value
203  * @exception Exception thrown if index is out of bounds
204  */
205 float
206 RobotinoSensorInterface::mot_current(unsigned int index) const
207 {
208  if (index > 3) {
209  throw Exception("Index value %u out of bounds (0..3)", index);
210  }
211  return data->mot_current[index];
212 }
213 
214 /** Get maximum length of mot_current value.
215  * @return length of mot_current value, can be length of the array or number of
216  * maximum number of characters for a string
217  */
218 size_t
219 RobotinoSensorInterface::maxlenof_mot_current() const
220 {
221  return 3;
222 }
223 
224 /** Set mot_current value.
225  * Motor currents.
226  * @param new_mot_current new mot_current value
227  */
228 void
229 RobotinoSensorInterface::set_mot_current(const float * new_mot_current)
230 {
231  memcpy(data->mot_current, new_mot_current, sizeof(float) * 3);
232  data_changed = true;
233 }
234 
235 /** Set mot_current value at given index.
236  * Motor currents.
237  * @param new_mot_current new mot_current value
238  * @param index index for of the value
239  */
240 void
241 RobotinoSensorInterface::set_mot_current(unsigned int index, const float new_mot_current)
242 {
243  if (index > 3) {
244  throw Exception("Index value %u out of bounds (0..3)", index);
245  }
246  data->mot_current[index] = new_mot_current;
247  data_changed = true;
248 }
249 /** Get bumper value.
250  * Bumper pressed indicator.
251  * @return bumper value
252  */
253 bool
254 RobotinoSensorInterface::is_bumper() const
255 {
256  return data->bumper;
257 }
258 
259 /** Get maximum length of bumper value.
260  * @return length of bumper value, can be length of the array or number of
261  * maximum number of characters for a string
262  */
263 size_t
264 RobotinoSensorInterface::maxlenof_bumper() const
265 {
266  return 1;
267 }
268 
269 /** Set bumper value.
270  * Bumper pressed indicator.
271  * @param new_bumper new bumper value
272  */
273 void
274 RobotinoSensorInterface::set_bumper(const bool new_bumper)
275 {
276  data->bumper = new_bumper;
277  data_changed = true;
278 }
279 
280 /** Get distance value.
281  * Distance sensor values.
282  * @return distance value
283  */
284 float *
285 RobotinoSensorInterface::distance() const
286 {
287  return data->distance;
288 }
289 
290 /** Get distance value at given index.
291  * Distance sensor values.
292  * @param index index of value
293  * @return distance value
294  * @exception Exception thrown if index is out of bounds
295  */
296 float
297 RobotinoSensorInterface::distance(unsigned int index) const
298 {
299  if (index > 9) {
300  throw Exception("Index value %u out of bounds (0..9)", index);
301  }
302  return data->distance[index];
303 }
304 
305 /** Get maximum length of distance value.
306  * @return length of distance value, can be length of the array or number of
307  * maximum number of characters for a string
308  */
309 size_t
310 RobotinoSensorInterface::maxlenof_distance() const
311 {
312  return 9;
313 }
314 
315 /** Set distance value.
316  * Distance sensor values.
317  * @param new_distance new distance value
318  */
319 void
320 RobotinoSensorInterface::set_distance(const float * new_distance)
321 {
322  memcpy(data->distance, new_distance, sizeof(float) * 9);
323  data_changed = true;
324 }
325 
326 /** Set distance value at given index.
327  * Distance sensor values.
328  * @param new_distance new distance value
329  * @param index index for of the value
330  */
331 void
332 RobotinoSensorInterface::set_distance(unsigned int index, const float new_distance)
333 {
334  if (index > 9) {
335  throw Exception("Index value %u out of bounds (0..9)", index);
336  }
337  data->distance[index] = new_distance;
338  data_changed = true;
339 }
340 /** Get digital_in value.
341  * Digital input values.
342  * @return digital_in value
343  */
344 bool *
345 RobotinoSensorInterface::is_digital_in() const
346 {
347  return data->digital_in;
348 }
349 
350 /** Get digital_in value at given index.
351  * Digital input values.
352  * @param index index of value
353  * @return digital_in value
354  * @exception Exception thrown if index is out of bounds
355  */
356 bool
357 RobotinoSensorInterface::is_digital_in(unsigned int index) const
358 {
359  if (index > 8) {
360  throw Exception("Index value %u out of bounds (0..8)", index);
361  }
362  return data->digital_in[index];
363 }
364 
365 /** Get maximum length of digital_in value.
366  * @return length of digital_in value, can be length of the array or number of
367  * maximum number of characters for a string
368  */
369 size_t
370 RobotinoSensorInterface::maxlenof_digital_in() const
371 {
372  return 8;
373 }
374 
375 /** Set digital_in value.
376  * Digital input values.
377  * @param new_digital_in new digital_in value
378  */
379 void
380 RobotinoSensorInterface::set_digital_in(const bool * new_digital_in)
381 {
382  memcpy(data->digital_in, new_digital_in, sizeof(bool) * 8);
383  data_changed = true;
384 }
385 
386 /** Set digital_in value at given index.
387  * Digital input values.
388  * @param new_digital_in new digital_in value
389  * @param index index for of the value
390  */
391 void
392 RobotinoSensorInterface::set_digital_in(unsigned int index, const bool new_digital_in)
393 {
394  if (index > 8) {
395  throw Exception("Index value %u out of bounds (0..8)", index);
396  }
397  data->digital_in[index] = new_digital_in;
398  data_changed = true;
399 }
400 /** Get analog_in value.
401  * Analog input values.
402  * @return analog_in value
403  */
404 float *
405 RobotinoSensorInterface::analog_in() const
406 {
407  return data->analog_in;
408 }
409 
410 /** Get analog_in value at given index.
411  * Analog input values.
412  * @param index index of value
413  * @return analog_in value
414  * @exception Exception thrown if index is out of bounds
415  */
416 float
417 RobotinoSensorInterface::analog_in(unsigned int index) const
418 {
419  if (index > 8) {
420  throw Exception("Index value %u out of bounds (0..8)", index);
421  }
422  return data->analog_in[index];
423 }
424 
425 /** Get maximum length of analog_in value.
426  * @return length of analog_in value, can be length of the array or number of
427  * maximum number of characters for a string
428  */
429 size_t
430 RobotinoSensorInterface::maxlenof_analog_in() const
431 {
432  return 8;
433 }
434 
435 /** Set analog_in value.
436  * Analog input values.
437  * @param new_analog_in new analog_in value
438  */
439 void
440 RobotinoSensorInterface::set_analog_in(const float * new_analog_in)
441 {
442  memcpy(data->analog_in, new_analog_in, sizeof(float) * 8);
443  data_changed = true;
444 }
445 
446 /** Set analog_in value at given index.
447  * Analog input values.
448  * @param new_analog_in new analog_in value
449  * @param index index for of the value
450  */
451 void
452 RobotinoSensorInterface::set_analog_in(unsigned int index, const float new_analog_in)
453 {
454  if (index > 8) {
455  throw Exception("Index value %u out of bounds (0..8)", index);
456  }
457  data->analog_in[index] = new_analog_in;
458  data_changed = true;
459 }
460 /** Get gyro_available value.
461  * True if gyro is available
462  * @return gyro_available value
463  */
464 bool
465 RobotinoSensorInterface::is_gyro_available() const
466 {
467  return data->gyro_available;
468 }
469 
470 /** Get maximum length of gyro_available value.
471  * @return length of gyro_available value, can be length of the array or number of
472  * maximum number of characters for a string
473  */
474 size_t
475 RobotinoSensorInterface::maxlenof_gyro_available() const
476 {
477  return 1;
478 }
479 
480 /** Set gyro_available value.
481  * True if gyro is available
482  * @param new_gyro_available new gyro_available value
483  */
484 void
485 RobotinoSensorInterface::set_gyro_available(const bool new_gyro_available)
486 {
487  data->gyro_available = new_gyro_available;
488  data_changed = true;
489 }
490 
491 /** Get gyro_angle value.
492  * Gyro angle value; rad
493  * @return gyro_angle value
494  */
495 float
496 RobotinoSensorInterface::gyro_angle() const
497 {
498  return data->gyro_angle;
499 }
500 
501 /** Get maximum length of gyro_angle value.
502  * @return length of gyro_angle value, can be length of the array or number of
503  * maximum number of characters for a string
504  */
505 size_t
506 RobotinoSensorInterface::maxlenof_gyro_angle() const
507 {
508  return 1;
509 }
510 
511 /** Set gyro_angle value.
512  * Gyro angle value; rad
513  * @param new_gyro_angle new gyro_angle value
514  */
515 void
516 RobotinoSensorInterface::set_gyro_angle(const float new_gyro_angle)
517 {
518  data->gyro_angle = new_gyro_angle;
519  data_changed = true;
520 }
521 
522 /** Get gyro_rate value.
523  * Gyro rate value; rad/sec
524  * @return gyro_rate value
525  */
526 float
527 RobotinoSensorInterface::gyro_rate() const
528 {
529  return data->gyro_rate;
530 }
531 
532 /** Get maximum length of gyro_rate value.
533  * @return length of gyro_rate value, can be length of the array or number of
534  * maximum number of characters for a string
535  */
536 size_t
537 RobotinoSensorInterface::maxlenof_gyro_rate() const
538 {
539  return 1;
540 }
541 
542 /** Set gyro_rate value.
543  * Gyro rate value; rad/sec
544  * @param new_gyro_rate new gyro_rate value
545  */
546 void
547 RobotinoSensorInterface::set_gyro_rate(const float new_gyro_rate)
548 {
549  data->gyro_rate = new_gyro_rate;
550  data_changed = true;
551 }
552 
553 /* =========== message create =========== */
554 Message *
555 RobotinoSensorInterface::create_message(const char *type) const
556 {
557  throw UnknownTypeException("The given type '%s' does not match any known "
558  "message type for this interface type.", type);
559 }
560 
561 
562 /** Copy values from other interface.
563  * @param other other interface to copy values from
564  */
565 void
566 RobotinoSensorInterface::copy_values(const Interface *other)
567 {
568  const RobotinoSensorInterface *oi = dynamic_cast<const RobotinoSensorInterface *>(other);
569  if (oi == NULL) {
570  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
571  type(), other->type());
572  }
573  memcpy(data, oi->data, sizeof(RobotinoSensorInterface_data_t));
574 }
575 
576 const char *
577 RobotinoSensorInterface::enum_tostring(const char *enumtype, int val) const
578 {
579  throw UnknownTypeException("Unknown enum type %s", enumtype);
580 }
581 
582 /* =========== messages =========== */
583 /** Check if message is valid and can be enqueued.
584  * @param message Message to check
585  * @return true if the message is valid, false otherwise.
586  */
587 bool
588 RobotinoSensorInterface::message_valid(const Message *message) const
589 {
590  return false;
591 }
592 
593 /// @cond INTERNALS
594 EXPORT_INTERFACE(RobotinoSensorInterface)
595 /// @endcond
596 
597 
598 } // end namespace fawkes
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:43
Fawkes library namespace.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
RobotinoSensorInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
const char * type() const
Get type of interface.
Definition: interface.cpp:635