Merge pull request #447 from k-okada/use_stable_rtc

Use stable rtc
- merged according to procedure described in #449

hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

@@ -10,9 +10,9 @@
   <!-- Set these values false when using HIRO -->
   <arg name="USE_COMMON" default="true" />
   <arg name="USE_ROBOTHARDWARE" default="false" />
-  <arg name="USE_WALKING" default="true" />
+  <arg name="USE_WALKING" default="false" />
   <arg name="USE_COLLISIONCHECK" default="true" />
-  <arg name="USE_IMPEDANCECONTROLLER" default="true" />
+  <arg name="USE_IMPEDANCECONTROLLER" default="false" />
   <arg name="USE_GRASPCONTROLLER" default="false" />
   <arg name="USE_TORQUECONTROLLER" default="false" />
   <arg name="USE_SOFTERRORLIMIT" default="true" />

hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py

@@ -21,7 +21,7 @@ def connecSensorRosBridgePort(url, rh, bridge, vs, afs):
             if rh.port(sen.name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
                 print program_name, "connect ", sen.name, rh.port(sen.name).get_port_profile().name, bridge.port(sen.name).get_port_profile().name
                 connectPorts(rh.port(sen.name), bridge.port(sen.name), "new")
-                if sen.type == 'Force':
+                if sen.type == 'Force' and afs != None:
                     print program_name, "connect ", sen.name, afs.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name
                     connectPorts(afs.port("off_" + sen.name), bridge.port("off_" + sen.name), "new") # for abs forces
         else:

hrpsys_ros_bridge/test/test-samplerobot.py

@@ -38,13 +38,13 @@ def setUp(self):
         rospy.Subscriber('/rfsensor', WrenchStamped, self.rfsensor_cb)
         self.listener = tf.TransformListener()
 
-    def test_tf_imu_floor_WAIST_LINK(self): # need to check if map/ is published?
+    def test_tf_odom_WAIST_LINK(self): # need to check if map/ is published?
         try:
-            self.listener.waitForTransform('/imu_floor', '/WAIST_LINK0', rospy.Time(), rospy.Duration(120))
+            self.listener.waitForTransform('/odom', '/WAIST_LINK0', rospy.Time(), rospy.Duration(120))
         except tf.Exception:
-            self.assertTrue(None, "could not found tf from /imu_floor to /WAIST_LINK0")
-        (trans,rot) = self.listener.lookupTransform('/imu_floor', '/WAIST_LINK0', rospy.Time(0))
-        rospy.logwarn("tf_echo /imu_floor /WAIST_LINK0 %r %r"%(trans,rot))
+            self.assertTrue(None, "could not found tf from /odom to /WAIST_LINK0")
+        (trans,rot) = self.listener.lookupTransform('/odom', '/WAIST_LINK0', rospy.Time(0))
+        rospy.logwarn("tf_echo /odom /WAIST_LINK0 %r %r"%(trans,rot))
         self.assertAlmostEqual(trans[2],0.7235,2)
         self.assertTrue(True,"ok")
 
@@ -68,7 +68,7 @@ def test_load_pattern(self):
         ret = wait_interpolation()
         t2 = rospy.get_time()
         rospy.logwarn("waitInterpolation %f"%(t2-t1))
-        (trans,rot) = self.listener.lookupTransform('/imu_floor', '/WAIST_LINK0', rospy.Time(0))
+        (trans,rot) = self.listener.lookupTransform('/odom', '/WAIST_LINK0', rospy.Time(0))
         rospy.logwarn("tf_echo /odom /WAIST_LINK0 %r %r"%(trans,rot))
         self.assertAlmostEqual(t2-t1, 20, delta=5)
         #self.assertNotAlmostEqual(trans[1],0,2)