Support odom topic

hrpsys_ros_bridge/catkin.cmake

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(hrpsys_ros_bridge)
 
 # call catkin depends
-find_package(catkin REQUIRED COMPONENTS rtmbuild roscpp rostest sensor_msgs robot_state_publisher actionlib control_msgs tf camera_info_manager hrpsys_tools image_transport dynamic_reconfigure hrpsys) # pr2_controllers_msgs robot_monitor
+find_package(catkin REQUIRED COMPONENTS rtmbuild roscpp rostest sensor_msgs robot_state_publisher actionlib control_msgs tf camera_info_manager hrpsys_tools image_transport dynamic_reconfigure hrpsys nav_msgs) # pr2_controllers_msgs robot_monitor
 catkin_python_setup()
 
 # download pr2_controllers_msgs for git
@@ -70,7 +70,7 @@ rtmbuild_init()
 # call catkin_package, after rtmbuild_init, before rtmbuild_gen*
 catkin_package(
     DEPENDS hrpsys # TODO
-    CATKIN_DEPENDS rtmbuild roscpp sensor_msgs robot_state_publisher actionlib control_msgs tf camera_info_manager image_transport dynamic_reconfigure # pr2_controllers_msgs robot_monitor
+    CATKIN_DEPENDS rtmbuild roscpp sensor_msgs robot_state_publisher actionlib control_msgs tf camera_info_manager image_transport dynamic_reconfigure nav_msgs # pr2_controllers_msgs robot_monitor
     INCLUDE_DIRS # TODO include
     LIBRARIES # TODO
     CFG_EXTRAS compile_robot_model.cmake

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

@@ -453,17 +453,9 @@
 (defmethod rtm-ros-robot-interface
   (:start-auto-balancer
    (&key (limbs '(:rleg :lleg)))
-   (send self :autobalancerservice_startABC
-         :alp (mapcar #'(lambda (limb)
-                          (let ((target2foot (send (send (send robot limb :end-coords) :parent) :transformation (send robot limb :end-coords))))
-                            (instance hrpsys_ros_bridge::openhrp_autobalancerservice_autobalancerlimbparam
-                                      :init :name (format nil "~A" limb)
-                                      ;;:target_name (send (send (send robot limb :end-coords) :parent) :joint :name)
-                                      ;;:base_name (if (send (send (send robot limb :root-link) :parent) :joint) (send (send (send robot limb :root-link) :parent) :joint :name) "WAIST")
-                                      :target2foot_offset_pos (scale 1e-3 (send target2foot :worldpos))
-                                      :target2foot_offset_rot (matrix2quaternion (send target2foot :worldrot)))))
-                      limbs)))
-  (:stop-auto-balancer () (send self :autobalancerservice_stopABC))
+   (send self :autobalancerservice_startAutoBalancer
+         :limbs (mapcar #'(lambda (x) (format nil "~A" x)) limbs)))
+  (:stop-auto-balancer () (send self :autobalancerservice_stopAutoBalancer))
   (:go-pos-no-wait
    (xx yy th)
    (send self :autobalancerservice_goPos :x xx :y yy :th th))

hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

@@ -85,7 +85,8 @@
     <rtconnect from="$(arg SIMULATOR_NAME).rtc:q" to="HrpsysSeqStateROSBridge0.rtc:rsangle" subscription_type="new"/>
     <rtconnect from="$(arg SIMULATOR_NAME).rtc:tau" to="HrpsysSeqStateROSBridge0.rtc:rstorque" subscription_type="new" unless="$(arg USE_TORQUEFILTER)"/>
     <rtconnect from="StateHolderServiceROSBridge.rtc:StateHolderService" to="sh.rtc:StateHolderService"  subscription_type="new"/>
-    <rtconnect from="sh.rtc:baseTformOut" to="HrpsysSeqStateROSBridge0.rtc:baseTform" subscription_type="new"/>
+    <!-- <rtconnect from="sh.rtc:baseTformOut" to="HrpsysSeqStateROSBridge0.rtc:baseTform" subscription_type="new"/> -->
+    <rtconnect from="abc.rtc:baseTformOut" to="HrpsysSeqStateROSBridge0.rtc:baseTform" subscription_type="new"/>
     <rtconnect from="sh.rtc:qOut" to="HrpsysSeqStateROSBridge0.rtc:mcangle" subscription_type="new"/>
     <rtconnect from="HrpsysSeqStateROSBridge0.rtc:SequencePlayerService" to="seq.rtc:SequencePlayerService" subscription_type="new"/>
     <rtconnect from="HrpsysJointTrajectoryBridge0.rtc:SequencePlayerService" to="seq.rtc:SequencePlayerService" subscription_type="new"/>
@@ -142,18 +143,18 @@
 
   <!-- USE_IMPEDANCECONTROLLER
        ImpedanceController
-       AbsoluteForceSensor
+       RemoveForceSensorLinkOffset
   -->
   <group if="$(arg USE_IMPEDANCECONTROLLER)" >
     <node pkg="hrpsys_ros_bridge" name="ImpedanceControllerServiceROSBridge" type="ImpedanceControllerServiceROSBridgeComp"
           output="screen" args ="$(arg openrtm_args)" />
-    <node pkg="hrpsys_ros_bridge" name="AbsoluteForceSensorServiceROSBridge" type="AbsoluteForceSensorServiceROSBridgeComp"
+    <node pkg="hrpsys_ros_bridge" name="RemoveForceSensorLinkOffsetServiceROSBridge" type="RemoveForceSensorLinkOffsetServiceROSBridgeComp"
           output="$(arg OUTPUT)" args="$(arg openrtm_args)" />
 
     <rtconnect from="ImpedanceControllerServiceROSBridge.rtc:ImpedanceControllerService" to="ic.rtc:ImpedanceControllerService"  subscription_type="new"/>
     <rtactivate component="ImpedanceControllerServiceROSBridge.rtc" />
-    <rtconnect from="AbsoluteForceSensorServiceROSBridge.rtc:AbsoluteForceSensorService" to="afs.rtc:AbsoluteForceSensorService"  subscription_type="new"/>
-    <rtactivate component="AbsoluteForceSensorServiceROSBridge.rtc" />
+    <rtconnect from="RemoveForceSensorLinkOffsetServiceROSBridge.rtc:RemoveForceSensorLinkOffsetService" to="rmfo.rtc:RemoveForceSensorLinkOffsetService"  subscription_type="new"/>
+    <rtactivate component="RemoveForceSensorLinkOffsetServiceROSBridge.rtc" />
   </group>
 
   <!-- USE_GRASPCONTROLLER

hrpsys_ros_bridge/manifest.xml

@@ -21,6 +21,7 @@ hrpsys_ros_bridge.launch : hrpsys and ROS bridge software core launch script, th
   <!-- seq state ros bridge -->
   <depend package="roscpp"/>
   <depend package="sensor_msgs"/>
+  <depend package="nav_msgs"/>
   <depend package="visualization_msgs"/>
   <depend package="robot_state_publisher"/>
   <depend package="actionlib"/>

hrpsys_ros_bridge/package.xml

@@ -54,6 +54,8 @@
   <build_depend>subversion</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>visualization_msgs</build_depend>
+  <build_depend>angles</build_depend>
+  <build_depend>nav_msgs</build_depend>
 
   <run_depend>actionlib</run_depend>
   <run_depend>camera_info_manager</run_depend>
@@ -75,7 +77,8 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>visualization_msgs</run_depend>
-
+  <run_depend>nav_msgs</run_depend>
+
   <!-- <test_depend>hrpsys</test_depend> -->
   <!-- <test_depend>hrpsys_tools</test_depend> -->
   <!-- <test_depend>openrtm_tools</test_depend> -->

hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py

@@ -15,15 +15,15 @@
 
 program_name = '[sensor_ros_bridge_connect.py] '
 
-def connecSensorRosBridgePort(url, rh, bridge, vs, afs):
+def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo):
     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), "new")
                 if sen.type == 'Force':
-                    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
+                    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), "new") # for abs forces
         else:
             continue
     if vs != None:
@@ -40,8 +40,8 @@ def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerho
         bridge = rtm.findRTC(rosbridge_name)
         print program_name, " wait for ", rosbridge_name, " : ",bridge
     vs=rtm.findRTC('vs')
-    afs=rtm.findRTC('afs')
-    connecSensorRosBridgePort(url, hcf.rh, bridge, vs, afs)
+    rmfo=rtm.findRTC('rmfo')
+    connecSensorRosBridgePort(url, hcf.rh, bridge, vs, rmfo)
 
 if __name__ == '__main__':
     print program_name, "start"

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

@@ -45,6 +45,7 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) :
   joint_state_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
   joint_controller_state_pub = nh.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("/fullbody_controller/state", 1);
   mot_states_pub = nh.advertise<hrpsys_ros_bridge::MotorStates>("/motor_states", 1);
+  odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 1);
 
   // is use_sim_time is set and no one publishes clock, publish clock time
   use_sim_time = ros::Time::isSimTime();
@@ -464,20 +465,51 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
   // m_baseTformIn
   if ( m_baseTformIn.isNew () ) {
     m_baseTformIn.read();
-    tf::Transform base;
+    // tf::Transform base;
     double *a = m_baseTform.data.get_buffer();
-    base.setOrigin( tf::Vector3(a[0], a[1], a[2]) );
+    // base.setOrigin( tf::Vector3(a[0], a[1], a[2]) );
     hrp::Matrix33 R;
     hrp::getMatrix33FromRowMajorArray(R, a, 3);
     hrp::Vector3 rpy = hrp::rpyFromRot(R);
-    base.setRotation( tf::createQuaternionFromRPY(rpy(0), rpy(1), rpy(2)) );
-
-    // odom publish
-    ros::Time base_time = tm_on_execute;
-    if ( use_hrpsys_time ) {
-        base_time = ros::Time(m_baseTform.tm.sec,m_baseTform.tm.nsec);
+    tf::Quaternion q = tf::createQuaternionFromRPY(rpy(0), rpy(1), rpy(2));
+    nav_msgs::Odometry odom;
+    //odom.header.frame_id = rootlink_name;
+    odom.header.frame_id = "odom";
+    odom.header.stamp = ros::Time(m_baseTform.tm.sec, m_baseTform.tm.nsec);
+    //odom.child_frame_id = "/odom";
+    odom.pose.pose.position.x = a[0];
+    odom.pose.pose.position.y = a[1];
+    odom.pose.pose.position.z = a[2];
+    odom.pose.pose.orientation.x = q.getX();
+    odom.pose.pose.orientation.y = q.getY();
+    odom.pose.pose.orientation.z = q.getZ();
+    odom.pose.pose.orientation.w = q.getW();
+
+    odom.pose.covariance[0] = 0.002 * 0.002;
+    odom.pose.covariance[7] = 0.002 * 0.002;
+    odom.pose.covariance[14] = 0.002 * 0.002;
+    odom.pose.covariance[21] = 0.002 * 0.002;
+    odom.pose.covariance[28] = 0.002 * 0.002;
+    odom.pose.covariance[35] = 0.002 * 0.002;
+    if (prev_odom_acquired) {
+      // calc velocity
+      double dt = (odom.header.stamp - prev_odom.header.stamp).toSec();
+      odom.twist.twist.linear.x = (odom.pose.pose.position.x - prev_odom.pose.pose.position.x) / dt;
+      odom.twist.twist.linear.y = (odom.pose.pose.position.y - prev_odom.pose.pose.position.y) / dt;
+      odom.twist.twist.linear.z = (odom.pose.pose.position.z - prev_odom.pose.pose.position.z) / dt;
+      odom.twist.twist.angular.x = (rpy(0) - prev_rpy(0)) / dt;
+      odom.twist.twist.angular.x = (rpy(1) - prev_rpy(1)) / dt;
+      odom.twist.twist.angular.x = (rpy(2) - prev_rpy(2)) / dt;
+      odom.twist.covariance = odom.pose.covariance;
+      prev_odom = odom;
+      prev_rpy = rpy;
+      odom_pub.publish(odom);
+    }
+    else {
+      prev_odom = odom;
+      prev_rpy = rpy;
+      prev_odom_acquired = true;
     }
-    br.sendTransform(tf::StampedTransform(base, base_time, "odom", rootlink_name));
   }  // end: m_baseTformIn
 
   bool updateTfImu = false;

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h

@@ -16,6 +16,7 @@
 #include "ros/ros.h"
 #include "rosgraph_msgs/Clock.h"
 #include "sensor_msgs/JointState.h"
+#include "nav_msgs/Odometry.h"
 #include "geometry_msgs/WrenchStamped.h"
 #include "actionlib/server/simple_action_server.h"
 #include "control_msgs/FollowJointTrajectoryAction.h"
@@ -47,7 +48,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
 
  private:
   ros::NodeHandle nh;
-  ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub;
+  ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, odom_pub;
   std::vector<ros::Publisher> fsensor_pub;
   actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> joint_trajectory_server;
   actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> follow_joint_trajectory_server;
@@ -63,6 +64,10 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   std::string rootlink_name;
 
   ros::Subscriber clock_sub;
+
+  nav_msgs::Odometry prev_odom;
+  bool prev_odom_acquired;
+  hrp::Vector3 prev_rpy;
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;

hrpsys_tools/launch/hrpsys.launch

@@ -56,7 +56,7 @@
 -o "example.TorqueFilter.config_file:$(arg CONF_FILE)"
 -o "example.TorqueController.config_file:$(arg CONF_FILE)"
 -o "example.VirtualForceSensor.config_file:$(arg CONF_FILE)"
--o "example.AbsoluteForceSensor.config_file:$(arg CONF_FILE)"
+-o "example.RemoveForceSensorLinkOffset.config_file:$(arg CONF_FILE)"
 -o "example.KalmanFilter.config_file:$(arg CONF_FILE)"
 -o "example.Stabilizer.config_file:$(arg CONF_FILE)"
 -o "example.CollisionDetector.config_file:$(arg CONF_FILE)"