iob.cpp
Go to the documentation of this file.
1 // -*- Mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 //
3 // iob.cpp for NEXTAGE OPEN
4 //
5 //
7 /*
8 * iob.cpp
9 * Copyright (c) 2014, Tokyo Opensource Robotics Kyokai Association
10 *
11 * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
12 * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
13 * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
14 * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
15 *
16 * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
17 * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
18 * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
19 * CONDITIONS.
20 * http://creativecommons.org/licenses/by-nc/4.0
21 */
22 
23 #include <cstdio>
24 #include <dlfcn.h>
25 
26 // depended by iob.h
27 #include <unistd.h> // pid_t, etc.
28 #include <pthread.h> // mutex
29 
30 #include <hrpsys/io/iob.h>
31 #include <nextage-open.hpp>
32 
33 #define JID_INVALID -2
34 
35 // private variables
37 
38 #define CHECK_JOINT_ID(id) if((id) < 0 || (id) >= number_of_joints()) return E_ID
39 #define CHECK_JOINT_ID_DOF(id) if((id) < 0 || (id) >= DOF) return E_ID
40 
42 // RENAME number_of_joints -> Done
44 {
45  return (15); // this might be called before open_iob().
46 }
47 
49 // ADD number_of_gyro_sensors - Done
51 {
52  return FALSE;
53 }
54 
56 // ADD number_of_force_sensors - Done
58 {
59  return 0;
60 }
61 
63 // ADD set_number_of_force_sensors - Done
65 {
66  return FALSE;
67 }
68 
70 // ADD number_of_gyro_sensors - Done
72 {
73  return 0;
74 }
75 
77 // ADD set_number_of_gyro_sensors - Done
79 {
80  return FALSE;
81 }
82 
84 // ADD number_of_accelerometers - Done
86 {
87  return 0;
88 }
89 
91 // ADD set_number_of_accelerometers - Done
93 {
94  return FALSE;
95 }
96 
98 // ADD number_of_attitude_sensors - Done
100 {
101  return 0;
102 }
103 
105 // RENAME read_actual_angle - TODO dynamic test
106 //int readActualAngle(const uint32_t jointid, double *o_angle)
107 int read_actual_angle(int id, double *angle)
108 {
109  if (nxifv1)
110  return nxifv1->read_actual_angle(id, angle);
111  else
112  return FALSE;
113 }
114 
116 // RENAME read_actual_angles - TODO dynamic check
118 {
119  if (nxifv1)
120  return nxifv1->read_actual_angles(angles);
121  else
122  return FALSE;
123 }
124 
126 // ADD read_angle_offset - Done
127 int read_angle_offset(int id, double *angle)
128 {
129  return FALSE;
130 }
131 
133 // ADD write_angle_offset - Done
134 int write_angle_offset(int id, double angle)
135 {
136  return FALSE;
137 }
138 
139 
140 
142 // RENAME: read_power_state - Done, TODO static test
143 int read_power_state(int id, int *s)
144 {
145  if (nxifv1)
146  return nxifv1->read_power_state(id, s);
147  else
148  return FALSE;
149 }
150 
151 
152 // RENAME write_power_command - Done. TODO dynamic test
153 int write_power_command(int id, int com)
154 {
155  if (nxifv1)
156  return nxifv1->write_power_command(id, com);
157  else
158  return FALSE;
159 }
160 
162 // ADD read_power_command - done, TODO dynamic
163 int read_power_command(int id, int *com)
164 {
165  if (nxifv1)
166  return nxifv1->read_power_command(id, com);
167  else
168  return FALSE;
169 }
170 
172 // RENAME read_servo_state - TODO dynamic test
173 int read_servo_state(int id, int *s)
174 {
175  if (nxifv1)
176  return nxifv1->read_servo_state(id, s);
177  else
178  return FALSE;
179 }
180 
182 // RENAME read_servo_alarm - done, TODO dynamic
183 int read_servo_alarm(int id, int *a)
184 {
185  if (nxifv1)
186  return nxifv1->read_servo_alarm(id, a);
187  else
188  return FALSE;
189 }
190 
192 // ADD read_control_mode - done, TODO static test
194 {
195  return FALSE;
196 }
197 
199 // ADD write_control_mode - done
201 {
202  return FALSE;
203 }
204 
206 // ADD read_actual_torque - done
207 int read_actual_torque(int id, double *angle)
208 {
209  return FALSE;
210 }
211 
213 // RENAME: read_actual_torques
214 int read_actual_torques(double *torques)
215 {
216  return FALSE;
217 }
218 
220 // ADD read_command_torque - done
221 int read_command_torque(int id, double *torque)
222 {
223  return FALSE;
224 }
225 
227 // ADD write_command_torque - done
228 int write_command_torque(int id, double torque)
229 {
230  return FALSE;
231 }
232 
234 // ADD read_command_torques - done
235 int read_command_torques(double *torques)
236 {
237  return FALSE;
238 }
239 
240 int write_command_torques(const double *torques)
241 {
242  return FALSE;
243 }
244 
245 
247 // RENAME read_command_angle - done, TODO dynamic
248 int read_command_angle(int id, double *angle)
249 {
250  if (nxifv1)
251  return nxifv1->read_command_angle(id, angle);
252  else
253  return FALSE;
254 }
255 
257 // RENAME read_command_angles - done, TODO dynamic
259 {
260  if (nxifv1)
261  return nxifv1->read_command_angles(angles);
262  else
263  return FALSE;
264 }
265 
267 // RENAME write_command_angle - done. TODO dynamic
268 int write_command_angle(int id, double angle)
269 {
270  if (nxifv1)
271  return nxifv1->write_command_angle(id, angle);
272  else
273  return FALSE;
274 }
275 
277 // RENAME write_command_angles - done. TODO dynamic
278 int write_command_angles(const double *angles)
279 {
280  if (nxifv1)
281  return nxifv1->write_command_angles(angles);
282  else
283  return FALSE;
284 }
285 
287 // RENAME read_pgain - done. TODO static
288 int read_pgain(int id, double *gain)
289 {
290  return E_ID; // FALSE ?
291 }
292 
294 // RENAME write_pgain - requires implementation
295 int write_pgain(int id, double gain)
296 {
297  return E_ID; // FALSE ?
298 }
299 
301 // RENAME read_dgain - done
302 int read_dgain(int id, double *gain)
303 {
304  return E_ID;
305 }
306 
308 // RENAME write_dgain - done
309 //int
310 int write_dgain(int id, double gain)
311 {
312  return E_ID;
313 }
314 
315 // WANT read_igain
316 // WANT write_igain
317 
318 int read_actual_velocity(int id, double *vel)
319 {
320  return FALSE;
321 }
322 
324 // ADD read_command_velocity - done
325 int read_command_velocity(int id, double *vel)
326 {
327  return E_ID;
328 }
329 
331 // ADD write_command_velocity - done
332 int write_command_velocity(int id, double vel)
333 {
334  return E_ID;
335 }
336 
338 // ADD read_command_velocities - done
339 int read_command_velocities(double *vels)
340 {
341  return FALSE;
342 }
343 
344 int read_actual_velocities(double *vels)
345 {
346  return FALSE;
347 }
348 
350 // ADD write_command_velocities - done
351 int write_command_velocities(const double *vels)
352 {
353  return FALSE;
354 }
355 
357 // RENAME write_servo TODO dynamic test
358 int write_servo(int id, int com)
359 {
360  if (nxifv1)
361  return nxifv1->write_servo(id, com);
362  else
363  return FALSE;
364 }
365 
367 // ADD read_driver_temperature - done
368 int read_driver_temperature(int id, unsigned char *v)
369 {
370  if (nxifv1)
371  return nxifv1->read_driver_temperature(id, v);
372  else
373  return FALSE;
374 }
375 
377 // RENAME read_calib_state - done. TODO dynamic
378 int read_calib_state(int id, int *s)
379 {
380  if (nxifv1)
381  return nxifv1->read_calib_state(id, s);
382  else
383  return FALSE;
384 }
385 
387 // ADD length_of_extra_servo_state - TODO implementation
389 {
390  return 0;
391 }
392 
394 // ADD read_extra_servo_state - TODO implementation
395 int read_extra_servo_state(int id, int *state)
396 {
397  return TRUE;
398 }
399 
401 // RENAME read_force_sensor - done
402 int read_force_sensor(int id, double *forces)
403 {
404  return E_ID;
405 }
406 
408 // Add read_force_offset - done
409 int read_force_offset(int id, double *offsets)
410 {
411  return E_ID;
412 }
413 
415 // ADD write_force_offset - done
416 int write_force_offset(int id, double *offsets)
417 {
418  return E_ID;
419 }
420 
422 // RENAME read_gyro_sensor - done
423 //int
424 int read_gyro_sensor(int id, double *rates)
425 {
426  return E_ID;
427 }
428 
430 // ADD read_gyro_sensor_offset - done
431 int read_gyro_sensor_offset(int id, double *offset)
432 {
433  return E_ID;
434 }
435 
437 // ADD write_gyro_sensor_offset - done
438 int write_gyro_sensor_offset(int id, double *offset)
439 {
440  return E_ID;
441 }
442 
444 // RENAME read_accelerometer - done
445 //int
446 int read_accelerometer(int id, double *accels)
447 {
448  return E_ID;
449 }
450 
452 // ADD read_accelerometer_offset - done
453 int read_accelerometer_offset(int id, double *offset)
454 {
455  return E_ID;
456 }
457 
459 // ADD write_accelerometer_offset - done
460 int write_accelerometer_offset(int id, double *offset)
461 {
462  return E_ID;
463 }
464 
466 // ADD read_attitude_sensor - done
467 int read_attitude_sensor(int id, double *att)
468 {
469  return E_ID;
470 }
471 
473 // ADD write_attitude_sensor_offset - done
474 int write_attitude_sensor_offset(int id, double *offset)
475 {
476  return E_ID;
477 }
478 
480 // ADD read_power - done. TODO implement
481 int read_power(double *voltage, double *current)
482 {
483  if (nxifv1)
484  return nxifv1->read_power(voltage, current);
485  else
486  return FALSE;
487 }
488 
490 // RENAME read_driver_temperature - done
491 //int
492 int read_temperature(int id, double *v)
493 {
494  return E_ID;
495 }
496 
497 
498 static pthread_mutex_t openLock = PTHREAD_MUTEX_INITIALIZER;
499 
500 // RENAME open_iob - done TODO check implementation
501 int open_iob(void)
502 {
503  pthread_mutex_lock (&openLock);
504  if (nxifv1 == NULL) {
505  void *lib = dlopen("libNxOpenCore.so", RTLD_LOCAL|RTLD_NOW);
506  if (lib == NULL) {
507  std::fprintf(stdout, "** failed in dlopen(libNxOpenCore.so): %s\n", dlerror());
508  pthread_mutex_unlock(&openLock);
509  return FALSE;
510  }
511 
512  bool (*check)(const char *);
513  check = reinterpret_cast<bool (*)(const char *)>(dlsym(lib, "nextage_open_supported"));
514  if (check==NULL) {
515  std::fprintf(stdout, "** failed to find nextage_open_supported(): %s\n", dlerror());
516  pthread_mutex_unlock(&openLock);
517  return FALSE;
518  }
519 
520  if (check("v1.0")==false) {
521  std::fprintf(stdout, "** v1.0 not supported\n");
522  pthread_mutex_unlock(&openLock);
523  return FALSE;
524  }
525 
526  NEXTAGE_OPEN::OpenIFv10 * (*getif)(void);
527  getif = reinterpret_cast<NEXTAGE_OPEN::OpenIFv10 * (*)(void)>(dlsym(lib, "nextage_open_getIFv10"));
528  if (getif==NULL) {
529  std::fprintf(stdout, "** failed to find nextage_open_getIFv10(): %s\n", dlerror());
530  pthread_mutex_unlock(&openLock);
531  return FALSE;
532  }
533 
534  nxifv1 = getif();
535  if (nxifv1 == NULL) {
536  std::fprintf(stdout, "** failed to getIF()\n");
537  pthread_mutex_unlock(&openLock);
538  return FALSE;
539  }
540 
541 
542  std::fprintf(stdout, "open_iob - NEXTAGE OPEN I/F v1 instance at 0x%x\n", nxifv1);
543  pthread_mutex_unlock(&openLock);
544  return nxifv1->open_iob();
545 
546  } else {
547  std::fprintf(stdout, "open_iob - NEXTAGE OPEN I/F instance is not NULL, ignored\n");
548  pthread_mutex_unlock(&openLock);
549  return TRUE;
550  }
551 
552  pthread_mutex_unlock(&openLock);
553  return FALSE; // unexpected
554 }
555 
556 // RENAME close_iob - done. TODO check implementation
557 //int
558 int close_iob(void)
559 {
560  // do nothing
561  return TRUE;
562 #if 0
563  if (nxifv1 != NULL) {
564  int res=nxifv1->close_iob();
565  delete nxifv1;
566  nxifv1 = NULL;
567  return res;
568  } else {
569  std::fprintf(stdout, "close_iob(): nxifv1 is NULL\n");
570  }
571 
572  return FALSE;
573 #endif
574 }
575 
576 
577 // ADD reset_body - TODO static test
578 int reset_body(void)
579 {
580  if (nxifv1)
581  return nxifv1->reset_body();
582  else
583  return FALSE;
584 }
585 
586 // RENAME: lock_iob - done. TODO check implementation
587 int lock_iob()
588 {
589  if (nxifv1)
590  return nxifv1->lock_iob();
591  else
592  return FALSE;
593 }
594 
595 // RENAME: unlock_iob - done. TODO check static test
596 //int
597 //ShMIF::unlock ()
599 {
600  if (nxifv1)
601  return nxifv1->unlock_iob();
602  else
603  return FALSE;
604 }
605 
606 // ADD read_lock_owner - done. TODO static test
608 {
609  if (nxifv1)
610  return nxifv1->read_lock_owner(pid);
611  else
612  return FALSE;
613 }
614 
615 // RENAME read_iob_frame - done. TODO static test
616 //WANT uint64_t read_iob_frame()
617 //int read_iob_frame()
618 unsigned long long read_iob_frame()
619 {
620  if (nxifv1)
621  return nxifv1->read_iob_frame();
622  else
623  return FALSE;
624 }
625 
626 // REMOVE getAddr
627 
628 // RENAME number_of_substeps
630 {
631  if (nxifv1)
632  return nxifv1->number_of_substeps();
633  else
634  return FALSE;
635  //return 1;
636  //return 5;
637 }
638 
639 // RENAME wait_for_iob_signal - done. TODO implementation
641 {
642  if (nxifv1)
643  return nxifv1->wait_for_iob_signal();
644  else
645  return FALSE;
646 }
647 
653 int set_signal_period(long period_ns)
654 {
655  return FALSE;
656 }
657 
663 {
664  if (nxifv1)
665  return nxifv1->get_signal_period();
666  else
667  return FALSE;
668  //return 5 * 1e6;
669 }
670 
671 
672 
674 // TODO move internal
675 
676 /*
677  * for safety
678  */
679 // TODO everything to do with calibrateJoints is to be moved here
680 // RENAME initializeJointAngle
681 int initializeJointAngle(const char *name, const char *option)
682 {
683  if (nxifv1)
684  return nxifv1->initializeJointAngle(name, option);
685  else
686  return FALSE;
687 }
688 
689 int read_digital_input(char *dIn)
690 {
691  if (nxifv1)
692  return nxifv1->read_digital_input(dIn);
693  else
694  return FALSE;
695 }
696 
698 {
699  if (nxifv1)
700  return nxifv1->length_digital_input();
701  else
702  return FALSE;
703 }
704 
705 int write_digital_output(const char *doutput)
706 {
707  if (nxifv1)
708  return nxifv1->write_digital_output(doutput);
709  else
710  return FALSE;
711 }
712 
713 int write_digital_output_with_mask(const char *doutput, const char *mask)
714 {
715  if (nxifv1)
716  return nxifv1->write_digital_output_with_mask(doutput, mask);
717  else
718  return FALSE;
719 }
720 
722 {
723  if (nxifv1)
724  return nxifv1->length_digital_output();
725  else
726  return FALSE;
727 }
728 
729 int read_digital_output(char *doutput)
730 {
731  if (nxifv1)
732  return nxifv1->read_digital_output(doutput);
733  else
734  return FALSE;
735 }
int write_dgain(int id, double gain)
Definition: iob.cpp:310
virtual int read_driver_temperature(int id, unsigned char *v)=0
int read_servo_state(int id, int *s)
Definition: iob.cpp:173
virtual int read_lock_owner(pid_t *pid)=0
int read_attitude_sensor(int id, double *att)
Definition: iob.cpp:467
int read_actual_torque(int id, double *angle)
Definition: iob.cpp:207
int write_power_command(int id, int com)
Definition: iob.cpp:153
int read_command_angle(int id, double *angle)
Definition: iob.cpp:248
int set_number_of_gyro_sensors(int num)
Definition: iob.cpp:78
int read_dgain(int id, double *gain)
Definition: iob.cpp:302
int read_calib_state(int id, int *s)
Definition: iob.cpp:378
ROSCPP_DECL bool check()
virtual int read_power_state(int id, int *s)=0
int read_command_torques(double *torques)
Definition: iob.cpp:235
int read_extra_servo_state(int id, int *state)
Definition: iob.cpp:395
virtual int length_digital_input(void)=0
#define E_ID
int write_digital_output_with_mask(const char *doutput, const char *mask)
Definition: iob.cpp:713
virtual int read_power_command(int id, int *com)=0
int read_gyro_sensor_offset(int id, double *offset)
Definition: iob.cpp:431
int read_control_mode(int id, joint_control_mode *s)
Definition: iob.cpp:193
int write_angle_offset(int id, double angle)
Definition: iob.cpp:134
int close_iob(void)
Definition: iob.cpp:558
int read_force_sensor(int id, double *forces)
Definition: iob.cpp:402
int number_of_accelerometers()
Definition: iob.cpp:85
virtual int read_servo_alarm(int id, int *a)=0
virtual int unlock_iob(void)=0
int read_power(double *voltage, double *current)
Definition: iob.cpp:481
virtual int write_digital_output(const char *doutput)=0
int read_angle_offset(int id, double *angle)
Definition: iob.cpp:127
virtual int open_iob(void)=0
int number_of_gyro_sensors()
Definition: iob.cpp:71
virtual int number_of_substeps(void)=0
int read_actual_angle(int id, double *angle)
Definition: iob.cpp:107
int open_iob(void)
Definition: iob.cpp:501
int write_command_torque(int id, double torque)
Definition: iob.cpp:228
int write_control_mode(int id, joint_control_mode s)
Definition: iob.cpp:200
int length_digital_input()
Definition: iob.cpp:697
int write_gyro_sensor_offset(int id, double *offset)
Definition: iob.cpp:438
virtual int initializeJointAngle(const char *name, const char *option)=0
virtual int write_servo(int id, int com)=0
size_t length_of_extra_servo_state(int id)
Definition: iob.cpp:388
int length_digital_output()
Definition: iob.cpp:721
virtual int read_command_angles(double *angles)=0
int read_force_offset(int id, double *offsets)
Definition: iob.cpp:409
int read_power_state(int id, int *s)
Definition: iob.cpp:143
int read_command_angles(double *angles)
Definition: iob.cpp:258
int write_pgain(int id, double gain)
Definition: iob.cpp:295
virtual int read_actual_angle(int id, double *angle)=0
int set_number_of_accelerometers(int num)
Definition: iob.cpp:92
int read_digital_input(char *dIn)
Definition: iob.cpp:689
lib
::pid_t pid_t
virtual int read_calib_state(int id, int *s)=0
int write_attitude_sensor_offset(int id, double *offset)
Definition: iob.cpp:474
virtual int write_command_angles(const double *angles)=0
int reset_body(void)
Definition: iob.cpp:578
virtual int read_digital_input(char *dIn)=0
unsigned long long read_iob_frame()
Definition: iob.cpp:618
virtual int lock_iob(void)=0
int write_digital_output(const char *doutput)
Definition: iob.cpp:705
typedef void(PNGAPI *png_error_ptr) PNGARG((png_structp
virtual int close_iob(void)=0
virtual int read_command_angle(int id, double *angle)=0
int read_actual_velocities(double *vels)
Definition: iob.cpp:344
int read_gyro_sensor(int id, double *rates)
Definition: iob.cpp:424
virtual unsigned long long read_iob_frame(void)=0
int read_lock_owner(pid_t *pid)
Definition: iob.cpp:607
int number_of_force_sensors()
Definition: iob.cpp:57
int wait_for_iob_signal()
Definition: iob.cpp:640
virtual int write_power_command(int id, int com)=0
int number_of_joints()
Definition: iob.cpp:43
int read_driver_temperature(int id, unsigned char *v)
Definition: iob.cpp:368
int set_signal_period(long period_ns)
set the period of signals issued by wait_for_iob_signal()
Definition: iob.cpp:653
static NEXTAGE_OPEN::OpenIFv10 * nxifv1
Definition: iob.cpp:36
int unlock_iob()
Definition: iob.cpp:598
int read_power_command(int id, int *com)
Definition: iob.cpp:163
virtual int read_digital_output(char *doutput)=0
int read_temperature(int id, double *v)
Definition: iob.cpp:492
int read_accelerometer(int id, double *accels)
Definition: iob.cpp:446
virtual int read_power(double *voltage, double *current)=0
virtual int wait_for_iob_signal(void)=0
int write_force_offset(int id, double *offsets)
Definition: iob.cpp:416
virtual long get_signal_period(void)=0
int write_command_velocity(int id, double vel)
Definition: iob.cpp:332
int FALSE
virtual int length_digital_output(void)=0
long get_signal_period()
get the period of signals issued by wait_for_iob_signal()
Definition: iob.cpp:662
int read_actual_velocity(int id, double *vel)
Definition: iob.cpp:318
res
joint_control_mode
int number_of_attitude_sensors()
Definition: iob.cpp:99
virtual int write_command_angle(int id, double angle)=0
int read_servo_alarm(int id, int *a)
Definition: iob.cpp:183
int read_command_velocities(double *vels)
Definition: iob.cpp:339
int read_digital_output(char *doutput)
Definition: iob.cpp:729
int read_command_torque(int id, double *torque)
Definition: iob.cpp:221
int number_of_substeps()
Definition: iob.cpp:629
int read_command_velocity(int id, double *vel)
Definition: iob.cpp:325
virtual int read_servo_state(int id, int *s)=0
HANDLE pthread_mutex_t
virtual int reset_body(void)=0
int initializeJointAngle(const char *name, const char *option)
Definition: iob.cpp:681
int set_number_of_joints(int num)
Definition: iob.cpp:50
int lock_iob()
Definition: iob.cpp:587
int write_servo(int id, int com)
Definition: iob.cpp:358
int read_accelerometer_offset(int id, double *offset)
Definition: iob.cpp:453
int set_number_of_force_sensors(int num)
Definition: iob.cpp:64
static pthread_mutex_t openLock
Definition: iob.cpp:498
int write_command_velocities(const double *vels)
Definition: iob.cpp:351
int TRUE
virtual int read_actual_angles(double *angles)=0
int write_command_angle(int id, double angle)
Definition: iob.cpp:268
int read_actual_angles(double *angles)
Definition: iob.cpp:117
virtual int write_digital_output_with_mask(const char *doutput, const char *mask)=0
int write_command_torques(const double *torques)
Definition: iob.cpp:240
int write_command_angles(const double *angles)
Definition: iob.cpp:278
int read_actual_torques(double *torques)
Definition: iob.cpp:214
int read_pgain(int id, double *gain)
Definition: iob.cpp:288
int write_accelerometer_offset(int id, double *offset)
Definition: iob.cpp:460


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15