kinect_tf_broadcaster.py
Go to the documentation of this file.
00001 #!/usr/bin/env python  
00002 import roslib
00003 roslib.load_manifest('pcl_to_scan')
00004 import rospy
00005 import std_msgs
00006 
00007 # Give ourselves the ability to run a dynamic reconfigure server.
00008 from dynamic_reconfigure.server import Server as DynamicReconfigureServer
00009 # Import dynamic reconfigure variables.
00010 from pcl_to_scan.cfg import kinect_tf_broadcaster_paramsConfig as Params
00011 
00012 import tf
00013 
00014 class kinect_tf_broadcaster():
00015     def __init__(self):
00016         self.roll = rospy.get_param("~roll", 0.0)
00017         self.pitch = rospy.get_param("~pitch", 0.0)
00018         
00019         self.server = DynamicReconfigureServer(Params, self.reconfigure)
00020         
00021         #print "R: %f P: %f" % (roll, pitch)
00022         #rospy.Subscriber('/cur_tilt_angle', std_msgs.msg.Float64, handle_kinect_tilt)
00023         #rospy.spin()
00024         self.angle_0 = tf.transformations.quaternion_from_euler(0, 0, 0)
00025         self.angle_1 = tf.transformations.quaternion_from_euler(-1.57, 0, -1.57)
00026         self.angle_2 = tf.transformations.quaternion_from_euler(-self.roll, -self.pitch, 0)
00027         while not rospy.is_shutdown():
00028             stamp = rospy.Time.now() + rospy.Duration(0.3)
00029             tf.TransformBroadcaster().sendTransform( (0, 0, 0.036), self.angle_0, stamp, "/openni_camera", "/kinect_rotated_base")
00030             tf.TransformBroadcaster().sendTransform( (0, -0.02, 0), self.angle_0, stamp, "/openni_depth_frame", "/openni_camera")
00031             tf.TransformBroadcaster().sendTransform( (0, -0.04, 0), self.angle_0, stamp, "/openni_rgb_frame", "/openni_camera")
00032             tf.TransformBroadcaster().sendTransform( (0, 0, 0),     self.angle_1, stamp, "/openni_depth_optical_frame", "/openni_depth_frame")
00033             tf.TransformBroadcaster().sendTransform( (0, 0, 0),     self.angle_1, stamp, "/openni_rgb_optical_frame", "/openni_rgb_frame")
00034             tf.TransformBroadcaster().sendTransform( (0, 0, 0),     self.angle_2, stamp, "/kinect_rotated_base", "/kinect_base")
00035             tf.TransformBroadcaster().sendTransform( (-0.077, 0, 0.35),  self.angle_0, stamp, "/kinect_base", "/base_link")
00036             rospy.sleep(0.2)
00037 
00038     # Create a callback function for the dynamic reconfigure server.
00039     def reconfigure(self, config, level):
00040         # Fill in local variables with values received from dynamic reconfigure clients (typically the GUI).
00041         self.roll = config["roll"]
00042         self.pitch = config["pitch"]
00043         self.angle_2 = tf.transformations.quaternion_from_euler(-self.roll, -self.pitch, 0)
00044         # Return the new variables.
00045         return config
00046 
00047 if __name__ == '__main__':
00048     rospy.init_node('kinect_tf_broadcaster')
00049     
00050     try:
00051         ktb = kinect_tf_broadcaster()
00052     except rospy.ROSInterruptException: pass
00053     
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_to_scan
Author(s): Maciej Stefanczyk
autogenerated on Thu Nov 14 2013 11:56:51