From 124b1e93e73d83d0bb8d45d1987b67d85fa23a82 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 21 Apr 2015 00:34:54 +0900 Subject: [PATCH 1/8] [.travis.yml, .travis.sh] enable hrpsys with 315.1.10 [.travis.sh] RULE CHANGED adding new function to iob.h is ok [.travis.sh] do not install test/share/samples/src of old hrpsys, use sample/test/launch of latest hrpsys [.travis.sh] display test results when failure --- .travis.sh | 29 ++++++++++++++++++++++++++--- .travis.yml | 4 ++-- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/.travis.sh b/.travis.sh index 922b5699160..f601f802a04 100755 --- a/.travis.sh +++ b/.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,7 +259,10 @@ 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 @@ -258,6 +270,7 @@ case $TEST_PACKAGE in 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 @@ -338,7 +355,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 diff --git a/.travis.yml b/.travis.yml index 8ad2c766a38..d31499bd291 100644 --- a/.travis.yml +++ b/.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 From d5c54e55c255aa8e9817d9da67e0e4dd37ab235b Mon Sep 17 00:00:00 2001 From: "au@leus" Date: Thu, 16 Apr 2015 20:43:33 +0900 Subject: [PATCH 2/8] [sample/RampleRobot,sample/environment] install pyhton scripts under deval --- sample/SampleRobot/CMakeLists.txt | 4 ++++ sample/environments/CMakeLists.txt | 8 ++------ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sample/SampleRobot/CMakeLists.txt b/sample/SampleRobot/CMakeLists.txt index ac8369272f4..1032dd149e1 100644 --- a/sample/SampleRobot/CMakeLists.txt +++ b/sample/SampleRobot/CMakeLists.txt @@ -60,5 +60,9 @@ install(FILES SampleRobot.DRCTestbed.xml DESTINATION share/hrpsys/samples/SampleRobot) +file(GLOB python_scripts RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.py) +install(PROGRAMS + ${python_scripts} + DESTINATION share/hrpsys/samples/SampleRobot) add_subdirectory(rtc) diff --git a/sample/environments/CMakeLists.txt b/sample/environments/CMakeLists.txt index 08d7da6b2a9..3486eb184a4 100644 --- a/sample/environments/CMakeLists.txt +++ b/sample/environments/CMakeLists.txt @@ -1,7 +1,3 @@ -install(FILES - TerrainFloor.wrl - DRCTestbedTerrainUSBlock.wrl - DRCTestbedTerrainJPBlock.wrl - DRCTestbedStair.wrl - DESTINATION share/hrpsys/samples/environments) +file(GLOB model_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.wrl) +install(FILES ${model_files} DESTINATION share/hrpsys/samples/environments) From 436287b6e9ab3c8020ed63e6a41acea756905ce1 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 21 Apr 2015 12:58:40 +0900 Subject: [PATCH 3/8] [hrpsys_config.py] #567 is wrong, do not need to decode --- python/hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index a0d27e8068f..2aa0a13c12b 100755 --- a/python/hrpsys_config.py +++ b/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 From 3b7889451ad2c2095341628f84507ee24635d6cc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Apr 2015 03:19:34 -0700 Subject: [PATCH 4/8] [RobotHardware, iob, BodyRTC] fix to compatible with old API --- idl/RobotHardwareService.idl | 37 +++++- lib/io/iob.cpp | 7 +- lib/io/iob.h | 10 +- lib/util/BodyRTC.cpp | 7 ++ lib/util/BodyRTC.h | 3 +- .../RobotHardwareService_impl.cpp | 107 ++++++++++-------- rtc/RobotHardware/RobotHardwareService_impl.h | 1 + rtc/RobotHardware/robot.cpp | 8 +- rtc/RobotHardware/robot.h | 3 +- 9 files changed, 130 insertions(+), 53 deletions(-) diff --git a/idl/RobotHardwareService.idl b/idl/RobotHardwareService.idl index 92956e4d4fb..f9da0a3d227 100644 --- a/idl/RobotHardwareService.idl +++ b/idl/RobotHardwareService.idl @@ -54,7 +54,6 @@ module OpenHRP sequence 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 force; ///< forces[N] and torques[Nm] + sequence rateGyro; ///< angular velocities[rad/s] + sequence 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); + }; }; diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index a8c3e4c14a5..e284a8d9361 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -562,10 +562,15 @@ 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; +} + +int read_battery(double *battery) +{ *battery = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+50; return TRUE; } diff --git a/lib/io/iob.h b/lib/io/iob.h index 1167b8f8f6f..5f49360e0c8 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -572,10 +572,18 @@ 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); + //@} + + //@{ + /** + * @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. //@} /** diff --git a/lib/util/BodyRTC.cpp b/lib/util/BodyRTC.cpp index f5fdaec57f7..4ee85987442 100644 --- a/lib/util/BodyRTC.cpp +++ b/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); diff --git a/lib/util/BodyRTC.h b/lib/util/BodyRTC.h index b748c4c751f..7885fd70629 100644 --- a/lib/util/BodyRTC.h +++ b/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(); diff --git a/rtc/RobotHardware/RobotHardwareService_impl.cpp b/rtc/RobotHardware/RobotHardwareService_impl.cpp index e90de29d057..44f482a6993 100644 --- a/rtc/RobotHardware/RobotHardwareService_impl.cpp +++ b/rtc/RobotHardware/RobotHardwareService_impl.cpp @@ -13,60 +13,73 @@ 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; itorque.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; irateGyro.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; iaccel.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; iforce.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; itorque.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; irateGyro.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; iaccel.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; iforce.length(); i++){ - rs->force[i].length(6); - m_robot->readForceSensor(i, rs->force[i].get_buffer()); - } + GetStatus m_robot->readPowerStatus(rs->voltage, rs->current, rs->battery); } diff --git a/rtc/RobotHardware/RobotHardwareService_impl.h b/rtc/RobotHardware/RobotHardwareService_impl.h index 8d0e09a11c0..e347c0a17df 100644 --- a/rtc/RobotHardware/RobotHardwareService_impl.h +++ b/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); diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 659cc7ba115..19bde795160 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -459,9 +459,15 @@ void robot::writeVelocityCommands(const double *i_commands) write_command_velocities(i_commands); } +void robot::readPowerStatus(double &o_voltage, double &o_current) +{ + read_power(&o_voltage, &o_current); +} + void robot::readPowerStatus(double &o_voltage, double &o_current, double &o_battery) { - read_power(&o_voltage, &o_current, &o_battery); + read_power(&o_voltage, &o_current); + read_battery(&o_battery); } int robot::readCalibState(int i) diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h index 1bcdfc37d6a..1141b015fe8 100644 --- a/rtc/RobotHardware/robot.h +++ b/rtc/RobotHardware/robot.h @@ -127,8 +127,9 @@ class robot : public hrp::Body \brief read voltage and current of the robot power source \param o_voltage voltage \param o_current current - \param o_battery remaining battery level + \param o_battery remaining battery level ( new feature on 315.4.0) */ + void readPowerStatus(double &o_voltage, double &o_current); void readPowerStatus(double &o_voltage, double &o_current, double &o_battery); /** From 55c1d28d9901635555e9f8cd4688e601407d8523 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Apr 2015 01:51:16 -0700 Subject: [PATCH 5/8] [test-colcheck.test] load modelfiles --- test/test-colcheck.test | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test/test-colcheck.test b/test/test-colcheck.test index 8c68ad3fa1e..4fc5fdaca5a 100644 --- a/test/test-colcheck.test +++ b/test/test-colcheck.test @@ -8,7 +8,11 @@ - + + + \ No newline at end of file From 46b27dea3ebfe57b12f44fe9bd3fd7e425c1f0cf Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 20 Apr 2015 01:52:11 -0700 Subject: [PATCH 6/8] [test-robot-hardware.test] add test to check robot-hardware service call --- test/test-robot-hardware.py | 39 +++++++++++++++++++++++++++++++++++ test/test-robot-hardware.test | 16 ++++++++++++++ 2 files changed, 55 insertions(+) create mode 100755 test/test-robot-hardware.py create mode 100644 test/test-robot-hardware.test diff --git a/test/test-robot-hardware.py b/test/test-robot-hardware.py new file mode 100755 index 00000000000..10fb51e4bd1 --- /dev/null +++ b/test/test-robot-hardware.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +PKG = 'hrpsys' +NAME = 'test_robothardware' + +from hrpsys import hrpsys_config +import OpenHRP + +import socket +import rtm + +import unittest +import rostest +import sys + +class TestHrpsysRobotHardware(unittest.TestCase): + + def test_rh_service(self): + try: + rh = rh_svc = None + rtm.nshost = 'localhost' + rtm.nsport = 2809 + rtm.initCORBA() + rh = rtm.findRTC("RobotHardware0") + rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService") + print "RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc) + self.assertTrue(rh and rh_svc) + rh.start() + self.assertTrue(rh.isActive()) + self.assertTrue(rh_svc.getStatus()) + + except Exception as e: + print "{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc) + self.fail() + pass + +#unittest.main() +if __name__ == '__main__': + rostest.run(PKG, NAME, TestHrpsysRobotHardware, sys.argv) diff --git a/test/test-robot-hardware.test b/test/test-robot-hardware.test new file mode 100644 index 00000000000..61c073b1b66 --- /dev/null +++ b/test/test-robot-hardware.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + \ No newline at end of file From 56805f5e0b94d7edb7b5c93c7b7bb1a513911a00 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 22 Apr 2015 15:13:43 +0900 Subject: [PATCH 7/8] [.travis.sh] hot fix for https://github.com/start-jsk/rtmros_hironx/pull/358 --- .travis.sh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index f601f802a04..355b65d0faa 100755 --- a/.travis.sh +++ b/.travis.sh @@ -344,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..." From c07ca55594a6cc44a001046c66fbc449c2fdcde9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 22 Apr 2015 20:03:09 +0900 Subject: [PATCH 8/8] [lib/io/iob{cpp,h},rtc/RobotHardware/{robot.cpp,robot.h,RobotHardwareService_impl.cpp,CMakeList.txt] add ROBOT_IOB_VERSION for backword compatibility --- CMakeLists.txt | 1 + lib/io/iob.cpp | 2 ++ lib/io/iob.h | 2 ++ rtc/RobotHardware/RobotHardwareService_impl.cpp | 4 ++++ rtc/RobotHardware/robot.cpp | 2 ++ rtc/RobotHardware/robot.h | 2 ++ 6 files changed, 13 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 460e15651fc..35951dfc4a7 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index e284a8d9361..c9614c2de44 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -569,11 +569,13 @@ int read_power(double *voltage, double *current) 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) { diff --git a/lib/io/iob.h b/lib/io/iob.h index 5f49360e0c8..9d2f3c2b68d 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -577,6 +577,7 @@ extern "C"{ 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 @@ -585,6 +586,7 @@ extern "C"{ */ int read_battery(double *b) /* {} */; // if you are compiling against old libiob.so, please uncomment to dummy define this function. //@} +#endif /** * @name thermometer diff --git a/rtc/RobotHardware/RobotHardwareService_impl.cpp b/rtc/RobotHardware/RobotHardwareService_impl.cpp index 44f482a6993..8d9aa77dbbd 100644 --- a/rtc/RobotHardware/RobotHardwareService_impl.cpp +++ b/rtc/RobotHardware/RobotHardwareService_impl.cpp @@ -81,7 +81,11 @@ void RobotHardwareService_impl::getStatus2(OpenHRP::RobotHardwareService::RobotS 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) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 19bde795160..364c54847c4 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -464,11 +464,13 @@ void robot::readPowerStatus(double &o_voltage, double &o_current) read_power(&o_voltage, &o_current); } +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2 void robot::readPowerStatus(double &o_voltage, double &o_current, double &o_battery) { read_power(&o_voltage, &o_current); read_battery(&o_battery); } +#endif int robot::readCalibState(int i) { diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h index 1141b015fe8..f06963a7d2b 100644 --- a/rtc/RobotHardware/robot.h +++ b/rtc/RobotHardware/robot.h @@ -130,7 +130,9 @@ class robot : public hrp::Body \param o_battery remaining battery level ( new feature on 315.4.0) */ void readPowerStatus(double &o_voltage, double &o_current); +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2 void readPowerStatus(double &o_voltage, double &o_current, double &o_battery); +#endif /** \brief read array of all joint angles[rad]