diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index e072e80b3..4654e2d6c 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -10,9 +10,9 @@
-
+
-
+
diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
index 3300572ee..d46d26e49 100755
--- a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
+++ b/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:
diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py
index 63fe58c8d..6373cbc9b 100755
--- a/hrpsys_ros_bridge/test/test-samplerobot.py
+++ b/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)