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)