If rmfo is available, connect rmfo' force sensor ports instead of rh's force sensor ports. Otherwise, connect rh's force sensor ports.

hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l

@@ -104,15 +104,19 @@
    (send self :tmp-force-moment-vector :moment limb))
   (:off-force-vector
    (&optional (limb))
-   "Returns offset-removed :force-vector [N] list for all limbs obtained by :state.
+   "!! This method is deprecated, use :force-vector !!
+    Returns offset-removed :force-vector [N] list for all limbs obtained by :state.
     This value corresponds to RemoveForceSensorLinkOffset RTC.
     If a limb argument is specified, returns a vector for the limb."
+   (warning-message 1 ";; !!~%;; !! :off-force-vector is deprecated, use :force-vector !!~%;; !!~%")
    (send self :tmp-force-moment-vector :force limb "off"))
   (:off-moment-vector
    (&optional (limb))
-   "Returns offset-removed :moment-vector [Nm] list for all limbs obtained by :state.
+   "!! This method is deprecated, use :moment-vector !!
+    Returns offset-removed :moment-vector [Nm] list for all limbs obtained by :state.
     This value corresponds to RemoveForceSensorLinkOffset RTC.
     If a limb argument is specified, returns a vector for the limb."
+   (warning-message 1 ";; !!~%;; !! :off-moment-vector is deprecated, use :moment-vector !!~%;; !!~%")
    (send self :tmp-force-moment-vector :moment limb "off"))
   (:reference-force-vector
    (&optional (limb))
@@ -132,20 +136,20 @@
     This value corresponds to RemoveForceSensorLinkOffset RTC.
     If a limb argument is specified, returns a vector for the limb."
    (if limb
-       (send (car (send robot limb :force-sensors)) :rotate-vector (send self :off-force-vector limb))
+       (send (car (send robot limb :force-sensors)) :rotate-vector (send self :force-vector limb))
      (mapcar #'(lambda (fs force)
                  (send fs :rotate-vector force))
-             (send robot :force-sensors) (send self :off-force-vector))))
+             (send robot :force-sensors) (send self :force-vector))))
   (:absolute-moment-vector
    (&optional (limb))
    "Returns offset-removed :moment-vector [Nm] list for all limbs in world frame obtained by :state.
     This value corresponds to RemoveForceSensorLinkOffset RTC.
     If a limb argument is specified, returns a vector for the limb."
    (if limb
-       (send (car (send robot limb :force-sensors)) :rotate-vector (send self :off-moment-vector limb))
+       (send (car (send robot limb :force-sensors)) :rotate-vector (send self :moment-vector limb))
      (mapcar #'(lambda (fs moment)
                  (send fs :rotate-vector moment))
-             (send robot :force-sensors) (send self :off-moment-vector))))
+             (send robot :force-sensors) (send self :moment-vector))))
   (:zmp-vector
    (&optional (wrt :local))
    "Returns zmp vector [mm].
@@ -1053,12 +1057,12 @@
    (let* ((params (mapcar #'(lambda (alimb) (send self :get-forcemoment-offset-param alimb)) limbs)))
      (labels ((calc-off
                (alimb)
-               (send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb))
+               (send self (if (eq f/m :force) :force-vector :moment-vector) alimb))
               (get-avg-fm
                ()
                (let ((fm (mapcar #'(lambda (i)
                                      (send self :state)
-                                     (mapcar #'(lambda (alimb) (send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb)) limbs))
+                                     (mapcar #'(lambda (alimb) (send self (if (eq f/m :force) :force-vector :moment-vector) alimb)) limbs))
                                  (make-list itr))))
                  (mapcar #'(lambda (alimb)
                              (let ((idx (position alimb limbs)))

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)
+                # For acceleration and rategyro sensors, use rh.
+                # For force sensors, 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

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)

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;