test_prior_tf_to_pose.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import tf
4 import numpy
5 import tf2_ros
6 from geometry_msgs.msg import PoseWithCovarianceStamped
7 
8 if __name__ == '__main__':
9  rospy.init_node('tf_to_pose', anonymous=True)
10  listener = tf.TransformListener()
11  frame = rospy.get_param('~frame', 'world')
12  childFrame = rospy.get_param('~child_frame', 'kinect_gt')
13  outputFrame = rospy.get_param('~output_frame', 'kinect')
14  cov = rospy.get_param('~cov', 1)
15  rateParam = rospy.get_param('~rate', 30) # 10hz
16  pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
17 
18  print 'start loop!'
19  rate = rospy.Rate(rateParam)
20  while not rospy.is_shutdown():
21  poseOut = PoseWithCovarianceStamped()
22  try:
23  now = rospy.get_rostime()
24  listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
25  (trans,rot) = listener.lookupTransform(frame, childFrame, now)
26  except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
27  print str(e)
28  rate.sleep()
29  continue
30 
31  poseOut.header.stamp.nsecs = now.nsecs
32  poseOut.header.stamp.secs = now.secs
33  poseOut.header.frame_id = outputFrame
34  poseOut.pose.pose.position.x = trans[0]
35  poseOut.pose.pose.position.y = trans[1]
36  poseOut.pose.pose.position.z = trans[2]
37  poseOut.pose.pose.orientation.x = rot[0]
38  poseOut.pose.pose.orientation.y = rot[1]
39  poseOut.pose.pose.orientation.z = rot[2]
40  poseOut.pose.pose.orientation.w = rot[3]
41  poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
42  poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
43 
44  print str(poseOut)
45  pub.publish(poseOut)
46  rate.sleep()


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40