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->msgid = new_msgid;
109  data_changed = true;
110 }
111 
112 /** Get final value.
113  * True, if the last command has been finished,
114  false if it is still running
115  * @return final value
116  */
117 bool
118 JacoBimanualInterface::is_final() const
119 {
120  return data->final;
121 }
122 
123 /** Get maximum length of final value.
124  * @return length of final value, can be length of the array or number of
125  * maximum number of characters for a string
126  */
127 size_t
128 JacoBimanualInterface::maxlenof_final() const
129 {
130  return 1;
131 }
132 
133 /** Set final value.
134  * True, if the last command has been finished,
135  false if it is still running
136  * @param new_final new final value
137  */
138 void
139 JacoBimanualInterface::set_final(const bool new_final)
140 {
141  data->final = new_final;
142  data_changed = true;
143 }
144 
145 /** Get error_code value.
146  * Error code, set if
147  final is true. 0 if no error occured, an error code from ERROR_*
148  constants otherwise.
149  * @return error_code value
150  */
151 uint32_t
152 JacoBimanualInterface::error_code() const
153 {
154  return data->error_code;
155 }
156 
157 /** Get maximum length of error_code value.
158  * @return length of error_code value, can be length of the array or number of
159  * maximum number of characters for a string
160  */
161 size_t
162 JacoBimanualInterface::maxlenof_error_code() const
163 {
164  return 1;
165 }
166 
167 /** Set error_code value.
168  * Error code, set if
169  final is true. 0 if no error occured, an error code from ERROR_*
170  constants otherwise.
171  * @param new_error_code new error_code value
172  */
173 void
174 JacoBimanualInterface::set_error_code(const uint32_t new_error_code)
175 {
176  data->error_code = new_error_code;
177  data_changed = true;
178 }
179 
180 /** Get constrained value.
181  * Wheter planning is using constraint-function.
182  This is an OpenRAVE internal constraint function.
183  * @return constrained value
184  */
185 bool
186 JacoBimanualInterface::is_constrained() const
187 {
188  return data->constrained;
189 }
190 
191 /** Get maximum length of constrained value.
192  * @return length of constrained value, can be length of the array or number of
193  * maximum number of characters for a string
194  */
195 size_t
196 JacoBimanualInterface::maxlenof_constrained() const
197 {
198  return 1;
199 }
200 
201 /** Set constrained value.
202  * Wheter planning is using constraint-function.
203  This is an OpenRAVE internal constraint function.
204  * @param new_constrained new constrained value
205  */
206 void
207 JacoBimanualInterface::set_constrained(const bool new_constrained)
208 {
209  data->constrained = new_constrained;
210  data_changed = true;
211 }
212 
213 /* =========== message create =========== */
214 Message *
215 JacoBimanualInterface::create_message(const char *type) const
216 {
217  if ( strncmp("CartesianGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
218  return new CartesianGotoMessage();
219  } else if ( strncmp("MoveGripperMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
220  return new MoveGripperMessage();
221  } else if ( strncmp("SetPlannerParamsMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
222  return new SetPlannerParamsMessage();
223  } else if ( strncmp("SetConstrainedMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
224  return new SetConstrainedMessage();
225  } else {
226  throw UnknownTypeException("The given type '%s' does not match any known "
227  "message type for this interface type.", type);
228  }
229 }
230 
231 
232 /** Copy values from other interface.
233  * @param other other interface to copy values from
234  */
235 void
236 JacoBimanualInterface::copy_values(const Interface *other)
237 {
238  const JacoBimanualInterface *oi = dynamic_cast<const JacoBimanualInterface *>(other);
239  if (oi == NULL) {
240  throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
241  type(), other->type());
242  }
243  memcpy(data, oi->data, sizeof(JacoBimanualInterface_data_t));
244 }
245 
246 const char *
247 JacoBimanualInterface::enum_tostring(const char *enumtype, int val) const
248 {
249  throw UnknownTypeException("Unknown enum type %s", enumtype);
250 }
251 
252 /* =========== messages =========== */
253 /** @class JacoBimanualInterface::CartesianGotoMessage <interfaces/JacoBimanualInterface.h>
254  * CartesianGotoMessage Fawkes BlackBoard Interface Message.
255  *
256 
257  */
258 
259 
260 /** Constructor with initial values.
261  * @param ini_l_x initial value for l_x
262  * @param ini_l_y initial value for l_y
263  * @param ini_l_z initial value for l_z
264  * @param ini_l_e1 initial value for l_e1
265  * @param ini_l_e2 initial value for l_e2
266  * @param ini_l_e3 initial value for l_e3
267  * @param ini_r_x initial value for r_x
268  * @param ini_r_y initial value for r_y
269  * @param ini_r_z initial value for r_z
270  * @param ini_r_e1 initial value for r_e1
271  * @param ini_r_e2 initial value for r_e2
272  * @param ini_r_e3 initial value for r_e3
273  */
274 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")
275 {
276  data_size = sizeof(CartesianGotoMessage_data_t);
277  data_ptr = malloc(data_size);
278  memset(data_ptr, 0, data_size);
279  data = (CartesianGotoMessage_data_t *)data_ptr;
281  data->l_x = ini_l_x;
282  data->l_y = ini_l_y;
283  data->l_z = ini_l_z;
284  data->l_e1 = ini_l_e1;
285  data->l_e2 = ini_l_e2;
286  data->l_e3 = ini_l_e3;
287  data->r_x = ini_r_x;
288  data->r_y = ini_r_y;
289  data->r_z = ini_r_z;
290  data->r_e1 = ini_r_e1;
291  data->r_e2 = ini_r_e2;
292  data->r_e3 = ini_r_e3;
293  add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
294  add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
295  add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
296  add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
297  add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
298  add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
299  add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
300  add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
301  add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
302  add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
303  add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
304  add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
305 }
306 /** Constructor */
308 {
309  data_size = sizeof(CartesianGotoMessage_data_t);
310  data_ptr = malloc(data_size);
311  memset(data_ptr, 0, data_size);
312  data = (CartesianGotoMessage_data_t *)data_ptr;
314  add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
315  add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
316  add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
317  add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
318  add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
319  add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
320  add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
321  add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
322  add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
323  add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
324  add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
325  add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
326 }
327 
328 /** Destructor */
330 {
331  free(data_ptr);
332 }
333 
334 /** Copy constructor.
335  * @param m message to copy from
336  */
338 {
339  data_size = m->data_size;
340  data_ptr = malloc(data_size);
341  memcpy(data_ptr, m->data_ptr, data_size);
342  data = (CartesianGotoMessage_data_t *)data_ptr;
344 }
345 
346 /* Methods */
347 /** Get l_x value.
348  * X-coordinate of left manipulator
349  * @return l_x value
350  */
351 float
353 {
354  return data->l_x;
355 }
356 
357 /** Get maximum length of l_x value.
358  * @return length of l_x value, can be length of the array or number of
359  * maximum number of characters for a string
360  */
361 size_t
363 {
364  return 1;
365 }
366 
367 /** Set l_x value.
368  * X-coordinate of left manipulator
369  * @param new_l_x new l_x value
370  */
371 void
373 {
374  data->l_x = new_l_x;
375 }
376 
377 /** Get l_y value.
378  * Y-coordinate of left manipulator
379  * @return l_y value
380  */
381 float
383 {
384  return data->l_y;
385 }
386 
387 /** Get maximum length of l_y value.
388  * @return length of l_y value, can be length of the array or number of
389  * maximum number of characters for a string
390  */
391 size_t
393 {
394  return 1;
395 }
396 
397 /** Set l_y value.
398  * Y-coordinate of left manipulator
399  * @param new_l_y new l_y value
400  */
401 void
403 {
404  data->l_y = new_l_y;
405 }
406 
407 /** Get l_z value.
408  * Z-coordinate of left manipulator
409  * @return l_z value
410  */
411 float
413 {
414  return data->l_z;
415 }
416 
417 /** Get maximum length of l_z value.
418  * @return length of l_z value, can be length of the array or number of
419  * maximum number of characters for a string
420  */
421 size_t
423 {
424  return 1;
425 }
426 
427 /** Set l_z value.
428  * Z-coordinate of left manipulator
429  * @param new_l_z new l_z value
430  */
431 void
433 {
434  data->l_z = new_l_z;
435 }
436 
437 /** Get l_e1 value.
438  * 1st Euler angle of left manipulator rotation
439  * @return l_e1 value
440  */
441 float
443 {
444  return data->l_e1;
445 }
446 
447 /** Get maximum length of l_e1 value.
448  * @return length of l_e1 value, can be length of the array or number of
449  * maximum number of characters for a string
450  */
451 size_t
453 {
454  return 1;
455 }
456 
457 /** Set l_e1 value.
458  * 1st Euler angle of left manipulator rotation
459  * @param new_l_e1 new l_e1 value
460  */
461 void
463 {
464  data->l_e1 = new_l_e1;
465 }
466 
467 /** Get l_e2 value.
468  * 2nd Euler angle of left manipulator rotation
469  * @return l_e2 value
470  */
471 float
473 {
474  return data->l_e2;
475 }
476 
477 /** Get maximum length of l_e2 value.
478  * @return length of l_e2 value, can be length of the array or number of
479  * maximum number of characters for a string
480  */
481 size_t
483 {
484  return 1;
485 }
486 
487 /** Set l_e2 value.
488  * 2nd Euler angle of left manipulator rotation
489  * @param new_l_e2 new l_e2 value
490  */
491 void
493 {
494  data->l_e2 = new_l_e2;
495 }
496 
497 /** Get l_e3 value.
498  * 3rd Euler angle of left manipulator rotation
499  * @return l_e3 value
500  */
501 float
503 {
504  return data->l_e3;
505 }
506 
507 /** Get maximum length of l_e3 value.
508  * @return length of l_e3 value, can be length of the array or number of
509  * maximum number of characters for a string
510  */
511 size_t
513 {
514  return 1;
515 }
516 
517 /** Set l_e3 value.
518  * 3rd Euler angle of left manipulator rotation
519  * @param new_l_e3 new l_e3 value
520  */
521 void
523 {
524  data->l_e3 = new_l_e3;
525 }
526 
527 /** Get r_x value.
528  * X-coordinate of right manipulator
529  * @return r_x value
530  */
531 float
533 {
534  return data->r_x;
535 }
536 
537 /** Get maximum length of r_x value.
538  * @return length of r_x value, can be length of the array or number of
539  * maximum number of characters for a string
540  */
541 size_t
543 {
544  return 1;
545 }
546 
547 /** Set r_x value.
548  * X-coordinate of right manipulator
549  * @param new_r_x new r_x value
550  */
551 void
553 {
554  data->r_x = new_r_x;
555 }
556 
557 /** Get r_y value.
558  * Y-coordinate of right manipulator
559  * @return r_y value
560  */
561 float
563 {
564  return data->r_y;
565 }
566 
567 /** Get maximum length of r_y value.
568  * @return length of r_y value, can be length of the array or number of
569  * maximum number of characters for a string
570  */
571 size_t
573 {
574  return 1;
575 }
576 
577 /** Set r_y value.
578  * Y-coordinate of right manipulator
579  * @param new_r_y new r_y value
580  */
581 void
583 {
584  data->r_y = new_r_y;
585 }
586 
587 /** Get r_z value.
588  * Z-coordinate of right manipulator
589  * @return r_z value
590  */
591 float
593 {
594  return data->r_z;
595 }
596 
597 /** Get maximum length of r_z value.
598  * @return length of r_z value, can be length of the array or number of
599  * maximum number of characters for a string
600  */
601 size_t
603 {
604  return 1;
605 }
606 
607 /** Set r_z value.
608  * Z-coordinate of right manipulator
609  * @param new_r_z new r_z value
610  */
611 void
613 {
614  data->r_z = new_r_z;
615 }
616 
617 /** Get r_e1 value.
618  * 1st Euler angle of right manipulator rotation
619  * @return r_e1 value
620  */
621 float
623 {
624  return data->r_e1;
625 }
626 
627 /** Get maximum length of r_e1 value.
628  * @return length of r_e1 value, can be length of the array or number of
629  * maximum number of characters for a string
630  */
631 size_t
633 {
634  return 1;
635 }
636 
637 /** Set r_e1 value.
638  * 1st Euler angle of right manipulator rotation
639  * @param new_r_e1 new r_e1 value
640  */
641 void
643 {
644  data->r_e1 = new_r_e1;
645 }
646 
647 /** Get r_e2 value.
648  * 2nd Euler angle of right manipulator rotation
649  * @return r_e2 value
650  */
651 float
653 {
654  return data->r_e2;
655 }
656 
657 /** Get maximum length of r_e2 value.
658  * @return length of r_e2 value, can be length of the array or number of
659  * maximum number of characters for a string
660  */
661 size_t
663 {
664  return 1;
665 }
666 
667 /** Set r_e2 value.
668  * 2nd Euler angle of right manipulator rotation
669  * @param new_r_e2 new r_e2 value
670  */
671 void
673 {
674  data->r_e2 = new_r_e2;
675 }
676 
677 /** Get r_e3 value.
678  * 3rd Euler angle of right manipulator rotation
679  * @return r_e3 value
680  */
681 float
683 {
684  return data->r_e3;
685 }
686 
687 /** Get maximum length of r_e3 value.
688  * @return length of r_e3 value, can be length of the array or number of
689  * maximum number of characters for a string
690  */
691 size_t
693 {
694  return 1;
695 }
696 
697 /** Set r_e3 value.
698  * 3rd Euler angle of right manipulator rotation
699  * @param new_r_e3 new r_e3 value
700  */
701 void
703 {
704  data->r_e3 = new_r_e3;
705 }
706 
707 /** Clone this message.
708  * Produces a message of the same type as this message and copies the
709  * data to the new message.
710  * @return clone of this message
711  */
712 Message *
714 {
716 }
717 /** @class JacoBimanualInterface::MoveGripperMessage <interfaces/JacoBimanualInterface.h>
718  * MoveGripperMessage Fawkes BlackBoard Interface Message.
719  *
720 
721  */
722 
723 
724 /** Constructor with initial values.
725  * @param ini_l_finger1 initial value for l_finger1
726  * @param ini_l_finger2 initial value for l_finger2
727  * @param ini_l_finger3 initial value for l_finger3
728  * @param ini_r_finger1 initial value for r_finger1
729  * @param ini_r_finger2 initial value for r_finger2
730  * @param ini_r_finger3 initial value for r_finger3
731  */
732 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")
733 {
734  data_size = sizeof(MoveGripperMessage_data_t);
735  data_ptr = malloc(data_size);
736  memset(data_ptr, 0, data_size);
737  data = (MoveGripperMessage_data_t *)data_ptr;
739  data->l_finger1 = ini_l_finger1;
740  data->l_finger2 = ini_l_finger2;
741  data->l_finger3 = ini_l_finger3;
742  data->r_finger1 = ini_r_finger1;
743  data->r_finger2 = ini_r_finger2;
744  data->r_finger3 = ini_r_finger3;
745  add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
746  add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
747  add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
748  add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
749  add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
750  add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
751 }
752 /** Constructor */
754 {
755  data_size = sizeof(MoveGripperMessage_data_t);
756  data_ptr = malloc(data_size);
757  memset(data_ptr, 0, data_size);
758  data = (MoveGripperMessage_data_t *)data_ptr;
760  add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
761  add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
762  add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
763  add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
764  add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
765  add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
766 }
767 
768 /** Destructor */
770 {
771  free(data_ptr);
772 }
773 
774 /** Copy constructor.
775  * @param m message to copy from
776  */
778 {
779  data_size = m->data_size;
780  data_ptr = malloc(data_size);
781  memcpy(data_ptr, m->data_ptr, data_size);
782  data = (MoveGripperMessage_data_t *)data_ptr;
784 }
785 
786 /* Methods */
787 /** Get l_finger1 value.
788  * Value of finger 1 on left gripper. Range [0,60]
789  * @return l_finger1 value
790  */
791 float
793 {
794  return data->l_finger1;
795 }
796 
797 /** Get maximum length of l_finger1 value.
798  * @return length of l_finger1 value, can be length of the array or number of
799  * maximum number of characters for a string
800  */
801 size_t
803 {
804  return 1;
805 }
806 
807 /** Set l_finger1 value.
808  * Value of finger 1 on left gripper. Range [0,60]
809  * @param new_l_finger1 new l_finger1 value
810  */
811 void
813 {
814  data->l_finger1 = new_l_finger1;
815 }
816 
817 /** Get l_finger2 value.
818  * Value of finger 2 on left gripper. Range [0,60]
819  * @return l_finger2 value
820  */
821 float
823 {
824  return data->l_finger2;
825 }
826 
827 /** Get maximum length of l_finger2 value.
828  * @return length of l_finger2 value, can be length of the array or number of
829  * maximum number of characters for a string
830  */
831 size_t
833 {
834  return 1;
835 }
836 
837 /** Set l_finger2 value.
838  * Value of finger 2 on left gripper. Range [0,60]
839  * @param new_l_finger2 new l_finger2 value
840  */
841 void
843 {
844  data->l_finger2 = new_l_finger2;
845 }
846 
847 /** Get l_finger3 value.
848  * Value of finger 3 on left gripper. Range [0,60]
849  * @return l_finger3 value
850  */
851 float
853 {
854  return data->l_finger3;
855 }
856 
857 /** Get maximum length of l_finger3 value.
858  * @return length of l_finger3 value, can be length of the array or number of
859  * maximum number of characters for a string
860  */
861 size_t
863 {
864  return 1;
865 }
866 
867 /** Set l_finger3 value.
868  * Value of finger 3 on left gripper. Range [0,60]
869  * @param new_l_finger3 new l_finger3 value
870  */
871 void
873 {
874  data->l_finger3 = new_l_finger3;
875 }
876 
877 /** Get r_finger1 value.
878  * Value of finger 1 on right gripper. Range [0,60]
879  * @return r_finger1 value
880  */
881 float
883 {
884  return data->r_finger1;
885 }
886 
887 /** Get maximum length of r_finger1 value.
888  * @return length of r_finger1 value, can be length of the array or number of
889  * maximum number of characters for a string
890  */
891 size_t
893 {
894  return 1;
895 }
896 
897 /** Set r_finger1 value.
898  * Value of finger 1 on right gripper. Range [0,60]
899  * @param new_r_finger1 new r_finger1 value
900  */
901 void
903 {
904  data->r_finger1 = new_r_finger1;
905 }
906 
907 /** Get r_finger2 value.
908  * Value of finger 2 on right gripper. Range [0,60]
909  * @return r_finger2 value
910  */
911 float
913 {
914  return data->r_finger2;
915 }
916 
917 /** Get maximum length of r_finger2 value.
918  * @return length of r_finger2 value, can be length of the array or number of
919  * maximum number of characters for a string
920  */
921 size_t
923 {
924  return 1;
925 }
926 
927 /** Set r_finger2 value.
928  * Value of finger 2 on right gripper. Range [0,60]
929  * @param new_r_finger2 new r_finger2 value
930  */
931 void
933 {
934  data->r_finger2 = new_r_finger2;
935 }
936 
937 /** Get r_finger3 value.
938  * Value of finger 3 on right gripper. Range [0,60]
939  * @return r_finger3 value
940  */
941 float
943 {
944  return data->r_finger3;
945 }
946 
947 /** Get maximum length of r_finger3 value.
948  * @return length of r_finger3 value, can be length of the array or number of
949  * maximum number of characters for a string
950  */
951 size_t
953 {
954  return 1;
955 }
956 
957 /** Set r_finger3 value.
958  * Value of finger 3 on right gripper. Range [0,60]
959  * @param new_r_finger3 new r_finger3 value
960  */
961 void
963 {
964  data->r_finger3 = new_r_finger3;
965 }
966 
967 /** Clone this message.
968  * Produces a message of the same type as this message and copies the
969  * data to the new message.
970  * @return clone of this message
971  */
972 Message *
974 {
976 }
977 /** @class JacoBimanualInterface::SetPlannerParamsMessage <interfaces/JacoBimanualInterface.h>
978  * SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
979  *
980 
981  */
982 
983 
984 /** Constructor with initial values.
985  * @param ini_params initial value for params
986  */
987 JacoBimanualInterface::SetPlannerParamsMessage::SetPlannerParamsMessage(const char * ini_params) : Message("SetPlannerParamsMessage")
988 {
989  data_size = sizeof(SetPlannerParamsMessage_data_t);
990  data_ptr = malloc(data_size);
991  memset(data_ptr, 0, data_size);
992  data = (SetPlannerParamsMessage_data_t *)data_ptr;
994  strncpy(data->params, ini_params, 1024);
995  add_fieldinfo(IFT_STRING, "params", 1024, data->params);
996 }
997 /** Constructor */
999 {
1000  data_size = sizeof(SetPlannerParamsMessage_data_t);
1001  data_ptr = malloc(data_size);
1002  memset(data_ptr, 0, data_size);
1003  data = (SetPlannerParamsMessage_data_t *)data_ptr;
1005  add_fieldinfo(IFT_STRING, "params", 1024, data->params);
1006 }
1007 
1008 /** Destructor */
1010 {
1011  free(data_ptr);
1012 }
1013 
1014 /** Copy constructor.
1015  * @param m message to copy from
1016  */
1018 {
1019  data_size = m->data_size;
1020  data_ptr = malloc(data_size);
1021  memcpy(data_ptr, m->data_ptr, data_size);
1022  data = (SetPlannerParamsMessage_data_t *)data_ptr;
1024 }
1025 
1026 /* Methods */
1027 /** Get params value.
1028  * Planner parameters
1029  * @return params value
1030  */
1031 char *
1033 {
1034  return data->params;
1035 }
1036 
1037 /** Get maximum length of params value.
1038  * @return length of params value, can be length of the array or number of
1039  * maximum number of characters for a string
1040  */
1041 size_t
1043 {
1044  return 1024;
1045 }
1046 
1047 /** Set params value.
1048  * Planner parameters
1049  * @param new_params new params value
1050  */
1051 void
1053 {
1054  strncpy(data->params, new_params, sizeof(data->params));
1055 }
1056 
1057 /** Clone this message.
1058  * Produces a message of the same type as this message and copies the
1059  * data to the new message.
1060  * @return clone of this message
1061  */
1062 Message *
1064 {
1066 }
1067 /** @class JacoBimanualInterface::SetConstrainedMessage <interfaces/JacoBimanualInterface.h>
1068  * SetConstrainedMessage Fawkes BlackBoard Interface Message.
1069  *
1070 
1071  */
1072 
1073 
1074 /** Constructor with initial values.
1075  * @param ini_constrained initial value for constrained
1076  */
1077 JacoBimanualInterface::SetConstrainedMessage::SetConstrainedMessage(const bool ini_constrained) : Message("SetConstrainedMessage")
1078 {
1079  data_size = sizeof(SetConstrainedMessage_data_t);
1080  data_ptr = malloc(data_size);
1081  memset(data_ptr, 0, data_size);
1082  data = (SetConstrainedMessage_data_t *)data_ptr;
1084  data->constrained = ini_constrained;
1085  add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1086 }
1087 /** Constructor */
1089 {
1090  data_size = sizeof(SetConstrainedMessage_data_t);
1091  data_ptr = malloc(data_size);
1092  memset(data_ptr, 0, data_size);
1093  data = (SetConstrainedMessage_data_t *)data_ptr;
1095  add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1096 }
1097 
1098 /** Destructor */
1100 {
1101  free(data_ptr);
1102 }
1103 
1104 /** Copy constructor.
1105  * @param m message to copy from
1106  */
1108 {
1109  data_size = m->data_size;
1110  data_ptr = malloc(data_size);
1111  memcpy(data_ptr, m->data_ptr, data_size);
1112  data = (SetConstrainedMessage_data_t *)data_ptr;
1114 }
1115 
1116 /* Methods */
1117 /** Get constrained value.
1118  * Wheter planning is using constraint-function.
1119  This is an OpenRAVE internal constraint function.
1120  * @return constrained value
1121  */
1122 bool
1124 {
1125  return data->constrained;
1126 }
1127 
1128 /** Get maximum length of constrained value.
1129  * @return length of constrained value, can be length of the array or number of
1130  * maximum number of characters for a string
1131  */
1132 size_t
1134 {
1135  return 1;
1136 }
1137 
1138 /** Set constrained value.
1139  * Wheter planning is using constraint-function.
1140  This is an OpenRAVE internal constraint function.
1141  * @param new_constrained new constrained value
1142  */
1143 void
1145 {
1146  data->constrained = new_constrained;
1147 }
1148 
1149 /** Clone this message.
1150  * Produces a message of the same type as this message and copies the
1151  * data to the new message.
1152  * @return clone of this message
1153  */
1154 Message *
1156 {
1158 }
1159 /** Check if message is valid and can be enqueued.
1160  * @param message Message to check
1161  * @return true if the message is valid, false otherwise.
1162  */
1163 bool
1165 {
1166  const CartesianGotoMessage *m0 = dynamic_cast<const CartesianGotoMessage *>(message);
1167  if ( m0 != NULL ) {
1168  return true;
1169  }
1170  const MoveGripperMessage *m1 = dynamic_cast<const MoveGripperMessage *>(message);
1171  if ( m1 != NULL ) {
1172  return true;
1173  }
1174  const SetPlannerParamsMessage *m2 = dynamic_cast<const SetPlannerParamsMessage *>(message);
1175  if ( m2 != NULL ) {
1176  return true;
1177  }
1178  const SetConstrainedMessage *m3 = dynamic_cast<const SetConstrainedMessage *>(message);
1179  if ( m3 != NULL ) {
1180  return true;
1181  }
1182  return false;
1183 }
1184 
1185 /// @cond INTERNALS
1186 EXPORT_INTERFACE(JacoBimanualInterface)
1187 /// @endcond
1188 
1189 
1190 } // end namespace fawkes
void set_l_x(const float new_l_x)
Set l_x value.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
void set_l_y(const float new_l_y)
Set l_y value.
void set_r_finger3(const float new_r_finger3)
Set r_finger3 value.
MoveGripperMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_y() const
Get maximum length of l_y value.
void set_r_z(const float new_r_z)
Set r_z value.
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:124
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
void set_l_finger1(const float new_l_finger1)
Set l_finger1 value.
void set_r_x(const float new_r_x)
Set r_x value.
size_t maxlenof_r_z() const
Get maximum length of r_z value.
size_t maxlenof_r_x() const
Get maximum length of r_x value.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
size_t maxlenof_r_y() const
Get maximum length of r_y value.
size_t maxlenof_r_e3() const
Get maximum length of r_e3 value.
Fawkes library namespace.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
Definition: message.h:129
JacoBimanualInterface Fawkes BlackBoard Interface.
virtual Message * clone() const
Clone this message.
void set_r_y(const float new_r_y)
Set r_y value.
void set_constrained(const bool new_constrained)
Set constrained value.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_x() const
Get maximum length of l_x value.
string field
Definition: types.h:47
void set_l_finger2(const float new_l_finger2)
Set l_finger2 value.
size_t maxlenof_l_e2() const
Get maximum length of l_e2 value.
size_t maxlenof_l_finger2() const
Get maximum length of l_finger2 value.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
size_t maxlenof_l_e3() const
Get maximum length of l_e3 value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_r_finger2() const
Get maximum length of r_finger2 value.
size_t maxlenof_r_e2() const
Get maximum length of r_e2 value.
size_t maxlenof_r_finger3() const
Get maximum length of r_finger3 value.
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:133
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:125
void set_r_e2(const float new_r_e2)
Set r_e2 value.
void set_r_e3(const float new_r_e3)
Set r_e3 value.
size_t maxlenof_constrained() const
Get maximum length of constrained value.
void set_r_finger2(const float new_r_finger2)
Set r_finger2 value.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
const char * type() const
Get type of interface.
Definition: interface.cpp:651
void set_r_e1(const float new_r_e1)
Set r_e1 value.
void set_l_e1(const float new_l_e1)
Set l_e1 value.
void set_l_e2(const float new_l_e2)
Set l_e2 value.
void set_l_finger3(const float new_l_finger3)
Set l_finger3 value.
size_t maxlenof_l_z() const
Get maximum length of l_z value.
float field
Definition: types.h:45
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
void set_params(const char *new_params)
Set params value.
size_t maxlenof_l_finger3() const
Get maximum length of l_finger3 value.
void set_r_finger1(const float new_r_finger1)
Set r_finger1 value.
size_t maxlenof_r_e1() const
Get maximum length of r_e1 value.
virtual Message * clone() const
Clone this message.
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:436
boolean field
Definition: types.h:36
size_t maxlenof_params() const
Get maximum length of params value.
void set_l_z(const float new_l_z)
Set l_z value.
size_t maxlenof_r_finger1() const
Get maximum length of r_finger1 value.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_finger1() const
Get maximum length of l_finger1 value.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
void set_l_e3(const float new_l_e3)
Set l_e3 value.
size_t maxlenof_l_e1() const
Get maximum length of l_e1 value.