iob.cpp
Go to the documentation of this file.
00001 // -*- Mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 //
00003 // iob.cpp for NEXTAGE OPEN
00004 //
00005 //
00007 /*
00008 * iob.cpp
00009 * Copyright (c) 2014, Tokyo Opensource Robotics Kyokai Association
00010 *
00011 * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
00012 * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
00013 * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
00014 * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
00015 *
00016 * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
00017 * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
00018 * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
00019 * CONDITIONS.
00020 * http://creativecommons.org/licenses/by-nc/4.0
00021 */
00022 
00023 #include <cstdio>
00024 #include <dlfcn.h>
00025 
00026 // depended by iob.h
00027 #include <unistd.h> // pid_t, etc.
00028 #include <pthread.h> // mutex
00029 
00030 #include <hrpsys/io/iob.h>
00031 #include <nextage-open.hpp>
00032 
00033 #define JID_INVALID -2
00034 
00035 // private variables
00036 static NEXTAGE_OPEN::OpenIFv10 *nxifv1 = NULL;
00037 
00038 #define CHECK_JOINT_ID(id) if((id) < 0 || (id) >= number_of_joints()) return E_ID
00039 #define CHECK_JOINT_ID_DOF(id) if((id) < 0 || (id) >= DOF) return E_ID
00040 
00042 // RENAME number_of_joints -> Done
00043 int number_of_joints()
00044 {
00045         return (15); // this might be called before open_iob(). 
00046 }
00047 
00049 // ADD number_of_gyro_sensors - Done
00050 int set_number_of_joints(int num)
00051 {
00052     return FALSE;
00053 }
00054 
00056 // ADD number_of_force_sensors - Done
00057 int number_of_force_sensors()
00058 {
00059     return 0;
00060 }
00061 
00063 // ADD set_number_of_force_sensors - Done
00064 int set_number_of_force_sensors(int num)
00065 {
00066     return FALSE;
00067 }
00068 
00070 // ADD number_of_gyro_sensors - Done
00071 int number_of_gyro_sensors()
00072 {
00073     return 0;
00074 }
00075 
00077 // ADD set_number_of_gyro_sensors - Done
00078 int set_number_of_gyro_sensors(int num)
00079 {
00080     return FALSE;
00081 }
00082 
00084 // ADD number_of_accelerometers - Done
00085 int number_of_accelerometers()
00086 {
00087     return 0;
00088 }
00089 
00091 // ADD set_number_of_accelerometers - Done
00092 int set_number_of_accelerometers(int num)
00093 {
00094     return FALSE;
00095 }
00096 
00098 // ADD number_of_attitude_sensors - Done
00099 int number_of_attitude_sensors()
00100 {
00101     return 0;
00102 }
00103 
00105 // RENAME read_actual_angle - TODO dynamic test
00106 //int readActualAngle(const uint32_t jointid, double *o_angle)
00107 int read_actual_angle(int id, double *angle)
00108 {
00109         if (nxifv1)
00110                 return nxifv1->read_actual_angle(id, angle);
00111         else
00112                 return FALSE;
00113 }
00114 
00116 // RENAME read_actual_angles - TODO dynamic check
00117 int read_actual_angles(double *angles)
00118 {
00119         if (nxifv1)
00120                 return nxifv1->read_actual_angles(angles);
00121         else
00122                 return FALSE;
00123 }
00124 
00126 // ADD read_angle_offset - Done
00127 int read_angle_offset(int id, double *angle)
00128 {
00129     return FALSE;
00130 }
00131 
00133 // ADD write_angle_offset - Done
00134 int write_angle_offset(int id, double angle)
00135 {
00136     return FALSE;
00137 }
00138 
00139 
00140 
00142 // RENAME: read_power_state - Done, TODO static test
00143 int read_power_state(int id, int *s)
00144 {
00145         if (nxifv1)
00146                 return nxifv1->read_power_state(id, s);
00147     else
00148                 return FALSE;
00149 }
00150 
00151 
00152 // RENAME write_power_command - Done. TODO dynamic test
00153 int write_power_command(int id, int com)
00154 {
00155         if (nxifv1)
00156                 return nxifv1->write_power_command(id, com);
00157     else
00158                 return FALSE;
00159 }
00160 
00162 // ADD read_power_command - done, TODO dynamic
00163 int read_power_command(int id, int *com)
00164 {
00165         if (nxifv1)
00166                 return nxifv1->read_power_command(id, com);
00167     else
00168                 return FALSE;
00169 }
00170 
00172 // RENAME read_servo_state - TODO dynamic test
00173 int read_servo_state(int id, int *s)
00174 {
00175         if (nxifv1)
00176                 return nxifv1->read_servo_state(id, s);
00177         else
00178                 return FALSE;
00179 }
00180 
00182 // RENAME read_servo_alarm - done, TODO dynamic
00183 int read_servo_alarm(int id, int *a)
00184 {
00185         if (nxifv1)
00186                 return nxifv1->read_servo_alarm(id, a);
00187         else
00188                 return FALSE;
00189 }
00190 
00192 // ADD read_control_mode - done, TODO static test
00193 int read_control_mode(int id, joint_control_mode *s)
00194 {
00195     return FALSE;
00196 }
00197 
00199 // ADD write_control_mode - done
00200 int write_control_mode(int id, joint_control_mode s)
00201 {
00202     return FALSE;
00203 }
00204 
00206 // ADD read_actual_torque - done
00207 int read_actual_torque(int id, double *angle)
00208 {
00209     return FALSE;
00210 }
00211 
00213 // RENAME: read_actual_torques
00214 int read_actual_torques(double *torques)
00215 {
00216     return FALSE;
00217 }
00218 
00220 // ADD read_command_torque - done
00221 int read_command_torque(int id, double *torque)
00222 {
00223     return FALSE;
00224 }
00225 
00227 // ADD write_command_torque - done
00228 int write_command_torque(int id, double torque)
00229 {
00230     return FALSE;
00231 }
00232 
00234 // ADD read_command_torques - done
00235 int read_command_torques(double *torques)
00236 {
00237     return FALSE;
00238 }
00239 
00240 int write_command_torques(const double *torques)
00241 {
00242     return FALSE;
00243 }
00244 
00245 
00247 // RENAME read_command_angle - done, TODO dynamic
00248 int read_command_angle(int id, double *angle)
00249 {
00250         if (nxifv1)
00251                 return nxifv1->read_command_angle(id, angle);
00252         else
00253                 return FALSE;
00254 }
00255 
00257 // RENAME read_command_angles - done, TODO dynamic
00258 int read_command_angles(double *angles)
00259 {
00260         if (nxifv1)
00261                 return nxifv1->read_command_angles(angles);
00262         else
00263                 return FALSE;
00264 }
00265 
00267 // RENAME write_command_angle - done. TODO dynamic
00268 int write_command_angle(int id, double angle)
00269 {
00270         if (nxifv1)
00271                 return nxifv1->write_command_angle(id, angle);
00272         else
00273                 return FALSE;
00274 }
00275 
00277 // RENAME write_command_angles - done. TODO dynamic
00278 int write_command_angles(const double *angles)
00279 {
00280         if (nxifv1)
00281                 return nxifv1->write_command_angles(angles);
00282         else
00283                 return FALSE;
00284 }
00285 
00287 // RENAME read_pgain - done. TODO static
00288 int read_pgain(int id, double *gain)
00289 {
00290     return E_ID; // FALSE ?
00291 }
00292 
00294 // RENAME write_pgain - requires implementation
00295 int write_pgain(int id, double gain)
00296 {
00297     return E_ID; // FALSE ?
00298 }
00299 
00301 // RENAME read_dgain - done
00302 int read_dgain(int id, double *gain)
00303 {
00304     return E_ID;
00305 }
00306 
00308 // RENAME write_dgain - done
00309 //int
00310 int write_dgain(int id, double gain)
00311 {
00312     return E_ID;
00313 }
00314 
00315 // WANT read_igain
00316 // WANT write_igain
00317 
00318 int read_actual_velocity(int id, double *vel)
00319 {
00320     return FALSE;
00321 }
00322 
00324 // ADD read_command_velocity - done
00325 int read_command_velocity(int id, double *vel)
00326 {
00327     return E_ID;
00328 }
00329 
00331 // ADD write_command_velocity - done
00332 int write_command_velocity(int id, double vel)
00333 {
00334     return E_ID;
00335 }
00336 
00338 // ADD read_command_velocities - done
00339 int read_command_velocities(double *vels)
00340 {
00341     return FALSE;
00342 }
00343 
00344 int read_actual_velocities(double *vels)
00345 {
00346     return FALSE;
00347 }
00348 
00350 // ADD write_command_velocities - done
00351 int write_command_velocities(const double *vels)
00352 {
00353     return FALSE;
00354 }
00355 
00357 // RENAME write_servo TODO dynamic test
00358 int write_servo(int id, int com)
00359 {
00360         if (nxifv1)
00361                 return nxifv1->write_servo(id, com);
00362         else
00363                 return FALSE;
00364 }
00365 
00367 // ADD read_driver_temperature - done
00368 int read_driver_temperature(int id, unsigned char *v)
00369 {
00370         if (nxifv1)
00371                 return nxifv1->read_driver_temperature(id, v);
00372         else
00373                 return FALSE;
00374 }
00375 
00377 // RENAME read_calib_state - done. TODO dynamic
00378 int read_calib_state(int id, int *s)
00379 {
00380         if (nxifv1)
00381                 return nxifv1->read_calib_state(id, s);
00382         else
00383                 return FALSE;
00384 }
00385 
00387 // ADD length_of_extra_servo_state - TODO implementation
00388 size_t length_of_extra_servo_state(int id)
00389 {
00390     return 0;
00391 }
00392 
00394 // ADD read_extra_servo_state - TODO implementation
00395 int read_extra_servo_state(int id, int *state)
00396 {
00397     return TRUE;
00398 }
00399 
00401 // RENAME read_force_sensor - done
00402 int read_force_sensor(int id, double *forces)
00403 {
00404     return E_ID;
00405 }
00406 
00408 // Add read_force_offset - done
00409 int read_force_offset(int id, double *offsets)
00410 {
00411     return E_ID;
00412 }
00413 
00415 // ADD write_force_offset - done
00416 int write_force_offset(int id, double *offsets)
00417 {
00418     return E_ID;
00419 }
00420 
00422 // RENAME read_gyro_sensor - done
00423 //int
00424 int read_gyro_sensor(int id, double *rates)
00425 {
00426     return E_ID;
00427 }
00428 
00430 // ADD read_gyro_sensor_offset - done
00431 int read_gyro_sensor_offset(int id, double *offset)
00432 {
00433     return E_ID;
00434 }
00435 
00437 // ADD write_gyro_sensor_offset - done
00438 int write_gyro_sensor_offset(int id, double *offset)
00439 {
00440     return E_ID;
00441 }
00442 
00444 // RENAME read_accelerometer - done
00445 //int
00446 int read_accelerometer(int id, double *accels)
00447 {
00448     return E_ID;
00449 }
00450 
00452 // ADD read_accelerometer_offset - done
00453 int read_accelerometer_offset(int id, double *offset)
00454 {
00455     return E_ID;
00456 }
00457 
00459 // ADD write_accelerometer_offset - done
00460 int write_accelerometer_offset(int id, double *offset)
00461 {
00462     return E_ID;
00463 }
00464 
00466 // ADD read_attitude_sensor - done
00467 int read_attitude_sensor(int id, double *att)
00468 {
00469     return E_ID;
00470 }
00471 
00473 // ADD write_attitude_sensor_offset - done
00474 int write_attitude_sensor_offset(int id, double *offset)
00475 {
00476     return E_ID;
00477 }
00478 
00480 // ADD read_power - done. TODO implement
00481 int read_power(double *voltage, double *current)
00482 {
00483         if (nxifv1)
00484                 return nxifv1->read_power(voltage, current);
00485         else
00486                 return FALSE;
00487 }
00488 
00490 // RENAME read_driver_temperature - done
00491 //int
00492 int read_temperature(int id, double *v)
00493 {
00494     return E_ID;
00495 }
00496 
00497 
00498 static pthread_mutex_t openLock = PTHREAD_MUTEX_INITIALIZER;
00499 
00500 // RENAME open_iob - done TODO check implementation
00501 int open_iob(void)
00502 {
00503         pthread_mutex_lock (&openLock);
00504         if (nxifv1 == NULL) {
00505                 void *lib = dlopen("libNxOpenCore.so", RTLD_LOCAL|RTLD_NOW);
00506                 if (lib == NULL) {
00507                         std::fprintf(stdout, "** failed in dlopen(libNxOpenCore.so): %s\n", dlerror());
00508                         pthread_mutex_unlock(&openLock);
00509                         return FALSE;
00510                 }
00511 
00512                 bool (*check)(const char *);
00513                 check = reinterpret_cast<bool (*)(const char *)>(dlsym(lib, "nextage_open_supported"));
00514                 if (check==NULL) {
00515                         std::fprintf(stdout, "** failed to find nextage_open_supported(): %s\n", dlerror());
00516                         pthread_mutex_unlock(&openLock);
00517                         return FALSE;
00518                 }
00519 
00520                 if (check("v1.0")==false) {
00521                         std::fprintf(stdout, "** v1.0 not supported\n");
00522                         pthread_mutex_unlock(&openLock);
00523                         return FALSE;
00524                 }
00525 
00526                 NEXTAGE_OPEN::OpenIFv10 * (*getif)(void);
00527                 getif = reinterpret_cast<NEXTAGE_OPEN::OpenIFv10 * (*)(void)>(dlsym(lib, "nextage_open_getIFv10"));
00528                 if (getif==NULL) {
00529                         std::fprintf(stdout, "** failed to find nextage_open_getIFv10(): %s\n", dlerror());
00530                         pthread_mutex_unlock(&openLock);
00531                         return FALSE;
00532                 }
00533 
00534                 nxifv1 = getif();
00535                 if (nxifv1 == NULL) {
00536                         std::fprintf(stdout, "** failed to getIF()\n");
00537                         pthread_mutex_unlock(&openLock);
00538                         return FALSE;
00539                 }
00540 
00541 
00542                 std::fprintf(stdout, "open_iob - NEXTAGE OPEN  I/F v1 instance at 0x%x\n", nxifv1);
00543                 pthread_mutex_unlock(&openLock);
00544         return nxifv1->open_iob();
00545 
00546     } else {
00547                 std::fprintf(stdout, "open_iob - NEXTAGE OPEN I/F instance is not NULL, ignored\n");
00548                 pthread_mutex_unlock(&openLock);
00549                 return TRUE;
00550         }
00551 
00552         pthread_mutex_unlock(&openLock);
00553     return FALSE; // unexpected
00554 }
00555 
00556 // RENAME close_iob - done. TODO check implementation
00557 //int
00558 int close_iob(void)
00559 {
00560         // do nothing
00561         return TRUE;
00562 #if 0
00563         if (nxifv1 != NULL) {
00564                 int res=nxifv1->close_iob();
00565                 delete nxifv1;
00566                 nxifv1 = NULL;
00567                 return res;
00568         } else {
00569                 std::fprintf(stdout, "close_iob(): nxifv1 is NULL\n");
00570         }
00571 
00572     return FALSE;
00573 #endif
00574 }
00575 
00576 
00577 // ADD reset_body - TODO static test
00578 int reset_body(void)
00579 {
00580         if (nxifv1) 
00581                 return nxifv1->reset_body();
00582         else
00583                 return FALSE;
00584 }
00585 
00586 // RENAME: lock_iob - done. TODO check implementation
00587 int lock_iob()
00588 {
00589         if (nxifv1) 
00590                 return nxifv1->lock_iob();
00591         else
00592                 return FALSE;
00593 }
00594 
00595 // RENAME: unlock_iob - done. TODO check static test
00596 //int
00597 //ShMIF::unlock ()
00598 int unlock_iob()
00599 {
00600         if (nxifv1) 
00601                 return nxifv1->unlock_iob();
00602         else
00603                 return FALSE;
00604 }
00605 
00606 // ADD read_lock_owner - done. TODO static test
00607 int read_lock_owner(pid_t *pid)
00608 {
00609         if (nxifv1) 
00610                 return nxifv1->read_lock_owner(pid);
00611         else
00612                 return FALSE;
00613 }
00614 
00615 // RENAME read_iob_frame - done. TODO static test
00616 //WANT uint64_t read_iob_frame()
00617 //int read_iob_frame()
00618 unsigned long long read_iob_frame()
00619 {
00620         if (nxifv1) 
00621                 return nxifv1->read_iob_frame();
00622         else
00623                 return FALSE;
00624 }
00625 
00626 // REMOVE getAddr
00627 
00628 // RENAME number_of_substeps
00629 int number_of_substeps()
00630 {
00631         if (nxifv1) 
00632                 return nxifv1->number_of_substeps();
00633         else
00634                 return FALSE;
00635     //return 1;
00636     //return 5;
00637 }
00638 
00639 // RENAME wait_for_iob_signal - done. TODO implementation
00640 int wait_for_iob_signal()
00641 {
00642         if (nxifv1) 
00643                 return nxifv1->wait_for_iob_signal();
00644         else
00645                 return FALSE;
00646 }
00647 
00653 int set_signal_period(long period_ns)
00654 {
00655     return FALSE;
00656 }
00657 
00662 long get_signal_period()
00663 {
00664         if (nxifv1) 
00665                 return nxifv1->get_signal_period();
00666         else
00667                 return FALSE;
00668     //return 5 * 1e6;
00669 }
00670 
00671 
00672 
00674 // TODO move internal
00675 
00676 /*
00677  *  for safety
00678  */
00679 // TODO everything to do with calibrateJoints is to be moved here
00680 // RENAME initializeJointAngle
00681 int initializeJointAngle(const char *name, const char *option)
00682 {
00683         if (nxifv1)
00684                 return nxifv1->initializeJointAngle(name, option);
00685         else
00686                 return FALSE;
00687 }
00688 
00689 int read_digital_input(char *dIn)
00690 {
00691         if (nxifv1)
00692                 return nxifv1->read_digital_input(dIn);
00693         else
00694                 return FALSE;
00695 }
00696 
00697 int length_digital_input()
00698 {
00699         if (nxifv1)
00700                 return nxifv1->length_digital_input();
00701         else
00702                 return FALSE;
00703 }
00704 
00705 int write_digital_output(const char *doutput)
00706 {
00707         if (nxifv1)
00708                 return nxifv1->write_digital_output(doutput);
00709         else
00710                 return FALSE;
00711 }
00712 
00713 int write_digital_output_with_mask(const char *doutput, const char *mask)
00714 {
00715         if (nxifv1)
00716                 return nxifv1->write_digital_output_with_mask(doutput, mask);
00717         else
00718                 return FALSE;
00719 }
00720 
00721 int length_digital_output()
00722 {
00723         if (nxifv1)
00724                 return nxifv1->length_digital_output();
00725         else
00726                 return FALSE;
00727 }
00728 
00729 int read_digital_output(char *doutput)
00730 {
00731         if (nxifv1)
00732                 return nxifv1->read_digital_output(doutput);
00733         else
00734                 return FALSE;
00735 }


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39