[hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py, hrpsys_ros_b…

…ridge/src/HrpsysSeqStateROSBridge.cpp,h] If rmfo is available, connect rmfo' force sensor ports instead of rh's force sensor ports. Otherwise, connect rh's force sensor ports.
start-jsk · Oct 24, 2016 · 87aaa38 · 87aaa38
1 parent 472f7c8
commit 87aaa38
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 3 deletions.
diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
@@ -19,11 +19,14 @@ def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo, sh, subscription_type =
     for sen in hcf.getSensors(url):
         if sen.type in ['Acceleration', 'RateGyro', 'Force']:
             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), subscription_type, rate=push_rate, pushpolicy=push_policy)
+                # If rmfo is available, connect rmfo' force sensor ports instead of rh's force sensor ports.
+                # Otherwise, connect rh's force sensor ports.
                 if sen.type == 'Force' and rmfo != None:
                     print program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name
                     connectPorts(rmfo.port("off_" + sen.name), bridge.port("off_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for abs forces
+                else:
+                    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), subscription_type, rate=push_rate, pushpolicy=push_policy)
                 if sen.type == 'Force' and sh.port(sen.name+"Out") and bridge.port("ref_" + sen.name):
                     print program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
                     connectPorts(sh.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for reference forces

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -34,7 +34,7 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) :
   use_sim_time(false), use_hrpsys_time(false),
   joint_trajectory_server(nh, "fullbody_controller/joint_trajectory_action", false),
   follow_joint_trajectory_server(nh, "fullbody_controller/follow_joint_trajectory_action", false),
-  HrpsysSeqStateROSBridgeImpl(manager), follow_action_initialized(false), prev_odom_acquired(false)
+  HrpsysSeqStateROSBridgeImpl(manager), follow_action_initialized(false), prev_odom_acquired(false), is_rsforce_connected(false)
 {
   // ros
   ros::NodeHandle pnh("~");
@@ -606,6 +606,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
 	  fsensor.wrench.torque.y = m_rsforce[i].data[4];
 	  fsensor.wrench.torque.z = m_rsforce[i].data[5];
 	  fsensor_pub[i].publish(fsensor);
+          is_rsforce_connected = true;
 	}
       }
       catch(const std::runtime_error &e)
@@ -634,6 +635,12 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
 	  fsensor.wrench.torque.y = m_offforce[i].data[4];
 	  fsensor.wrench.torque.z = m_offforce[i].data[5];
 	  fsensor_pub[i+m_rsforceIn.size()].publish(fsensor);
+          // Publish same force as rsforce for backward compatibility.
+          //   For example, if off_rfsensor is available, publish off_rfsensor and rfsensor.
+          if (!is_rsforce_connected) {
+            fsensor.header.frame_id = m_rsforceName[i];
+            fsensor_pub[i].publish(fsensor);
+          }
 	}
       }
       catch(const std::runtime_error &e)

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -79,6 +79,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;
+  bool is_rsforce_connected;
 
   boost::mutex tf_mutex;
   double tf_rate;