ros bridge does not publish any topics when st is enabled and robot is on the ground

[garaemon]

Sometimes ROS bridge does not publish any topics when st is enabled and robot is on the ground.

If I hang robot off the ground, ROS bridge works again.

If I disable st and re-enable st, ROS bridge works again even though robot is on the ground.

[garaemon]

hrpsys does not output anything unusual.

[garaemon]

Today I found similar problem. 1) rostopic echo /lfsensor outputs in 1-0.001 Hz. it's super slow. 2) Remove robot from tne ground, /lfsensor topic gets back to normal. 3) I ran rtprint hrp2007c:15005/sh.rtc:rfsensorOut with robot standing on the ground and while rtprint outputs something, rostopic echo /lfsensor works well (!)

It looks like RTM/CORBA problem?

[garaemon]

I found that servo off -> servo on solves this issue.

Following steps does not resolve this issue.

• Restarting st
• Restarting abc

[garaemon]

I found that ports which does not output anything are connected via hrpsys.connectPorts and the ports which keep working are connected via rtcon.connect_ports.

hrpsys.connectPorts sets several default value, it might be a problem.

I will keep digging this issue.

[garaemon]

diff for testing: Use rtmlaunch instead of sensor_connect

diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index 838dc03..ff46b6f 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -65,8 +65,26 @@
   <node pkg="hrpsys_ros_bridge" name="hrpsys_profile" type="hrpsys_profile.py" args="$(arg omniorb_args)"
         if="$(arg USE_HRPSYS_PROFILE)"/>

+  <!-- Use rtconnect instead of sensor_ros_bridge_connect -->
   <node pkg="hrpsys_ros_bridge" name="sensor_ros_bridge_connect" type="sensor_ros_bridge_connect.py"
-        args="$(arg MODEL_FILE) $(arg SIMULATOR_NAME) HrpsysSeqStateROSBridge0 $(arg nameserver) $(arg omniorb_args)" output="$(arg OUTPUT)"/
+        args="$(arg MODEL_FILE) $(arg SIMULATOR_NAME) HrpsysSeqStateROSBridge0 $(arg nameserver) $(arg omniorb_args)" output="$(arg OUTPUT)"
+        if="false" />
+
+  <rtconnect from="RobotHardware0.rtc:rfsensor" to="HrpsysSeqStateROSBridge0.rtc:rfsensor" subscription_type="new" />
+  <rtconnect from="rmfo.rtc:off_rfsensor" to="HrpsysSeqStateROSBridge0.rtc:off_rfsensor" subscription_type="new" />
+  <rtconnect from="sh.rtc:rfsensorOut" to="HrpsysSeqStateROSBridge0.rtc:ref_rfsensor" subscription_type="new" />
+  <rtconnect from="RobotHardware0.rtc:lfsensor" to="HrpsysSeqStateROSBridge0.rtc:lfsensor" subscription_type="new" />
+  <rtconnect from="rmfo.rtc:off_lfsensor" to="HrpsysSeqStateROSBridge0.rtc:off_lfsensor" subscription_type="new" />
+  <rtconnect from="sh.rtc:lfsensorOut" to="HrpsysSeqStateROSBridge0.rtc:ref_lfsensor" subscription_type="new" />
+  <rtconnect from="RobotHardware0.rtc:gsensor" to="HrpsysSeqStateROSBridge0.rtc:gsensor" subscription_type="new" />
+  <rtconnect from="RobotHardware0.rtc:gyrometer" to="HrpsysSeqStateROSBridge0.rtc:gyrometer" subscription_type="new" />
+  <rtconnect from="RobotHardware0.rtc:rhsensor" to="HrpsysSeqStateROSBridge0.rtc:rhsensor" subscription_type="new" />
+  <rtconnect from="rmfo.rtc:off_rhsensor" to="HrpsysSeqStateROSBridge0.rtc:off_rhsensor" subscription_type="new" />
+  <rtconnect from="sh.rtc:rhsensorOut" to="HrpsysSeqStateROSBridge0.rtc:ref_rhsensor" subscription_type="new" />
+  <rtconnect from="RobotHardware0.rtc:lhsensor" to="HrpsysSeqStateROSBridge0.rtc:lhsensor" subscription_type="new" />
+  <rtconnect from="rmfo.rtc:off_lhsensor" to="HrpsysSeqStateROSBridge0.rtc:off_lhsensor" subscription_type="new" />
+  <rtconnect from="sh.rtc:lhsensorOut" to="HrpsysSeqStateROSBridge0.rtc:ref_lhsensor" subscription_type="new" />
+
   <!-- BEGIN:openrtm connection -->
   <env name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
   <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.l

[garaemon]

looks better than before if I applied the patch above.

Default parameter in connectPorts is not good? https://github.com/fkanehiro/hrpsys-base/blob/master/python/rtm.py#L502

[garaemon]

usint rtshell does not solve this issue

[garaemon]

Problem of state holder? https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/StateHolder/StateHolder.cpp#L364

[garaemon]

@snozawa Can we use other port which do not belong to stateholder such as robot hardware?

[snozawa]

@snozawa Can we use other port which do not belong to stateholder such as robot hardware?

For what port? Basically, we cannot replace StateHolder ports by RobotHardware ports, because

• StateHolder is for command values
• RobotHardawre is for actual values

[garaemon]

Ooops, sorry I misunderstood. We are using force sensors from robot hardware.