From 8665017714847a9d5428e81c626eb896c81bd4bc Mon Sep 17 00:00:00 2001 From: xiachu Date: Fri, 20 Mar 2026 23:09:34 -0400 Subject: [PATCH 1/3] fix tf tree, now compatible with f1tenth_stack --- particle_filter/particle_filter.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index 0f68cb8..f657c20 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -243,24 +243,28 @@ def publish_tf(self, pose, stamp=None): t = TransformStamped() # header t.header.stamp = stamp - t.header.frame_id = '/map' - t.child_frame_id = '/laser' + t.header.frame_id = 'laser' + t.child_frame_id = 'map' # translation - t.transform.translation.x = pose[0] - t.transform.translation.y = pose[1] - t.transform.translation.z = 0.0 q = tf_transformations.quaternion_from_euler(0., 0., pose[2]) + inv_q = tf_transformations.quaternion_inverse(q) + t_quat = [-pose[0], -pose[1], 0.0, 0.0] + t_quat_rotated = tf_transformations.quaternion_multiply(tf_transformations.quaternion_multiply(inv_q, t_quat), q) + t.transform.translation.x = t_quat_rotated[0] + t.transform.translation.y = t_quat_rotated[1] + t.transform.translation.z = 0.0 # rotation - t.transform.rotation.x = q[0] - t.transform.rotation.y = q[1] - t.transform.rotation.z = q[2] - t.transform.rotation.w = q[3] + t.transform.rotation.x = inv_q[0] + t.transform.rotation.y = inv_q[1] + t.transform.rotation.z = inv_q[2] + t.transform.rotation.w = inv_q[3] self.pub_tf.sendTransform(t) # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header.stamp = self.get_clock().now().to_msg() - odom.header.frame_id = '/map' + odom.header.frame_id = 'map' + odom.child_frame_id = 'laser' odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) From 391d9c1c479cae711f172bbb4aecff88543c0632 Mon Sep 17 00:00:00 2001 From: xiachu Date: Tue, 24 Mar 2026 17:10:10 -0400 Subject: [PATCH 2/3] update setup.cfg --- setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index d0c7c95..6cad085 100755 --- a/setup.cfg +++ b/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/particle_filter +script_dir=$base/lib/particle_filter [install] -install-scripts=$base/lib/particle_filter +install_scripts=$base/lib/particle_filter From d69604cf7b4919e7646d1f21528d124599cba15f Mon Sep 17 00:00:00 2001 From: xiachu Date: Thu, 26 Mar 2026 12:42:55 -0400 Subject: [PATCH 3/3] fix timestamp of /pf/pose/odom --- particle_filter/particle_filter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index f657c20..1687778 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -262,7 +262,7 @@ def publish_tf(self, pose, stamp=None): # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() - odom.header.stamp = self.get_clock().now().to_msg() + odom.header.stamp = stamp odom.header.frame_id = 'map' odom.child_frame_id = 'laser' odom.pose.pose.position.x = pose[0]