Fawkes API  Fawkes Development Version
JacoBimanualInterface.cpp
1 
2 /***************************************************************************
3  * JacoBimanualInterface.cpp - Fawkes BlackBoard Interface - JacoBimanualInterface
4  *
5  * Templated created: Thu Oct 12 10:49:19 2006
6  * Copyright 2014 Bahram Maleki-Fard
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/JacoBimanualInterface.h>
25 
26 #include <core/exceptions/software.h>
27 
28 #include <map>
29 #include <string>
30 #include <cstring>
31 #include <cstdlib>
32 
33 namespace fawkes {
34 
35 /** @class JacoBimanualInterface <interfaces/JacoBimanualInterface.h>
36  * JacoBimanualInterface Fawkes BlackBoard Interface.
37  *
38  Interface for coordinate bimanual manipulation with a Kinova Jaco arm.
39 
40  * @ingroup FawkesInterfaces
41  */
42 
43 
44 /** ERROR_NONE constant */
45 const uint32_t JacoBimanualInterface::ERROR_NONE = 0u;
46 /** ERROR_UNSPECIFIC constant */
47 const uint32_t JacoBimanualInterface::ERROR_UNSPECIFIC = 1u;
48 /** ERROR_NO_IK constant */
49 const uint32_t JacoBimanualInterface::ERROR_NO_IK = 2u;
50 /** ERROR_PLANNING constant */
51 const uint32_t JacoBimanualInterface::ERROR_PLANNING = 4u;
52 
53 /** Constructor */
54 JacoBimanualInterface::JacoBimanualInterface() : Interface()
55 {
56  data_size = sizeof(JacoBimanualInterface_data_t);
57  data_ptr = malloc(data_size);
58  data = (JacoBimanualInterface_data_t *)data_ptr;
59  data_ts = (interface_data_ts_t *)data_ptr;
60  memset(data_ptr, 0, data_size);
61  add_fieldinfo(IFT_UINT32, "msgid", 1, &data->msgid);
62  add_fieldinfo(IFT_BOOL, "final", 1, &data->final);
63  add_fieldinfo(IFT_UINT32, "error_code", 1, &data->error_code);
64  add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
65  add_messageinfo("CartesianGotoMessage");
66  add_messageinfo("MoveGripperMessage");
67  add_messageinfo("SetPlannerParamsMessage");
68  add_messageinfo("SetConstrainedMessage");
69  unsigned char tmp_hash[] = {0x7c, 0x62, 0x7a, 0x5a, 0xc6, 0xc1, 0xb4, 0x12, 0x6f, 0xa4, 0x89, 0x89, 0xb8, 0xe5, 0x1, 0x66};
70  set_hash(tmp_hash);
71 }
72 
73 /** Destructor */
74 JacoBimanualInterface::~JacoBimanualInterface()
75 {
76  free(data_ptr);
77 }
78 /* Methods */
79 /** Get msgid value.
80  * The ID of the message that is currently being
81  processed, or 0 if no message is being processed.
82  * @return msgid value
83  */
84 uint32_t
85 JacoBimanualInterface::msgid() const
86 {
87  return data->msgid;
88 }
89 
90 /** Get maximum length of msgid value.
91  * @return length of msgid value, can be length of the array or number of
92  * maximum number of characters for a string
93  */
94 size_t
95 JacoBimanualInterface::maxlenof_msgid() const
96 {
97  return 1;
98 }
99 
100 /** Set msgid value.
101  * The ID of the message that is currently being
102  processed, or 0 if no message is being processed.
103  * @param new_msgid new msgid value
104  */
105 void
106 JacoBimanualInterface::set_msgid(const uint32_t new_msgid)
107 {
108  data_changed |= change_field(data->msgid, new_msgid);
109 }
110 
111 /** Get final value.
112  * True, if the last command has been finished,
113  false if it is still running
114  * @return final value
115  */
116 bool
117 JacoBimanualInterface::is_final() const
118 {
119  return data->final;
120 }
121 
122 /** Get maximum length of final value.
123  * @return length of final value, can be length of the array or number of
124  * maximum number of characters for a string
125  */
126 size_t
127 JacoBimanualInterface::maxlenof_final() const
128 {
129  return 1;
130 }
131 
132 /** Set final value.
133  * True, if the last command has been finished,
134  false if it is still running
135  * @param new_final new final value
136  */
137 void
138 JacoBimanualInterface::set_final(const bool new_final)
139 {
140  data_changed |= change_field(data->final, new_final);
141 }
142 
143 /** Get error_code value.
144  * Error code, set if
145  final is true. 0 if no error occured, an error code from ERROR_*
146  constants otherwise.
147  * @return error_code value
148  */
149 uint32_t
150 JacoBimanualInterface::error_code() const
151 {
152  return data->error_code;
153 }
154 
155 /** Get maximum length of error_code value.
156  * @return length of error_code value, can be length of the array or number of
157  * maximum number of characters for a string
158  */
159 size_t
160 JacoBimanualInterface::maxlenof_error_code() const
161 {
162  return 1;
163 }
164 
165 /** Set error_code value.
166  * Error code, set if
167  final is true. 0 if no error occured, an error code from ERROR_*
168  constants otherwise.
169  * @param new_error_code new error_code value
170  */
171 void
172 JacoBimanualInterface::set_error_code(const uint32_t new_error_code)
173 {
174  data_changed |= change_field(data->error_code, new_error_code);
175 }
176 
177 /** Get constrained value.
178  * Wheter planning is using constraint-function.
179  This is an OpenRAVE internal constraint function.
180  * @return constrained value
181  */
182 bool
183 JacoBimanualInterface::is_constrained() const
184 {
185  return data->constrained;
186 }
187 
188 /** Get maximum length of constrained value.
189  * @return length of constrained value, can be length of the array or number of
190  * maximum number of characters for a string
191  */
192 size_t
193 JacoBimanualInterface::maxlenof_constrained() const
194 {
195  return 1;
196 }
197 
198 /** Set constrained value.
199  * Wheter planning is using constraint-function.
200  This is an OpenRAVE internal constraint function.
201  * @param new_constrained new constrained value
202  */
203 void
204 JacoBimanualInterface::set_constrained(const bool new_constrained)
205 {
206  data_changed |= change_field(data->constrained, new_constrained);
207 }
208 
209 /* =========== message create =========== */
210 Message *
211 JacoBimanualInterface::create_message(const char *type) const
212 {
213  if ( strncmp("CartesianGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
214  return new CartesianGotoMessage();
215  } else if ( strncmp("MoveGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
216  return new MoveGripperMessage();
217  } else if ( strncmp("SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
218  return new SetPlannerParamsMessage();
219  } else if ( strncmp("SetConstrainedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
220  return new SetConstrainedMessage();
221  } else {
222  throw UnknownTypeException("The given type '%s' does not match any known "
223  "message type for this interface type.", type);
224  }
225 }
226 
227 
228 /** Copy values from other interface.
229  * @param other other interface to copy values from
230  */
231 void
232 JacoBimanualInterface::copy_values(const Interface *other)
233 {
234  const JacoBimanualInterface *oi = dynamic_cast<const JacoBimanualInterface *>(other);
235  if (oi == NULL) {
236  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
237  type(), other->type());
238  }
239  memcpy(data, oi->data, sizeof(JacoBimanualInterface_data_t));
240 }
241 
242 const char *
243 JacoBimanualInterface::enum_tostring(const char *enumtype, int val) const
244 {
245  throw UnknownTypeException("Unknown enum type %s", enumtype);
246 }
247 
248 /* =========== messages =========== */
249 /** @class JacoBimanualInterface::CartesianGotoMessage <interfaces/JacoBimanualInterface.h>
250  * CartesianGotoMessage Fawkes BlackBoard Interface Message.
251  *
252 
253  */
254 
255 
256 /** Constructor with initial values.
257  * @param ini_l_x initial value for l_x
258  * @param ini_l_y initial value for l_y
259  * @param ini_l_z initial value for l_z
260  * @param ini_l_e1 initial value for l_e1
261  * @param ini_l_e2 initial value for l_e2
262  * @param ini_l_e3 initial value for l_e3
263  * @param ini_r_x initial value for r_x
264  * @param ini_r_y initial value for r_y
265  * @param ini_r_z initial value for r_z
266  * @param ini_r_e1 initial value for r_e1
267  * @param ini_r_e2 initial value for r_e2
268  * @param ini_r_e3 initial value for r_e3
269  */
270 JacoBimanualInterface::CartesianGotoMessage::CartesianGotoMessage(const float ini_l_x, const float ini_l_y, const float ini_l_z, const float ini_l_e1, const float ini_l_e2, const float ini_l_e3, const float ini_r_x, const float ini_r_y, const float ini_r_z, const float ini_r_e1, const float ini_r_e2, const float ini_r_e3) : Message("CartesianGotoMessage")
271 {
272  data_size = sizeof(CartesianGotoMessage_data_t);
273  data_ptr = malloc(data_size);
274  memset(data_ptr, 0, data_size);
275  data = (CartesianGotoMessage_data_t *)data_ptr;
277  data->l_x = ini_l_x;
278  data->l_y = ini_l_y;
279  data->l_z = ini_l_z;
280  data->l_e1 = ini_l_e1;
281  data->l_e2 = ini_l_e2;
282  data->l_e3 = ini_l_e3;
283  data->r_x = ini_r_x;
284  data->r_y = ini_r_y;
285  data->r_z = ini_r_z;
286  data->r_e1 = ini_r_e1;
287  data->r_e2 = ini_r_e2;
288  data->r_e3 = ini_r_e3;
289  add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
290  add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
291  add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
292  add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
293  add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
294  add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
295  add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
296  add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
297  add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
298  add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
299  add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
300  add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
301 }
302 /** Constructor */
304 {
305  data_size = sizeof(CartesianGotoMessage_data_t);
306  data_ptr = malloc(data_size);
307  memset(data_ptr, 0, data_size);
308  data = (CartesianGotoMessage_data_t *)data_ptr;
310  add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
311  add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
312  add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
313  add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
314  add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
315  add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
316  add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
317  add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
318  add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
319  add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
320  add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
321  add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
322 }
323 
324 /** Destructor */
326 {
327  free(data_ptr);
328 }
329 
330 /** Copy constructor.
331  * @param m message to copy from
332  */
334 {
335  data_size = m->data_size;
336  data_ptr = malloc(data_size);
337  memcpy(data_ptr, m->data_ptr, data_size);
338  data = (CartesianGotoMessage_data_t *)data_ptr;
340 }
341 
342 /* Methods */
343 /** Get l_x value.
344  * X-coordinate of left manipulator
345  * @return l_x value
346  */
347 float
349 {
350  return data->l_x;
351 }
352 
353 /** Get maximum length of l_x value.
354  * @return length of l_x value, can be length of the array or number of
355  * maximum number of characters for a string
356  */
357 size_t
359 {
360  return 1;
361 }
362 
363 /** Set l_x value.
364  * X-coordinate of left manipulator
365  * @param new_l_x new l_x value
366  */
367 void
369 {
370  change_field(data->l_x, new_l_x);
371 }
372 
373 /** Get l_y value.
374  * Y-coordinate of left manipulator
375  * @return l_y value
376  */
377 float
379 {
380  return data->l_y;
381 }
382 
383 /** Get maximum length of l_y value.
384  * @return length of l_y value, can be length of the array or number of
385  * maximum number of characters for a string
386  */
387 size_t
389 {
390  return 1;
391 }
392 
393 /** Set l_y value.
394  * Y-coordinate of left manipulator
395  * @param new_l_y new l_y value
396  */
397 void
399 {
400  change_field(data->l_y, new_l_y);
401 }
402 
403 /** Get l_z value.
404  * Z-coordinate of left manipulator
405  * @return l_z value
406  */
407 float
409 {
410  return data->l_z;
411 }
412 
413 /** Get maximum length of l_z value.
414  * @return length of l_z value, can be length of the array or number of
415  * maximum number of characters for a string
416  */
417 size_t
419 {
420  return 1;
421 }
422 
423 /** Set l_z value.
424  * Z-coordinate of left manipulator
425  * @param new_l_z new l_z value
426  */
427 void
429 {
430  change_field(data->l_z, new_l_z);
431 }
432 
433 /** Get l_e1 value.
434  * 1st Euler angle of left manipulator rotation
435  * @return l_e1 value
436  */
437 float
439 {
440  return data->l_e1;
441 }
442 
443 /** Get maximum length of l_e1 value.
444  * @return length of l_e1 value, can be length of the array or number of
445  * maximum number of characters for a string
446  */
447 size_t
449 {
450  return 1;
451 }
452 
453 /** Set l_e1 value.
454  * 1st Euler angle of left manipulator rotation
455  * @param new_l_e1 new l_e1 value
456  */
457 void
459 {
460  change_field(data->l_e1, new_l_e1);
461 }
462 
463 /** Get l_e2 value.
464  * 2nd Euler angle of left manipulator rotation
465  * @return l_e2 value
466  */
467 float
469 {
470  return data->l_e2;
471 }
472 
473 /** Get maximum length of l_e2 value.
474  * @return length of l_e2 value, can be length of the array or number of
475  * maximum number of characters for a string
476  */
477 size_t
479 {
480  return 1;
481 }
482 
483 /** Set l_e2 value.
484  * 2nd Euler angle of left manipulator rotation
485  * @param new_l_e2 new l_e2 value
486  */
487 void
489 {
490  change_field(data->l_e2, new_l_e2);
491 }
492 
493 /** Get l_e3 value.
494  * 3rd Euler angle of left manipulator rotation
495  * @return l_e3 value
496  */
497 float
499 {
500  return data->l_e3;
501 }
502 
503 /** Get maximum length of l_e3 value.
504  * @return length of l_e3 value, can be length of the array or number of
505  * maximum number of characters for a string
506  */
507 size_t
509 {
510  return 1;
511 }
512 
513 /** Set l_e3 value.
514  * 3rd Euler angle of left manipulator rotation
515  * @param new_l_e3 new l_e3 value
516  */
517 void
519 {
520  change_field(data->l_e3, new_l_e3);
521 }
522 
523 /** Get r_x value.
524  * X-coordinate of right manipulator
525  * @return r_x value
526  */
527 float
529 {
530  return data->r_x;
531 }
532 
533 /** Get maximum length of r_x value.
534  * @return length of r_x value, can be length of the array or number of
535  * maximum number of characters for a string
536  */
537 size_t
539 {
540  return 1;
541 }
542 
543 /** Set r_x value.
544  * X-coordinate of right manipulator
545  * @param new_r_x new r_x value
546  */
547 void
549 {
550  change_field(data->r_x, new_r_x);
551 }
552 
553 /** Get r_y value.
554  * Y-coordinate of right manipulator
555  * @return r_y value
556  */
557 float
559 {
560  return data->r_y;
561 }
562 
563 /** Get maximum length of r_y value.
564  * @return length of r_y value, can be length of the array or number of
565  * maximum number of characters for a string
566  */
567 size_t
569 {
570  return 1;
571 }
572 
573 /** Set r_y value.
574  * Y-coordinate of right manipulator
575  * @param new_r_y new r_y value
576  */
577 void
579 {
580  change_field(data->r_y, new_r_y);
581 }
582 
583 /** Get r_z value.
584  * Z-coordinate of right manipulator
585  * @return r_z value
586  */
587 float
589 {
590  return data->r_z;
591 }
592 
593 /** Get maximum length of r_z value.
594  * @return length of r_z value, can be length of the array or number of
595  * maximum number of characters for a string
596  */
597 size_t
599 {
600  return 1;
601 }
602 
603 /** Set r_z value.
604  * Z-coordinate of right manipulator
605  * @param new_r_z new r_z value
606  */
607 void
609 {
610  change_field(data->r_z, new_r_z);
611 }
612 
613 /** Get r_e1 value.
614  * 1st Euler angle of right manipulator rotation
615  * @return r_e1 value
616  */
617 float
619 {
620  return data->r_e1;
621 }
622 
623 /** Get maximum length of r_e1 value.
624  * @return length of r_e1 value, can be length of the array or number of
625  * maximum number of characters for a string
626  */
627 size_t
629 {
630  return 1;
631 }
632 
633 /** Set r_e1 value.
634  * 1st Euler angle of right manipulator rotation
635  * @param new_r_e1 new r_e1 value
636  */
637 void
639 {
640  change_field(data->r_e1, new_r_e1);
641 }
642 
643 /** Get r_e2 value.
644  * 2nd Euler angle of right manipulator rotation
645  * @return r_e2 value
646  */
647 float
649 {
650  return data->r_e2;
651 }
652 
653 /** Get maximum length of r_e2 value.
654  * @return length of r_e2 value, can be length of the array or number of
655  * maximum number of characters for a string
656  */
657 size_t
659 {
660  return 1;
661 }
662 
663 /** Set r_e2 value.
664  * 2nd Euler angle of right manipulator rotation
665  * @param new_r_e2 new r_e2 value
666  */
667 void
669 {
670  change_field(data->r_e2, new_r_e2);
671 }
672 
673 /** Get r_e3 value.
674  * 3rd Euler angle of right manipulator rotation
675  * @return r_e3 value
676  */
677 float
679 {
680  return data->r_e3;
681 }
682 
683 /** Get maximum length of r_e3 value.
684  * @return length of r_e3 value, can be length of the array or number of
685  * maximum number of characters for a string
686  */
687 size_t
689 {
690  return 1;
691 }
692 
693 /** Set r_e3 value.
694  * 3rd Euler angle of right manipulator rotation
695  * @param new_r_e3 new r_e3 value
696  */
697 void
699 {
700  change_field(data->r_e3, new_r_e3);
701 }
702 
703 /** Clone this message.
704  * Produces a message of the same type as this message and copies the
705  * data to the new message.
706  * @return clone of this message
707  */
708 Message *
710 {
712 }
713 /** @class JacoBimanualInterface::MoveGripperMessage <interfaces/JacoBimanualInterface.h>
714  * MoveGripperMessage Fawkes BlackBoard Interface Message.
715  *
716 
717  */
718 
719 
720 /** Constructor with initial values.
721  * @param ini_l_finger1 initial value for l_finger1
722  * @param ini_l_finger2 initial value for l_finger2
723  * @param ini_l_finger3 initial value for l_finger3
724  * @param ini_r_finger1 initial value for r_finger1
725  * @param ini_r_finger2 initial value for r_finger2
726  * @param ini_r_finger3 initial value for r_finger3
727  */
728 JacoBimanualInterface::MoveGripperMessage::MoveGripperMessage(const float ini_l_finger1, const float ini_l_finger2, const float ini_l_finger3, const float ini_r_finger1, const float ini_r_finger2, const float ini_r_finger3) : Message("MoveGripperMessage")
729 {
730  data_size = sizeof(MoveGripperMessage_data_t);
731  data_ptr = malloc(data_size);
732  memset(data_ptr, 0, data_size);
733  data = (MoveGripperMessage_data_t *)data_ptr;
735  data->l_finger1 = ini_l_finger1;
736  data->l_finger2 = ini_l_finger2;
737  data->l_finger3 = ini_l_finger3;
738  data->r_finger1 = ini_r_finger1;
739  data->r_finger2 = ini_r_finger2;
740  data->r_finger3 = ini_r_finger3;
741  add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
742  add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
743  add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
744  add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
745  add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
746  add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
747 }
748 /** Constructor */
750 {
751  data_size = sizeof(MoveGripperMessage_data_t);
752  data_ptr = malloc(data_size);
753  memset(data_ptr, 0, data_size);
754  data = (MoveGripperMessage_data_t *)data_ptr;
756  add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
757  add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
758  add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
759  add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
760  add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
761  add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
762 }
763 
764 /** Destructor */
766 {
767  free(data_ptr);
768 }
769 
770 /** Copy constructor.
771  * @param m message to copy from
772  */
774 {
775  data_size = m->data_size;
776  data_ptr = malloc(data_size);
777  memcpy(data_ptr, m->data_ptr, data_size);
778  data = (MoveGripperMessage_data_t *)data_ptr;
780 }
781 
782 /* Methods */
783 /** Get l_finger1 value.
784  * Value of finger 1 on left gripper. Range [0,60]
785  * @return l_finger1 value
786  */
787 float
789 {
790  return data->l_finger1;
791 }
792 
793 /** Get maximum length of l_finger1 value.
794  * @return length of l_finger1 value, can be length of the array or number of
795  * maximum number of characters for a string
796  */
797 size_t
799 {
800  return 1;
801 }
802 
803 /** Set l_finger1 value.
804  * Value of finger 1 on left gripper. Range [0,60]
805  * @param new_l_finger1 new l_finger1 value
806  */
807 void
809 {
810  change_field(data->l_finger1, new_l_finger1);
811 }
812 
813 /** Get l_finger2 value.
814  * Value of finger 2 on left gripper. Range [0,60]
815  * @return l_finger2 value
816  */
817 float
819 {
820  return data->l_finger2;
821 }
822 
823 /** Get maximum length of l_finger2 value.
824  * @return length of l_finger2 value, can be length of the array or number of
825  * maximum number of characters for a string
826  */
827 size_t
829 {
830  return 1;
831 }
832 
833 /** Set l_finger2 value.
834  * Value of finger 2 on left gripper. Range [0,60]
835  * @param new_l_finger2 new l_finger2 value
836  */
837 void
839 {
840  change_field(data->l_finger2, new_l_finger2);
841 }
842 
843 /** Get l_finger3 value.
844  * Value of finger 3 on left gripper. Range [0,60]
845  * @return l_finger3 value
846  */
847 float
849 {
850  return data->l_finger3;
851 }
852 
853 /** Get maximum length of l_finger3 value.
854  * @return length of l_finger3 value, can be length of the array or number of
855  * maximum number of characters for a string
856  */
857 size_t
859 {
860  return 1;
861 }
862 
863 /** Set l_finger3 value.
864  * Value of finger 3 on left gripper. Range [0,60]
865  * @param new_l_finger3 new l_finger3 value
866  */
867 void
869 {
870  change_field(data->l_finger3, new_l_finger3);
871 }
872 
873 /** Get r_finger1 value.
874  * Value of finger 1 on right gripper. Range [0,60]
875  * @return r_finger1 value
876  */
877 float
879 {
880  return data->r_finger1;
881 }
882 
883 /** Get maximum length of r_finger1 value.
884  * @return length of r_finger1 value, can be length of the array or number of
885  * maximum number of characters for a string
886  */
887 size_t
889 {
890  return 1;
891 }
892 
893 /** Set r_finger1 value.
894  * Value of finger 1 on right gripper. Range [0,60]
895  * @param new_r_finger1 new r_finger1 value
896  */
897 void
899 {
900  change_field(data->r_finger1, new_r_finger1);
901 }
902 
903 /** Get r_finger2 value.
904  * Value of finger 2 on right gripper. Range [0,60]
905  * @return r_finger2 value
906  */
907 float
909 {
910  return data->r_finger2;
911 }
912 
913 /** Get maximum length of r_finger2 value.
914  * @return length of r_finger2 value, can be length of the array or number of
915  * maximum number of characters for a string
916  */
917 size_t
919 {
920  return 1;
921 }
922 
923 /** Set r_finger2 value.
924  * Value of finger 2 on right gripper. Range [0,60]
925  * @param new_r_finger2 new r_finger2 value
926  */
927 void
929 {
930  change_field(data->r_finger2, new_r_finger2);
931 }
932 
933 /** Get r_finger3 value.
934  * Value of finger 3 on right gripper. Range [0,60]
935  * @return r_finger3 value
936  */
937 float
939 {
940  return data->r_finger3;
941 }
942 
943 /** Get maximum length of r_finger3 value.
944  * @return length of r_finger3 value, can be length of the array or number of
945  * maximum number of characters for a string
946  */
947 size_t
949 {
950  return 1;
951 }
952 
953 /** Set r_finger3 value.
954  * Value of finger 3 on right gripper. Range [0,60]
955  * @param new_r_finger3 new r_finger3 value
956  */
957 void
959 {
960  change_field(data->r_finger3, new_r_finger3);
961 }
962 
963 /** Clone this message.
964  * Produces a message of the same type as this message and copies the
965  * data to the new message.
966  * @return clone of this message
967  */
968 Message *
970 {
972 }
973 /** @class JacoBimanualInterface::SetPlannerParamsMessage <interfaces/JacoBimanualInterface.h>
974  * SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
975  *
976 
977  */
978 
979 
980 /** Constructor with initial values.
981  * @param ini_params initial value for params
982  */
983 JacoBimanualInterface::SetPlannerParamsMessage::SetPlannerParamsMessage(const char * ini_params) : Message("SetPlannerParamsMessage")
984 {
985  data_size = sizeof(SetPlannerParamsMessage_data_t);
986  data_ptr = malloc(data_size);
987  memset(data_ptr, 0, data_size);
988  data = (SetPlannerParamsMessage_data_t *)data_ptr;
990  strncpy(data->params, ini_params, 1024-1);
991  data->params[1024-1] = 0;
992  add_fieldinfo(IFT_STRING, "params", 1024, data->params);
993 }
994 /** Constructor */
996 {
997  data_size = sizeof(SetPlannerParamsMessage_data_t);
998  data_ptr = malloc(data_size);
999  memset(data_ptr, 0, data_size);
1000  data = (SetPlannerParamsMessage_data_t *)data_ptr;
1002  add_fieldinfo(IFT_STRING, "params", 1024, data->params);
1003 }
1004 
1005 /** Destructor */
1007 {
1008  free(data_ptr);
1009 }
1010 
1011 /** Copy constructor.
1012  * @param m message to copy from
1013  */
1015 {
1016  data_size = m->data_size;
1017  data_ptr = malloc(data_size);
1018  memcpy(data_ptr, m->data_ptr, data_size);
1019  data = (SetPlannerParamsMessage_data_t *)data_ptr;
1021 }
1022 
1023 /* Methods */
1024 /** Get params value.
1025  * Planner parameters
1026  * @return params value
1027  */
1028 char *
1030 {
1031  return data->params;
1032 }
1033 
1034 /** Get maximum length of params value.
1035  * @return length of params value, can be length of the array or number of
1036  * maximum number of characters for a string
1037  */
1038 size_t
1040 {
1041  return 1024;
1042 }
1043 
1044 /** Set params value.
1045  * Planner parameters
1046  * @param new_params new params value
1047  */
1048 void
1050 {
1051  change_field(data->params, new_params);
1052 }
1053 
1054 /** Clone this message.
1055  * Produces a message of the same type as this message and copies the
1056  * data to the new message.
1057  * @return clone of this message
1058  */
1059 Message *
1061 {
1063 }
1064 /** @class JacoBimanualInterface::SetConstrainedMessage <interfaces/JacoBimanualInterface.h>
1065  * SetConstrainedMessage Fawkes BlackBoard Interface Message.
1066  *
1067 
1068  */
1069 
1070 
1071 /** Constructor with initial values.
1072  * @param ini_constrained initial value for constrained
1073  */
1074 JacoBimanualInterface::SetConstrainedMessage::SetConstrainedMessage(const bool ini_constrained) : Message("SetConstrainedMessage")
1075 {
1076  data_size = sizeof(SetConstrainedMessage_data_t);
1077  data_ptr = malloc(data_size);
1078  memset(data_ptr, 0, data_size);
1079  data = (SetConstrainedMessage_data_t *)data_ptr;
1081  data->constrained = ini_constrained;
1082  add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1083 }
1084 /** Constructor */
1086 {
1087  data_size = sizeof(SetConstrainedMessage_data_t);
1088  data_ptr = malloc(data_size);
1089  memset(data_ptr, 0, data_size);
1090  data = (SetConstrainedMessage_data_t *)data_ptr;
1092  add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1093 }
1094 
1095 /** Destructor */
1097 {
1098  free(data_ptr);
1099 }
1100 
1101 /** Copy constructor.
1102  * @param m message to copy from
1103  */
1105 {
1106  data_size = m->data_size;
1107  data_ptr = malloc(data_size);
1108  memcpy(data_ptr, m->data_ptr, data_size);
1109  data = (SetConstrainedMessage_data_t *)data_ptr;
1111 }
1112 
1113 /* Methods */
1114 /** Get constrained value.
1115  * Wheter planning is using constraint-function.
1116  This is an OpenRAVE internal constraint function.
1117  * @return constrained value
1118  */
1119 bool
1121 {
1122  return data->constrained;
1123 }
1124 
1125 /** Get maximum length of constrained value.
1126  * @return length of constrained value, can be length of the array or number of
1127  * maximum number of characters for a string
1128  */
1129 size_t
1131 {
1132  return 1;
1133 }
1134 
1135 /** Set constrained value.
1136  * Wheter planning is using constraint-function.
1137  This is an OpenRAVE internal constraint function.
1138  * @param new_constrained new constrained value
1139  */
1140 void
1142 {
1143  change_field(data->constrained, new_constrained);
1144 }
1145 
1146 /** Clone this message.
1147  * Produces a message of the same type as this message and copies the
1148  * data to the new message.
1149  * @return clone of this message
1150  */
1151 Message *
1153 {
1155 }
1156 /** Check if message is valid and can be enqueued.
1157  * @param message Message to check
1158  * @return true if the message is valid, false otherwise.
1159  */
1160 bool
1162 {
1163  const CartesianGotoMessage *m0 = dynamic_cast<const CartesianGotoMessage *>(message);
1164  if ( m0 != NULL ) {
1165  return true;
1166  }
1167  const MoveGripperMessage *m1 = dynamic_cast<const MoveGripperMessage *>(message);
1168  if ( m1 != NULL ) {
1169  return true;
1170  }
1171  const SetPlannerParamsMessage *m2 = dynamic_cast<const SetPlannerParamsMessage *>(message);
1172  if ( m2 != NULL ) {
1173  return true;
1174  }
1175  const SetConstrainedMessage *m3 = dynamic_cast<const SetConstrainedMessage *>(message);
1176  if ( m3 != NULL ) {
1177  return true;
1178  }
1179  return false;
1180 }
1181 
1182 /// @cond INTERNALS
1183 EXPORT_INTERFACE(JacoBimanualInterface)
1184 /// @endcond
1185 
1186 
1187 } // end namespace fawkes
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
const char * type() const
Get type of interface.
Definition: interface.cpp:643
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:224
CartesianGotoMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_e1() const
Get maximum length of l_e1 value.
void set_l_e2(const float new_l_e2)
Set l_e2 value.
void set_r_e1(const float new_r_e1)
Set r_e1 value.
size_t maxlenof_l_e2() const
Get maximum length of l_e2 value.
size_t maxlenof_r_z() const
Get maximum length of r_z value.
void set_r_e3(const float new_r_e3)
Set r_e3 value.
void set_r_e2(const float new_r_e2)
Set r_e2 value.
size_t maxlenof_r_e3() const
Get maximum length of r_e3 value.
size_t maxlenof_r_x() const
Get maximum length of r_x value.
void set_l_e1(const float new_l_e1)
Set l_e1 value.
size_t maxlenof_r_e2() const
Get maximum length of r_e2 value.
virtual Message * clone() const
Clone this message.
void set_l_e3(const float new_l_e3)
Set l_e3 value.
size_t maxlenof_l_z() const
Get maximum length of l_z value.
size_t maxlenof_l_e3() const
Get maximum length of l_e3 value.
size_t maxlenof_r_e1() const
Get maximum length of r_e1 value.
size_t maxlenof_l_y() const
Get maximum length of l_y value.
size_t maxlenof_l_x() const
Get maximum length of l_x value.
size_t maxlenof_r_y() const
Get maximum length of r_y value.
MoveGripperMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_finger2() const
Get maximum length of l_finger2 value.
size_t maxlenof_l_finger1() const
Get maximum length of l_finger1 value.
void set_r_finger2(const float new_r_finger2)
Set r_finger2 value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_l_finger3() const
Get maximum length of l_finger3 value.
void set_l_finger1(const float new_l_finger1)
Set l_finger1 value.
size_t maxlenof_r_finger1() const
Get maximum length of r_finger1 value.
void set_l_finger3(const float new_l_finger3)
Set l_finger3 value.
size_t maxlenof_r_finger3() const
Get maximum length of r_finger3 value.
void set_r_finger3(const float new_r_finger3)
Set r_finger3 value.
void set_r_finger1(const float new_r_finger1)
Set r_finger1 value.
void set_l_finger2(const float new_l_finger2)
Set l_finger2 value.
size_t maxlenof_r_finger2() const
Get maximum length of r_finger2 value.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
void set_constrained(const bool new_constrained)
Set constrained value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_constrained() const
Get maximum length of constrained value.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_params() const
Get maximum length of params value.
virtual Message * clone() const
Clone this message.
void set_params(const char *new_params)
Set params value.
JacoBimanualInterface Fawkes BlackBoard Interface.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the info list.
Definition: message.cpp:400
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:128
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:138
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:129
Fawkes library namespace.
@ IFT_FLOAT
float field
Definition: types.h:46
@ IFT_STRING
string field
Definition: types.h:48
@ IFT_BOOL
boolean field
Definition: types.h:37
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Definition: message.h:167
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:134