From a735069ba9a94b39c1f058b76eac1ac2a3f1419c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 9 Feb 2018 16:42:48 -0500 Subject: [PATCH] Added example of using global_pose topic (edge prior, https://github.com/introlab/rtabmap/pull/196) --- launch/tests/test_prior.launch | 82 +++++++++++++++++++ .../tests/test_prior_rename_kinect_bag_tf.py | 19 +++++ launch/tests/test_prior_tf_to_pose.py | 46 +++++++++++ 3 files changed, 147 insertions(+) create mode 100644 launch/tests/test_prior.launch create mode 100644 launch/tests/test_prior_rename_kinect_bag_tf.py create mode 100755 launch/tests/test_prior_tf_to_pose.py diff --git a/launch/tests/test_prior.launch b/launch/tests/test_prior.launch new file mode 100644 index 00000000..fc98731a --- /dev/null +++ b/launch/tests/test_prior.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tests/test_prior_rename_kinect_bag_tf.py b/launch/tests/test_prior_rename_kinect_bag_tf.py new file mode 100644 index 00000000..e0aae871 --- /dev/null +++ b/launch/tests/test_prior_rename_kinect_bag_tf.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python +import rosbag +from tf.msg import tfMessage +with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag: + for topic, msg, t in rosbag.Bag('rgbd_dataset_freiburg3_long_office_household.bag').read_messages(): + if topic == "/tf" and msg.transforms: + newList = []; + for m in msg.transforms: + if m.child_frame_id != "/kinect": + newList.append(m) + else: + m.child_frame_id = "/kinect_gt" + newList.append(m) + print 'kinect frame renamed!' + if len(newList)>0: + msg.transforms = newList + outbag.write(topic, msg, t) + else: + outbag.write(topic, msg, t) diff --git a/launch/tests/test_prior_tf_to_pose.py b/launch/tests/test_prior_tf_to_pose.py new file mode 100755 index 00000000..126557af --- /dev/null +++ b/launch/tests/test_prior_tf_to_pose.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +import rospy +import tf +import numpy +import tf2_ros +from geometry_msgs.msg import PoseWithCovarianceStamped + +if __name__ == '__main__': + rospy.init_node('tf_to_pose', anonymous=True) + listener = tf.TransformListener() + frame = rospy.get_param('~frame', 'world') + childFrame = rospy.get_param('~child_frame', 'kinect_gt') + outputFrame = rospy.get_param('~output_frame', 'kinect') + cov = rospy.get_param('~cov', 1) + rateParam = rospy.get_param('~rate', 30) # 10hz + pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1) + + print 'start loop!' + rate = rospy.Rate(rateParam) + while not rospy.is_shutdown(): + poseOut = PoseWithCovarianceStamped() + try: + now = rospy.get_rostime() + listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033)) + (trans,rot) = listener.lookupTransform(frame, childFrame, now) + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e: + print str(e) + rate.sleep() + continue + + poseOut.header.stamp.nsecs = now.nsecs + poseOut.header.stamp.secs = now.secs + poseOut.header.frame_id = outputFrame + poseOut.pose.pose.position.x = trans[0] + poseOut.pose.pose.position.y = trans[1] + poseOut.pose.pose.position.z = trans[2] + poseOut.pose.pose.orientation.x = rot[0] + poseOut.pose.pose.orientation.y = rot[1] + poseOut.pose.pose.orientation.z = rot[2] + poseOut.pose.pose.orientation.w = rot[3] + poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist() + poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist] + + print str(poseOut) + pub.publish(poseOut) + rate.sleep()