iob.cpp
Go to the documentation of this file.
00001 #include <unistd.h>
00002 #include <iostream>
00003 #include <cstdlib>
00004 #include <cstring>
00005 #include <vector>
00006 #include "io/iob.h"
00007 
00008 #include <ros/ros.h>
00009 #include <boost/algorithm/string.hpp>
00010 
00011 #include <hrpsys_gazebo_msgs/JointCommand.h>
00012 #include <hrpsys_gazebo_msgs/RobotState.h>
00013 #include <hrpsys_gazebo_msgs/SyncCommand.h>
00014 
00015 #include <hrpUtil/Eigen3d.h>
00016 
00017 typedef hrpsys_gazebo_msgs::JointCommand JointCommand;
00018 typedef hrpsys_gazebo_msgs::RobotState RobotState;
00019 
00020 static ros::NodeHandle* rosnode;
00021 
00022 static ros::ServiceClient serv_command;
00023 static ros::Publisher pub_joint_command;
00024 static ros::Subscriber sub_robot_state;
00025 static bool iob_synchronized;
00026 static bool start_robothw = false;
00027 static bool use_velocity_feedback = false;
00028 
00029 static JointCommand jointcommand;
00030 static JointCommand initial_jointcommand;
00031 
00032 static RobotState js;
00033 static bool init_sub_flag = false;
00034 
00035 static std::vector<double> command;
00036 static std::vector<double> prev_command;
00037 static std::vector<std::vector<double> > forces;
00038 static std::vector<std::vector<double> > gyros;
00039 static std::vector<std::vector<double> > accelerometers;
00040 static std::vector<std::vector<double> > attitude_sensors;
00041 static std::vector<std::vector<double> > force_offset;
00042 static std::vector<std::vector<double> > gyro_offset;
00043 static std::vector<std::vector<double> > accel_offset;
00044 static std::vector<int> power;
00045 static std::vector<int> servo;
00046 static bool isLocked = false;
00047 static unsigned long long frame = 0;
00048 static timespec g_ts;
00049 static long g_period_ns=1000000;
00050 static long overwrite_g_period_ns = -1;
00051 static ros::Time rg_ts;
00052 static int num_of_substeps = 1;
00053 
00054 #define CHECK_JOINT_ID(id) if ((id) < 0 || (id) >= number_of_joints()) return E_ID
00055 #define CHECK_FORCE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_force_sensors()) return E_ID
00056 #define CHECK_ACCELEROMETER_ID(id) if ((id) < 0 || (id) >= number_of_accelerometers()) return E_ID
00057 #define CHECK_GYRO_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_gyro_sensors()) return E_ID
00058 #define CHECK_ATTITUDE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID
00059 
00060 #define JOINT_ID_REAL2MODEL(id) joint_real2model_vec[id]
00061 #define JOINT_ID_MODEL2REAL(id) joint_id_model2real(id)
00062 #define NUM_OF_REAL_JOINT (joint_real2model_vec.size())
00063 
00064 #define USE_SERVO_ON 0
00065 
00066 static std::map<int, int> joint_model2real_map;
00067 static std::vector<int>   joint_real2model_vec;
00068 
00069 static inline int joint_id_model2real(int id)
00070 {
00071   std::map< int, int>::iterator it = joint_model2real_map.find (id);
00072 
00073   if (it == joint_model2real_map.end()) {
00074     return -1;
00075   } else {
00076     return it->second;
00077   }
00078 }
00079 
00080 static inline void tick_service_command()
00081 {
00082   // tick gazebo ...
00083   hrpsys_gazebo_msgs::SyncCommandRequest req;
00084   // req.joint_command = jointcommand; // do not need, because just tick gazebo time
00085   hrpsys_gazebo_msgs::SyncCommandResponse res;
00086   // std::cerr << "[iob] tick srv c" << std::endl;
00087   serv_command.call(req, res);
00088   // std::cerr << "[iob] tick srv r" << std::endl;
00089   js = res.robot_state;
00090 }
00091 
00092 #ifdef __APPLE__
00093 typedef int clockid_t;
00094 #define CLOCK_MONOTONIC 0
00095 #include <mach/mach_time.h>
00096 int clock_gettime(clockid_t clk_id, struct timespec *tp)
00097 {
00098     if (clk_id != CLOCK_MONOTONIC) return -1;
00099 
00100     uint64_t clk;
00101     clk = mach_absolute_time();
00102 
00103     static mach_timebase_info_data_t info = {0,0};
00104     if (info.denom == 0) mach_timebase_info(&info);
00105 
00106     uint64_t elapsednano = clk * (info.numer / info.denom);
00107 
00108     tp->tv_sec = elapsednano * 1e-9;
00109     tp->tv_nsec = elapsednano - (tp->tv_sec * 1e9);
00110     return 0;
00111 }
00112 
00113 #define TIMER_ABSTIME 0
00114 int clock_nanosleep(clockid_t clk_id, int flags, struct timespec *tp,
00115     struct timespec *remain)
00116 {
00117     if (clk_id != CLOCK_MONOTONIC || flags != TIMER_ABSTIME) return -1;
00118 
00119     static mach_timebase_info_data_t info = {0,0};
00120     if (info.denom == 0) mach_timebase_info(&info);
00121 
00122     uint64_t clk = (tp->tv_sec*1e9 + tp->tv_nsec)/(info.numer/info.denom);
00123 
00124     mach_wait_until(clk);
00125     return 0;
00126 }
00127 #endif
00128 
00129 
00130 int number_of_joints()
00131 {
00132     return (int)command.size();
00133 }
00134 
00135 int number_of_force_sensors()
00136 {
00137     return (int)forces.size();
00138 }
00139 
00140 int number_of_gyro_sensors()
00141 {
00142     return (int)gyros.size();
00143 }
00144 
00145 int number_of_accelerometers()
00146 {
00147     return (int)accelerometers.size();
00148 }
00149 
00150 int number_of_attitude_sensors()
00151 {
00152     return (int)attitude_sensors.size();
00153 }
00154 
00155 int set_number_of_joints(int num)
00156 {
00157     std::cerr << ";; IOB / set num of joint = " << num << std::endl;
00158     command.resize(num);
00159     prev_command.resize(num);
00160     power.resize(num);
00161     servo.resize(num);
00162     for (int i=0; i<num; i++){
00163         command[i] = power[i] = servo[i] = prev_command[i] = 0;
00164     }
00165     return TRUE;
00166 }
00167 
00168 int set_number_of_force_sensors(int num)
00169 {
00170     std::cerr << ";; set_number_of_force_sensors = " << num << std::endl;
00171     forces.resize(num);
00172     force_offset.resize(num);
00173     for (unsigned int i=0; i<forces.size();i++){
00174         forces[i].resize(6);
00175         force_offset[i].resize(6);
00176         for (int j=0; j<6; j++){
00177             forces[i][j] = 0;
00178             force_offset[i][j] = 0;
00179         }
00180     }
00181     return TRUE;
00182 }
00183 
00184 int set_number_of_gyro_sensors(int num)
00185 {
00186     std::cerr << ";; set_number_of_gyro_sensors = " << num << std::endl;
00187     gyros.resize(num);
00188     gyro_offset.resize(num);
00189     for (unsigned int i=0; i<gyros.size();i++){
00190         gyros[i].resize(3);
00191         gyro_offset[i].resize(3);
00192         for (int j=0; j<3; j++){
00193             gyros[i][j] = 0.0;
00194             gyro_offset[i][j] = 0.0;
00195         }
00196     }
00197     return TRUE;
00198 }
00199 
00200 int set_number_of_accelerometers(int num)
00201 {
00202   std::cerr << ";; set_number_of_number_of_accelerometers = " << num << std::endl;
00203     accelerometers.resize(num);
00204     accel_offset.resize(num);
00205     for (unsigned int i=0; i<accelerometers.size();i++){
00206         accelerometers[i].resize(3);
00207         accel_offset[i].resize(3);
00208         for (int j=0; j<3; j++){
00209             accelerometers[i][j] = j == 2 ? 9.81 : 0.0; // z direction should be virtical???
00210             accel_offset[i][j] = 0;
00211         }
00212     }
00213     return TRUE;
00214 }
00215 
00216 int set_number_of_attitude_sensors(int num)
00217 {
00218     attitude_sensors.resize(num);
00219     for (unsigned int i=0; i<attitude_sensors.size();i++){
00220         attitude_sensors[i].resize(3);
00221         for (int j=0; j<3; j++){
00222             attitude_sensors[i][j] = 0.0;
00223         }
00224     }
00225     return TRUE;
00226 }
00227 
00228 int read_power_state(int id, int *s)
00229 {
00230     CHECK_JOINT_ID(id);
00231     *s = power[id];
00232     return TRUE;
00233 }
00234 
00235 int write_power_command(int id, int com)
00236 {
00237     CHECK_JOINT_ID(id);
00238     power[id] = com;
00239     return TRUE;
00240 }
00241 
00242 int read_power_command(int id, int *com)
00243 {
00244     CHECK_JOINT_ID(id);
00245     *com = power[id];
00246     return TRUE;
00247 }
00248 
00249 int read_servo_state(int id, int *s)
00250 {
00251     CHECK_JOINT_ID(id);
00252     *s = servo[id];
00253     return TRUE;
00254 }
00255 
00256 int read_servo_alarm(int id, int *a)
00257 {
00258     CHECK_JOINT_ID(id);
00259     *a = 0;
00260     return TRUE;
00261 }
00262 
00263 int read_control_mode(int id, joint_control_mode *s)
00264 {
00265     CHECK_JOINT_ID(id);
00266     *s = JCM_POSITION;
00267     return TRUE;
00268 }
00269 
00270 int write_control_mode(int id, joint_control_mode s)
00271 {
00272     CHECK_JOINT_ID(id);
00273     return TRUE;
00274 }
00275 
00276 int read_actual_angle(int id, double *angle)
00277 {
00278   CHECK_JOINT_ID(id);
00279 
00280   if(init_sub_flag){
00281     if (JOINT_ID_MODEL2REAL(id) < 0) {
00282       *angle = command[id];
00283     }else{
00284       *angle = js.position[JOINT_ID_MODEL2REAL(id)];
00285     }
00286   }else{
00287     *angle = command[id];
00288   }
00289   return TRUE;
00290 }
00291 
00292 int read_actual_angles(double *angles)
00293 {
00294     for (int i=0; i<number_of_joints(); i++){
00295         read_actual_angle(i, &angles[i]);
00296     }
00297     return TRUE;
00298 }
00299 
00300 int read_actual_torques(double *torques)
00301 {
00302   if(init_sub_flag) {
00303     for(int i=0; i<number_of_joints(); i++){
00304       if(JOINT_ID_MODEL2REAL(i) < 0) {
00305         *(torques+i) = -1;
00306       }else{
00307         *(torques+i) = js.effort[JOINT_ID_MODEL2REAL(i)];
00308       }
00309     }
00310   }
00311 
00312   return TRUE;
00313 }
00314 
00315 int read_command_torque(int id, double *torque)
00316 {
00317     return FALSE;
00318 }
00319 
00320 int write_command_torque(int id, double torque)
00321 {
00322     return FALSE;
00323 }
00324 
00325 int read_command_torques(double *torques)
00326 {
00327     return FALSE;
00328 }
00329 
00330 int write_command_torques(const double *torques)
00331 {
00332     return FALSE;
00333 }
00334 
00335 int read_command_angle(int id, double *angle)
00336 {
00337     CHECK_JOINT_ID(id);
00338     *angle = command[id];
00339     return TRUE;
00340 }
00341 
00342 int write_command_angle(int id, double angle)
00343 {
00344     CHECK_JOINT_ID(id);
00345     command[id] = angle;
00346     return TRUE;
00347 }
00348 
00349 int read_command_angles(double *angles)
00350 {
00351     for (int i=0; i<number_of_joints(); i++){
00352         angles[i] = command[i];
00353     }
00354     return TRUE;
00355 }
00356 
00357 int write_command_angles(const double *angles)
00358 {
00359     //std::cerr << "[iob] write command[" << frame << "]" << std::endl;
00360     for (int i=0; i<number_of_joints(); i++){
00361       prev_command[i] = command[i];
00362       command[i] = angles[i];
00363     }
00364 
00365     JointCommand send_com(jointcommand);
00366     bool servo_on = true;
00367 
00368     send_com.header.stamp = ros::Time::now();
00369 
00370     for (int i=0; i<NUM_OF_REAL_JOINT; i++) {
00371 #if USE_SERVO_ON
00372       if (servo[JOINT_ID_REAL2MODEL(i)] > 0) {
00373         send_com.position[i] = command[JOINT_ID_REAL2MODEL(i)];
00374         send_com.velocity[i] = (command[JOINT_ID_REAL2MODEL(i)] - prev_command[JOINT_ID_REAL2MODEL(i)]) / (overwrite_g_period_ns * 1e-9);
00375       } else {
00376         servo_on = false;
00377       }
00378 #else
00379       send_com.position[i] = command[JOINT_ID_REAL2MODEL(i)];
00380       send_com.velocity[i] = (command[JOINT_ID_REAL2MODEL(i)] - prev_command[JOINT_ID_REAL2MODEL(i)]) / (overwrite_g_period_ns * 1e-9);
00381 #endif
00382     }
00383 
00384     if (iob_synchronized) {
00385       hrpsys_gazebo_msgs::SyncCommandRequest req;
00386       if (servo_on) {
00387         req.joint_command = send_com;
00388       }
00389       hrpsys_gazebo_msgs::SyncCommandResponse res;
00390       // std::cerr << "[iob] srv c" << std::endl;
00391       serv_command.call(req, res);
00392       // std::cerr << "[iob] srv r" << std::endl;
00393       js = res.robot_state;
00394       init_sub_flag = true;
00395     } else {
00396       if(servo_on) {
00397         pub_joint_command.publish(send_com);
00398       }
00399       ros::spinOnce();
00400     }
00401     if (!start_robothw) {
00402       frame = 0;
00403       start_robothw = true;
00404     }
00405 
00406     return TRUE;
00407 }
00408 
00409 int read_pgain(int id, double *gain)
00410 {
00411   if(JOINT_ID_MODEL2REAL(id) < 0) {
00412     std::cerr << ";;; read pgain: " << id << " failed." << std::endl;
00413   }else{
00414     int iid = JOINT_ID_MODEL2REAL(id);
00415     if (!use_velocity_feedback) {
00416       *(gain) = jointcommand.kp_position[iid] / initial_jointcommand.kp_position[iid];
00417       //std::cerr << ";;; read gain: " << id << " = " << *gain << std::endl;
00418     } else {
00419       if (initial_jointcommand.kpv_position[iid] <= 0) {
00420         *(gain) = 1.0;
00421       } else {
00422         *(gain) = jointcommand.kpv_position[iid] / initial_jointcommand.kpv_position[iid];
00423       }
00424     }
00425   }
00426   return TRUE;
00427 }
00428 
00429 int write_pgain(int id, double gain)
00430 {
00431 
00432   if(JOINT_ID_MODEL2REAL(id) < 0) {
00433     std::cerr << ";;; write pgain: " << id << " failed." << std::endl;
00434   }else{
00435     int iid = JOINT_ID_MODEL2REAL(id);
00436     if(!use_velocity_feedback) {
00437       jointcommand.kp_position[iid] = gain * initial_jointcommand.kp_position[iid];
00438       //std::cerr << ";;; write pgain: " << id << " = " << gain << std::endl;
00439     } else {
00440       jointcommand.kpv_position[iid] = gain * initial_jointcommand.kpv_position[iid];
00441     }
00442   }
00443   return TRUE;
00444 }
00445 
00446 int read_dgain(int id, double *gain)
00447 {
00448   if(JOINT_ID_MODEL2REAL(id) < 0) {
00449     //
00450   }else{
00451     int iid = JOINT_ID_MODEL2REAL(id);
00452     if (!use_velocity_feedback) {
00453       *(gain) = jointcommand.kd_position[iid] / initial_jointcommand.kd_position[iid];
00454       //std::cerr << ";;; read dgain: " << id << " = " << *gain << std::endl;
00455     } else {
00456       if(initial_jointcommand.kpv_velocity[iid] <= 0) {
00457         *(gain) = 1.0;
00458       } else {
00459         *(gain) = jointcommand.kpv_velocity[iid] / initial_jointcommand.kpv_velocity[iid];
00460       }
00461     }
00462   }
00463   return TRUE;
00464 }
00465 
00466 int write_dgain(int id, double gain)
00467 {
00468   if(JOINT_ID_MODEL2REAL(id) < 0) {
00469     //
00470   }else{
00471     int iid = JOINT_ID_MODEL2REAL(id);
00472     if (!use_velocity_feedback) {
00473       jointcommand.kd_position[iid] =
00474         gain * initial_jointcommand.kd_position[iid];
00475       //std::cerr << ";;; write dgain: " << id << " = " << gain << std::endl;
00476     } else {
00477       jointcommand.kpv_velocity[iid] =
00478         gain * initial_jointcommand.kpv_velocity[iid];
00479     }
00480   }
00481   return TRUE;
00482 }
00483 
00484 int read_force_sensor(int id, double *forces)
00485 {
00486   CHECK_FORCE_SENSOR_ID(id);
00487 
00488   if (id >= js.sensors.size()) {
00489     return E_ID;
00490   }
00491   if (init_sub_flag) {
00492     forces[0] = js.sensors[id].force.x + force_offset[id][0];
00493     forces[1] = js.sensors[id].force.y + force_offset[id][1];
00494     forces[2] = js.sensors[id].force.z + force_offset[id][2];
00495     forces[3] = js.sensors[id].torque.x + force_offset[id][3];
00496     forces[4] = js.sensors[id].torque.y + force_offset[id][4];
00497     forces[5] = js.sensors[id].torque.z + force_offset[id][5];
00498   } else {
00499     forces[0] = forces[1] = forces[2] = forces[3] = forces[4] = forces[5] = 0.0;
00500   }
00501   return TRUE;
00502 }
00503 
00504 int read_gyro_sensor(int id, double *rates)
00505 {
00506   CHECK_GYRO_SENSOR_ID(id);
00507   if(init_sub_flag){
00508     if (id >= js.Imus.size()) {
00509       return E_ID;
00510     }
00511 #if 0
00512     Eigen::Quaternion<double> q(js.Imus[id].orientation.w,
00513                                 js.Imus[id].orientation.x,
00514                                 js.Imus[id].orientation.y,
00515                                 js.Imus[id].orientation.z);
00516     hrp::Vector3 rpy = hrp::rpyFromRot(q.toRotationMatrix());
00517     rates[0] = rpy[0];
00518     rates[1] = rpy[1];
00519     rates[2] = rpy[2];
00520 #endif
00521     rates[0] = js.Imus[id].angular_velocity.x + gyro_offset[id][0];
00522     rates[1] = js.Imus[id].angular_velocity.y + gyro_offset[id][1];
00523     rates[2] = js.Imus[id].angular_velocity.z + gyro_offset[id][2];
00524   } else {
00525     // tempolary values when sensor is not ready.
00526     rates[0] = rates[1] = rates[2] = 0.0;
00527   }
00528   //fprintf(stderr, "rates[%ld]: %f %f %f\n", frame, rates[0], rates[1], rates[2]);
00529   return TRUE;
00530 }
00531 
00532 int read_accelerometer(int id, double *accels)
00533 {
00534   CHECK_ACCELEROMETER_ID(id);
00535 
00536   if(init_sub_flag){
00537     if (id >= js.Imus.size()) {
00538       return E_ID;
00539     }
00540     accels[0] = js.Imus[id].linear_acceleration.x + accel_offset[id][0];
00541     accels[1] = js.Imus[id].linear_acceleration.y + accel_offset[id][1];
00542     accels[2] = js.Imus[id].linear_acceleration.z + accel_offset[id][2];
00543   } else {
00544     // tempolary values when sensor is not ready.
00545     //accels[0] = accels[1] = accels[2] = 0.0;
00546     accels[0] = accels[1] = 0.0;
00547     accels[2] = -9.8;
00548   }
00549   return TRUE;
00550 }
00551 
00552 int read_touch_sensors(unsigned short *onoff)
00553 {
00554     return FALSE;
00555 }
00556 
00557 int read_attitude_sensor(int id, double *att)
00558 {
00559   return FALSE;
00560 }
00561 
00562 int read_current(int id, double *mcurrent)
00563 {
00564     return FALSE;
00565 }
00566 
00567 int read_current_limit(int id, double *v)
00568 {
00569     return FALSE;
00570 }
00571 
00572 int read_currents(double *currents)
00573 {
00574     return FALSE;
00575 }
00576 
00577 int read_gauges(double *gauges)
00578 {
00579     return FALSE;
00580 }
00581 
00582 int read_actual_velocity(int id, double *vel)
00583 {
00584     // TODO: impliment here
00585     return FALSE;
00586 }
00587 
00588 int read_command_velocity(int id, double *vel)
00589 {
00590     // TODO: impliment here
00591     return FALSE;
00592 }
00593 
00594 int write_command_velocity(int id, double vel)
00595 {
00596     // TODO: impliment here
00597     return FALSE;
00598 }
00599 
00600 int read_actual_velocities(double *vels)
00601 {
00602     // TODO: impliment here
00603     return FALSE;
00604 }
00605 
00606 int read_command_velocities(double *vels)
00607 {
00608     // TODO: impliment here
00609     return FALSE;
00610 }
00611 
00612 int write_command_velocities(const double *vels)
00613 {
00614     // TODO: impliment here
00615     return FALSE;
00616 }
00617 
00618 int read_temperature(int id, double *v)
00619 {
00620     return FALSE;
00621 }
00622 
00623 int write_servo(int id, int com)
00624 {
00625   std::cerr << "servo: id: " << id;
00626   std::cerr << "(" << servo.size() << ")";
00627   std::cerr << " - " << com << std::endl;
00628     servo[id] = com;
00629     return TRUE;
00630 }
00631 
00632 int write_dio(unsigned short buf)
00633 {
00634     return FALSE;
00635 }
00636 
00637 // callback
00638 static void setJointStates(const RobotState::ConstPtr &_js) {
00639   ROS_DEBUG("[iob] subscribe RobotState");
00640   js = *_js;
00641   init_sub_flag = true;
00642   //if (iob_synchronized) {
00643   //ROS_WARN("iob subscribed wrong topic ...");
00644   //}
00645 }
00646 
00647 int open_iob(void)
00648 {
00649     static bool isInitialized = false;
00650     if ( isInitialized ) return TRUE;
00651     isInitialized = true;
00652 
00653     std::cerr << "[iob] Open IOB / start " << std::endl;
00654 
00655     std::string node_name;
00656     {
00657       char *ret = getenv("HRPSYS_GAZEBO_IOB_NAME");
00658       if (ret != NULL) {
00659         node_name.assign(ret);
00660       } else {
00661         node_name = "hrpsys_gazebo_iob";
00662       }
00663       std::cerr << "[iob] set node name : " << node_name << std::endl;
00664     }
00665 
00666     std::map<std::string, std::string> arg;
00667     ros::init(arg, "hrpsys_gazebo_iob", ros::init_options::NoSigintHandler);
00668     rosnode = new ros::NodeHandle();
00669     ros::WallDuration(0.5).sleep(); // wait for initializing ros
00670 
00671     std::string controller_name;
00672     { // setting configuration name
00673       char *ret = getenv("HRPSYS_GAZEBO_CONFIGURATION");
00674       if (ret != NULL) {
00675         controller_name.assign(ret);
00676       } else {
00677         controller_name = "hrpsys_gazebo_configuration";
00678       }
00679       ROS_INFO_STREAM( "[iob] set controller_name : " << controller_name);
00680     }
00681     std::string robotname;
00682     { // setting configuration name
00683       char *ret = getenv("HRPSYS_GAZEBO_ROBOTNAME");
00684       if (ret != NULL) {
00685         robotname.assign(ret);
00686         // controller_name -> robotname/controller_name
00687         controller_name = robotname + "/" + controller_name;
00688       } else {
00689         std::string rname_str = std::string(controller_name) + "/robotname";
00690         rosnode->getParam(rname_str, robotname);
00691       }
00692       if (robotname.empty()) {
00693         ROS_ERROR("[iob] did not find robot_name");
00694         robotname = "default";
00695       }
00696       ROS_INFO_STREAM( "[iob] set robot_name : " << robotname);
00697     }
00698 
00699     { // setting synchronized
00700       char *ret = getenv("HRPSYS_GAZEBO_IOB_SYNCHRONIZED");
00701       if (ret != NULL) {
00702         std::string ret_str(ret);
00703         if (ret_str.size() > 0) {
00704           iob_synchronized = true;
00705         }
00706       } else {
00707         iob_synchronized = false;
00708       }
00709       if(rosnode->hasParam(controller_name + "/use_synchronized_command")) {
00710         rosnode->getParam(controller_name + "/use_synchronized_command", iob_synchronized);
00711       }
00712       if(iob_synchronized) ROS_INFO("[iob] use synchronized command");
00713     }
00714     { // setting substeps
00715       char *ret = getenv("HRPSYS_GAZEBO_IOB_SUBSTEPS");
00716       if (ret != NULL) {
00717         int num = atoi(ret);
00718         if (num > 0) {
00719           num_of_substeps = num;
00720           ROS_INFO("[iob] use substeps %d", num);
00721         }
00722       }
00723       if(rosnode->hasParam(controller_name + "/iob_substeps")) {
00724         rosnode->getParam(controller_name + "/iob_substeps", num_of_substeps);
00725         ROS_INFO("[iob] use substeps %d", num_of_substeps);
00726       }
00727     }
00728     { // settting rate
00729       if(rosnode->hasParam(controller_name + "/iob_rate")) {
00730         double rate = 0;
00731         rosnode->getParam(controller_name + "/iob_rate", rate);
00732         overwrite_g_period_ns = (long) ((1000 * 1000 * 1000) / rate);
00733         fprintf(stderr, "iob::period %d\n", overwrite_g_period_ns);
00734         ROS_INFO("[iob] period_ns %d", overwrite_g_period_ns);
00735       }
00736     }
00737 
00738     joint_real2model_vec.resize(0);
00739 
00740     if (rosnode->hasParam(controller_name + "/joint_id_list")) {
00741       XmlRpc::XmlRpcValue param_val;
00742       rosnode->getParam(controller_name + "/joint_id_list", param_val);
00743       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00744         for(int i = 0; i < param_val.size(); i++) {
00745           int num = param_val[i];
00746           joint_real2model_vec.push_back(num);
00747         }
00748       } else {
00749         ROS_WARN("[iob] %s/joint_id_list is not list of integer", controller_name.c_str());
00750       }
00751     } else {
00752       ROS_DEBUG("[iob] %s/joint_id_list is nothing", controller_name.c_str());
00753     }
00754     if (rosnode->hasParam(controller_name + "/use_velocity_feedback")) {
00755         bool ret;
00756         rosnode->getParam(controller_name + "/use_velocity_feedback", ret);
00757         use_velocity_feedback = ret;
00758         if(ret) {
00759           ROS_INFO("[iob] use_velocity_feedback");
00760         }
00761     }
00762     XmlRpc::XmlRpcValue param_val;
00763     std::vector<std::string> joint_lst;
00764     rosnode->getParam(controller_name + "/joints", param_val);
00765     if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00766       for(unsigned int s = 0; s < param_val.size(); s++) {
00767           std::string nstr = param_val[s];
00768           ROS_DEBUG("add joint: %s", nstr.c_str());
00769           joint_lst.push_back(nstr);
00770         }
00771     } else {
00772       ROS_ERROR("[iob] %s/joints is not list of joint name", controller_name.c_str());
00773     }
00774 
00775     if (joint_real2model_vec.size() == 0) {
00776       for(unsigned int i = 0; i < joint_lst.size(); i++) {
00777         joint_real2model_vec.push_back(i);
00778       }
00779     } else if (joint_real2model_vec.size() != joint_lst.size()) {
00780       ROS_ERROR("[iob] size differece on joint_id_list and joints (%ld,  %ld)",
00781                 joint_real2model_vec.size(), joint_lst.size());
00782     }
00783 
00784     for(unsigned int i = 0; i < joint_real2model_vec.size(); i++) {
00785       joint_model2real_map[joint_real2model_vec[i]] = i;
00786     }
00787 
00788     unsigned int n = NUM_OF_REAL_JOINT;
00789 
00790     initial_jointcommand.position.resize(n);
00791     initial_jointcommand.velocity.resize(n);
00792     //initial_jointcommand.effort.resize(n);
00793     initial_jointcommand.effort.resize(0);
00794 
00795     if(!use_velocity_feedback) {
00796       initial_jointcommand.kp_position.resize(n);
00797       initial_jointcommand.ki_position.resize(n);
00798       initial_jointcommand.kd_position.resize(n);
00799       initial_jointcommand.kp_velocity.resize(n);
00800       initial_jointcommand.i_effort_min.resize(n);
00801       initial_jointcommand.i_effort_max.resize(n);
00802     } else {
00803       initial_jointcommand.kpv_position.resize(n);
00804       initial_jointcommand.kpv_velocity.resize(n);
00805     }
00806 
00807     for (unsigned int i = 0; i < joint_lst.size(); ++i) {
00808       std::string joint_ns(controller_name);
00809       joint_ns += ("/gains/" + joint_lst[i] + "/");
00810 
00811       if (!use_velocity_feedback) {
00812         double p_val = 0, i_val = 0, d_val = 0, i_clamp_val = 0, vp_val = 0;
00813         std::string p_str = std::string(joint_ns)+"p";
00814         std::string i_str = std::string(joint_ns)+"i";
00815         std::string d_str = std::string(joint_ns)+"d";
00816         std::string i_clamp_str = std::string(joint_ns)+"i_clamp";
00817         std::string vp_str = std::string(joint_ns)+"vp";
00818         if (!rosnode->getParam(p_str, p_val)) {
00819           ROS_WARN("[iob] couldn't find a P param for %s", joint_ns.c_str());
00820         }
00821         if (!rosnode->getParam(i_str, i_val)) {
00822           ROS_WARN("[iob] couldn't find a I param for %s", joint_ns.c_str());
00823         }
00824         if (!rosnode->getParam(d_str, d_val)) {
00825           ROS_WARN("[iob] couldn't find a D param for %s", joint_ns.c_str());
00826         }
00827         if (!rosnode->getParam(i_clamp_str, i_clamp_val)) {
00828           ROS_WARN("[iob] couldn't find a I_CLAMP param for %s", joint_ns.c_str());
00829         }
00830         if (!rosnode->getParam(vp_str, vp_val)) {
00831           ROS_WARN("[iob] couldn't find a VP param for %s", joint_ns.c_str());
00832         }
00833         // store these directly on altasState, more efficient for pub later
00834         initial_jointcommand.kp_position[i] = p_val;
00835         initial_jointcommand.ki_position[i] = i_val;
00836         initial_jointcommand.kd_position[i] = d_val;
00837         initial_jointcommand.i_effort_min[i] = -i_clamp_val;
00838         initial_jointcommand.i_effort_max[i] = i_clamp_val;
00839         initial_jointcommand.velocity[i]     = 0;
00840         //initial_jointcommand.effort[i]       = 0;
00841         initial_jointcommand.kp_velocity[i]  = vp_val;
00842       } else {
00843         // velocity feedback
00844         double p_v_val = 0, vp_v_val = 0;
00845         std::string p_v_str  = std::string(joint_ns) + "p_v";
00846         std::string vp_v_str = std::string(joint_ns) + "vp_v";
00847         if (!rosnode->getParam(p_v_str, p_v_val)) {
00848           ROS_WARN("[iob] couldn't find a P_V param for %s", joint_ns.c_str());
00849         }
00850         if (!rosnode->getParam(vp_v_str, vp_v_val)) {
00851           ROS_WARN("[iob] couldn't find a VP_V param for %s", joint_ns.c_str());
00852         }
00853         initial_jointcommand.kpv_position[i] = p_v_val;
00854         initial_jointcommand.kpv_velocity[i] = vp_v_val;
00855       }
00856     }
00857 
00858     initial_jointcommand.desired_controller_period_ms =
00859       static_cast<unsigned int>(overwrite_g_period_ns * 1e-6);
00860 
00861     if (iob_synchronized) {
00862       serv_command =
00863         ros::service::createClient<hrpsys_gazebo_msgs::SyncCommand> (robotname + "/iob_command", true); // persistent = true,
00864       ROS_INFO_STREAM("[iob] waiting service " << robotname << "/iob_command");
00865       serv_command.waitForExistence();
00866       ROS_INFO_STREAM("[iob] found service " <<  robotname << "/iob_command");
00867     } else {
00868       pub_joint_command = rosnode->advertise <JointCommand> (robotname + "/joint_command", 1, true);
00869 
00870       // ros topic subscribtions
00871       ros::SubscribeOptions jointStatesSo =
00872         ros::SubscribeOptions::create<RobotState>(robotname + "/robot_state", 1, setJointStates,
00873                                                   ros::VoidPtr(), rosnode->getCallbackQueue());
00874       // Because TCP causes bursty communication with high jitter,
00875       // declare a preference on UDP connections for receiving
00876       // joint states, which we want to get at a high rate.
00877       // Note that we'll still accept TCP connections for this topic
00878       // (e.g., from rospy nodes, which don't support UDP);
00879       // we just prefer UDP.
00880       // temporary use TCP / Using UDP occured some problem when message size more than 1500.
00881       //jointStatesSo.transport_hints =
00882       //ros::TransportHints().maxDatagramSize(3000).unreliable().reliable().tcpNoDelay(true);
00883       jointStatesSo.transport_hints =
00884         ros::TransportHints().reliable().tcpNoDelay(true);
00885       sub_robot_state = rosnode->subscribe(jointStatesSo);
00886     }
00887 
00888     for (int i=0; i < number_of_joints(); i++){
00889         command[i] = 0.0;
00890         power[i] = OFF;
00891         servo[i] = OFF;
00892     }
00893     clock_gettime(CLOCK_MONOTONIC, &g_ts);
00894     rg_ts = ros::Time::now();
00895 
00896     jointcommand = initial_jointcommand;
00897     std::cerr << "[iob]  " << number_of_joints() << " / " << initial_jointcommand.position.size() << " / " << NUM_OF_REAL_JOINT << std::endl;
00898 
00899     if (iob_synchronized) {
00900       hrpsys_gazebo_msgs::SyncCommandRequest req;
00901       req.joint_command = jointcommand;
00902       hrpsys_gazebo_msgs::SyncCommandResponse res;
00903       std::cerr << "[iob] first service call" << std::endl;
00904       serv_command.call(req, res);
00905       std::cerr << "[iob] first service returned" << std::endl;
00906       js = res.robot_state;
00907       init_sub_flag = true;
00908     } else {
00909       std::cerr << "[iob] block until subscribing first robot_state";
00910       ros::Time start_tm = ros::Time::now();
00911       ros::Rate rr(100);
00912       ros::spinOnce();
00913       while (!init_sub_flag) {
00914         if ((ros::Time::now() - start_tm).toSec() > 5.0) {
00915           std::cerr << "[iob] timeout for waiting robot_state";
00916           break;
00917         }
00918         std::cerr << ".";
00919         rr.sleep();
00920         ros::spinOnce();
00921       }
00922     }
00923 
00924     std::cerr << std::endl << "[iob] Open IOB / finish " << std::endl;
00925 
00926     return TRUE;
00927 }
00928 
00929 int close_iob(void)
00930 {
00931     std::cerr << "[iob] IOB is closed" << std::endl;
00932     return TRUE;
00933 }
00934 
00935 int reset_body(void)
00936 {
00937     for (int i=0; i<number_of_joints(); i++){
00938         power[i] = servo[i] = OFF;
00939     }
00940     return TRUE;
00941 }
00942 
00943 int joint_calibration(int id, double angle)
00944 {
00945     return FALSE;
00946 }
00947 
00948 int read_gyro_sensor_offset(int id, double *offset)
00949 {
00950     for (int i=0; i<3; i++){
00951         offset[i] = gyro_offset[id][i];
00952     }
00953     return TRUE;
00954 }
00955 
00956 int write_gyro_sensor_offset(int id, double *offset)
00957 {
00958     for (int i=0; i<3; i++){
00959         gyro_offset[id][i] = offset[i];
00960     }
00961     return TRUE;
00962 }
00963 
00964 int read_accelerometer_offset(int id, double *offset)
00965 {
00966     for (int i=0; i<3; i++){
00967         offset[i] = accel_offset[id][i];
00968     }
00969     return TRUE;
00970 }
00971 
00972 int write_accelerometer_offset(int id, double *offset)
00973 {
00974     for (int i=0; i<3; i++){
00975         accel_offset[id][i] = offset[i];
00976     }
00977     return TRUE;
00978 }
00979 
00980 int read_force_offset(int id, double *offsets)
00981 {
00982     for (int i=0; i<6; i++){
00983         offsets[i] = force_offset[id][i];
00984     }
00985     return TRUE;
00986 }
00987 
00988 int write_force_offset(int id, double *offsets)
00989 {
00990     for (int i=0; i<6; i++){
00991         force_offset[id][i] = offsets[i];
00992     }
00993     return TRUE;
00994 }
00995 
00996 int write_attitude_sensor_offset(int id, double *offset)
00997 {
00998     return FALSE;
00999 }
01000 
01001 int read_calib_state(int id, int *s)
01002 {
01003     CHECK_JOINT_ID(id);
01004     int v = id/2;
01005     *s = v%2==0 ? ON : OFF;
01006     return TRUE;
01007 }
01008 
01009 int lock_iob()
01010 {
01011     if (isLocked) return FALSE;
01012 
01013     //isLocked = true;
01014     return TRUE;
01015 }
01016 int unlock_iob()
01017 {
01018     isLocked = false;
01019     return TRUE;
01020 }
01021 
01022 int read_lock_owner(pid_t *pid)
01023 {
01024   return FALSE;
01025 }
01026 
01027 int read_limit_angle(int id, double *angle)
01028 {
01029   return FALSE;
01030 }
01031 
01032 int read_angle_offset(int id, double *angle)
01033 {
01034   return FALSE;
01035 }
01036 
01037 int write_angle_offset(int id, double angle)
01038 {
01039   return FALSE;
01040 }
01041 
01042 int read_ulimit_angle(int id, double *angle)
01043 {
01044   return FALSE;
01045 }
01046 int read_llimit_angle(int id, double *angle)
01047 {
01048   return FALSE;
01049 }
01050 int read_encoder_pulse(int id, double *ec)
01051 {
01052   return FALSE;
01053 }
01054 int read_gear_ratio(int id, double *gr)
01055 {
01056   return FALSE;
01057 }
01058 int read_torque_const(int id, double *tc)
01059 {
01060   return FALSE;
01061 }
01062 int read_torque_limit(int id, double *limit)
01063 {
01064   return FALSE;
01065 }
01066 
01067 unsigned long long read_iob_frame()
01068 {
01069     ++frame;
01070     if (iob_synchronized && start_robothw) {
01071       if(frame % num_of_substeps != 0) {
01072         tick_service_command();
01073       } // else break to executing RobotHardware
01074     }
01075     return frame;
01076 }
01077 
01078 int number_of_substeps()
01079 {
01080   if (iob_synchronized) {
01081     return num_of_substeps;
01082   } else {
01083     return 1;
01084   }
01085 }
01086 
01087 int read_power(double *voltage, double *current)
01088 {
01089     *voltage = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*1+48;
01090     *current = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+1;
01091     return TRUE;
01092 }
01093 
01094 int read_driver_temperature(int id, unsigned char *v)
01095 {
01096     *v = id * 2;
01097     return TRUE;
01098 }
01099 
01100 void timespec_add_ns(timespec *ts, long ns)
01101 {
01102     ts->tv_nsec += ns;
01103     while (ts->tv_nsec > 1e9){
01104         ts->tv_sec += 1;
01105         ts->tv_nsec -= 1e9;
01106     }
01107 }
01108 
01109 double timespec_compare(timespec *ts1, timespec *ts2)
01110 {
01111     double dts = ts1->tv_sec - ts2->tv_sec;
01112     double dtn = ts1->tv_nsec - ts2->tv_nsec;
01113     return dts*1e9+dtn;
01114 }
01115 
01116 int wait_for_iob_signal()
01117 {
01118   if (iob_synchronized) {
01119     //std::cerr << "wait>" << std::endl;
01120     if (start_robothw) {
01121       // no wait
01122     }
01123     //std::cerr << "wait<" << std::endl;
01124     return 0;
01125   } else {
01126     //
01127     ros::Time rnow;
01128     ros::Duration tm = ros::Duration(0, overwrite_g_period_ns);
01129     ros::WallDuration wtm = ros::WallDuration(0, 100000); // 0.1 ms
01130     while ((rnow = ros::Time::now()) < rg_ts) {
01131       wtm.sleep();
01132     }
01133 
01134     if ((rg_ts - rnow).toSec() < 0) {
01135       if((rg_ts + tm).toSec() - rnow.toSec() < 0) {
01136         fprintf(stderr, "iob::critical overrun (%f[ms]), w:%f -> %f\n",
01137                 (rnow - rg_ts).toSec()*1000, rnow.toSec(), rg_ts.toSec());
01138       }
01139       do {
01140         rg_ts += tm;
01141       } while ((rg_ts - rnow).toSec() <= 0);
01142     } else {
01143       rg_ts += tm;
01144     }
01145     // fprintf(stderr, "iob:: %f\n", rg_ts.toSec()); // debug
01146 
01147     return 0;
01148   }
01149 
01150   return 0;
01151 }
01152 
01153 size_t length_of_extra_servo_state(int id)
01154 {
01155     return 0;
01156 }
01157 
01158 int read_extra_servo_state(int id, int *state)
01159 {
01160     return TRUE;
01161 }
01162 
01163 int set_signal_period(long period_ns)
01164 {
01165     g_period_ns = period_ns;
01166     if(overwrite_g_period_ns < 0) {
01167       overwrite_g_period_ns = g_period_ns;
01168     }
01169     return TRUE;
01170 }
01171 
01172 long get_signal_period()
01173 {
01174     return g_period_ns;
01175 }
01176 
01177 int initializeJointAngle(const char *name, const char *option)
01178 {
01179     sleep(3);
01180     return TRUE;
01181 }
01182 
01183 int read_digital_input(char *dinput)
01184 {
01185   return 0;
01186 }
01187 
01188 int write_digital_output(const char *doutput)
01189 {
01190   return 0;
01191 }
01192 
01193 int length_digital_input(void)
01194 {
01195   return 0;
01196 }
01197 
01198 int write_digital_output_with_mask(const char *doutput, const char *mask)
01199 {
01200   return FALSE;
01201 }
01202 
01203 int length_digital_output(void)
01204 {
01205   return 0;
01206 }
01207 
01208 int read_digital_output(char *doutput)
01209 {
01210   return 0;
01211 }
01212 


hrpsys_gazebo_general
Author(s): Yohei Kakiuchi , Kei Okada , Masaki Murooka
autogenerated on Thu Jun 6 2019 20:56:53