diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index b32a3a1b8..585556704 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/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))) diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py index 6283d4797..9c2e184f4 100755 --- 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) + # 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 diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index c9a40e844..1bf990e3b 100644 --- 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 index 4ee06ebc9..fdc34a067 100644 --- 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;