From e11abd1fe889faf16556e05098be1bc70f1e10b6 Mon Sep 17 00:00:00 2001 From: Kevin Gmelin Date: Tue, 24 Nov 2020 12:07:16 -0500 Subject: [PATCH] Particle_filter.py: Added the option to publish a map to odom transform in order to make this particle filter use the same standard as the AMCL package and be compatible with ROS navigation libraries like move_base. --- src/particle_filter.py | 45 +++++++++++++++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/src/particle_filter.py b/src/particle_filter.py index 05082f8..c17cbf3 100755 --- a/src/particle_filter.py +++ b/src/particle_filter.py @@ -51,6 +51,8 @@ def __init__(self): self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.DO_VIZ = bool(rospy.get_param("~viz")) + self.PUBLISH_MAP_TO_ODOM = bool(rospy.get_param("~publish_map_to_odom", "0")) + self.TRANSFORM_TOLERANCE = float(rospy.get_param("~transform_tolerance", 0.5)) # sensor model constants self.Z_SHORT = float(rospy.get_param("~z_short", 0.01)) @@ -116,6 +118,9 @@ def __init__(self): # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() + if self.PUBLISH_MAP_TO_ODOM: + self.tf_listener = tf.TransformListener() + # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.lidarCB, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param("~odometry_topic", "/odom"), Odometry, self.odomCB, queue_size=1) @@ -170,8 +175,9 @@ def publish_tf(self,pose, stamp=None): stamp = rospy.Time.now() # this may cause issues with the TF tree. If so, see the below code. - self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), - stamp , "/laser", "/map") + if not self.PUBLISH_MAP_TO_ODOM: + self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), + stamp , "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: @@ -181,7 +187,40 @@ def publish_tf(self,pose, stamp=None): odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) self.odom_pub.publish(odom) - + + """ + Our particle filter provides estimates for the "laser" frame + since that is where our laser range estimates are measure from. Thus, + it provides the "map" -> "laser" transform. + + However, to make this compatible with libraries such as move_base + in the ROS navigation stack, we need to publish a "map" -> "odom" transform. + """ + + if self.PUBLISH_MAP_TO_ODOM: + # Get map -> laser transform. + map_laser_pos = np.array((pose[0], pose[1], 0)) + map_laser_rotation = np.array(tf.transformations.quaternion_from_euler(0, 0, pose[2])) + # Get laser -> odom transform. + t = self.tf_listener.getLatestCommonTime("/laser", "/odom") + laser_odom_pos, laser_odom_quaternion = self.tf_listener.lookupTransform("/laser", "/odom", t) + + # Apply laser -> odom transform to map -> laser transform + # This gives a map -> odom transform + map_laser_matrix = self.tf_listener.fromTranslationRotation(map_laser_pos, map_laser_rotation) + laser_odom_matrix = self.tf_listener.fromTranslationRotation(laser_odom_pos, laser_odom_quaternion) + map_odom_matrix = np.dot(map_laser_matrix, laser_odom_matrix) + + map_odom_pos = map_odom_matrix[:3, 3] + map_odom_rotation = tf.transformations.quaternion_from_matrix(map_odom_matrix) + + # Transform tolerance is the time with which to post-date the transform that is published + # to indicate that this transform is valid into the future + stamp = stamp + rospy.Duration.from_sec(self.TRANSFORM_TOLERANCE) + + # Publish transform + self.pub_tf.sendTransform(map_odom_pos, map_odom_rotation, stamp, "/odom", "/map") + return # below this line is disabled """