rtabmap_ros: test_prior_tf_to_pose.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python  
00002 import rospy
00003 import tf
00004 import numpy
00005 import tf2_ros
00006 from geometry_msgs.msg import PoseWithCovarianceStamped
00007 
00008 if __name__ == '__main__':
00009     rospy.init_node('tf_to_pose', anonymous=True)
00010     listener = tf.TransformListener()
00011     frame = rospy.get_param('~frame', 'world')
00012     childFrame = rospy.get_param('~child_frame', 'kinect_gt')
00013     outputFrame = rospy.get_param('~output_frame', 'kinect')
00014     cov = rospy.get_param('~cov', 1)
00015     rateParam = rospy.get_param('~rate', 30) # 10hz
00016     pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
00017    
00018     print 'start loop!'
00019     rate = rospy.Rate(rateParam) 
00020     while not rospy.is_shutdown():
00021         poseOut = PoseWithCovarianceStamped()
00022         try:
00023             now = rospy.get_rostime()
00024             listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
00025             (trans,rot) = listener.lookupTransform(frame, childFrame, now)
00026         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
00027             print str(e)
00028             rate.sleep()
00029             continue
00030         
00031         poseOut.header.stamp.nsecs = now.nsecs
00032         poseOut.header.stamp.secs = now.secs
00033         poseOut.header.frame_id = outputFrame
00034         poseOut.pose.pose.position.x = trans[0]
00035         poseOut.pose.pose.position.y = trans[1]
00036         poseOut.pose.pose.position.z = trans[2]
00037         poseOut.pose.pose.orientation.x = rot[0]
00038         poseOut.pose.pose.orientation.y = rot[1]
00039         poseOut.pose.pose.orientation.z = rot[2]
00040         poseOut.pose.pose.orientation.w = rot[3]
00041         poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
00042         poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
00043 
00044         print str(poseOut)
00045         pub.publish(poseOut)
00046         rate.sleep()