[hironx_ros_bridge] Add test to disable/enable RTCs from conf

start-jsk · Nov 23, 2019 · 2feb99b · 2feb99b
1 parent b6336f9
commit 2feb99b
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 1 deletion.
diff --git a/hironx_ros_bridge/CMakeLists.txt b/hironx_ros_bridge/CMakeLists.txt
@@ -44,7 +44,8 @@ configure_file(conf/xml.in                      ${PROJECT_SOURCE_DIR}/conf/${ROB
 configure_file(conf/nosim.xml.in                ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.xml)
 configure_file(conf/conf.in                     ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.conf)
 configure_file(conf/nosim.conf.in               ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.conf)
-add_custom_target(${PROJECT_NAME}_model_files ALL DEPENDS ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.RobotHardware.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.RobotHardware.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.xml ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.xml ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.conf)
+configure_file(conf/rtc_on_off_test.conf.in     ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_rtc_on_off_test.conf)
+add_custom_target(${PROJECT_NAME}_model_files ALL DEPENDS ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.RobotHardware.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.RobotHardware.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.xml ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.xml ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_nosim.conf ${PROJECT_SOURCE_DIR}/conf/${ROBOT_NAME}_rtc_on_off_test.conf)
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
@@ -104,6 +105,7 @@ add_rostest(test/test-hironx-ros-bridge.test)
 add_rostest(test/test-hironx-ros-bridge-send.test)
 add_rostest(test/test-hironx-ros-bridge-pose.test)
 add_rostest(test/test-hironx-ros-bridge-controller.test)
+add_rostest(test/test-hironx-rtc-on-off.test)
 
 if (CATKIN_ENABLE_TESTING)
   catkin_add_nosetests(test/test_no_ros.py)

diff --git a/hironx_ros_bridge/conf/rtc_on_off_test.conf.in b/hironx_ros_bridge/conf/rtc_on_off_test.conf.in
@@ -0,0 +1,10 @@
+model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
+dt: 0.005
+collision_pair: RARM_JOINT2:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5
+end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,0.0,0.0,0.0,-0.57735027,-0.57735027,-0.57735027,-2.0943951023931953,larm,LARM_JOINT5,CHEST_JOINT0,0.0,0.0,0.0,-0.57735027,-0.57735027,-0.57735027,-2.0943951023931953,
+
+# RTC disable/enable settings which will be loaded via RobotHardware
+CollisionDetector.enable: YES
+ServoController.enable: NO
+DataLogger.enable: NOO  # Test case when typo exists
+
diff --git a/hironx_ros_bridge/test/test-hironx-rtc-on-off.test b/hironx_ros_bridge/test/test-hironx-rtc-on-off.test
@@ -0,0 +1,18 @@
+<launch>
+  <arg name="corbaport" default="2809" />
+  <arg name="GUI" default="false" />
+
+  <!-- See https://github.com/start-jsk/rtmros_common/issues/416#issuecomment-46846623 -->
+  <node name="start_omninames" pkg="rtmbuild" type="start_omninames.sh" args="$(arg corbaport)" />
+
+  <include file="$(find hironx_ros_bridge)/launch/hironx_startup.launch" >
+    <arg name="GUI" value="$(arg GUI)" />
+    <arg name="LAUNCH_HRPSYSPY" value="false" />
+    <arg name="REALTIME" value="false" />
+    <arg name="CONF_FILE" value="$(find hironx_ros_bridge)/conf/kawada-hironx_rtc_on_off_test.conf" />
+    <arg name="corbaport" value="$(arg corbaport)" />
+  </include>
+
+  <test type="test_hironx_rtc_on_off.py" pkg="hironx_ros_bridge" test-name="test_hironx_rtc_on_off" time-limit="200" retry="4"
+        args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
+</launch>
diff --git a/hironx_ros_bridge/test/test_hironx_rtc_on_off.py b/hironx_ros_bridge/test/test_hironx_rtc_on_off.py
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from distutils.version import StrictVersion
+
+from test_hironx import *
+
+class TestHironxRtcOnOff(TestHiro):
+
+    def test_rtc_on_off(self):
+        self.assertTrue(['co', "CollisionDetector"] in self.robot.getRTCList())
+        self.assertFalse(['sc', "ServoController"] in self.robot.getRTCList())
+        self.assertTrue(['log', "DataLogger"] in self.robot.getRTCList())
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'test_hironx_rtc_on_off', TestHironxRtcOnOff)