fix iob for downstream packages

.travis.sh

@@ -84,7 +84,7 @@ case $TEST_PACKAGE in
             iob)
                 travis_time_start  install_wget
 
-                sudo apt-get install -qq -y cproto wget
+                sudo apt-get install -qq -y cproto wget diffstat
 
                 travis_time_end
                 travis_time_start  iob_test
@@ -94,7 +94,7 @@ case $TEST_PACKAGE in
                 echo -e "#define pid_t int\n#define size_t int\n#include \"iob.h.315.1.9\"" | cproto -x - | sort > iob.h.stable
                 cat iob.h.current
                 cat iob.h.stable
-                diff iob.h.current iob.h.stable || exit 1
+                diff iob.h.stable iob.h.current | tee >(cat - 1>&2)  | diffstat | grep -c deletion && exit 1
 
                 travis_time_end
                 ;;
@@ -161,6 +161,9 @@ case $TEST_PACKAGE in
         sudo apt-get install -qq -y freeglut3-dev python-tk jython doxygen libboost-all-dev libsdl1.2-dev libglew1.6-dev libqhull-dev libirrlicht-dev libxmu-dev libcv-dev libhighgui-dev libopencv-contrib-dev
         # check rtmros_common
 
+        if [ "$TEST_PACKAGE" == "hrpsys-base" ]; then
+            TEST_PACKAGE="hrpsys"
+        fi
         travis_time_end
         travis_time_start  install_$TEST_PACKAGE
 
@@ -232,6 +235,12 @@ case $TEST_PACKAGE in
             # do not copile hrpsys because we wan to use them
             sed -i "1imacro(dummy_install)\nmessage(\"install(\${ARGN})\")\nendmacro()" src/hrpsys/CMakeLists.txt
             sed -i "s@install(@dummy_install(@g" src/hrpsys/CMakeLists.txt
+            sed -i "\$iinstall(DIRECTORY test launch sample DESTINATION share/hrpsys USE_SOURCE_PERMISSIONS)" src/hrpsys/CMakeLists.txt
+            sed -i "\$iinstall(FILES package.xml DESTINATION share/hrpsys/)" src/hrpsys/CMakeLists.txt
+            sed -i "\$iinstall(CODE \"execute_process(COMMAND cmake -E make_directory share/hrpsys WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/share/hrpsys)\")" src/hrpsys/CMakeLists.txt
+            sed -i "\$iinstall(CODE \"execute_process(COMMAND cmake -E create_symlink ../../../hrpsys/idl     share/hrpsys/idl WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/share/hrpsys)\")" src/hrpsys/CMakeLists.txt
+            sed -i "\$iinstall(CODE \"execute_process(COMMAND cmake -E create_symlink ../../../hrpsys/samples share/hrpsys/samples WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/share/hrpsys)\")" src/hrpsys/CMakeLists.txt
+            cat src/hrpsys/CMakeLists.txt
 
             travis_time_end
             travis_time_start  compile_new_version
@@ -250,14 +259,18 @@ case $TEST_PACKAGE in
             wstool set hrpsys http://github.com/start-jsk/hrpsys -v 315.1.9 --git -y
             wstool update
             #
+            sed -i "1imacro(dummy_macro)\nmessage(\"dummy(\${ARGN})\")\nendmacro()" hrpsys/catkin.cmake
+            sed -i "s@install(DIRECTORY test share@dummy_macro(DIRECTORY test share@" hrpsys/catkin.cmake
             sed -i "s@find_package(catkin REQUIRED COMPONENTS rostest mk openrtm_aist openhrp3)@find_package(catkin REQUIRED COMPONENTS rostest mk)\nset(openrtm_aist_PREFIX /opt/ros/hydro/)\nset(openhrp3_PREFIX /opt/ros/hydro/)@"  hrpsys/catkin.cmake
+            cat hrpsys/catkin.cmake
             sed -i "s@NUM_OF_CPUS = \$(shell grep -c '^processor' /proc/cpuinfo)@NUM_OF_CPUS = 2@" hrpsys/Makefile.hrpsys-base
             sed -i "s@touch installed@@" hrpsys/Makefile.hrpsys-base
             cat hrpsys/Makefile.hrpsys-base
             # use git repository, instead of svn due to googlecode shoutdown
             git clone http://github.com/fkanehiro/hrpsys-base --depth 1 -b 315.1.9 ../build_isolated/hrpsys/build/hrpsys-base-source
             # we use latest hrpsys_ocnfig.py for this case, so do not install them
             sed -i -e 's/\(add_subdirectory(python)\)/#\1/' ../build_isolated/hrpsys/build/hrpsys-base-source/CMakeLists.txt
+            sed -i -e 's/\(add_subdirectory(test)\)/#\1/' ../build_isolated/hrpsys/build/hrpsys-base-source/CMakeLists.txt
             find ../build_isolated/hrpsys/build/hrpsys-base-source -name CMakeLists.txt -exec sed -i "s@PCL_FOUND@0@" {} \; # disable PCL
             find ../build_isolated/hrpsys/build/hrpsys-base-source -name CMakeLists.txt -exec sed -i "s@OCTOMAP_FOUND@0@" {} \; # disable OCTOMAP
             find ../build_isolated/hrpsys/build/hrpsys-base-source -name CMakeLists.txt -exec sed -i "s@IRRLIGHT_FOUND@0@" {} \; # disable IRRLIGHT
@@ -300,6 +313,10 @@ case $TEST_PACKAGE in
             trap error ERR
 
             #cp ~/catkin_ws/src/hrpsys/package.xml install_isolated/share/hrpsys/ # old hrpsys did not do this
+            mkdir -p install_isolated/share/hrpsys/share/hrpsys/
+            cp -r ~/catkin_ws/install_isolated/share/hrpsys/idl install_isolated/share/hrpsys/share/hrpsys/
+            cp -r ~/catkin_ws/install_isolated/share/hrpsys/{test,launch,samples} install_isolated/share/hrpsys/ # cp latest script
+
             source install_isolated/setup.bash
 
             #echo $ROS_PACKAGE_PATH
@@ -327,7 +344,10 @@ case $TEST_PACKAGE in
         if [ -e /opt/ros/hydro/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py ]; then
             sudo sed -i "s@test_tf_and_controller@_test_tf_and_controller@" /opt/ros/hydro/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py
         fi
-
+        #https://github.com/start-jsk/rtmros_hironx/pull/358
+        if [ -e /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py ]; then
+            sudo wget https://raw.githubusercontent.com/k-okada/rtmros_hironx/stop_unfinished_battle/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py -O /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py
+        fi
         travis_time_end
 
         sudo /etc/init.d/omniorb4-nameserver stop || echo "stop omniserver just in case..."
@@ -338,7 +358,13 @@ case $TEST_PACKAGE in
         else
             for test_file in `find $pkg_path/test -iname "*.test" -print`; do
                 travis_time_start $(echo $test_file | sed 's@.*/\([a-zA-Z0-9-]*\).test$@\1@' | sed 's@-@_@g')
-                rostest $test_file && travis_time_end || (travis_time_end 31; export EXIT_STATUS=$?)
+                export TMP_EXIT_STATUS=0
+                rostest $test_file && travis_time_end || export TMP_EXIT_STATUS=$?
+                if [ "$TMP_EXIT_STATUS" != 0 ]; then
+                    export EXIT_STATUS=$TMP_EXIT_STATUS
+                    find ~/.ros/test_results -type f -iname "*`basename $test_file .test`.xml" -print -exec echo "=== {} ===" \; -exec cat {} \;
+                    travis_time_end 31
+                fi
             done
         fi
 

.travis.yml

@@ -17,8 +17,8 @@ env:
     - TEST_TYPE=python      TEST_PACKAGE=hrpsys
     - TEST_TYPE=iob         TEST_PACKAGE=hrpsys
     - TEST_TYPE=stable_rtc  TEST_PACKAGE=hrpsys
-    - TEST_TYPE=work_with_downstream  TEST_PACKAGE=hrpsys
-    - TEST_TYPE=work_with_315_1_10    TEST_PACKAGE=hrpsys
+    - TEST_TYPE=work_with_downstream  TEST_PACKAGE=hrpsys-base
+    - TEST_TYPE=work_with_315_1_10    TEST_PACKAGE=hrpsys-base
     - TEST_TYPE=work_with_downstream  TEST_PACKAGE=hrpsys-tools
     - TEST_TYPE=work_with_315_1_10    TEST_PACKAGE=hrpsys-tools
     - TEST_TYPE=work_with_downstream  TEST_PACKAGE=hrpsys-ros-bridge

CMakeLists.txt

@@ -152,6 +152,7 @@ install(FILES
   DESTINATION lib/pkgconfig)
 
 add_definitions(-DHRPSYS_PACKAGE_VERSION=\"\\"${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}\\"\")
+add_definitions(-DROBOT_IOB_VERSION=2)
 
 if(COMPILE_JAVA_STUFF)
   add_subdirectory(jython)

idl/RobotHardwareService.idl

@@ -54,7 +54,6 @@ module OpenHRP
       sequence<DblSequence3>	accel;    ///< accelerations[m/(s^2)]
       double			voltage;  ///< voltage of power supply[V]
       double			current;  ///< current[A] 
-      double			battery;  ///< remingin battery level[%]
     };
 
     /**
@@ -163,5 +162,41 @@ module OpenHRP
      * @return true if applicable, false otherwise
      */
     boolean readDigitalOutput(out OctSequence dOut);
+
+    /* robot status version 2.0 */
+    /**
+     * @brief status of the robot
+     */
+    struct RobotState2
+    {
+      DblSequence		angle;  ///< current joint angles[rad]
+      DblSequence		command;///< reference joint angles[rad]
+      DblSequence		torque; ///< joint torques[Nm]
+      /**
+       * @brief servo statuses(32bit+extra states)
+       * 
+       * 0: calib status ( 1 => done )\n 
+       * 1: servo status ( 1 => on )\n
+       * 2: power status ( 1 => supplied )\n
+       * 3-18: servo alarms (see @ref iob.h)\n
+       * 19-23: unused
+       * 24-31: driver temperature (deg)
+       */
+      LongSequenceSequence		servoState;
+
+      sequence<DblSequence6>	force;    ///< forces[N] and torques[Nm]
+      sequence<DblSequence3>	rateGyro; ///< angular velocities[rad/s]
+      sequence<DblSequence3>	accel;    ///< accelerations[m/(s^2)]
+      double			voltage;  ///< voltage of power supply[V]
+      double			current;  ///< current[A] 
+      double                    battery;  ///< remingin battery level[%]
+    };
+
+    /**
+     * @brief get status of the robot
+     * @param rs status of the robot
+     */
+    void getStatus2(out RobotState2 rs);
+
   };
 };

lib/io/iob.cpp

@@ -562,13 +562,20 @@ int number_of_substeps()
     return 5;
 }
 
-int read_power(double *voltage, double *current, double *battery)
+int read_power(double *voltage, double *current)
 {
     *voltage = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*1+48;
     *current = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+1;
+    return TRUE;
+}
+
+#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2
+int read_battery(double *battery)
+{
     *battery = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+50;
     return TRUE;
 }
+#endif
 
 int read_driver_temperature(int id, unsigned char *v)
 {

lib/io/iob.h

@@ -572,11 +572,21 @@ extern "C"{
      * @brief		read status of power source
      * @param v		voltage[V]
      * @param a		current[A]
+     * @return		TRUE or FALSE
+     */
+    int read_power(double *v, double *a);
+    //@}
+
+#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2
+    //@{
+    /**
+     * @brief		read status of battery source this is new API since 315.4.0
      * @param b		remaining battery level[%]
      * @return		TRUE or FALSE
      */
-    int read_power(double *v, double *a, double *b);
+    int read_battery(double *b) /* {} */; // if you are compiling against old libiob.so, please uncomment to dummy define this function.
     //@}
+#endif
 
     /**
      * @name thermometer

lib/util/BodyRTC.cpp

@@ -438,6 +438,9 @@ void BodyRTC::getStatus(OpenHRP::RobotHardwareService::RobotState* rs) {
     //readPowerStatus(rs->voltage, rs->current);
 }
 
+void BodyRTC::getStatus2(OpenHRP::RobotHardwareService::RobotState2* rs) {
+}
+
 bool BodyRTC::setServoErrorLimit(const char *i_jname, double i_limit)
 {
     Link *l = NULL;
@@ -603,6 +606,10 @@ void RobotHardwareServicePort::getStatus(OpenHRP::RobotHardwareService::RobotSta
     m_robot->getStatus(rs);
 }
 
+void RobotHardwareServicePort::getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs) {
+    rs = new OpenHRP::RobotHardwareService::RobotState2();
+    m_robot->getStatus2(rs);
+}
 
 CORBA::Boolean RobotHardwareServicePort::power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus turnon) {
     m_robot->power(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON);

lib/util/BodyRTC.h

@@ -26,6 +26,7 @@ class RobotHardwareServicePort
     RobotHardwareServicePort();
     ~RobotHardwareServicePort();
     void getStatus(OpenHRP::RobotHardwareService::RobotState_out rs);
+    void getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs);
     CORBA::Boolean power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss);
     CORBA::Boolean servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss);
     void setServoGainPercentage(const char *jname, double limit);
@@ -71,7 +72,7 @@ class BodyRTC : virtual public hrp::Body, public RTC::DataFlowComponentBase
     static void moduleInit(RTC::Manager*);
 
     void getStatus(OpenHRP::RobotHardwareService::RobotState* rs);
-
+    void getStatus2(OpenHRP::RobotHardwareService::RobotState2* rs);
 
     bool preOneStep();
     bool postOneStep();

python/hrpsys_config.py

@@ -670,10 +670,10 @@ def getRTCInstanceList(self, verbose=True):
     # private method to replace $(OPENHRP_DIR) or $(PROJECT_DIR)
     def parseUrl(self, url):
         if '$(OPENHRP_DIR)' in url:
-            path = subprocess.Popen(['pkg-config', 'openhrp3.1', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip().decode('utf-8')
+            path = subprocess.Popen(['pkg-config', 'openhrp3.1', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip()
             url = url.replace('$(OPENHRP_DIR)', path)
         if '$(PROJECT_DIR)' in url:
-            path = subprocess.Popen(['pkg-config', 'hrpsys-base', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip().decode('utf-8')
+            path = subprocess.Popen(['pkg-config', 'hrpsys-base', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip()
             url = url.replace('$(PROJECT_DIR)', path)
         return url
 

rtc/RobotHardware/RobotHardwareService_impl.cpp

@@ -13,62 +13,79 @@ RobotHardwareService_impl::~RobotHardwareService_impl()
 {
 }
 
+#define GetStatus                                                       \
+                                                                        \
+    rs->angle.length(m_robot->numJoints());                             \
+    m_robot->readJointAngles(rs->angle.get_buffer());                   \
+                                                                        \
+    rs->command.length(m_robot->numJoints());                           \
+    m_robot->readJointCommands(rs->command.get_buffer());               \
+                                                                        \
+    rs->torque.length(m_robot->numJoints());                            \
+    if (!m_robot->readJointTorques(rs->torque.get_buffer())){           \
+        for (unsigned int i=0; i<rs->torque.length(); i++){             \
+            rs->torque[i] = 0.0;                                        \
+        }                                                               \
+    }                                                                   \
+                                                                        \
+    rs->servoState.length(m_robot->numJoints());                        \
+    int v, status;                                                      \
+    for(unsigned int i=0; i < rs->servoState.length(); ++i){            \
+        size_t len = m_robot->lengthOfExtraServoState(i)+1;             \
+        rs->servoState[i].length(len);                                  \
+	status = 0;                                                     \
+        v = m_robot->readCalibState(i);                                 \
+        status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; \
+        v = m_robot->readPowerState(i);                                 \
+        status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT; \
+        v = m_robot->readServoState(i);                                 \
+        status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; \
+        v = m_robot->readServoAlarm(i);                                 \
+        status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT; \
+        v = m_robot->readDriverTemperature(i);                          \
+        status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT; \
+        rs->servoState[i][0] = status;                                  \
+        m_robot->readExtraServoState(i, (int *)(rs->servoState[i].get_buffer()+1)); \
+    }                                                                   \
+                                                                        \
+    rs->rateGyro.length(m_robot->numSensors(Sensor::RATE_GYRO));        \
+    for (unsigned int i=0; i<rs->rateGyro.length(); i++){               \
+        rs->rateGyro[i].length(3);                                      \
+        m_robot->readGyroSensor(i, rs->rateGyro[i].get_buffer());       \
+    }                                                                   \
+                                                                        \
+    rs->accel.length(m_robot->numSensors(Sensor::ACCELERATION));        \
+    for (unsigned int i=0; i<rs->accel.length(); i++){                  \
+        rs->accel[i].length(3);                                         \
+        m_robot->readAccelerometer(i, rs->accel[i].get_buffer());       \
+    }                                                                   \
+                                                                        \
+    rs->force.length(m_robot->numSensors(Sensor::FORCE));               \
+    for (unsigned int i=0; i<rs->force.length(); i++){                  \
+        rs->force[i].length(6);                                         \
+        m_robot->readForceSensor(i, rs->force[i].get_buffer());         \
+    }
+
 void RobotHardwareService_impl::getStatus(OpenHRP::RobotHardwareService::RobotState_out rs)
 {
     rs = new OpenHRP::RobotHardwareService::RobotState();
 
-    rs->angle.length(m_robot->numJoints());
-    m_robot->readJointAngles(rs->angle.get_buffer());
-
-    rs->command.length(m_robot->numJoints());
-    m_robot->readJointCommands(rs->command.get_buffer());
-
-    rs->torque.length(m_robot->numJoints());
-    if (!m_robot->readJointTorques(rs->torque.get_buffer())){
-        for (unsigned int i=0; i<rs->torque.length(); i++){
-            rs->torque[i] = 0.0;
-        }
-    }
-
-    rs->servoState.length(m_robot->numJoints());
-    int v, status;
-    for(unsigned int i=0; i < rs->servoState.length(); ++i){
-        size_t len = m_robot->lengthOfExtraServoState(i)+1;
-        rs->servoState[i].length(len);
-	status = 0;
-        v = m_robot->readCalibState(i);
-        status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
-        v = m_robot->readPowerState(i);
-        status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
-        v = m_robot->readServoState(i);
-        status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
-        v = m_robot->readServoAlarm(i);
-        status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
-        v = m_robot->readDriverTemperature(i);
-        status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
-        rs->servoState[i][0] = status;
-        m_robot->readExtraServoState(i, (int *)(rs->servoState[i].get_buffer()+1));
-    }
+    GetStatus
 
-    rs->rateGyro.length(m_robot->numSensors(Sensor::RATE_GYRO));
-    for (unsigned int i=0; i<rs->rateGyro.length(); i++){
-        rs->rateGyro[i].length(3);
-        m_robot->readGyroSensor(i, rs->rateGyro[i].get_buffer());
-    }
+    m_robot->readPowerStatus(rs->voltage, rs->current);
+}
 
-    rs->accel.length(m_robot->numSensors(Sensor::ACCELERATION));
-    for (unsigned int i=0; i<rs->accel.length(); i++){
-        rs->accel[i].length(3);
-        m_robot->readAccelerometer(i, rs->accel[i].get_buffer());
-    }
+void RobotHardwareService_impl::getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs)
+{
+    rs = new OpenHRP::RobotHardwareService::RobotState2();
 
-    rs->force.length(m_robot->numSensors(Sensor::FORCE));
-    for (unsigned int i=0; i<rs->force.length(); i++){
-        rs->force[i].length(6);
-        m_robot->readForceSensor(i, rs->force[i].get_buffer());
-    }
+    GetStatus
 
+#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2
     m_robot->readPowerStatus(rs->voltage, rs->current, rs->battery);
+#else
+    m_robot->readPowerStatus(rs->voltage, rs->current);
+#endif
 }
 
 CORBA::Boolean RobotHardwareService_impl::power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss)

rtc/RobotHardware/RobotHardwareService_impl.h

@@ -17,6 +17,7 @@ class RobotHardwareService_impl
     virtual ~RobotHardwareService_impl();
 
     void getStatus(OpenHRP::RobotHardwareService::RobotState_out rs);
+    void getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs);
 
     CORBA::Boolean power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss);
     CORBA::Boolean servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss);