Creates ROS frame transformations on the fly, adapted from John Schulman code

change_transforms.py

#!/usr/bin/python
from numpy import *
from traitsui.key_bindings import KeyBinding,KeyBindings
from traitsui.api import *
from traits.api import *
import numpy as np
import roslib
roslib.load_manifest("sensor_msgs")
roslib.load_manifest("rospy")
roslib.load_manifest("tf")
import sensor_msgs.msg as sm
import geometry_msgs.msg as gm
import tf
from tf.transformations import quaternion_from_euler, quaternion_multiply
import tf
from threading import Thread
import rospy
import sys

#PARENT_FRAME = "/head_plate_frame"

def trans_rot_to_pose(trans,rot):
    pose = gm.Pose()
    pose.position.x, pose.position.y, pose.position.z = trans
    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.z = rot
    return pose
def normalize(x):
    return x / np.linalg.norm(x)

def qm(a,b): 
    return normalize(quaternion_multiply(a,b))

class Transform(HasTraits):
    t_step = .005
    
    R = Array(float,(3,))
    reset = Button
    T = Array(float,(3,))    
    
    _key_bindings = []    

    def xqp(self,_):
        t = r_[0.,0,0]
        t[0] = 1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="o",method_name="xqp"))


    def xqn(self,_):
        t = r_[0.,0,0]
        t[0] = -1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="u",method_name="xqn"))


    def yqp(self,_):
        t = r_[0.,0,0]
        t[1] = 1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="i",method_name="yqp"))


    def yqn(self,_):
        t = r_[0.,0,0]
        t[1] = -1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="k",method_name="yqn"))


    def zqp(self,_):
        t = r_[0.,0,0]
        t[2] = 1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="j",method_name="zqp"))


    def zqn(self,_):
        t = r_[0.,0,0]
        t[2] = -1*self.t_step
        self.R = self.R + t
    _key_bindings.append(KeyBinding(binding1="l",method_name="zqn"))


    def xtp(self,_):
        t = r_[0.,0,0]
        t[0] = 1*self.t_step
        self.T = self.T + t

    _key_bindings.append(KeyBinding(binding1="w",method_name="xtp"))


    def xtn(self,_):
        t = r_[0.,0,0]
        t[0] = -1*self.t_step
        self.T = self.T + t

    _key_bindings.append(KeyBinding(binding1="s",method_name="xtn"))

    def ytp(self,_):
        t = r_[0.,0,0]
        t[1] = 1*self.t_step
        self.T = self.T + t
        
    _key_bindings.append(KeyBinding(binding1="a",method_name="ytp"))

    def ytn(self,_):
        t = r_[0.,0,0]
        t[1] = -1*self.t_step
        self.T = self.T + t

    _key_bindings.append(KeyBinding(binding1="d",method_name="ytn"))

    def ztp(self,_):
        t = r_[0.,0,0]
        t[2] = 1*self.t_step
        self.T = self.T + t

    _key_bindings.append(KeyBinding(binding1="z",method_name="ztp"))

    def ztn(self,_):
        t = r_[0.,0,0]
        t[2] = -1*self.t_step
        self.T = self.T + t

    _key_bindings.append(KeyBinding(binding1="x",method_name="ztn"))

    def __init__(self,T=(0,0,0),R=(0,0,0)):
        self.T = T
        self.R = R
        HasTraits.__init__(self)

    def _reset_fired(self):        
        self.set(T=[0,0,0],R = [0,0,0], trait_change_notify=True)

    view = View(
        VGroup(
            Item("reset"),            
            Item("T"),
            Item("R")),
        key_bindings = KeyBindings(*_key_bindings),
        buttons = OKCancelButtons,
        width = 500)        

class TransformBroadcasterThread(Thread):
    wants_exit = False
    def __init__(self,transform, parent_frame, child_frame, pose_name = None):
        self.parent_frame = parent_frame
        self.child_frame = child_frame
        if pose_name is None:
            pose_name = child_frame + "_pose"
        else:
            pose_name = pose_name
        self.transform = transform
        self.pose_pub = rospy.Publisher(pose_name, gm.PoseStamped)
        self.br = tf.TransformBroadcaster()
        Thread.__init__(self)
    def run(self):
        while not rospy.is_shutdown() and not self.wants_exit:
            rot = quaternion_from_euler(*self.transform.R)
            trans = self.transform.T
            self.br.sendTransform(trans, rot, rospy.Time.now(), self.child_frame, self.parent_frame)
            ps = gm.PoseStamped()
            ps.pose = trans_rot_to_pose(trans,rot)
            ps.header.frame_id = self.parent_frame
            ps.header.stamp = rospy.Time.now()
            self.pose_pub.publish(ps)
            rospy.sleep(.01)
        
if __name__ == "__main__":    
    rospy.init_node('calibration')
    
    try:
        parent_frame = rospy.get_param("~parent_frame")
        child_frame = rospy.get_param("~child_frame")
    except KeyError, e:
        rospy.logerr("The parameter %s has to be specified", e)
        rospy.signal_shutdown("Not enough parameters passed")
        sys.exit(1)
        
    if rospy.has_param("pose_name"):
        pose_name = rospy.get_param("pose_name")        
    else:
        pose_name = None
        
    rospy.loginfo("Publishing the transform between %s (parent) and %s (child)", parent_frame, child_frame)
    rospy.loginfo("Keys are: w,a,s,d for forward backward right and left, "
                 "z,x for up down, u,i,o,j,k,l for the rotations (hint: it's easier to see on the keyboard!)")
    
    tra = Transform()
    tf_thread = TransformBroadcasterThread(tra, parent_frame, child_frame)
    tf_thread.start()
    tra.configure_traits()
    tf_thread.wants_exit = True