Remove unnecessary tf to reduce tf execution rate

jsk_robot_common/jsk_robot_startup/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_python_setup()
 
 generate_dynamic_reconfigure_options(
+  config/OdometryOffsetReconfigure.cfg
   config/OdometryFeedbackWrapperReconfigure.cfg
   config/ConstantHeightFramePublisherReconfigure.cfg
 )

jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml

@@ -14,6 +14,7 @@ viso_odom_calculator:
 biped_particle_odometry:
   particle_num: 80
   rate: 50
+  use_imu: true
 
 biped_odometry_offset:
   use_twist_filter: true

jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg

@@ -5,12 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("sigma_x", double_t, 0, "sigma of distribution on x velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_y", double_t, 0, "sigma of distribution on y velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_z", double_t, 0, "sigma of distribution on z velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_roll", double_t, 0, "sigma of distribution on roll angular velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_pitch", double_t, 0, "sigma of distribution on pitch angular velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_yaw", double_t, 0, "sigma of distribution on yaw angular velocity", 0.3, 0.0001, 10.0)
 gen.add("force_feedback_sigma", double_t, 0, "Odometry feedback is forcely enabled when sigma is larger than this value", 0.0001, 0.3, 10.0)
 gen.add("distribution_feedback_minimum_sigma", double_t, 0, "Distribution check is enabled when sigma is larger than this value", 0.0001, 0.3, 10.0)
 

jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+PACKAGE = "jsk_robot_startup"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("mean_x", double_t, 0, "Average for linear x", -10.0, 0.0, 10.0)
+gen.add("mean_y", double_t, 0, "Average for linear y", -10.0, 0.0, 10.0)
+gen.add("mean_z", double_t, 0, "Average for linear z", -10.0, 0.0, 10.0)
+gen.add("mean_roll", double_t, 0, "Average for angular x", -10.0, 0.0, 10.0)
+gen.add("mean_pitch", double_t, 0, "Average for angular y", -10.0, 0.0, 10.0)
+gen.add("mean_yaw", double_t, 0, "Average for angular z", -10.0, 0.0, 10.0)
+gen.add("sigma_x", double_t, 0, "Standard diviation for linear x", 1e-6, 0.05, 10.0)
+gen.add("sigma_y", double_t, 0, "Standard diviation for linear y", 1e-6, 0.05, 10.0)
+gen.add("sigma_z", double_t, 0, "Standard diviation for linear z", 1e-6, 0.05, 10.0)
+gen.add("sigma_roll", double_t, 0, "Standard diviation for angular x", 1e-6, 0.05, 10.0)
+gen.add("sigma_pitch", double_t, 0, "Standard diviation for angular y", 1e-6, 0.05, 10.0)
+gen.add("sigma_yaw", double_t, 0, "Standard diviation for angular z", 1e-6, 0.05, 10.0)
+
+exit(gen.generate(PACKAGE, "constant_height_frame_publisher_reconfigure", "OdometryOffsetReconfigure"))

jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml

@@ -1,11 +1,15 @@
-viso_odom_calculator:
-  rate: 10
+viso_odometry_offset:
+  overwrite_pdf: true
+  twist_proportional_sigma: true
   sigma_x: 1.0
   sigma_y: 1.0
   sigma_z: 1.0
   sigma_roll: 0.5
   sigma_pitch: 0.5
   sigma_yaw: 0.5
+
+viso_odom_calculator:
+  rate: 10
   twist_proportional_sigma: true
   force_feedback_sigma: 1.0
   distribution_feedback_minimum_sigma: 0.1
@@ -14,3 +18,4 @@ viso_odom_calculator:
 biped_particle_odometry:
   particle_num: 20
   rate: 100
+  use_imu: false

jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch

@@ -1,7 +1,6 @@
 <launch>
   <arg name="set_slam_laser_params" default="true"/>
   <arg name="use_particle_odom" default="true"/>
-  <arg name="use_imu" default="false"/>
   <arg name="use_slam_feedback" default="false"/>
   <arg name="nodelet_index" default="2"/>
   <arg name="stereo_namespace" default="multisense"/>
@@ -25,16 +24,14 @@
     <arg name="stereo" default="$(arg stereo_namespace)" />
     <arg name="image" default="image_rect" />
     <arg name="use_robot_pose_ekf" default="false" />
-    <arg name="publish_viso_tf" default="true" />
+    <arg name="publish_viso_tf" default="false" />
     <arg name="invert_viso_tf" default="true" /> 
   </include>
 
   <!-- <include file="$(find jsk_robot_startup)/launch/odometry_integration.launch"/>     -->
-
   <group if="$(arg use_particle_odom)">
     <include file="$(find jsk_robot_startup)/launch/particle_odometry.launch">
       <arg name="robot" value="$(arg robot)"/>
-      <arg name="use_imu" value="$(arg use_imu)"/>
       <arg name="use_slam_feedback" value="$(arg use_slam_feedback)"/>
     </include>
   </group>

jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch

@@ -1,5 +1,4 @@
 <launch>
-  <arg name="use_imu" default="false"/>
   <arg name="use_slam_feedback" default="false"/>
   <arg name="robot" default="default"/>
 
@@ -16,6 +15,7 @@
       base_odom_frame: odom_init
       odom_frame: biped_odom_offset
       base_link_frame: BODY
+      publish_tf: false
     </rosparam>
   </node>
 
@@ -28,6 +28,7 @@
       base_odom_frame: odom_init
       odom_frame: viso_odom_offset
       base_link_frame: BODY
+      publish_tf: false      
     </rosparam>
   </node>
 
@@ -43,6 +44,7 @@
     <rosparam>
       odom_frame: viso_odom_integrated
       base_link_frame: BODY
+      publish_tf: false
     </rosparam>
   </node>
 
@@ -59,7 +61,6 @@
       odom_frame: biped_odom_particle
       base_link_frame: BODY
       publish_tf: false
-      use_imu: $(arg use_imu)
     </rosparam>
   </node>
 

jsk_robot_common/jsk_robot_startup/launch/viso.launch

@@ -37,6 +37,7 @@
     <param name="~base_link_frame" value="$(arg base_link_frame_id)" />
     <param name="~camera_frame" value="$(arg sensor_frame_id)"/>
     <param name="~odom_frame" value="$(arg odom_frame_id)"/>
+    <param name="~publish_tf" value="$(arg publish_viso_tf)" />
   </node>
 
   <!-- EKF -->

jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py

@@ -9,6 +9,7 @@
 import time
 import threading
 import copy
+from jsk_robot_startup.odometry_utils import broadcast_transform, make_homogeneous_matrix
 
 class CameraToBaseOffset(object):
     def __init__(self):
@@ -17,12 +18,13 @@ def __init__(self):
         self.rate = float(rospy.get_param("~rate", 100))
         # tf parameters
         self.publish_tf = rospy.get_param("~publish_tf", True)
-        self.invert_tf = rospy.get_param("~invert_tf", True)
+        if self.publish_tf:
+            self.invert_tf = rospy.get_param("~invert_tf", True)
+            self.broadcast = tf.TransformBroadcaster()
         self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
         self.camera_frame = rospy.get_param("~camera_frame", "left_camera_optical_frame")
         self.odom_frame = rospy.get_param("~odom_frame", "viso_odom")        
         self.tf_duration = rospy.get_param("~tf_duration", 1)
-        self.broadcast = tf.TransformBroadcaster()
         self.listener = tf.TransformListener(True, rospy.Duration(10))
         self.r = rospy.Rate(self.rate)
         self.initial_matrix = None
@@ -44,54 +46,36 @@ def source_odom_callback(self, msg):
         with self.lock:
             # calculate camera transform
             current_camera_to_base = self.calculate_camera_to_base_transform(msg.header.stamp)
+            if current_camera_to_base == None:
+                return
+
             if self.initial_matrix == None:
-                self.initial_matrix = current_camera_to_base
+                self.initial_matrix = numpy.linalg.inv(current_camera_to_base)
 
-            if self.initial_matrix != None:
-                camera_relative_base_transformation = numpy.dot(numpy.linalg.inv(self.initial_matrix), current_camera_to_base) # base_link transformation in camera coords
-
-                # calculate offseted odometry
-                source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
-                                                                  [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
-                                                                   msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
-                new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix)
+            camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords
+
+            # calculate offseted odometry
+            source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
+                                                         [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
+                                                          msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+            new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix)
 
-                # make odometry msg. twist is copied from source_odom
-                new_odom = copy.deepcopy(msg)
-                new_odom.header.frame_id = self.odom_frame
-                new_odom.child_frame_id = self.base_link_frame
-                new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
-                new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))
-                new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
-                rotation_matrix = camera_relative_base_transformation[:3, :3]
-                new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
-                new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
-                new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist()
+            # make odometry msg. twist is copied from source_odom
+            new_odom = copy.deepcopy(msg)
+            new_odom.header.frame_id = self.odom_frame
+            new_odom.child_frame_id = self.base_link_frame
+            new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
+            new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))
+            new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
+            rotation_matrix = camera_relative_base_transformation[:3, :3]
+            new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
+            new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
+            new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist()
 
-                # publish
-                self.pub.publish(new_odom)
-                if self.publish_tf:
-                    self.broadcast_transform(new_odom)
-
-    def make_homogeneous_matrix(self, trans, rot):
-        homogeneous_matrix = tf.transformations.quaternion_matrix(rot)
-        homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3)
-        return homogeneous_matrix
-
-    def broadcast_transform(self, odom):
-        position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z]
-        orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
-        if self.invert_tf:
-            homogeneous_matrix = self.make_homogeneous_matrix(position, orientation)
-            homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
-            position = list(homogeneous_matrix_inv[:3, 3])
-            orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
-            parent_frame = odom.child_frame_id
-            target_frame = odom.header.frame_id
-        else:
-            parent_frame = odom.header.frame_id
-            target_frame = odom.child_frame_id
-        self.broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame)
+            # publish
+            self.pub.publish(new_odom)
+            if self.publish_tf:
+                broadcast_transform(self.broadcast, new_odom, self.invert_tf)
 
     def calculate_camera_to_base_transform(self, stamp):
         try:
@@ -103,7 +87,7 @@ def calculate_camera_to_base_transform(self, stamp):
             except:
                 rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame)
                 return None
-        camera_to_base_link = self.make_homogeneous_matrix(trans, rot) # camera -> base_link
+        camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link
         return camera_to_base_link
 
 if __name__ == '__main__':