initial commit of a tool for publishing a tf frame from a twist message

Author: jbohren

CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

launch/twist_frame_integrator.launch

@@ -0,0 +1,14 @@
+<launch>
+  <node pkg="lcsr_tf_tools" type="twist_frame_integrator.py" name="twist_frame_integrator" output="screen">
+    <param name="linear_multiplier" value="0.0001"/>
+    <param name="angular_multiplier" value="0.0001"/>
+    <param name="broadcast_rate" value="50"/>
+
+    <param name="child_frame_id" value="integrated_twist_frame"/>
+    <param name="frame_id" value="/world"/>
+
+    <param name="body_fixed" value="true"/>
+
+    <remap from="/twist" to="/spacenav/twist"/>
+  </node>
+</launch>

mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b lcsr_tf_tools 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

manifest.xml

@@ -0,0 +1,18 @@
+<package>
+  <description brief="lcsr_tf_tools">
+
+     lcsr_tf_tools
+
+  </description>
+  <author>Jonathan Bohren</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/lcsr_tf_tools</url>
+
+  <depend package="rospy"/>
+  <depend package="tf"/>
+  <depend package="geometry_msgs"/>
+  <depend package="python_orocos_kdl"/>
+</package>
+
+

scripts/twist_frame_integrator.py

@@ -0,0 +1,143 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('lcsr_tf_tools')
+import rospy
+import tf
+import geometry_msgs.msg as geometry_msgs
+import threading
+import math
+import PyKDL
+import copy
+
+def multiply_quat(q0,q1):
+    [x0,y0,z0,w0] = q0
+    [x1,y1,z1,w1] = q1
+    return ( (w0*x1 + x0*w1 + y0*z1 - z0*y1),
+             (w0*y1 - x0*z1 + y0*w1 + z0*x1),
+             (w0*z1 + x0*y1 - y0*x1 + z0*w1),
+             (w0*w1 - x0*x1 - y0*y1 - z0*z1) )
+
+class TwistFrameIntegrator(object):
+    def __init__(self):
+        # Get the node params
+        self.linear_multiplier = rospy.get_param('~linear_multiplier')
+        self.angular_multiplier = rospy.get_param('~angular_multiplier')
+        self.broadcast_rate = rospy.get_param('~broadcast_rate')
+        self.body_fixed = rospy.get_param('~body_fixed')
+
+        # Initialize the frame we're going to publish
+        self.transform = PyKDL.Frame()
+        self.transform_out = PyKDL.Frame()
+        self.translation = (0,0,0)
+        self.rotation = (0,0,0,1)
+        self.time = rospy.Time.now()
+        self.frame_id = rospy.get_param('~frame_id')
+        self.child_frame_id = rospy.get_param('~child_frame_id')
+
+        # Synchronization
+        self.broadcast_lock = threading.Lock()
+
+        # Initialze TF structures
+        self.broadcaster = tf.TransformBroadcaster()
+
+        # Subscribe to twist inputs
+        self.twist_sub = rospy.Subscriber(
+                "twist", geometry_msgs.Twist,
+                self.twist_cb)
+        self.twiststamped_sub = rospy.Subscriber(
+                "twist_stamped", geometry_msgs.TwistStamped,
+                self.twiststamped_cb)
+
+        # Broadcast the frame at a given rate
+        self.broadcast_thread = threading.Thread(target=self.broadcast_loop)
+        self.broadcast_thread.start()
+
+    def broadcast_loop(self):
+        """
+        Broadcast the integrated TF frame at a fixed rate.
+        """
+        rate = rospy.Rate(self.broadcast_rate)
+        while not rospy.is_shutdown():
+            # Acquire broadcast lock
+            self.broadcast_lock.acquire()
+            # Broadcast the frame
+            try:
+                self.broadcaster.sendTransform(
+                        (self.transform_out.p.x(), self.transform_out.p.y(), self.transform_out.p.z()),
+                        self.transform_out.M.GetQuaternion(),
+                        rospy.Time.now(),
+                        self.child_frame_id,
+                        self.frame_id)
+            except Exception as ex:
+                rospy.logerr("Failed to broadcast transform: "+str(ex))
+            # Release broadcast lock
+            self.broadcast_lock.release()
+            # Don't spam TF
+            rate.sleep()
+
+    def twiststamped_cb(self,msg):
+        """
+        This callback pulls the twist out of a TwistStamped message. For now,
+        it disregards the origin frame.
+        """
+        twist_cb(msg.twist)
+
+    def twist_cb(self,msg):
+        """
+        This callback integrates the linear and angular velocities into a TF
+        frame, and then broadcasts the frame.
+        """
+        try:
+            # Acquire broadcast lock
+            self.broadcast_lock.acquire()
+
+            # Convert angular rotation vector to quaternion
+            q_velocity = (
+                    msg.angular.x * math.sin(self.angular_multiplier/2),
+                    msg.angular.y * math.sin(self.angular_multiplier/2),
+                    msg.angular.z * math.sin(self.angular_multiplier/2),
+                    math.cos(self.angular_multiplier/2))
+
+            q_velocity_norm = math.sqrt(math.pow(q_velocity[0],2) + math.pow(q_velocity[1],2) + math.pow(q_velocity[2],2) + math.pow(q_velocity[3],2))
+            q_velocity = [i/q_velocity_norm for i in q_velocity]
+
+            if self.body_fixed:
+                # Integrate linear velocity in local frame
+                self.transform.p += self.transform.M*(self.linear_multiplier*PyKDL.Vector(msg.linear.x,msg.linear.y,msg.linear.z))
+                # Integrate angular velocity in local frame
+                self.rotation = multiply_quat(self.rotation,q_velocity)
+                self.transform.M = PyKDL.Rotation.Quaternion(*self.rotation)
+                
+                # Copy this transform
+                self.transform_out = PyKDL.Frame(self.transform)
+            else:
+                # Integrate linear velocity
+                self.transform.p += self.linear_multiplier*PyKDL.Vector(msg.linear.x,msg.linear.y,msg.linear.z)
+                # Integrate angular velocity
+                self.rotation = multiply_quat(q_velocity,self.rotation)
+                self.transform.M = PyKDL.Rotation.Quaternion(*self.rotation)
+
+                # Invert the transform to get parent-frame relative transform
+                self.transform_out = self.transform.Inverse()
+
+            self.transform_out.M.DoRotZ(-math.pi/2)
+            self.transform_out.M.DoRotX(-math.pi/2)
+
+
+
+        finally:
+            # Release broadcast lock
+            self.broadcast_lock.release()
+
+def main():
+    rospy.init_node('twist_frame_integrator')
+    
+    twist_frame_integrator = TwistFrameIntegrator()
+
+    rospy.loginfo("Spinning...")
+    rospy.spin()
+    rospy.loginfo("Done.")
+
+
+if __name__ == '__main__':
+    main()