From 2de762519442d4b25f3e1dcf24e475e94ba88112 Mon Sep 17 00:00:00 2001 From: Boluwatife Olabiran Date: Mon, 2 Jun 2025 11:12:03 -0400 Subject: [PATCH 1/5] 1. Modified tf tree to conform to ROS standards to fix the pf node breaking tf trees. 2. Removed global namespaces, tf names and topics. --- config/localize.yaml | 14 +- particle_filter/particle_filter.py | 284 +++++++++++++++++++++++------ 2 files changed, 245 insertions(+), 53 deletions(-) diff --git a/config/localize.yaml b/config/localize.yaml index 1db77f6..ea2a5ba 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -1,8 +1,8 @@ particle_filter: ros__parameters: # topic names - scan_topic: '/scan' - odometry_topic: '/odom' + scan_topic: 'scan_filtered' # scan + odometry_topic: 'odometry/local' # odom, vehicle/vesc_odom # range data downsampling angle_step: 18 max_particles: 4000 @@ -30,6 +30,16 @@ particle_filter: # sensor model variant, variant 2 good for rmgpu, 3 doesn't work for rmgpu rangelib_variant: 2 + # Frame IDs + global_frame_id: 'map' + # odom_frame_id: '' + base_frame_id: 'base_link' + # laser_frame_id: '' + publish_map_to_odom: True + project_to_baselink: True + static_laser_to_base_link: True + transform_tolerance: 0.5 + map_server: ros__parameters: # assuming map file is in particle_filter/maps diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index 0f68cb8..5bd6ffc 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -1,3 +1,7 @@ +""" +Todo: get odom pose from topic if tf not present +""" + # MIT License # Copyright (c) 2020 Hongrui Zheng, Corey Walsh @@ -23,6 +27,7 @@ # ros2 python import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy # libraries import numpy as np @@ -34,7 +39,8 @@ # TF # import tf.transformations # import tf -from tf2_ros import TransformBroadcaster +from tf2_ros import Buffer, TransformListener, TransformBroadcaster +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException import tf_transformations # messages @@ -64,27 +70,35 @@ def __init__(self): super().__init__('particle_filter') # declare parameters - self.declare_parameter('angle_step') - self.declare_parameter('max_particles') - self.declare_parameter('max_viz_particles') - self.declare_parameter('squash_factor') - self.declare_parameter('max_range') - self.declare_parameter('theta_discretization') - self.declare_parameter('range_method') - self.declare_parameter('rangelib_variant') - self.declare_parameter('fine_timing') - self.declare_parameter('publish_odom') - self.declare_parameter('viz') - self.declare_parameter('z_short') - self.declare_parameter('z_max') - self.declare_parameter('z_rand') - self.declare_parameter('z_hit') - self.declare_parameter('sigma_hit') - self.declare_parameter('motion_dispersion_x') - self.declare_parameter('motion_dispersion_y') - self.declare_parameter('motion_dispersion_theta') - self.declare_parameter('scan_topic') - self.declare_parameter('odometry_topic') + self.declare_parameter('angle_step', 18) + self.declare_parameter('max_particles', 4000) + self.declare_parameter('max_viz_particles', 60) + self.declare_parameter('squash_factor', 2.2) + self.declare_parameter('max_range', 10) + self.declare_parameter('theta_discretization', 112) + self.declare_parameter('range_method', 'rmgpu') + self.declare_parameter('rangelib_variant', 2) + self.declare_parameter('fine_timing', 0) + self.declare_parameter('publish_odom', 1) + self.declare_parameter('viz', 1) + self.declare_parameter('z_short', 0.01) + self.declare_parameter('z_max', 0.07) + self.declare_parameter('z_rand', 0.12) + self.declare_parameter('z_hit', 0.75) + self.declare_parameter('sigma_hit', 8.0) + self.declare_parameter('motion_dispersion_x', 0.05) + self.declare_parameter('motion_dispersion_y', 0.025) + self.declare_parameter('motion_dispersion_theta', 0.25) + self.declare_parameter('global_frame_id', 'map') + # self.declare_parameter('odom_frame_id', '') # odom + self.declare_parameter('base_frame_id', 'base_link') # 'base_link', '' + # self.declare_parameter('laser_frame_id', '') # laser + self.declare_parameter('publish_map_to_odom', True) + self.declare_parameter('project_to_baselink', True) + self.declare_parameter('static_laser_to_base_link', True) + self.declare_parameter('transform_tolerance', 0.5) + self.declare_parameter('scan_topic', 'scan') + self.declare_parameter('odometry_topic', 'odom') # parameters self.ANGLE_STEP = self.get_parameter('angle_step').value @@ -110,6 +124,18 @@ def __init__(self): self.MOTION_DISPERSION_X = self.get_parameter('motion_dispersion_x').value self.MOTION_DISPERSION_Y = self.get_parameter('motion_dispersion_y').value self.MOTION_DISPERSION_THETA = self.get_parameter('motion_dispersion_theta').value + + # frame ids + self.GLOBAL_FRAME_ID = self.get_parameter('global_frame_id').value + self.ODOM_FRAME_ID = '' # self.get_parameter('odom_frame_id').value + self.BASE_FRAME_ID = self.get_parameter('base_frame_id').value + self.LASER_FRAME_ID = '' # self.get_parameter('laser_frame_id').value + self.PUBLISH_MAP_TO_ODOM = self.get_parameter('publish_map_to_odom').value + self.PROJECT_TO_BASELINK = self.get_parameter('project_to_baselink').value + self.STATIC_LASER_TO_BASE_LINK = self.get_parameter('static_laser_to_base_link').value + self.TRANSFORM_TOLERANCE = self.get_parameter('transform_tolerance').value + self.laser_to_base_link_tf = None + self.odom_pose = None # used to store odom pose received from message if tf is not available # various data containers used in the MCL algorithm self.MAX_RANGE_PX = None @@ -167,14 +193,16 @@ def __init__(self): self.odom_pub = self.create_publisher(Odometry, '/pf/pose/odom', 1) # these topics are for coordinate space things - self.pub_tf = TransformBroadcaster(self) + self.pub_tf = TransformBroadcaster(self) # tf broadcaster + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) # these topics are to receive data from the racecar self.laser_sub = self.create_subscription( LaserScan, self.get_parameter('scan_topic').value, self.lidarCB, - 1) + QoSProfile(depth=1, reliability=QoSReliabilityPolicy.BEST_EFFORT)) self.odom_sub = self.create_subscription( Odometry, self.get_parameter('odometry_topic').value, @@ -236,15 +264,17 @@ def get_omap(self): self.map_initialized = True def publish_tf(self, pose, stamp=None): - ''' Publish a tf for the car. This tells ROS where the car is with respect to the map. ''' - if stamp == None: - stamp = self.get_clock().now().to_msg() + """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """ + if stamp is None: + stamp = self.get_clock().now() + else: + stamp = rclpy.time.Time.from_msg(stamp) t = TransformStamped() # header - t.header.stamp = stamp - t.header.frame_id = '/map' - t.child_frame_id = '/laser' + t.header.stamp = stamp.to_msg() + t.header.frame_id = self.GLOBAL_FRAME_ID + t.child_frame_id = self.LASER_FRAME_ID # translation t.transform.translation.x = pose[0] t.transform.translation.y = pose[1] @@ -255,21 +285,168 @@ def publish_tf(self, pose, stamp=None): t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] - self.pub_tf.sendTransform(t) + + # Get map -> laser transform, i.e the pose from this PF node. + map_laser_pos = np.array((pose[0], pose[1], 0.0)) + map_laser_quat = tf_transformations.quaternion_from_euler(0, 0, pose[2]) + map_laser_rotation = np.array(map_laser_quat) + + map_laser_mat = tf_transformations.concatenate_matrices( + tf_transformations.translation_matrix(map_laser_pos), + tf_transformations.quaternion_matrix(map_laser_quat) + ) + + # # same as above but might be faster + # map_laser_matrix = tf_transformations.quaternion_matrix(map_laser_quat) + # map_laser_matrix[:3, 3] = map_laser_pos + + if not self.PUBLISH_MAP_TO_ODOM: + # Apply laser -> base_link transform to map -> laser transform + if self.PROJECT_TO_BASELINK and (self.BASE_FRAME_ID != self.LASER_FRAME_ID): + if (not self.STATIC_LASER_TO_BASE_LINK) or (self.laser_to_base_link_tf is None): + try: + tf_stamped = self.tf_buffer.lookup_transform( + self.BASE_FRAME_ID, + self.LASER_FRAME_ID, + rclpy.time.Time(), + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)) + laser_bl_pos = np.array([ + tf_stamped.transform.translation.x, + tf_stamped.transform.translation.y, + tf_stamped.transform.translation.z + ]) + laser_bl_quat = np.array([ + tf_stamped.transform.rotation.x, + tf_stamped.transform.rotation.y, + tf_stamped.transform.rotation.z, + tf_stamped.transform.rotation.w + ]) + + # Get map -> base_link transformation via matrix multiplication and inversion + laser_bl_mat = tf_transformations.concatenate_matrices( + tf_transformations.translation_matrix(laser_bl_pos), + tf_transformations.quaternion_matrix(laser_bl_quat) + ) + map_bl_mat = np.dot(map_laser_mat, laser_bl_mat) + + # Extract translation and rotation back from the combined map_bl_mat + map_bl_trans = tf_transformations.translation_from_matrix(map_bl_mat) + map_bl_quat = tf_transformations.quaternion_from_matrix(map_bl_mat) + + t.header.stamp = (stamp + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)).to_msg() + t.header.frame_id = self.GLOBAL_FRAME_ID + t.child_frame_id = self.BASE_FRAME_ID + t.transform.translation.x = float(map_bl_trans[0]) + t.transform.translation.y = float(map_bl_trans[1]) + t.transform.translation.z = float(map_bl_trans[2]) + t.transform.rotation.x = float(map_bl_quat[0]) + t.transform.rotation.y = float(map_bl_quat[1]) + t.transform.rotation.z = float(map_bl_quat[2]) + t.transform.rotation.w = float(map_bl_quat[3]) + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f'Failed to get laser→base_link: {e}') + + # publish the map -> (base_link or laser) transform + 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.pose.pose.position.x = pose[0] - odom.pose.pose.position.y = pose[1] - odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) + odom.header.stamp = stamp.to_msg() + odom.header.frame_id = self.GLOBAL_FRAME_ID + if self.PROJECT_TO_BASELINK and (self.BASE_FRAME_ID != self.LASER_FRAME_ID) and (not self.PUBLISH_MAP_TO_ODOM): + odom.child_frame_id = self.BASE_FRAME_ID + odom.pose.pose.position.x = float(map_bl_trans[0]) + odom.pose.pose.position.y = float(map_bl_trans[1]) + odom.pose.pose.position.z = float(map_bl_trans[2]) + odom.pose.pose.orientation.x = float(map_bl_quat[0]) + odom.pose.pose.orientation.y = float(map_bl_quat[1]) + odom.pose.pose.orientation.z = float(map_bl_quat[2]) + odom.pose.pose.orientation.w = float(map_bl_quat[3]) + else: + odom.child_frame_id = self.LASER_FRAME_ID + odom.pose.pose.position.x = pose[0] + odom.pose.pose.position.y = pose[1] + odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) + cov_mat = np.cov(self.particles, rowvar=False, ddof=0, aweights=self.weights).flatten() odom.pose.covariance[:cov_mat.shape[0]] = cov_mat odom.twist.twist.linear.x = self.current_speed self.odom_pub.publish(odom) - - return + + if self.PUBLISH_MAP_TO_ODOM: + """ + Our particle filter provides estimates for the "laser" frame + since that is where our laser range estimates are measured from. Thus, + we want to publish a "map" -> "laser" transform. + + However, the car's position is measured with respect to the "base_link" + frame (it is the root of the TF tree). Thus, we should actually define + a "map" -> "base_link" transform as to not break the TF tree. + """ + + # Lookup laser → odom transform + # Note: we could also get the current map->base_link transform by transforming the odom pose to the map frame + try: + # target_frame='odom', source_frame='laser' + trans = self.tf_buffer.lookup_transform( + self.ODOM_FRAME_ID, self.LASER_FRAME_ID, # works for laser -> (base_link) -> odom + rclpy.time.Time(), + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)) + laser_odom_pos = np.array([ + trans.transform.translation.x, + trans.transform.translation.y, + trans.transform.translation.z + ]) + laser_odom_quat = np.array([ + trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w + ]) + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f'Could not get laser→odom transform: {e}') + # get odom pose from message + if self.odom_pose is None: + self.pub_tf.sendTransform(t) + return + laser_odom_pos = np.array([ + self.odom_pose.position.x, + self.odom_pose.position.y, + self.odom_pose.position.z + ]) + laser_odom_quat = np.array([ + self.odom_pose.orientation.x, + self.odom_pose.orientation.y, + self.odom_pose.orientation.z, + self.odom_pose.orientation.w + ]) + + # Get map -> odom transformation via matrix multiplication and inversion + laser_odom_mat = tf_transformations.concatenate_matrices( + tf_transformations.translation_matrix(laser_odom_pos), + tf_transformations.quaternion_matrix(laser_odom_quat) + ) + map_odom_mat = np.dot(map_laser_mat, laser_odom_mat) + + map_odom_trans = tf_transformations.translation_from_matrix(map_odom_mat) + map_odom_quat = tf_transformations.quaternion_from_matrix(map_odom_mat) + + # Publish map -> odom transform + tfs = TransformStamped() + tfs.header.stamp = (stamp + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)).to_msg() + tfs.header.frame_id = self.GLOBAL_FRAME_ID + tfs.child_frame_id = self.ODOM_FRAME_ID + tfs.transform.translation.x = float(map_odom_trans[0]) + tfs.transform.translation.y = float(map_odom_trans[1]) + tfs.transform.translation.z = float(map_odom_trans[2]) + tfs.transform.rotation.x = float(map_odom_quat[0]) + tfs.transform.rotation.y = float(map_odom_quat[1]) + tfs.transform.rotation.z = float(map_odom_quat[2]) + tfs.transform.rotation.w = float(map_odom_quat[3]) + + # publish the transform + self.pub_tf.sendTransform(tfs) def visualize(self): ''' @@ -282,7 +459,7 @@ def visualize(self): # Publish the inferred pose for visualization ps = PoseStamped() ps.header.stamp = self.get_clock().now().to_msg() - ps.header.frame_id = '/map' + ps.header.frame_id = self.GLOBAL_FRAME_ID ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) @@ -310,7 +487,7 @@ def publish_particles(self, particles): # publish the given particles as a PoseArray object pa = PoseArray() pa.header.stamp = self.get_clock().now().to_msg() - pa.header.frame_id = '/map' + pa.header.frame_id = self.GLOBAL_FRAME_ID pa.poses = Utils.particles_to_poses(particles) self.particle_pub.publish(pa) @@ -318,19 +495,20 @@ def publish_scan(self, angles, ranges): # publish the given angels and ranges as a laser scan message ls = LaserScan() ls.header.stamp = self.last_stamp - ls.header.frame_id = '/laser' - ls.angle_min = np.min(angles) - ls.angle_max = np.max(angles) - ls.angle_increment = np.abs(angles[0] - angles[1]) - ls.range_min = 0 - ls.range_max = np.max(ranges) - ls.ranges = ranges + ls.header.frame_id = self.LASER_FRAME_ID + ls.angle_min = np.min(angles).astype(float) + ls.angle_max = np.max(angles).astype(float) + ls.angle_increment = np.abs(angles[0] - angles[1]).astype(float) + ls.range_min = 0.0 + ls.range_max = np.max(ranges).astype(float) + ls.ranges = ranges.tolist() self.pub_fake_scan.publish(ls) def lidarCB(self, msg): - ''' + """ Initializes reused buffers, and stores the relevant laser scanner data for later use. - ''' + """ + self.LASER_FRAME_ID = msg.header.frame_id if not isinstance(self.laser_angles, np.ndarray): self.get_logger().info('...Received first LiDAR message') self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) @@ -345,11 +523,15 @@ def lidarCB(self, msg): # self.update() def odomCB(self, msg): - ''' + """ Store deltas between consecutive odometry messages in the coordinate space of the car. Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own. - ''' + """ + self.BASE_FRAME_ID = msg.child_frame_id + self.ODOM_FRAME_ID = msg.header.frame_id + self.odom_pose = msg.pose.pose + position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y]) From 20f7fabb755ee2aaee4703d289ce37a44f3bb0f6 Mon Sep 17 00:00:00 2001 From: Boluwatife Olabiran Date: Wed, 27 May 2026 09:42:47 -0400 Subject: [PATCH 2/5] =?UTF-8?q?fix(tf):=20publish=20map=E2=86=92odom=20ins?= =?UTF-8?q?tead=20of=20map=E2=86=92laser;=20remove=20hardcoded=20frame=20n?= =?UTF-8?q?ames?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The particle filter was publishing a map→laser transform directly. On a robot with a complete TF tree (base_link → sensor_kit → laser), this created a cycle and broke ROS's TF tree — any node trying to resolve odom→laser or map→base_link would get a "would create a loop" error or incorrect poses. Changes in particle_filter/particle_filter.py ───────────────────────────────────────────── • TF output mode: when publish_map_to_odom=True (new default), the node computes map→odom by composing the PF's map→laser estimate with the live laser→odom TF from the robot's own tree. This is the nav-stack standard: the robot owns odom→base_link and the PF corrects drift by publishing map→odom. • Fallback: if the laser→odom TF lookup fails (e.g. on first startup), the node falls back to the odom pose received from the /odom topic message (stored as odom_pose) so it doesn't silently drop transforms. • Frame IDs are no longer hardcoded strings ('/map', '/laser'). global_frame_id, base_frame_id, odom_frame_id, and laser_frame_id are now ROS parameters, all with sensible defaults. The laser frame is additionally auto-detected from the incoming LaserScan header, and the odom/base frames are auto-detected from the Odometry message header — so the node adapts to the robot's actual TF tree without manual config in most cases. • project_to_baselink=True path: when not publishing map→odom, the node looks up laser→base_link, composes map→base_link, and publishes that instead of map→laser — correct for single-frame robots. • Static TF cache: static_laser_to_base_link=True caches the first successful laser→base_link lookup and reuses it on every cycle, saving one TF lookup per MCL tick. • LiDAR subscription QoS changed from depth-1 reliable to BEST_EFFORT (depth=1) to match real hardware publishers that use best-effort (required for YDLIDAR X4 and most lidar drivers). • LaserScan fields (angle_min, angle_max, angle_increment, range_max) are now explicitly cast to Python float / .tolist() before publishing, fixing a numpy scalar serialization error seen on some ROS 2 builds. • All published message frame_id fields updated to use the parameterized frame ID constants instead of hardcoded '/map', '/laser' strings. • Added tf2_ros Buffer + TransformListener for live TF lookups. • declare_parameter() calls now include default values so the node starts without a config file (useful for integration testing). Changes in config/localize.yaml ──────────────────────────────── • scan_topic updated to scan_filtered (actual filtered scan topic on the robot); odometry_topic updated to odometry/local (EKF output). Old values kept as inline comments for reference. • Added frame ID block: global_frame_id, base_frame_id, publish_map_to_odom, project_to_baselink, static_laser_to_base_link, transform_tolerance. odom_frame_id and laser_frame_id are commented out because they are auto-detected from incoming messages at runtime. --- README.md | 400 +++++++- config/localize.yaml | 69 +- config/ydlidar_x4.yaml | 29 + launch/localize_launch.py | 47 +- particle_filter/particle_filter.py | 1542 +++++++++++++++------------- particle_filter/utils.py | 270 ++++- 6 files changed, 1522 insertions(+), 835 deletions(-) create mode 100644 config/ydlidar_x4.yaml diff --git a/README.md b/README.md index e259e17..65d218e 100644 --- a/README.md +++ b/README.md @@ -1,65 +1,395 @@ # Particle Filter Localization -This code implements the MCL algorithm for the RACECAR. +Monte Carlo Localization (MCL) for F1tenth / Pacifica on **ROS 2 (Foxy/Humble)**. Fuses any `sensor_msgs/LaserScan`-compatible 2D LiDAR with wheel odometry to maintain a probability distribution (particle cloud) over robot pose (x, y, θ) on a pre-built occupancy grid. [![YouTube Demo](./media/thumb.jpg)](https://www.youtube.com/watch?v=-c_0hSjgLYw) -For high efficiency in Python, it uses Numpy arrays and [RangeLibc](https://github.com/f1tenth/range_libc) for fast 2D ray casting. +For high efficiency in Python it uses NumPy arrays and [RangeLibc](https://github.com/f1tenth/range_libc) for fast 2D ray casting. A GPU ray-casting backend (`rmgpu`) via CUDA is supported for maximum throughput on Jetson platforms. -# Installation +--- -To run this, you need to ensure that both the map_server ROS package, and the python wrappers for RangeLibc are installed. +## Table of Contents -For the map server: +- [Changes from upstream](#changes-from-upstream) +- [Installation](#installation) +- [Usage](#usage) +- [Configuration](#configuration) +- [Architecture](#architecture) +- [ROS Interface](#ros-interface) +- [TF Tree](#tf-tree) +- [Global Localization](#global-localization) +- [Ray-Casting Backends](#ray-casting-backends) +- [Sensor Model Variants](#sensor-model-variants) +- [Performance Notes](#performance-notes) +- [Future Work](#future-work) +- [Citation](#citation) + +--- + +## Changes from upstream + +This fork targets **real-robot deployment** on platforms with a complete ROS 2 TF tree +(e.g. Pacifica / Autonole). The upstream `foxy-devel` / `humble-devel` branches work in +simulation but have several issues that surface only on hardware. The changes below fix +those issues and make the node a drop-in replacement for any nav-stack-compatible robot. + +### Bug fixes + +#### TF tree corruption (critical) +The upstream node publishes a `map → laser` transform directly. On a robot whose URDF +already defines `odom → base_link → sensor_kit → laser`, this creates a cycle in the TF +tree and breaks every downstream node (`amcl`, `nav2`, `move_base`) that tries to look +up `odom → laser` or `map → base_link`. + +**Fix:** the node now publishes `map → odom` by composing its `map → laser` estimate +with the robot's own `laser → odom` TF chain. This is the nav-stack standard: the robot +owns `odom → base_link`; the PF corrects accumulated drift by publishing `map → odom`. +The old `map → laser` / `map → base_link` modes are still available via the +`publish_map_to_odom` parameter. + +#### Hardcoded frame names +The upstream code uses literal strings `'/map'` and `'/laser'` throughout — including +the leading `/` that ROS 2 deprecates. These must match your robot's URDF exactly or +every published message lands in the wrong frame. + +**Fix:** all frame names are now ROS parameters (`global_frame_id`, `base_frame_id`, +`odom_frame_id`). The laser frame is **auto-detected** from the incoming `LaserScan` +header, and the odom / base frames are **auto-detected** from the incoming `Odometry` +message, so the node adapts to any robot's TF tree without manual config changes. + +#### LiDAR subscription QoS mismatch +The upstream subscription uses `reliable` QoS. Most real LiDAR drivers (YDLIDAR, +Hokuyo, Velodyne) publish on `best_effort`. The mismatch silently drops every scan — +the node appears to run but never receives data. + +**Fix:** the LaserScan subscription now uses `BEST_EFFORT` QoS, matching standard +hardware drivers. + +#### NumPy scalar serialization in fake scan +`LaserScan.angle_min/max/angle_increment/range_max` were published as NumPy scalars. +On some ROS 2 / rcl_interfaces builds this raises a `TypeError` at serialize time, +crashing the visualization. + +**Fix:** all `LaserScan` fields are explicitly cast to Python `float` (scalar fields) +or `.tolist()` (range array) before publishing. + +#### Missing default parameter values +The upstream `declare_parameter()` calls have no defaults, so the node raises +`ParameterUninitializedException` and crashes if launched without a config file (e.g. +during integration testing or with a partial YAML overlay). + +**Fix:** all parameters have inline defaults matching `config/localize.yaml`. + +### Improvements + +#### TF lookup fallback +If the `laser → odom` TF lookup fails at startup (e.g. `static_transformations` is not +yet running), the node falls back to the pose from the latest `/odom` topic message +rather than silently dropping the transform broadcast. + +#### Static TF cache +When `static_laser_to_base_link: True` (default), the `laser → base_link` TF is looked +up once and cached for the lifetime of the node. This avoids one TF buffer query per +MCL tick (~40 Hz) and removes a potential source of jitter. + +#### Topic names updated for real hardware +Default `scan_topic` and `odometry_topic` are updated to `scan_filtered` and +`odometry/local` — the actual topics produced by the Autonole / Pacifica platform's +LiDAR pipeline and EKF. The original names (`scan`, `odom`) are kept as inline comments +for simulation use. + +--- + +## Installation + +### Dependencies + +**Map server (nav2):** +```bash +sudo apt update +rosdep update +source /opt/ros/${ROS_DISTRO}/setup.bash +rosdep install --from-paths src --ignore-src -r -y -q ``` -sudo apt-get update -rosdep install -r --from-paths src --ignore-src --rosdistro kinetic -y + +**[RangeLibc](https://github.com/f1tenth/range_libc) — required C++ ray-casting library with Python bindings:** +```bash +git clone https://github.com/f1tenth/range_libc.git -b humble-devel +cd range_libc && mkdir build && cd build +cmake -DCMAKE_BUILD_TYPE=Release .. +make -j$(nproc) && sudo make install +python3 -m pip install Cython==3.0.12 +cd ../pywrapper && WITH_CUDA=ON python3 setup.py install --user ``` -For [RangeLibc](https://github.com/f1tenth/range_libc): +> Drop `WITH_CUDA=ON` if you do not have a CUDA-capable GPU. The `rmgpu` backend will not be available, but all CPU backends still work. -``` -sudo pip install cython -git clone http://github.com/kctess5/range_libc -cd range_libc/pywrappers -# on VM -./compile.sh -# on car - compiles GPU ray casting methods -./compile_with_cuda.sh +### Build + +Run from the **ROS 2 workspace root** (not this package directory): +```bash +colcon build --packages-select particle_filter +source install/setup.bash ``` -# Usage +--- -The majority of parameters you might want to tweak are in the launch/localize.launch file. You may have to modify the "odometry_topic" or "scan_topic" parameters to match your environment. +## Usage +```bash +ros2 launch particle_filter localize_launch.py ``` -roslaunch particle_filter localize.launch + +Override the map at launch time: +```bash +ros2 launch particle_filter localize_launch.py map_name:=my_map ``` -Once the particle filter is running, you can visualize the map and other particle filter visualization message in RViz. Use the "2D Pose Estimate" tool from the RViz toolbar to initialize the particle locations. +Once running, open **RViz** to visualize the map, particle cloud, inferred pose, and fake scan. Use the **"2D Pose Estimate"** tool from the RViz toolbar to initialize particle locations. + +If `set_initial_pose: False` and no pose estimate is provided within `global_loc_timeout` seconds, the node automatically triggers hybrid global localization (see [Global Localization](#global-localization)). + +--- + +## Configuration + +All parameters live in `config/localize.yaml`. An example sensor-specific overlay is in `config/ydlidar_x4.yaml` (overrides only sensor model params for the YDLIDAR X4). + +### General parameters + +| Parameter | Default | Description | +| --- | --- | --- | +| `scan_topic` | `scan` | LiDAR scan topic | +| `odometry_topic` | `odom` | Odometry topic | +| `scan_qos_reliability` | `best_effort` | `best_effort` for real hardware; `reliable` for simulation | +| `max_particles` | `2000` | Number of particles; trades accuracy for CPU cost | +| `angle_step` | `18` | Subsample every Nth LiDAR beam before scoring | +| `range_method` | `rmgpu` | Ray-casting backend (see [Ray-Casting Backends](#ray-casting-backends)) | +| `rangelib_variant` | `2` | Sensor model computation path (see [Sensor Model Variants](#sensor-model-variants)) | +| `mcl_hz` | `40.0` | MCL timer rate (Hz); decoupled from odometry rate | +| `scan_max_age` | `0.5` | Skip MCL cycle if cached scan is older than this (seconds) | +| `viz_throttle` | `4` | Publish visualization every Nth MCL cycle | +| `seed` | `-1` | RNG seed; `>= 0` for reproducible runs, `-1` for random | +| `tf_broadcast` | `True` | Set `False` to suppress all `/tf` publishing | + +### Sensor model parameters + +These depend on your specific sensor. The values below are tuned for the YDLIDAR X4 and are provided as a starting point — adjust them to match your sensor's noise characteristics. + +| Parameter | Example value | Description | +| --- | --- | --- | +| `z_hit` | `0.75` | Probability of hitting the intended surface | +| `z_short` | `0.01` | Short-reading probability (e.g. crosstalk, glass) | +| `z_max` | `0.07` | Max-range miss probability | +| `z_rand` | `0.12` | Random noise return probability | +| `sigma_hit` | `4.0` | Sensor noise standard deviation **in pixels** | +| `lambda_short` | `0.05` | Exponential rate for the short-reading beam component (Thrun model) | +| `max_range` | `10.0` | Maximum valid range in metres | + +> **Pixel units for `sigma_hit`**: multiply your sensor's range accuracy (metres) by `1 / map_resolution`. For example, ±0.10 m at 0.05 m/px ≈ 2 px; `4.0` is a conservative starting value. + +### Motion model parameters + +| Parameter | Default | Description | +| --- | --- | --- | +| `motion_dispersion_x` | `0.05` | Per-step noise in forward direction | +| `motion_dispersion_y` | `0.025` | Per-step noise in lateral direction | +| `motion_dispersion_theta` | `0.25` | Per-step noise in heading | + +### Frame IDs + +| Parameter | Default | Description | +| --- | --- | --- | +| `global_frame_id` | `map` | Fixed map frame | +| `odom_frame_id` | `odom` | Odom frame; set to `''` to publish `map → base_link` directly | +| `base_frame_id` | `base_link` | Robot base frame | +| `static_laser_to_base_link` | `True` | Cache the `laser → base_link` TF once; avoids repeated lookups | + +--- + +## Architecture + +### MCL algorithm (one cycle of `_mcl_timer` at `mcl_hz` Hz) + +1. **Resample** — draw `max_particles` indices using systematic (low-variance) resampling weighted by particle weights +2. **Motion model** — apply accumulated odometry delta in car-local frame + Gaussian noise per axis +3. **Sensor model** — ray-cast each particle via RangeLibc; score against a precomputed `P(observed | true_range)` LUT +4. **Normalize** — divide weights by sum; compute `inferred_pose = Σ wᵢ · pᵢ` +5. **Publish** — broadcast TF (`map → odom` or `map → base_link`) and optional odometry topic +6. **Diagnostics** — compute effective particle count `N_eff = 1 / Σ wᵢ²`; warn if `N_eff < 10 % of max_particles` + +### Concurrency model + +MCL is **timer-driven**. `odomCB` accumulates odometry deltas between ticks (no MCL triggered from odometry). `lidarCB` caches the latest scan only. The node runs on a `SingleThreadedExecutor`. -See [launch/localize.launch](/particle_filter/launch/localize.launch) for docs on available parameters and arguments. +Two locks provide defensive concurrency: +- `state_lock` — guards the particle array and weights; MCL cycle is skipped (with a warning) if the lock cannot be acquired immediately +- `_odom_lock` — guards odometry accumulation and snapshot in `update()` -The "range_method" parameter determines which RangeLibc ray casting method to use. The default is cddt because it is fast and has a low initialization time. The fastest option on the CPU is "glt" but it has a slow startup. The fastest version if you have can compile RangeLibc with CUDA enabled is "rmgpu". See this performance comparison chart: +### Map setup + +Maps live in `maps/.yaml` + `maps/.pgm`. The launch file reads the default map name from `map_server.ros__parameters.map` in `localize.yaml` and falls back to `levine`. Override at launch time with `map_name:=`. + +--- + +## ROS Interface + +### Subscribed topics + +| Topic | Type | Description | +| --- | --- | --- | +| `scan` (configurable) | `sensor_msgs/LaserScan` | LiDAR scan input | +| `odom` (configurable) | `nav_msgs/Odometry` | Wheel odometry | +| `initialpose` | `geometry_msgs/PoseWithCovarianceStamped` | Manual pose initialization from RViz | +| `clicked_point` | `geometry_msgs/PointStamped` | Triggers hybrid global localization | + +### Published topics + +| Topic | Type | Description | +| --- | --- | --- | +| `pf/viz/inferred_pose` | `geometry_msgs/PoseStamped` | Best-estimate pose | +| `pf/viz/particles` | `geometry_msgs/PoseArray` | Full particle cloud | +| `pf/viz/fake_scan` | `sensor_msgs/LaserScan` | Ray-cast scan at inferred pose | +| `pf/pose/odom` | `nav_msgs/Odometry` | Estimated odometry (if `publish_odom: 1`) | +| `/tf` | — | `map → odom` or `map → base_link` | + +### Services + +| Service | Type | Description | +| --- | --- | --- | +| `global_localize` | `std_srvs/Empty` | Trigger hybrid global localization manually | + +```bash +ros2 service call /global_localize std_srvs/srv/Empty +``` + +--- + +## TF Tree + +`publish_tf()` selects the transform to broadcast based on which frame IDs are configured: + +| `odom_frame_id` | `base_frame_id` | Published transform | Notes | +| --- | --- | --- | --- | +| set (e.g. `odom`) | set | `map → odom` | **Nav-stack standard**; requires existing `odom → base_link` | +| `''` | set (e.g. `base_link`) | `map → base_link` | Uses cached static `laser → base_link` TF | +| `''` | `''` | `map → laser` | Fallback only; non-standard, breaks most nav stacks | + +The default config sets both frame IDs, so the node publishes the nav-stack-standard `map → odom` transform. The physical robot TF tree (`base_link → sensor_kit → lidar/IMU/wheels`) is published separately by `launch/static_transformations.launch.py`. + +--- + +## Global Localization + +When the robot pose is unknown (no `/initialpose` received), the node can locate itself automatically from the current laser scan using **hybrid global localization**: + +1. Build a coarse candidate grid over all free map cells at `global_loc_coarse_res` metre and `global_loc_theta_res` degree spacing +2. Score every candidate by ray-casting against the live scan using the existing sensor model LUT +3. Pick the top-K highest-scoring, spatially separated hypotheses (non-maximum suppression with `global_loc_min_dist` radius) +4. Seed `max_particles / K` particles around each hypothesis with small Gaussian noise; hand off to normal MCL + +### Triggers + +- **Automatic** — fires after `global_loc_timeout` seconds at startup if no `/initialpose` message arrives +- **RViz click** — click any point in the map (publishes to `clicked_point`) +- **ROS 2 service** — `ros2 service call /global_localize std_srvs/srv/Empty` + +### Parameters + +| Parameter | Default | Description | +| --- | --- | --- | +| `global_loc_coarse_res` | `0.3` | Metres between x/y candidates in coarse grid | +| `global_loc_theta_res` | `30.0` | Degrees between candidate orientations | +| `global_loc_top_k` | `3` | Number of best hypotheses to seed particles around | +| `global_loc_min_dist` | `1.0` | Minimum metres between chosen hypotheses | +| `global_loc_max_candidates` | `50000` | Safety cap; resolution is doubled automatically if exceeded | +| `global_loc_timeout` | `5.0` | Seconds to wait for `/initialpose` before auto-triggering | + +### Estimated cost (20 × 20 m indoor map, 0.05 m/px, 35 beams) + +| Backend | Approx. time | +| --- | --- | +| `cddt` (CPU) | ~0.3 – 0.8 s | +| `rmgpu` (GPU) | < 0.1 s | + +--- + +## Ray-Casting Backends + +Controlled by the `range_method` parameter: + +| Method | Description | +| --- | --- | +| `cddt` | **Default** — fast with low initialization time | +| `pcddt` | Pruned CDDT; slightly faster at runtime | +| `glt` | Giant LUT — fastest CPU option; slow startup | +| `rmgpu` | GPU ray marching via CUDA — fastest overall (requires CUDA build of RangeLibc) | +| `bl` | Bresenham's line — simplest; reference/educational use | ![Range Method Performance Comparison](./media/comparison.png) -# Docs +--- + +## Sensor Model Variants + +Controlled by the `rangelib_variant` parameter: + +| Value | Method | Notes | +| --- | --- | --- | +| 0 | Python loop over LUT | Slow; educational | +| 1 | `calc_range_many` + `eval_sensor_model` | Moderate | +| 2 | `calc_range_repeat_angles` + `eval_sensor_model` | **Default** — works with `rmgpu` | +| 3 | `calc_range_repeat_angles_eval_sensor_model` (one-shot) | **Does not work with `rmgpu`** | +| 4 | Radial CDDT optimization | CDDT / PCDDT only | + +--- + +## Performance Notes + +- **Executor**: `SingleThreadedExecutor` is used (switched from `MultiThreadedExecutor` after observing 446 % CPU on a Jetson Orin Nano). Do not re-introduce `MultiThreadedExecutor` without profiling first. +- **Resampling**: Systematic (low-variance) resampler replaces multinomial `np.random.choice`; O(N) cost with significantly reduced particle impoverishment. +- **Pre-allocation**: Working arrays are allocated at init time; MCL cycles use in-place NumPy operations to avoid per-cycle heap allocation. +- **Visualization throttle**: `viz_throttle: 4` publishes visualization every 4th MCL cycle (~10 Hz at 40 Hz MCL). +- **Scan staleness guard**: MCL cycle is skipped with a log warning if the cached scan is older than `scan_max_age` seconds. +- **N_eff diagnostic**: Effective particle count `N_eff = 1 / Σ wᵢ²` is logged every 10 cycles; a warning fires if it drops below 10 % of `max_particles`, indicating weight collapse. + +--- + +## Future Work + +### Near-term + +- **Automatic kidnapped-robot recovery** — the node already computes `N_eff = 1 / Σ wᵢ²` every 10 MCL cycles and logs a warning when it drops below 10 % of `max_particles`. The missing piece is acting on that signal: when `N_eff` falls below a configurable threshold, automatically invoke `_hybrid_global_localize()` to re-seed particles from scratch. The global localizer already exists and works; this only requires wiring the two together and adding a `kidnap_recovery_threshold` parameter (e.g. `0.05`). +- **Speed-dependent motion noise** — scale dispersion by vehicle speed from odometry; currently dispersion is constant per timestep +- **Dynamic parameter updates** — runtime tuning of `max_particles`, `motion_dispersion_*`, `z_hit`, `sigma_hit` via `add_on_set_parameters_callback` without restarting the node +- **QoS parameterization for non-scan topics** — `scan_qos_reliability` is implemented; `/odom` and other subscriptions use hardcoded QoS + +### Longer-term + +- **EDT / PyTorch ray marching backend** — replace RangeLibc with a Euclidean Distance Transform sphere-tracer implemented in PyTorch (`torch.vmap`), enabling GPU-accelerated localization without per-architecture CUDA compilation. The sensor model interface is already factored to support a backend swap without changing the MCL logic. + +### Cleanup + +- Remove the four vestigial `MutuallyExclusiveCallbackGroup`s left over from the `MultiThreadedExecutor` era (harmless but unused under `SingleThreadedExecutor`) + +--- -This code is the staff solution to the lab guide found in the [/docs](/particle_filter/docs) folder. A mathematical derivation of MCL is available in that guide. +## Docs -There is also documentation on RangeLibc in the [/docs](/particle_filter/docs) folder. +A mathematical derivation of MCL is in [docs/Lab5.pdf](docs/Lab5.pdf) along with [RangeLibc documentation](docs/RangeLibcUsageandInformation.pdf). -The code itself also contains comments describing purpose of each method. +--- -# Cite +## Citation -This library accompanies the following [publication](http://arxiv.org/abs/1705.01167). +This library accompanies the following [publication](https://arxiv.org/abs/1705.01167): - @article{walsh17, - author = {Corey Walsh and - Sertac Karaman}, - title = {CDDT: Fast Approximate 2D Ray Casting for Accelerated Localization}, - volume = {abs/1705.01167}, - url = {http://arxiv.org/abs/1705.01167}, - year = {2017}} +```bibtex +@article{walsh17, + author = {Corey Walsh and Sertac Karaman}, + title = {CDDT: Fast Approximate 2D Ray Casting for Accelerated Localization}, + volume = {abs/1705.01167}, + url = {https://arxiv.org/abs/1705.01167}, + year = {2017} +} +``` \ No newline at end of file diff --git a/config/localize.yaml b/config/localize.yaml index ea2a5ba..8728a6f 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -1,42 +1,75 @@ -particle_filter: +/**: ros__parameters: + set_initial_pose: True # True: use pose below, False: Use RViz 2D Pose Estimate + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 # topic names - scan_topic: 'scan_filtered' # scan - odometry_topic: 'odometry/local' # odom, vehicle/vesc_odom + scan_topic: 'scan' + odometry_topic: 'odom' + # scan QoS: 'best_effort' for real hardware, 'reliable' for simulation (P-9) + scan_qos_reliability: 'best_effort' # range data downsampling + # YDLIDAR X4: ~625 beams/rev; step=18 → ~35 beams per scan angle_step: 18 - max_particles: 4000 + max_particles: 2000 # reduced from 4000; halves all O(N) MCL work; validate N_eff in logs. 1500 (Autoware), 3000 (ForzaETH) squash_factor: 2.2 # visualization viz: 1 max_viz_particles: 60 + # publish viz every Nth MCL cycle; default 4 → ~10 Hz at 40 Hz MCL (P-6) + viz_throttle: 4 # ray marching method range_method: 'rmgpu' - theta_discretization: 112 - # range data filtering - max_range: 10 + theta_discretization: 112 # todo (test) 150 (ForzaETH) + # range data filtering — YDLIDAR X4 max range (m) + max_range: 10 # YDLIDAR X4 fine_timing: 0 publish_odom: 1 - # sensor model constants - z_short: 0.01 - z_max: 0.07 - z_rand: 0.12 - z_hit: 0.75 - sigma_hit: 8.0 + # MCL update rate in Hz; decoupled from odometry topic rate (P-2) + mcl_hz: 40.0 + # seconds; MCL skips cycle if scan is older than this (F-4) + scan_max_age: 0.5 + # sensor model constants (YDLIDAR X4 tuned values) + z_short: 0.01 # YDLIDAR X4: low glass/crosstalk return probability + z_max: 0.07 # YDLIDAR X4: occasional max-range misses + z_rand: 0.12 # YDLIDAR X4: random noise returns + z_hit: 0.75 # YDLIDAR X4: probability of hitting the intended target + # sigma_hit in pixels (map_resolution × metres); X4 ±2% at 5m ≈ 10cm ≈ 2px at 0.05m/px + sigma_hit: 4.0 # YDLIDAR X4 (pixels) + # rate parameter for the z_short exponential beam component (Thrun beam model) + lambda_short: 0.05 # YDLIDAR X4 # motion model dispersion constants motion_dispersion_x: 0.05 motion_dispersion_y: 0.025 - motion_dispersion_theta: 0.25 + motion_dispersion_theta: 0.25 # todo (test): 0.20 (f1tenth jax) # sensor model variant, variant 2 good for rmgpu, 3 doesn't work for rmgpu rangelib_variant: 2 + # -1 = random RNG; set >= 0 for reproducible particle divergence/convergence (E-7) + seed: -1 + + # Task 9: hybrid global localization + # meters between candidate x/y positions in coarse grid + global_loc_coarse_res: 0.3 + # degrees between candidate orientations + global_loc_theta_res: 30.0 + # number of best-scoring hypotheses to seed particles around + global_loc_top_k: 3 + # minimum meters between chosen hypotheses (prevents clustering) + global_loc_min_dist: 1.0 + # safety cap on candidate count; resolution is doubled if exceeded + global_loc_max_candidates: 50000 + # seconds to wait for /initialpose at startup before auto-triggering (G-7) + global_loc_timeout: 5.0 # Frame IDs + # TF mode: set odom_frame_id to publish map→odom (nav-stack standard). + # set to '' to publish map→base_link directly (no odom frame). global_frame_id: 'map' - # odom_frame_id: '' + odom_frame_id: 'odom' base_frame_id: 'base_link' - # laser_frame_id: '' - publish_map_to_odom: True - project_to_baselink: True static_laser_to_base_link: True transform_tolerance: 0.5 diff --git a/config/ydlidar_x4.yaml b/config/ydlidar_x4.yaml new file mode 100644 index 0000000..ad33964 --- /dev/null +++ b/config/ydlidar_x4.yaml @@ -0,0 +1,29 @@ +# YDLIDAR X4 sensor overlay +# Use as: ros2 launch particle_filter localize_launch.py params_file:=config/ydlidar_x4.yaml +# +# Sensor specs: 0.12–10 m range, ±2% accuracy, ~5000 Hz sample rate, +# ~625 beams at 8 Hz rotation, 360° FOV. +# +# This file overrides only sensor-model parameters. All other settings +# (frame IDs, MCL rate, map, etc.) are loaded from localize.yaml first. + +/**: + ros__parameters: + # YDLIDAR X4 max range (m) + max_range: 10 + + # step=18 → ~35 beams per scan at 625 beams/rev + angle_step: 18 + + # Beam model weights + z_hit: 0.75 # probability of hitting the intended target + z_rand: 0.12 # random noise returns + z_short: 0.01 # low glass/crosstalk return probability + z_max: 0.07 # occasional max-range misses + + # sigma_hit in pixels (map_resolution × metres) + # X4 ±2% at 5 m ≈ 10 cm; at 0.05 m/px → ~2 px; 4.0 is a conservative start + sigma_hit: 4.0 + + # exponential rate for the z_short beam component + lambda_short: 0.05 diff --git a/launch/localize_launch.py b/launch/localize_launch.py index dfed8d2..c3a49a2 100644 --- a/launch/localize_launch.py +++ b/launch/localize_launch.py @@ -22,44 +22,62 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch.actions import DeclareLaunchArgument from ament_index_python.packages import get_package_share_directory import os import yaml + def generate_launch_description(): - # config and args - localize_config = os.path.join( - get_package_share_directory('particle_filter'), - 'config', - 'localize.yaml' - ) - localize_config_dict = yaml.safe_load(open(localize_config, 'r')) - map_name = localize_config_dict['map_server']['ros__parameters']['map'] + pkg_share = get_package_share_directory('particle_filter') + localize_config = os.path.join(pkg_share, 'config', 'localize.yaml') + maps_dir = os.path.join(pkg_share, 'maps') + + try: + cfg = yaml.safe_load(open(localize_config, 'r')) + default_map = (cfg.get('map_server', {}) + .get('ros__parameters', {}) + .get('map', 'levine')) + except Exception: + default_map = 'levine' + localize_la = DeclareLaunchArgument( 'localize_config', default_value=localize_config, - description='Localization configs') - ld = LaunchDescription([localize_la]) + description='Path to localization config YAML') + map_name_la = DeclareLaunchArgument( + 'map_name', + default_value=default_map, + description='Map name (no extension) inside particle_filter/maps/; ' + 'overrides the map_server.ros__parameters.map entry in localize.yaml') + + ld = LaunchDescription([localize_la, map_name_la]) - # nodes pf_node = Node( package='particle_filter', executable='particle_filter', name='particle_filter', parameters=[LaunchConfiguration('localize_config')] ) + + # PythonExpression concatenates the maps directory (resolved at launch-file load + # time) with the overridable map_name launch argument and the .yaml suffix. + map_yaml_path = PythonExpression( + ["'", maps_dir + '/', "' + '", LaunchConfiguration('map_name'), "' + '.yaml'"] + ) + map_server_node = Node( package='nav2_map_server', executable='map_server', name='map_server', - parameters=[{'yaml_filename': os.path.join(get_package_share_directory('particle_filter'), 'maps', map_name + '.yaml')}, + parameters=[{'yaml_filename': map_yaml_path}, {'topic': 'map'}, {'frame_id': 'map'}, {'output': 'screen'}, {'use_sim_time': True}] ) + nav_lifecycle_node = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -70,9 +88,8 @@ def generate_launch_description(): {'node_names': ['map_server']}] ) - # finalize ld.add_action(nav_lifecycle_node) ld.add_action(map_server_node) ld.add_action(pf_node) - return ld \ No newline at end of file + return ld diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index 5bd6ffc..8cfeb93 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -1,10 +1,6 @@ -""" -Todo: get odom pose from topic if tf not present -""" - # MIT License -# Copyright (c) 2020 Hongrui Zheng, Corey Walsh +# Copyright (c) 2025 Boluwatife Olabiran # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the 'Software'), to deal @@ -24,36 +20,34 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -# ros2 python -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +import time +from threading import Lock -# libraries import numpy as np import range_libc -import time -from threading import Lock -from particle_filter import utils as Utils +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy +from rclpy.executors import SingleThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -# TF -# import tf.transformations -# import tf +from std_srvs.srv import Empty from tf2_ros import Buffer, TransformListener, TransformBroadcaster from tf2_ros import LookupException, ConnectivityException, ExtrapolationException -import tf_transformations -# messages -from std_msgs.msg import String, Header, Float32MultiArray from sensor_msgs.msg import LaserScan -from visualization_msgs.msg import Marker -from geometry_msgs.msg import Point, Pose, PoseStamped, PoseArray, Quaternion, PolygonStamped, Polygon, Point32, PoseWithCovarianceStamped, PointStamped, TransformStamped +from geometry_msgs.msg import (PoseStamped, PoseWithCovarianceStamped, PoseArray, + Quaternion, PolygonStamped, PointStamped) from nav_msgs.msg import Odometry from nav_msgs.srv import GetMap -''' -These flags indicate several variants of the sensor model. Only one of them is used at a time. -''' +from particle_filter import utils as Utils +from particle_filter.utils import ( + quat_trans_to_matrix, quaternion_from_euler, pose_msg_to_matrix, + transformstamped_to_matrix, quaternion_from_matrix, matrix_to_transformstamped, + cov_mat_to_list, +) + VAR_NO_EVAL_SENSOR_MODEL = 0 VAR_CALC_RANGE_MANY_EVAL_SENSOR = 1 VAR_REPEAT_ANGLES_EVAL_SENSOR = 2 @@ -61,15 +55,13 @@ VAR_RADIAL_CDDT_OPTIMIZATIONS = 4 -class ParticleFiler(Node): - ''' - This class implements Monte Carlo Localization based on odometry and a laser scanner. - ''' +class ParticleFilter(Node): + """Monte Carlo Localization fusing 2D LiDAR with wheel odometry.""" def __init__(self): super().__init__('particle_filter') - # declare parameters + # ── parameters ──────────────────────────────────────────────────────── self.declare_parameter('angle_step', 18) self.declare_parameter('max_particles', 4000) self.declare_parameter('max_viz_particles', 60) @@ -81,156 +73,286 @@ def __init__(self): self.declare_parameter('fine_timing', 0) self.declare_parameter('publish_odom', 1) self.declare_parameter('viz', 1) + self.declare_parameter('viz_throttle', 4) self.declare_parameter('z_short', 0.01) self.declare_parameter('z_max', 0.07) self.declare_parameter('z_rand', 0.12) self.declare_parameter('z_hit', 0.75) - self.declare_parameter('sigma_hit', 8.0) + self.declare_parameter('sigma_hit', 4.0) + self.declare_parameter('lambda_short', 0.05) self.declare_parameter('motion_dispersion_x', 0.05) self.declare_parameter('motion_dispersion_y', 0.025) self.declare_parameter('motion_dispersion_theta', 0.25) + self.declare_parameter('scan_topic', 'scan') + self.declare_parameter('odometry_topic', 'odom') + self.declare_parameter('scan_qos_reliability', 'best_effort') + self.declare_parameter('scan_max_age', 0.5) + self.declare_parameter('mcl_hz', 40.0) self.declare_parameter('global_frame_id', 'map') - # self.declare_parameter('odom_frame_id', '') # odom - self.declare_parameter('base_frame_id', 'base_link') # 'base_link', '' - # self.declare_parameter('laser_frame_id', '') # laser - self.declare_parameter('publish_map_to_odom', True) - self.declare_parameter('project_to_baselink', True) + self.declare_parameter('odom_frame_id', '') + self.declare_parameter('base_frame_id', 'base_link') self.declare_parameter('static_laser_to_base_link', True) self.declare_parameter('transform_tolerance', 0.5) - self.declare_parameter('scan_topic', 'scan') - self.declare_parameter('odometry_topic', 'odom') - - # parameters - self.ANGLE_STEP = self.get_parameter('angle_step').value - self.MAX_PARTICLES = self.get_parameter('max_particles').value - self.MAX_VIZ_PARTICLES = self.get_parameter('max_viz_particles').value - self.INV_SQUASH_FACTOR = 1.0 / self.get_parameter('squash_factor').value - self.MAX_RANGE_METERS = self.get_parameter('max_range').value + self.declare_parameter('tf_broadcast', True) + self.declare_parameter('set_initial_pose', False) + self.declare_parameter('initial_pose.x', 0.0) + self.declare_parameter('initial_pose.y', 0.0) + self.declare_parameter('initial_pose.z', 0.0) + self.declare_parameter('initial_pose.yaw', 0.0) + self.declare_parameter('seed', -1) + # Task 9: hybrid global localization + self.declare_parameter('global_loc_coarse_res', 0.3) + self.declare_parameter('global_loc_theta_res', 30.0) + self.declare_parameter('global_loc_top_k', 3) + self.declare_parameter('global_loc_min_dist', 1.0) + self.declare_parameter('global_loc_max_candidates', 50000) + self.declare_parameter('global_loc_timeout', 5.0) + + self.ANGLE_STEP = self.get_parameter('angle_step').value + self.MAX_PARTICLES = self.get_parameter('max_particles').value + self.MAX_VIZ_PARTICLES = self.get_parameter('max_viz_particles').value + self.INV_SQUASH_FACTOR = 1.0 / self.get_parameter('squash_factor').value + self.MAX_RANGE_METERS = self.get_parameter('max_range').value self.THETA_DISCRETIZATION = self.get_parameter('theta_discretization').value - self.WHICH_RM = self.get_parameter('range_method').value - self.RANGELIB_VAR = self.get_parameter('rangelib_variant').value - self.SHOW_FINE_TIMING = self.get_parameter('fine_timing').value - self.PUBLISH_ODOM = self.get_parameter('publish_odom').value - self.DO_VIZ = self.get_parameter('viz').value - - # sensor model constants - self.Z_SHORT = self.get_parameter('z_short').value - self.Z_MAX = self.get_parameter('z_max').value - self.Z_RAND = self.get_parameter('z_rand').value - self.Z_HIT = self.get_parameter('z_hit').value + self.WHICH_RM = self.get_parameter('range_method').value + self.RANGELIB_VAR = self.get_parameter('rangelib_variant').value + self.SHOW_FINE_TIMING = self.get_parameter('fine_timing').value + self.PUBLISH_ODOM = self.get_parameter('publish_odom').value + self.DO_VIZ = self.get_parameter('viz').value + self.VIZ_THROTTLE = self.get_parameter('viz_throttle').value + self.Z_SHORT = self.get_parameter('z_short').value + self.Z_MAX = self.get_parameter('z_max').value + self.Z_RAND = self.get_parameter('z_rand').value + self.Z_HIT = self.get_parameter('z_hit').value self.SIGMA_HIT = self.get_parameter('sigma_hit').value - - # motion model constants - self.MOTION_DISPERSION_X = self.get_parameter('motion_dispersion_x').value - self.MOTION_DISPERSION_Y = self.get_parameter('motion_dispersion_y').value + self.LAMBDA_SHORT = self.get_parameter('lambda_short').value + self.MOTION_DISPERSION_X = self.get_parameter('motion_dispersion_x').value + self.MOTION_DISPERSION_Y = self.get_parameter('motion_dispersion_y').value self.MOTION_DISPERSION_THETA = self.get_parameter('motion_dispersion_theta').value - - # frame ids - self.GLOBAL_FRAME_ID = self.get_parameter('global_frame_id').value - self.ODOM_FRAME_ID = '' # self.get_parameter('odom_frame_id').value - self.BASE_FRAME_ID = self.get_parameter('base_frame_id').value - self.LASER_FRAME_ID = '' # self.get_parameter('laser_frame_id').value - self.PUBLISH_MAP_TO_ODOM = self.get_parameter('publish_map_to_odom').value - self.PROJECT_TO_BASELINK = self.get_parameter('project_to_baselink').value + self.GLOBAL_FRAME_ID = self.get_parameter('global_frame_id').value + self.ODOM_FRAME_ID = self.get_parameter('odom_frame_id').value + self.BASE_FRAME_ID = self.get_parameter('base_frame_id').value + self.LASER_FRAME_ID = '' self.STATIC_LASER_TO_BASE_LINK = self.get_parameter('static_laser_to_base_link').value self.TRANSFORM_TOLERANCE = self.get_parameter('transform_tolerance').value - self.laser_to_base_link_tf = None - self.odom_pose = None # used to store odom pose received from message if tf is not available - - # various data containers used in the MCL algorithm + self.tf_broadcast = self.get_parameter('tf_broadcast').value + self.set_initial_pose = self.get_parameter('set_initial_pose').value + self.SCAN_MAX_AGE_SEC = self.get_parameter('scan_max_age').value + self.MCL_HZ = self.get_parameter('mcl_hz').value + scan_qos_rel_str = self.get_parameter('scan_qos_reliability').value + self.GLOBAL_LOC_COARSE_RES = self.get_parameter('global_loc_coarse_res').value + self.GLOBAL_LOC_THETA_RES = self.get_parameter('global_loc_theta_res').value + self.GLOBAL_LOC_TOP_K = self.get_parameter('global_loc_top_k').value + self.GLOBAL_LOC_MIN_DIST = self.get_parameter('global_loc_min_dist').value + self.GLOBAL_LOC_MAX_CANDIDATES = self.get_parameter('global_loc_max_candidates').value + self.GLOBAL_LOC_TIMEOUT = self.get_parameter('global_loc_timeout').value + + # E-7: reproducible RNG seed + seed = self.get_parameter('seed').value + if seed >= 0: + np.random.seed(seed) # legacy API: covers resampling, particle init, viz + # Fix 4: independent Generator for motion noise (supports out= parameter) + self._rng = np.random.default_rng(seed if seed >= 0 else None) + + # ── callback groups (P-1) ───────────────────────────────────────────── + self._lidar_group = MutuallyExclusiveCallbackGroup() + self._odom_group = MutuallyExclusiveCallbackGroup() + self._click_group = MutuallyExclusiveCallbackGroup() + self._mcl_group = MutuallyExclusiveCallbackGroup() + + # ── MCL state ───────────────────────────────────────────────────────── self.MAX_RANGE_PX = None - self.odometry_data = np.array([0.0, 0.0, 0.0]) - self.laser = None self.iters = 0 self.map_info = None + self.permissible_region = None self.map_initialized = False self.lidar_initialized = False self.odom_initialized = False - self.last_pose = None self.laser_angles = None self.downsampled_angles = None + self.downsampled_ranges = None self.range_method = None - self.last_time = None self.last_stamp = None self.first_sensor_update = True + self.last_pose = None + self.odom_pose = None + self.current_speed = 0.0 + self.cov_3x3 = np.zeros((3, 3)) + + # F-3: readiness flags; MCL timer checks all before running + self._map_ready = False + self._laser_ready = False + self._pose_inited = False + self._warned_not_ready = False + + # F-4: scan staleness tracking + self._last_scan_stamp = None + + # E-5: range_min populated from each scan message + self._range_min = 0.0 + + # P-6: viz throttle counter + self._viz_counter = 0 + + # E-3: N_eff iteration counter + self._n_eff_iter = 0 + + # cached transforms (static) + self.base_frame_to_scan_tf = None # T_base_scan + self.laser_to_base_frame_tf = None # T_scan_base = inv(T_base_scan) + + # reused homogeneous matrix for map→scan estimate (avoids per-cycle alloc) + self.T_map_to_scan = np.eye(4, dtype=np.float64) + self.state_lock = Lock() + self._odom_lock = Lock() - # cache this to avoid memory allocation in motion model + # pre-allocated buffers for motion model and systematic resampling (P-4) self.local_deltas = np.zeros((self.MAX_PARTICLES, 3)) + self._proposal = np.zeros((self.MAX_PARTICLES, 3)) + + # Fix 4: pre-allocated noise buffer; filled via rng.standard_normal(out=) each cycle + self._noise = np.empty((self.MAX_PARTICLES, 3), dtype=np.float64) + self._noise_scale = np.array( + [self.MOTION_DISPERSION_X, self.MOTION_DISPERSION_Y, self.MOTION_DISPERSION_THETA], + dtype=np.float64) + + # Fix 3: pre-allocated SE(3) inverse buffer; avoids LAPACK call in publish_tf + self._T_odom_scan_inv = np.eye(4, dtype=np.float64) + + # accumulated odometry delta; reset to zero after each MCL tick (P-2) + self.odometry_data = np.zeros(3) - # cache this for the sensor model computation + # sensor-model buffers (allocated on first update, Step 2) self.queries = None self.ranges = None self.tiled_angles = None self.sensor_model_table = None + self._sensor_model_dispatch = None - # particle poses and weights self.inferred_pose = None self.particle_indices = np.arange(self.MAX_PARTICLES) self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) - # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) - # map service client - self.map_client = self.create_client(GetMap, '/map_server/map') - self.get_omap() - self.precompute_sensor_model() - self.initialize_global() - - # keep track of speed from input odom - self.current_speed = 0.0 - # Pub Subs - # these topics are for visualization - self.pose_pub = self.create_publisher(PoseStamped, '/pf/viz/inferred_pose', 1) - self.particle_pub = self.create_publisher(PoseArray, '/pf/viz/particles', 1) - self.pub_fake_scan = self.create_publisher(LaserScan, '/pf/viz/fake_scan', 1) - self.rect_pub = self.create_publisher(PolygonStamped, '/pf/viz/poly1', 1) + # G-8: global-loc range buffer; deferred to lidarCB (downsampled_angles not yet known) + self._global_ranges_buf = None + # G-5: set when uniform-spread fallback ran before scan arrived; cleared on first scan + self._pending_global_loc = False + # ── startup sequence ─────────────────────────────────────────────────── + self.map_client = self.create_client(GetMap, 'map_server/map') + self.get_omap() + self.precompute_sensor_model() + # Step 1: unified init; scan not ready yet → uniform spread + _pending_global_loc + self._reset_particles() + + if self.set_initial_pose: + q = quaternion_from_euler( + 0.0, 0.0, self.get_parameter('initial_pose.yaw').value) + ip = PoseWithCovarianceStamped() + ip.header.frame_id = self.GLOBAL_FRAME_ID + ip.header.stamp = self.get_clock().now().to_msg() + ip.pose.pose.position.x = self.get_parameter('initial_pose.x').value + ip.pose.pose.position.y = self.get_parameter('initial_pose.y').value + ip.pose.pose.position.z = self.get_parameter('initial_pose.z').value + ip.pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + self._reset_particles(ip.pose.pose) + + # ── publishers ──────────────────────────────────────────────────────── + self.pose_pub = self.create_publisher(PoseStamped, 'pf/viz/inferred_pose', 1) + self.particle_pub = self.create_publisher(PoseArray, 'pf/viz/particles', 1) + self.pub_fake_scan = self.create_publisher(LaserScan, 'pf/viz/fake_scan', 1) + self.rect_pub = self.create_publisher(PolygonStamped, 'pf/viz/poly1', 1) if self.PUBLISH_ODOM: - self.odom_pub = self.create_publisher(Odometry, '/pf/pose/odom', 1) + self.odom_pub = self.create_publisher(Odometry, 'pf/pose/odom', 1) - # these topics are for coordinate space things - self.pub_tf = TransformBroadcaster(self) # tf broadcaster + self.pub_tf = TransformBroadcaster(self) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - # these topics are to receive data from the racecar + # ── scan QoS (P-9) ──────────────────────────────────────────────────── + qos_rel = (QoSReliabilityPolicy.BEST_EFFORT + if scan_qos_rel_str.lower() == 'best_effort' + else QoSReliabilityPolicy.RELIABLE) + + # ── subscribers ─────────────────────────────────────────────────────── self.laser_sub = self.create_subscription( LaserScan, self.get_parameter('scan_topic').value, self.lidarCB, - QoSProfile(depth=1, reliability=QoSReliabilityPolicy.BEST_EFFORT)) + QoSProfile(depth=1, reliability=qos_rel), + callback_group=self._lidar_group) + # Fix 1: depth=10 so odom messages queued during ~25 ms MCL cycle are not dropped self.odom_sub = self.create_subscription( Odometry, self.get_parameter('odometry_topic').value, self.odomCB, - 1) + 10, + callback_group=self._odom_group) self.pose_sub = self.create_subscription( - PoseWithCovarianceStamped, - '/initialpose', - self.clicked_pose, - 1) + PoseWithCovarianceStamped, 'initialpose', self.clicked_pose, 1, + callback_group=self._click_group) self.click_sub = self.create_subscription( - PointStamped, - '/clicked_point', - self.clicked_pose, - 1) + PointStamped, 'clicked_point', self.clicked_pose, 1, + callback_group=self._click_group) + + # ── G-6: global localization service ────────────────────────────────── + self._global_loc_srv = self.create_service( + Empty, 'global_localize', self._global_localize_srv_cb, + callback_group=self._click_group) + + # ── MCL timer (P-2) ─────────────────────────────────────────────────── + self._mcl_timer = self.create_timer( + 1.0 / self.MCL_HZ, self._mcl_timer_cb, + callback_group=self._mcl_group) + + # ── G-7: one-shot startup timer; fires if no pose init within timeout ─ + if not self.set_initial_pose: + self._startup_timer = self.create_timer( + self.GLOBAL_LOC_TIMEOUT, self._startup_global_loc_cb, + callback_group=self._click_group) self.get_logger().info('Finished initializing, waiting on messages...') - def get_omap(self): - ''' - Fetch the occupancy grid map from the map_server instance, and initialize the correct - RangeLibc method. Also stores a matrix which indicates the permissible region of the map - ''' + # ── helpers ─────────────────────────────────────────────────────────────── + + def _get_stamp(self, stamp=None): + if stamp is None: + return self.get_clock().now().to_msg() + return stamp + + def _lookup_tf(self, source_frame, target_frame, timestamp=None, timeout=0.05): + if timestamp is None: + timestamp = rclpy.time.Time() + try: + return self.tf_buffer.lookup_transform( + target_frame, source_frame, timestamp, + rclpy.duration.Duration(seconds=timeout)) + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f'TF {source_frame}→{target_frame}: {e}') + return None + + def _cache_base_to_scan_tf(self, timestamp=None): + """Look up base_link → laser and cache; skips if already cached and static (P-3).""" + if self.base_frame_to_scan_tf is not None and self.STATIC_LASER_TO_BASE_LINK: + return + tf = self._lookup_tf(self.BASE_FRAME_ID, self.LASER_FRAME_ID, + timestamp, self.TRANSFORM_TOLERANCE) + if tf is not None: + self.base_frame_to_scan_tf = transformstamped_to_matrix(tf) + self.laser_to_base_frame_tf = np.linalg.inv(self.base_frame_to_scan_tf) + + # ── map loading ─────────────────────────────────────────────────────────── + def get_omap(self): + """Fetch the occupancy grid from map_server and initialise the RangeLibc backend.""" while not self.map_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Get map service not available, waiting...') - req = GetMap.Request() - future = self.map_client.call_async(req) + self.get_logger().info('Waiting for map_server/map service...') + future = self.map_client.call_async(GetMap.Request()) rclpy.spin_until_future_complete(self, future) map_msg = future.result().map self.map_info = map_msg.info @@ -238,671 +360,663 @@ def get_omap(self): oMap = range_libc.PyOMap(map_msg) self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution) - # initialize range method - self.get_logger().info('Initializing range method: ' + self.WHICH_RM) + self.get_logger().info(f'Initialising range method: {self.WHICH_RM}') if self.WHICH_RM == 'bl': self.range_method = range_libc.PyBresenhamsLine(oMap, self.MAX_RANGE_PX) elif 'cddt' in self.WHICH_RM: - self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) + self.range_method = range_libc.PyCDDTCast( + oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if self.WHICH_RM == 'pcddt': - self.get_logger().info('Pruning...') + self.get_logger().info('Pruning CDDT...') self.range_method.prune() elif self.WHICH_RM == 'rm': self.range_method = range_libc.PyRayMarching(oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == 'rmgpu': self.range_method = range_libc.PyRayMarchingGPU(oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == 'glt': - self.range_method = range_libc.PyGiantLUTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) - self.get_logger().info('Done loading map') - - # 0: permissible, -1: unmapped, 100: blocked - array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) + self.range_method = range_libc.PyGiantLUTCast( + oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) + self.get_logger().info('Map loaded') - # 0: not permissible, 1: permissible - self.permissible_region = np.zeros_like(array_255, dtype=bool) - self.permissible_region[array_255==0] = 1 + array_255 = np.array(map_msg.data).reshape( + (map_msg.info.height, map_msg.info.width)) + self.permissible_region = (array_255 == 0) self.map_initialized = True + self._map_ready = True - def publish_tf(self, pose, stamp=None): - """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """ - if stamp is None: - stamp = self.get_clock().now() - else: - stamp = rclpy.time.Time.from_msg(stamp) + # ── sensor model ────────────────────────────────────────────────────────── - t = TransformStamped() - # header - t.header.stamp = stamp.to_msg() - t.header.frame_id = self.GLOBAL_FRAME_ID - t.child_frame_id = self.LASER_FRAME_ID - # 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]) - # 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] - - # Get map -> laser transform, i.e the pose from this PF node. - map_laser_pos = np.array((pose[0], pose[1], 0.0)) - map_laser_quat = tf_transformations.quaternion_from_euler(0, 0, pose[2]) - map_laser_rotation = np.array(map_laser_quat) - - map_laser_mat = tf_transformations.concatenate_matrices( - tf_transformations.translation_matrix(map_laser_pos), - tf_transformations.quaternion_matrix(map_laser_quat) - ) - - # # same as above but might be faster - # map_laser_matrix = tf_transformations.quaternion_matrix(map_laser_quat) - # map_laser_matrix[:3, 3] = map_laser_pos - - if not self.PUBLISH_MAP_TO_ODOM: - # Apply laser -> base_link transform to map -> laser transform - if self.PROJECT_TO_BASELINK and (self.BASE_FRAME_ID != self.LASER_FRAME_ID): - if (not self.STATIC_LASER_TO_BASE_LINK) or (self.laser_to_base_link_tf is None): - try: - tf_stamped = self.tf_buffer.lookup_transform( - self.BASE_FRAME_ID, - self.LASER_FRAME_ID, - rclpy.time.Time(), - rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)) - laser_bl_pos = np.array([ - tf_stamped.transform.translation.x, - tf_stamped.transform.translation.y, - tf_stamped.transform.translation.z - ]) - laser_bl_quat = np.array([ - tf_stamped.transform.rotation.x, - tf_stamped.transform.rotation.y, - tf_stamped.transform.rotation.z, - tf_stamped.transform.rotation.w - ]) - - # Get map -> base_link transformation via matrix multiplication and inversion - laser_bl_mat = tf_transformations.concatenate_matrices( - tf_transformations.translation_matrix(laser_bl_pos), - tf_transformations.quaternion_matrix(laser_bl_quat) - ) - map_bl_mat = np.dot(map_laser_mat, laser_bl_mat) - - # Extract translation and rotation back from the combined map_bl_mat - map_bl_trans = tf_transformations.translation_from_matrix(map_bl_mat) - map_bl_quat = tf_transformations.quaternion_from_matrix(map_bl_mat) - - t.header.stamp = (stamp + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)).to_msg() - t.header.frame_id = self.GLOBAL_FRAME_ID - t.child_frame_id = self.BASE_FRAME_ID - t.transform.translation.x = float(map_bl_trans[0]) - t.transform.translation.y = float(map_bl_trans[1]) - t.transform.translation.z = float(map_bl_trans[2]) - t.transform.rotation.x = float(map_bl_quat[0]) - t.transform.rotation.y = float(map_bl_quat[1]) - t.transform.rotation.z = float(map_bl_quat[2]) - t.transform.rotation.w = float(map_bl_quat[3]) - except (LookupException, ConnectivityException, ExtrapolationException) as e: - self.get_logger().warn(f'Failed to get laser→base_link: {e}') - - # publish the map -> (base_link or laser) transform - self.pub_tf.sendTransform(t) - - # also publish odometry to facilitate getting the localization pose - if self.PUBLISH_ODOM: - odom = Odometry() - odom.header.stamp = stamp.to_msg() - odom.header.frame_id = self.GLOBAL_FRAME_ID - if self.PROJECT_TO_BASELINK and (self.BASE_FRAME_ID != self.LASER_FRAME_ID) and (not self.PUBLISH_MAP_TO_ODOM): - odom.child_frame_id = self.BASE_FRAME_ID - odom.pose.pose.position.x = float(map_bl_trans[0]) - odom.pose.pose.position.y = float(map_bl_trans[1]) - odom.pose.pose.position.z = float(map_bl_trans[2]) - odom.pose.pose.orientation.x = float(map_bl_quat[0]) - odom.pose.pose.orientation.y = float(map_bl_quat[1]) - odom.pose.pose.orientation.z = float(map_bl_quat[2]) - odom.pose.pose.orientation.w = float(map_bl_quat[3]) - else: - odom.child_frame_id = self.LASER_FRAME_ID - odom.pose.pose.position.x = pose[0] - odom.pose.pose.position.y = pose[1] - odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) + def precompute_sensor_model(self): + """ + Build sensor_model_table[observed_px, computed_px] = P(observed | computed). + Beam model: z_hit (Gaussian) + z_short (exponential, BUG-10) + z_max + z_rand. + sigma_hit is in pixels (map_resolution × metres). + lambda_short is the exponential rate for the z_short beam component. + """ + self.get_logger().info('Precomputing sensor model') + table_width = int(self.MAX_RANGE_PX) + 1 + self.sensor_model_table = np.zeros((table_width, table_width)) - cov_mat = np.cov(self.particles, rowvar=False, ddof=0, aweights=self.weights).flatten() - odom.pose.covariance[:cov_mat.shape[0]] = cov_mat - odom.twist.twist.linear.x = self.current_speed - self.odom_pub.publish(odom) + for d in range(table_width): + norm = 0.0 + for r in range(table_width): + z = float(r - d) + prob = (self.Z_HIT + * np.exp(-(z * z) / (2.0 * self.SIGMA_HIT ** 2)) + / (self.SIGMA_HIT * np.sqrt(2.0 * np.pi))) + # d > 0 is guaranteed when r < d; canonical exponential z_short model + if r < d: + prob += self.Z_SHORT * self.LAMBDA_SHORT * np.exp(-self.LAMBDA_SHORT * r) + if r == self.MAX_RANGE_PX: + prob += self.Z_MAX + if r < self.MAX_RANGE_PX: + prob += self.Z_RAND / float(self.MAX_RANGE_PX) + norm += prob + self.sensor_model_table[r, d] = prob + self.sensor_model_table[:, d] /= norm - if self.PUBLISH_MAP_TO_ODOM: - """ - Our particle filter provides estimates for the "laser" frame - since that is where our laser range estimates are measured from. Thus, - we want to publish a "map" -> "laser" transform. - - However, the car's position is measured with respect to the "base_link" - frame (it is the root of the TF tree). Thus, we should actually define - a "map" -> "base_link" transform as to not break the TF tree. - """ - - # Lookup laser → odom transform - # Note: we could also get the current map->base_link transform by transforming the odom pose to the map frame - try: - # target_frame='odom', source_frame='laser' - trans = self.tf_buffer.lookup_transform( - self.ODOM_FRAME_ID, self.LASER_FRAME_ID, # works for laser -> (base_link) -> odom - rclpy.time.Time(), - rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)) - laser_odom_pos = np.array([ - trans.transform.translation.x, - trans.transform.translation.y, - trans.transform.translation.z - ]) - laser_odom_quat = np.array([ - trans.transform.rotation.x, - trans.transform.rotation.y, - trans.transform.rotation.z, - trans.transform.rotation.w - ]) - except (LookupException, ConnectivityException, ExtrapolationException) as e: - self.get_logger().warn(f'Could not get laser→odom transform: {e}') - # get odom pose from message - if self.odom_pose is None: - self.pub_tf.sendTransform(t) - return - laser_odom_pos = np.array([ - self.odom_pose.position.x, - self.odom_pose.position.y, - self.odom_pose.position.z - ]) - laser_odom_quat = np.array([ - self.odom_pose.orientation.x, - self.odom_pose.orientation.y, - self.odom_pose.orientation.z, - self.odom_pose.orientation.w - ]) - - # Get map -> odom transformation via matrix multiplication and inversion - laser_odom_mat = tf_transformations.concatenate_matrices( - tf_transformations.translation_matrix(laser_odom_pos), - tf_transformations.quaternion_matrix(laser_odom_quat) - ) - map_odom_mat = np.dot(map_laser_mat, laser_odom_mat) - - map_odom_trans = tf_transformations.translation_from_matrix(map_odom_mat) - map_odom_quat = tf_transformations.quaternion_from_matrix(map_odom_mat) - - # Publish map -> odom transform - tfs = TransformStamped() - tfs.header.stamp = (stamp + rclpy.duration.Duration(seconds=self.TRANSFORM_TOLERANCE)).to_msg() - tfs.header.frame_id = self.GLOBAL_FRAME_ID - tfs.child_frame_id = self.ODOM_FRAME_ID - tfs.transform.translation.x = float(map_odom_trans[0]) - tfs.transform.translation.y = float(map_odom_trans[1]) - tfs.transform.translation.z = float(map_odom_trans[2]) - tfs.transform.rotation.x = float(map_odom_quat[0]) - tfs.transform.rotation.y = float(map_odom_quat[1]) - tfs.transform.rotation.z = float(map_odom_quat[2]) - tfs.transform.rotation.w = float(map_odom_quat[3]) - - # publish the transform - self.pub_tf.sendTransform(tfs) - - def visualize(self): - ''' - Publish various visualization messages. - ''' - if not self.DO_VIZ: + if self.RANGELIB_VAR > 0: + self.range_method.set_sensor_model(self.sensor_model_table) + + # ── particle initialisation (Step 1: unified entry point) ───────────────── + + def _reset_particles(self, pose=None): + """Unified init: pose set → Gaussian around pose; pose=None → hybrid global + search if scan ready, else uniform spread (upgraded on first scan via G-5).""" + if pose is not None: + self._pending_global_loc = False + with self.state_lock: + self.weights[:] = 1.0 / self.MAX_PARTICLES + self.particles[:, 0] = (pose.position.x + + np.random.normal(0.0, 0.5, self.MAX_PARTICLES)) + self.particles[:, 1] = (pose.position.y + + np.random.normal(0.0, 0.5, self.MAX_PARTICLES)) + self.particles[:, 2] = (Utils.quaternion_to_angle(pose.orientation) + + np.random.normal(0.0, 0.4, self.MAX_PARTICLES)) + self.get_logger().info( + f'Pose init: [{pose.position.x:.2f}, {pose.position.y:.2f}]') + self._pose_inited = True + elif self._laser_ready: + # G-5: scan available — run coarse search immediately + self._hybrid_global_localize(self.downsampled_ranges) + else: + # G-5: scan not yet available — uniform spread so MCL can start, upgrade later + with self.state_lock: + py, px = np.where(self.permissible_region) + idx = np.random.randint(0, len(px), size=self.MAX_PARTICLES) + states = np.zeros((self.MAX_PARTICLES, 3)) + states[:, 0] = px[idx] + states[:, 1] = py[idx] + states[:, 2] = np.random.uniform(0.0, 2.0 * np.pi, self.MAX_PARTICLES) + Utils.map_to_world(states, self.map_info) + self.particles = states + self.weights[:] = 1.0 / self.MAX_PARTICLES + self.get_logger().info('Global particle initialisation (uniform; scan not ready)') + self._pending_global_loc = True + # _pose_inited stays False until hybrid search succeeds or manual pose arrives + + # ── MCL core ────────────────────────────────────────────────────────────── + + def _systematic_resample(self, weights): + """O(N) systematic (low-variance) resampling (E-1).""" + n = len(weights) + positions = (np.arange(n) + np.random.uniform()) / n + cumsum = np.cumsum(weights) + return np.searchsorted(cumsum, positions) + + def motion_model(self, proposal_dist, action): + """Apply odometry action in each particle's local frame, then add Gaussian noise.""" + cosines = np.cos(proposal_dist[:, 2]) + sines = np.sin(proposal_dist[:, 2]) + self.local_deltas[:, 0] = cosines * action[0] - sines * action[1] + self.local_deltas[:, 1] = sines * action[0] + cosines * action[1] + self.local_deltas[:, 2] = action[2] + proposal_dist += self.local_deltas + # Fix 4: single PRNG call into pre-allocated buffer; zero heap allocations per cycle + self._rng.standard_normal(out=self._noise) # shape (MAX_PARTICLES, 3), N(0,1) + self._noise *= self._noise_scale # broadcast scale per axis in place + proposal_dist += self._noise + + # ── sensor model dispatch (Step 2) ──────────────────────────────────────── + + def _init_sensor_buffers(self, num_rays): + """Allocate MCL ray-cast buffers and build dispatch dict on first sensor update.""" + if self.RANGELIB_VAR <= 1: + self.queries = np.zeros((num_rays * self.MAX_PARTICLES, 3), dtype=np.float32) + else: + self.queries = np.zeros((self.MAX_PARTICLES, 3), dtype=np.float32) + self.ranges = np.zeros(num_rays * self.MAX_PARTICLES, dtype=np.float32) + self.tiled_angles = np.tile(self.downsampled_angles, self.MAX_PARTICLES) + self._sensor_model_dispatch = { + VAR_NO_EVAL_SENSOR_MODEL: self._sm_v0, + VAR_CALC_RANGE_MANY_EVAL_SENSOR: self._sm_v1, + VAR_REPEAT_ANGLES_EVAL_SENSOR: self._sm_v2, + VAR_REPEAT_ANGLES_EVAL_SENSOR_ONE_SHOT: self._sm_v3, + VAR_RADIAL_CDDT_OPTIMIZATIONS: self._sm_v4, + } + self.first_sensor_update = False + + def _sm_v0(self, proposal_dist, obs, weights, num_rays): + """Python loop over sensor model table; slow, educational.""" + self.queries[:, 0] = np.repeat(proposal_dist[:, 0], num_rays) + self.queries[:, 1] = np.repeat(proposal_dist[:, 1], num_rays) + self.queries[:, 2] = np.repeat(proposal_dist[:, 2], num_rays) + self.tiled_angles + self.range_method.calc_range_many(self.queries, self.ranges) + obs_px = np.clip(obs / self.map_info.resolution, 0, self.MAX_RANGE_PX) + rng_px = np.clip(self.ranges / self.map_info.resolution, 0, self.MAX_RANGE_PX) + intobs = np.rint(obs_px).astype(np.uint16) + intrng = np.rint(rng_px).astype(np.uint16) + for i in range(self.MAX_PARTICLES): + w = np.prod( + self.sensor_model_table[intobs, intrng[i * num_rays:(i + 1) * num_rays]]) + weights[i] = np.power(w, self.INV_SQUASH_FACTOR) + + def _sm_v1(self, proposal_dist, obs, weights, num_rays): + """calc_range_many + eval_sensor_model.""" + self.queries[:, 0] = np.repeat(proposal_dist[:, 0], num_rays) + self.queries[:, 1] = np.repeat(proposal_dist[:, 1], num_rays) + self.queries[:, 2] = np.repeat(proposal_dist[:, 2], num_rays) + self.tiled_angles + self.range_method.calc_range_many(self.queries, self.ranges) + self.range_method.eval_sensor_model( + obs, self.ranges, weights, num_rays, self.MAX_PARTICLES) + np.power(weights, self.INV_SQUASH_FACTOR, weights) + + def _sm_v2(self, proposal_dist, obs, weights, num_rays): + """calc_range_repeat_angles + eval_sensor_model. Default for rmgpu.""" + self.queries[:, :] = proposal_dist[:, :] + self.range_method.calc_range_repeat_angles( + self.queries, self.downsampled_angles, self.ranges) + self.range_method.eval_sensor_model( + obs, self.ranges, weights, num_rays, self.MAX_PARTICLES) + np.power(weights, self.INV_SQUASH_FACTOR, weights) + if self.SHOW_FINE_TIMING and self.iters % 10 == 0: + self.get_logger().info(f'sensor_model variant={self.RANGELIB_VAR}') + + def _sm_v3(self, proposal_dist, obs, weights, num_rays): + """calc_range_repeat_angles_eval_sensor_model one-shot; incompatible with rmgpu.""" + self.queries[:, :] = proposal_dist[:, :] + self.range_method.calc_range_repeat_angles_eval_sensor_model( + self.queries, self.downsampled_angles, obs, weights) + np.power(weights, self.INV_SQUASH_FACTOR, weights) + + def _sm_v4(self, proposal_dist, obs, weights, num_rays): + """Radial CDDT optimization; silently degrades to variant 2 for non-CDDT backends.""" + if 'cddt' not in self.WHICH_RM: + self.get_logger().warn( + 'rangelib_variant 4 requires cddt/pcddt; falling back to variant 2') + self.RANGELIB_VAR = VAR_REPEAT_ANGLES_EVAL_SENSOR + self._sm_v2(proposal_dist, obs, weights, num_rays) return + self.queries[:, :] = proposal_dist[:, :] + self.range_method.calc_range_many_radial_optimized( + num_rays, self.downsampled_angles[0], self.downsampled_angles[-1], + self.queries, self.ranges) + self.range_method.eval_sensor_model( + obs, self.ranges, weights, num_rays, self.MAX_PARTICLES) + np.power(weights, self.INV_SQUASH_FACTOR, weights) - if self.pose_pub.get_subscription_count() > 0 and isinstance(self.inferred_pose, np.ndarray): - # Publish the inferred pose for visualization - ps = PoseStamped() - ps.header.stamp = self.get_clock().now().to_msg() - ps.header.frame_id = self.GLOBAL_FRAME_ID - ps.pose.position.x = self.inferred_pose[0] - ps.pose.position.y = self.inferred_pose[1] - ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) - self.pose_pub.publish(ps) + def sensor_model(self, proposal_dist, obs, weights): + """ + Score particles via RangeLibc ray-casting + beam model LUT. + rangelib_variant selects the ray-casting strategy (see CLAUDE.md for table). + Variant 3 is incompatible with rmgpu and raises AssertionError at startup. + """ + assert not (self.RANGELIB_VAR == VAR_REPEAT_ANGLES_EVAL_SENSOR_ONE_SHOT + and self.WHICH_RM == 'rmgpu'), \ + 'rangelib_variant 3 is incompatible with rmgpu; use variant 2' - if self.particle_pub.get_subscription_count() > 0: - # publish a downsampled version of the particle distribution to avoid a lot of latency - if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: - # randomly downsample particles - proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) - # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) - self.publish_particles(self.particles[proposal_indices,:]) - else: - self.publish_particles(self.particles) + num_rays = self.downsampled_angles.shape[0] + if self.first_sensor_update: + self._init_sensor_buffers(num_rays) - if self.pub_fake_scan.get_subscription_count() > 0 and isinstance(self.ranges, np.ndarray): - # generate the scan from the point of view of the inferred position for visualization - self.viz_queries[:,0] = self.inferred_pose[0] - self.viz_queries[:,1] = self.inferred_pose[1] - self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2] - self.range_method.calc_range_many(self.viz_queries, self.viz_ranges) - self.publish_scan(self.downsampled_angles, self.viz_ranges) + fn = self._sensor_model_dispatch.get(self.RANGELIB_VAR) + if fn is None: + self.get_logger().error(f'Unknown rangelib_variant {self.RANGELIB_VAR}; set 0–4') + return + fn(proposal_dist, obs, weights, num_rays) + + def MCL(self, action, obs): + """One MCL step: resample → motion model → sensor model → normalise.""" + # E-1: systematic (low-variance) resampling; P-4: copy into pre-allocated buffer + proposal_indices = self._systematic_resample(self.weights) + np.take(self.particles, proposal_indices, axis=0, out=self._proposal) + self.motion_model(self._proposal, action) + self.sensor_model(self._proposal, obs, self.weights) + self.weights /= np.sum(self.weights) + np.copyto(self.particles, self._proposal) - def publish_particles(self, particles): - # publish the given particles as a PoseArray object - pa = PoseArray() - pa.header.stamp = self.get_clock().now().to_msg() - pa.header.frame_id = self.GLOBAL_FRAME_ID - pa.poses = Utils.particles_to_poses(particles) - self.particle_pub.publish(pa) + # E-3: effective particle count diagnostic every 10 iters + self._n_eff_iter += 1 + if self._n_eff_iter % 10 == 0: + n_eff = 1.0 / np.sum(self.weights ** 2) + if n_eff < self.MAX_PARTICLES * 0.1: + self.get_logger().warn( + f'N_eff={n_eff:.0f} < {self.MAX_PARTICLES * 0.1:.0f}; ' + 'filter may be degenerate') - def publish_scan(self, angles, ranges): - # publish the given angels and ranges as a laser scan message - ls = LaserScan() - ls.header.stamp = self.last_stamp - ls.header.frame_id = self.LASER_FRAME_ID - ls.angle_min = np.min(angles).astype(float) - ls.angle_max = np.max(angles).astype(float) - ls.angle_increment = np.abs(angles[0] - angles[1]).astype(float) - ls.range_min = 0.0 - ls.range_max = np.max(ranges).astype(float) - ls.ranges = ranges.tolist() - self.pub_fake_scan.publish(ls) + def expected_pose(self): + return np.dot(self.particles.T, self.weights) + + # ── callbacks ───────────────────────────────────────────────────────────── def lidarCB(self, msg): - """ - Initializes reused buffers, and stores the relevant laser scanner data for later use. - """ + """Store latest scan; on first message initialise angle/buffer arrays.""" self.LASER_FRAME_ID = msg.header.frame_id + + if self.BASE_FRAME_ID and self.BASE_FRAME_ID == self.LASER_FRAME_ID: + self.get_logger().warn( + f'base_frame_id == laser_frame_id ({self.LASER_FRAME_ID}); ' + 'clearing base_frame_id') + self.BASE_FRAME_ID = '' + + if self.BASE_FRAME_ID: + self._cache_base_to_scan_tf( + timestamp=rclpy.time.Time.from_msg(msg.header.stamp)) + + # P-8: guard against misconfigured angle_step + assert len(msg.ranges) > self.ANGLE_STEP, \ + f'angle_step={self.ANGLE_STEP} >= num_ranges={len(msg.ranges)}' + if not isinstance(self.laser_angles, np.ndarray): - self.get_logger().info('...Received first LiDAR message') + self.get_logger().info('Received first LiDAR message') self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) - self.downsampled_angles = np.copy(self.laser_angles[0::self.ANGLE_STEP]).astype(np.float32) - self.viz_queries = np.zeros((self.downsampled_angles.shape[0],3), dtype=np.float32) + self.downsampled_angles = self.laser_angles[::self.ANGLE_STEP].astype(np.float32) + self.viz_queries = np.zeros((self.downsampled_angles.shape[0], 3), dtype=np.float32) self.viz_ranges = np.zeros(self.downsampled_angles.shape[0], dtype=np.float32) - self.get_logger().info(str(self.downsampled_angles.shape[0])) - - # store the necessary scanner information for later processing - self.downsampled_ranges = np.array(msg.ranges[::self.ANGLE_STEP]) + self.get_logger().info( + f'{self.downsampled_angles.shape[0]} downsampled rays ' + f'(angle_step={self.ANGLE_STEP})') + # G-8: allocate global-loc buffer now that num_beams is known + self._global_ranges_buf = np.zeros( + self.GLOBAL_LOC_MAX_CANDIDATES * self.downsampled_angles.shape[0], + dtype=np.float32) + + # E-5: read range_min from message; clip readings to valid sensor range. + # nan_to_num first: np.clip propagates NaN unchanged, which would poison + # the weight vector or cause undefined behaviour in RangeLibc C++ variants. + self._range_min = msg.range_min + self.downsampled_ranges = np.clip( + np.nan_to_num( + np.array(msg.ranges[::self.ANGLE_STEP], dtype=np.float32), + nan=self.MAX_RANGE_METERS, + posinf=self.MAX_RANGE_METERS, + neginf=self._range_min, + ), + self._range_min, + self.MAX_RANGE_METERS) + + self._last_scan_stamp = msg.header.stamp # F-4 self.lidar_initialized = True - # self.update() + self._laser_ready = True + + # G-5: upgrade from uniform spread to hybrid global search on first scan + if self._pending_global_loc: + self._pending_global_loc = False + self._hybrid_global_localize(self.downsampled_ranges) def odomCB(self, msg): """ - Store deltas between consecutive odometry messages in the coordinate space of the car. - - Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own. + Accumulate car-local odometry delta between consecutive messages. + The MCL timer drives updates at fixed Hz; this callback only accumulates (P-2). """ - self.BASE_FRAME_ID = msg.child_frame_id - self.ODOM_FRAME_ID = msg.header.frame_id self.odom_pose = msg.pose.pose - - position = np.array([ - msg.pose.pose.position.x, - msg.pose.pose.position.y]) - + position = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y]) orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation) pose = np.array([position[0], position[1], orientation]) self.current_speed = msg.twist.twist.linear.x - if isinstance(self.last_pose, np.ndarray): - # changes in x,y,theta in local coordinate system of the car - rot = Utils.rotation_matrix(-self.last_pose[2]) - delta = np.array([position - self.last_pose[0:2]]).transpose() - local_delta = (rot*delta).transpose() - - self.odometry_data = np.array([local_delta[0,0], local_delta[0,1], orientation - self.last_pose[2]]) - self.last_pose = pose - self.last_stamp = msg.header.stamp - self.odom_initialized = True - else: - self.get_logger().info('...Received first Odometry message') - self.last_pose = pose - - # this topic is slower than lidar, so update every time we receive a message - self.update() + with self._odom_lock: + if isinstance(self.last_pose, np.ndarray): + rot = Utils.rotation_matrix(-self.last_pose[2]) + delta = position - self.last_pose[:2] + local_delta = rot @ delta + # accumulate so no delta is lost between MCL timer ticks + self.odometry_data[0] += local_delta[0] + self.odometry_data[1] += local_delta[1] + # Fix 7: angle_diff handles ±pi wrap (raw difference can be ±2pi) + self.odometry_data[2] += Utils.angle_diff(orientation, self.last_pose[2]) + self.last_pose = pose + self.last_stamp = msg.header.stamp + self.odom_initialized = True + else: + self.get_logger().info('Received first Odometry message') + self.last_pose = pose def clicked_pose(self, msg): - ''' - Receive pose messages from RViz and initialize the particle distribution in response. - ''' + """RViz 2D Pose Estimate → init near pose; point click → global search.""" if isinstance(msg, PointStamped): - self.initialize_global() + self._reset_particles() elif isinstance(msg, PoseWithCovarianceStamped): - self.initialize_particles_pose(msg.pose.pose) - - def initialize_particles_pose(self, pose): - ''' - Initialize particles in the general region of the provided pose. - ''' - self.get_logger().info('SETTING POSE') - self.get_logger().info(str([pose.position.x, pose.position.y])) - self.state_lock.acquire() - self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) - self.particles[:,0] = pose.position.x + np.random.normal(loc=0.0,scale=0.5,size=self.MAX_PARTICLES) - self.particles[:,1] = pose.position.y + np.random.normal(loc=0.0,scale=0.5,size=self.MAX_PARTICLES) - self.particles[:,2] = Utils.quaternion_to_angle(pose.orientation) + np.random.normal(loc=0.0,scale=0.4,size=self.MAX_PARTICLES) - self.state_lock.release() - - def initialize_global(self): - ''' - Spread the particle distribution over the permissible region of the state space. - ''' - self.get_logger().info('GLOBAL INITIALIZATION') - # randomize over grid coordinate space - self.state_lock.acquire() - permissible_x, permissible_y = np.where(self.permissible_region == 1) - indices = np.random.randint(0, len(permissible_x), size=self.MAX_PARTICLES) - - permissible_states = np.zeros((self.MAX_PARTICLES,3)) - permissible_states[:,0] = permissible_y[indices] - permissible_states[:,1] = permissible_x[indices] - permissible_states[:,2] = np.random.random(self.MAX_PARTICLES) * np.pi * 2.0 - - Utils.map_to_world(permissible_states, self.map_info) - self.particles = permissible_states - self.weights[:] = 1.0 / self.MAX_PARTICLES - self.state_lock.release() + self._reset_particles(msg.pose.pose) + + # ── MCL timer callback (P-2) ────────────────────────────────────────────── + + def _mcl_timer_cb(self): + """Drive MCL at fixed Hz; guard against missing data (F-3) and stale scan (F-4).""" + if not (self._map_ready and self._laser_ready and self._pose_inited + and self.odom_initialized): + if not self._warned_not_ready: + missing = [name for name, ready in [ + ('map', self._map_ready), + ('laser', self._laser_ready), + ('pose_init', self._pose_inited), + ('odom', self.odom_initialized)] if not ready] + self.get_logger().warn(f'MCL not ready: waiting for {missing}') + self._warned_not_ready = True + return + self._warned_not_ready = False - def precompute_sensor_model(self): - ''' - Generate and store a table which represents the sensor model. For each discrete computed - range value, this provides the probability of measuring any (discrete) range. + # F-4: skip cycle if scan is stale + if self._last_scan_stamp is not None: + age = (self.get_clock().now() + - rclpy.time.Time.from_msg(self._last_scan_stamp)).nanoseconds * 1e-9 + if age > self.SCAN_MAX_AGE_SEC: + self.get_logger().warn( + f'Stale scan ({age:.2f}s > {self.SCAN_MAX_AGE_SEC}s); skipping MCL') + return - This table is indexed by the sensor model at runtime by discretizing the measurements - and computed ranges from RangeLibc. - ''' - self.get_logger().info('Precomputing sensor model') - # sensor model constants - z_short = self.Z_SHORT - z_max = self.Z_MAX - z_rand = self.Z_RAND - z_hit = self.Z_HIT - sigma_hit = self.SIGMA_HIT - - table_width = int(self.MAX_RANGE_PX) + 1 - self.sensor_model_table = np.zeros((table_width,table_width)) + self.update() - t = time.time() - # d is the computed range from RangeLibc - for d in range(table_width): - norm = 0.0 - sum_unkown = 0.0 - # r is the observed range from the lidar unit - for r in range(table_width): - prob = 0.0 - z = float(r-d) - # reflects from the intended object - prob += z_hit * np.exp(-(z*z)/(2.0*sigma_hit*sigma_hit)) / (sigma_hit * np.sqrt(2.0*np.pi)) + # ── update loop ─────────────────────────────────────────────────────────── - # observed range is less than the predicted range - short reading - if r < d: - prob += 2.0 * z_short * (d - r) / float(d) + def update(self): + """Run one MCL cycle; guards are in _mcl_timer_cb.""" + if self.state_lock.locked(): + self.get_logger().warn('MCL update skipped: state lock held') + return - # erroneous max range measurement - if int(r) == int(self.MAX_RANGE_PX): - prob += z_max + with self.state_lock: + self.timer.tick() + self.iters += 1 + t1 = time.time() - # random measurement - if r < int(self.MAX_RANGE_PX): - prob += z_rand * 1.0/float(self.MAX_RANGE_PX) + observation = np.copy(self.downsampled_ranges).astype(np.float32) + with self._odom_lock: + action = np.copy(self.odometry_data) + self.odometry_data[:] = 0.0 + last_stamp = self.last_stamp - norm += prob - self.sensor_model_table[int(r),int(d)] = prob + self.MCL(action, observation) + self.inferred_pose = self.expected_pose() + t2 = time.time() - # normalize - self.sensor_model_table[:,int(d)] /= norm + self.publish_tf(self.inferred_pose, last_stamp) - # upload the sensor model to RangeLib for ultra fast resolution - if self.RANGELIB_VAR > 0: - self.range_method.set_sensor_model(self.sensor_model_table) + ips = 1.0 / (t2 - t1) + self.smoothing.append(ips) + if self.iters % 10 == 0: + self.get_logger().info( + f'MCL iters/s: {int(self.timer.fps())} ' + f'(possible: {int(self.smoothing.mean())})') - def motion_model(self, proposal_dist, action): - ''' - The motion model applies the odometry to the particle distribution. Since there the odometry - data is inaccurate, the motion model mixes in gaussian noise to spread out the distribution. - - Vectorized motion model. Computing the motion model over all particles is thousands of times - faster than doing it for each particle individually due to vectorization and reduction in - function call overhead - - TODO this could be better, but it works for now - - fixed random noise is not very realistic - - ackermann model provides bad estimates at high speed - ''' - # rotate the action into the coordinate space of each particle - # t1 = time.time() - cosines = np.cos(proposal_dist[:,2]) - sines = np.sin(proposal_dist[:,2]) - - self.local_deltas[:,0] = cosines*action[0] - sines*action[1] - self.local_deltas[:,1] = sines*action[0] + cosines*action[1] - self.local_deltas[:,2] = action[2] - - proposal_dist[:,:] += self.local_deltas - proposal_dist[:,0] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_X,size=self.MAX_PARTICLES) - proposal_dist[:,1] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_Y,size=self.MAX_PARTICLES) - proposal_dist[:,2] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_THETA,size=self.MAX_PARTICLES) + self.visualize() - def sensor_model(self, proposal_dist, obs, weights): - ''' - This function computes a probablistic weight for each particle in the proposal distribution. - These weights represent how probable each proposed (x,y,theta) pose is given the measured - ranges from the lidar scanner. - - There are 4 different variants using various features of RangeLibc for demonstration purposes. - - VAR_REPEAT_ANGLES_EVAL_SENSOR is the most stable, and is very fast. - - VAR_NO_EVAL_SENSOR_MODEL directly indexes the precomputed sensor model. This is slow - but it demonstrates what self.range_method.eval_sensor_model does - - VAR_RADIAL_CDDT_OPTIMIZATIONS is only compatible with CDDT or PCDDT, it implments the radial - optimizations to CDDT which simultaneously performs ray casting - in two directions, reducing the amount of work by roughly a third - ''' - - num_rays = self.downsampled_angles.shape[0] - # only allocate buffers once to avoid slowness - if self.first_sensor_update: - if self.RANGELIB_VAR <= 1: - self.queries = np.zeros((num_rays*self.MAX_PARTICLES,3), dtype=np.float32) - else: - self.queries = np.zeros((self.MAX_PARTICLES,3), dtype=np.float32) + # ── TF / pose publishing ────────────────────────────────────────────────── + + def publish_tf(self, pose, stamp=None): + """ + Publish the localisation result as a TF transform and (optionally) Odometry. + + Three modes determined by parameters: + ODOM_FRAME_ID set → map → odom (relies on odom→base_link from VESC/DBW) + ODOM_FRAME_ID '' → map → base_link (BASE_FRAME_ID set) + both '' → map → laser (fallback; non-standard, breaks most nav stacks) + + For the map→odom mode, T_odom_scan is built from the latest odometry message pose + (T_odom_base) composed with the cached static T_base_scan, avoiding a dynamic + TF lookup on every cycle. + """ + if stamp is None: + stamp = self.get_clock().now() + else: + stamp = rclpy.time.Time.from_msg(stamp) + + # Fix 2: skip np.cov when nobody is subscribed to the odom topic + if self.PUBLISH_ODOM and self.odom_pub.get_subscription_count() > 0: + self.cov_3x3 = np.cov(self.particles, rowvar=False, ddof=0, aweights=self.weights) + + q_ms = quaternion_from_euler(0.0, 0.0, pose[2]) + self.T_map_to_scan = quat_trans_to_matrix( + q_ms, [pose[0], pose[1], 0.0], + dtype=np.float64, homogenous_matrix=self.T_map_to_scan) + + has_base = bool(self.BASE_FRAME_ID and self.BASE_FRAME_ID != self.LASER_FRAME_ID) + has_odom = bool(self.ODOM_FRAME_ID and self.ODOM_FRAME_ID != self.GLOBAL_FRAME_ID) - self.ranges = np.zeros(num_rays*self.MAX_PARTICLES, dtype=np.float32) - self.tiled_angles = np.tile(self.downsampled_angles, self.MAX_PARTICLES) - self.first_sensor_update = False + if has_odom: + # ── map → odom mode ─────────────────────────────────────────────── + if self.odom_pose is None: + self.get_logger().warn('No odometry message yet; skipping TF publish') + return - if self.RANGELIB_VAR == VAR_RADIAL_CDDT_OPTIMIZATIONS: - if 'cddt' in self.WHICH_RM: - self.queries[:,:] = proposal_dist[:,:] - self.range_method.calc_range_many_radial_optimized(num_rays, self.downsampled_angles[0], self.downsampled_angles[-1], self.queries, self.ranges) + T_odom_base = pose_msg_to_matrix(self.odom_pose) - # evaluate the sensor model - self.range_method.eval_sensor_model(obs, self.ranges, self.weights, num_rays, self.MAX_PARTICLES) - # apply the squash factor - self.weights = np.power(self.weights, self.INV_SQUASH_FACTOR) + if has_base: + if self.base_frame_to_scan_tf is None: + self.get_logger().warn('base→scan TF not cached yet; skipping TF publish') + return + T_odom_scan = T_odom_base @ self.base_frame_to_scan_tf else: - self.get_logger().info('Cannot use radial optimizations with non-CDDT based methods, use rangelib_variant 2') - elif self.RANGELIB_VAR == VAR_REPEAT_ANGLES_EVAL_SENSOR_ONE_SHOT: - self.queries[:,:] = proposal_dist[:,:] - self.range_method.calc_range_repeat_angles_eval_sensor_model(self.queries, self.downsampled_angles, obs, self.weights) - np.power(self.weights, self.INV_SQUASH_FACTOR, self.weights) - elif self.RANGELIB_VAR == VAR_REPEAT_ANGLES_EVAL_SENSOR: - if self.SHOW_FINE_TIMING: - t_start = time.time() - # this version demonstrates what this would look like with coordinate space conversion pushed to rangelib - self.queries[:,:] = proposal_dist[:,:] - if self.SHOW_FINE_TIMING: - t_init = time.time() - self.range_method.calc_range_repeat_angles(self.queries, self.downsampled_angles, self.ranges) - if self.SHOW_FINE_TIMING: - t_range = time.time() - # evaluate the sensor model on the GPU - self.range_method.eval_sensor_model(obs, self.ranges, self.weights, num_rays, self.MAX_PARTICLES) - if self.SHOW_FINE_TIMING: - t_eval = time.time() - np.power(self.weights, self.INV_SQUASH_FACTOR, self.weights) - if self.SHOW_FINE_TIMING: - t_squash = time.time() - t_total = (t_squash - t_start) / 100.0 - - if self.SHOW_FINE_TIMING and self.iters % 10 == 0: - self.get_logger().info(str(['sensor_model: init: ', np.round((t_init-t_start)/t_total, 2), 'range:', np.round((t_range-t_init)/t_total, 2), \ - 'eval:', np.round((t_eval-t_range)/t_total, 2), 'squash:', np.round((t_squash-t_eval)/t_total, 2)])) - elif self.RANGELIB_VAR == VAR_CALC_RANGE_MANY_EVAL_SENSOR: - # this version demonstrates what this would look like with coordinate space conversion pushed to rangelib - # this part is inefficient since it requires a lot of effort to construct this redundant array - self.queries[:,0] = np.repeat(proposal_dist[:,0], num_rays) - self.queries[:,1] = np.repeat(proposal_dist[:,1], num_rays) - self.queries[:,2] = np.repeat(proposal_dist[:,2], num_rays) - self.queries[:,2] += self.tiled_angles - - self.range_method.calc_range_many(self.queries, self.ranges) - - # evaluate the sensor model on the GPU - self.range_method.eval_sensor_model(obs, self.ranges, self.weights, num_rays, self.MAX_PARTICLES) - np.power(self.weights, self.INV_SQUASH_FACTOR, self.weights) - elif self.RANGELIB_VAR == VAR_NO_EVAL_SENSOR_MODEL: - # this version directly uses the sensor model in Python, at a significant computational cost - self.queries[:,0] = np.repeat(proposal_dist[:,0], num_rays) - self.queries[:,1] = np.repeat(proposal_dist[:,1], num_rays) - self.queries[:,2] = np.repeat(proposal_dist[:,2], num_rays) - self.queries[:,2] += self.tiled_angles - - # compute the ranges for all the particles in a single functon call - self.range_method.calc_range_many(self.queries, self.ranges) - - # resolve the sensor model by discretizing and indexing into the precomputed table - obs /= float(self.map_info.resolution) - ranges = self.ranges / float(self.map_info.resolution) - obs[obs > self.MAX_RANGE_PX] = self.MAX_RANGE_PX - ranges[ranges > self.MAX_RANGE_PX] = self.MAX_RANGE_PX - - intobs = np.rint(obs).astype(np.uint16) - intrng = np.rint(ranges).astype(np.uint16) - - # compute the weight for each particle - for i in range(self.MAX_PARTICLES): - weight = np.product(self.sensor_model_table[intobs,intrng[i*num_rays:(i+1)*num_rays]]) - weight = np.power(weight, self.INV_SQUASH_FACTOR) - weights[i] = weight + T_odom_scan = T_odom_base + + # Fix 3: guard for non-finite values (NaN/Inf from bad odom/scan data); + # np.linalg.inv propagates NaN silently rather than raising LinAlgError, + # so an explicit isfinite check is the correct guard for both cases. + if not np.isfinite(T_odom_scan).all(): + self.get_logger().error('T_odom_scan contains non-finite values; skipping TF publish') + return + # Analytical SE(3) inverse: T=[R|t] → T_inv=[R^T | -R^T@t]. + # Valid for any finite rigid-body transform; no LAPACK call needed. + _R = T_odom_scan[:3, :3] + self._T_odom_scan_inv[:3, :3] = _R.T + self._T_odom_scan_inv[:3, 3] = -(_R.T @ T_odom_scan[:3, 3]) + T_publish = self.T_map_to_scan @ self._T_odom_scan_inv + publish_child = self.ODOM_FRAME_ID + + elif has_base: + # ── map → base_link mode ────────────────────────────────────────── + if self.laser_to_base_frame_tf is None: + self.get_logger().warn('laser→base TF not cached yet; skipping TF publish') + return + T_publish = self.T_map_to_scan @ self.laser_to_base_frame_tf + publish_child = self.BASE_FRAME_ID + else: - self.get_logger().info('PLEASE SET rangelib_variant PARAM to 0-4') - - def MCL(self, a, o): - ''' - Performs one step of Monte Carlo Localization. - 1. resample particle distribution to form the proposal distribution - 2. apply the motion model - 3. apply the sensor model - 4. normalize particle weights - - This is in the critical path of code execution, so it is optimized for speed. - ''' - if self.SHOW_FINE_TIMING: - t = time.time() - # draw the proposal distribution from the old particles - proposal_indices = np.random.choice(self.particle_indices, self.MAX_PARTICLES, p=self.weights) - proposal_distribution = self.particles[proposal_indices,:] - if self.SHOW_FINE_TIMING: - t_propose = time.time() - - # compute the motion model to update the proposal distribution - self.motion_model(proposal_distribution, a) - if self.SHOW_FINE_TIMING: - t_motion = time.time() - - # compute the sensor model - self.sensor_model(proposal_distribution, o, self.weights) - if self.SHOW_FINE_TIMING: - t_sensor = time.time() - - # normalize importance weights - self.weights /= np.sum(self.weights) - if self.SHOW_FINE_TIMING: - t_norm = time.time() - t_total = (t_norm - t)/100.0 + # ── map → laser fallback ────────────────────────────────────────── + T_publish = self.T_map_to_scan + publish_child = self.LASER_FRAME_ID - if self.SHOW_FINE_TIMING and self.iters % 10 == 0: - self.get_logger().info(str(['MCL: propose: ', np.round((t_propose-t)/t_total, 2), 'motion:', np.round((t_motion-t_propose)/t_total, 2), \ - 'sensor:', np.round((t_sensor-t_motion)/t_total, 2), 'norm:', np.round((t_norm-t_sensor)/t_total, 2)])) + if self.tf_broadcast: + stamp_fwd = (stamp + rclpy.duration.Duration( + seconds=self.TRANSFORM_TOLERANCE)).to_msg() + self.pub_tf.sendTransform( + matrix_to_transformstamped( + T_publish, self.GLOBAL_FRAME_ID, publish_child, stamp_fwd)) - # save the particles - self.particles = proposal_distribution - - def expected_pose(self): - # returns the expected value of the pose given the particle distribution - return np.dot(self.particles.transpose(), self.weights) + if self.PUBLISH_ODOM: + quat = quaternion_from_matrix(T_publish[:3, :3]) + odom = Odometry() + odom.header.stamp = stamp.to_msg() + odom.header.frame_id = self.GLOBAL_FRAME_ID + odom.child_frame_id = publish_child + odom.pose.pose.position.x = float(T_publish[0, 3]) + odom.pose.pose.position.y = float(T_publish[1, 3]) + odom.pose.pose.position.z = float(T_publish[2, 3]) + odom.pose.pose.orientation = Quaternion( + x=float(quat[0]), y=float(quat[1]), + z=float(quat[2]), w=float(quat[3])) + odom.pose.covariance = cov_6x6.flatten().tolist() + odom.twist.twist.linear.x = self.current_speed + self.odom_pub.publish(odom) - def update(self): - ''' - Apply the MCL function to update particle filter state. - - Ensures the state is correctly initialized, and acquires the state lock before proceeding. - ''' - if self.lidar_initialized and self.odom_initialized and self.map_initialized: - if self.state_lock.locked(): - self.get_logger().info('Concurrency error avoided') + # ── Task 9: hybrid global localization ─────────────────────────────────── + + def _build_candidate_poses(self): + """G-1: sample permissible cells at coarse_res spacing × theta orientations.""" + step_px = max(1, int(self.GLOBAL_LOC_COARSE_RES / self.map_info.resolution)) + while True: + ys, xs = np.where(self.permissible_region) + mask = (ys % step_px == 0) & (xs % step_px == 0) + cells = np.stack([xs[mask], ys[mask]], axis=1) + n_theta = max(1, int(360.0 / self.GLOBAL_LOC_THETA_RES)) + thetas = np.linspace(0.0, 2.0 * np.pi, n_theta, endpoint=False) + n_candidates = len(cells) * n_theta + if n_candidates <= self.GLOBAL_LOC_MAX_CANDIDATES: + break + self.get_logger().warn( + f'Global loc: {n_candidates} candidates exceed ' + f'global_loc_max_candidates={self.GLOBAL_LOC_MAX_CANDIDATES}; ' + f'doubling step_px to {step_px * 2}') + step_px *= 2 + + candidates = np.repeat(cells, n_theta, axis=0).astype(np.float64) + candidates = np.column_stack([candidates, np.tile(thetas, len(cells))]) + Utils.map_to_world(candidates, self.map_info) + return candidates + + def _score_candidates(self, candidates, scan_ranges): + """G-2: ray-cast all candidates with variant-1 approach, sum sensor model scores.""" + num_beams = len(self.downsampled_angles) + n = len(candidates) + queries = np.zeros((n * num_beams, 3), dtype=np.float32) + queries[:, 0] = np.repeat(candidates[:, 0], num_beams).astype(np.float32) + queries[:, 1] = np.repeat(candidates[:, 1], num_beams).astype(np.float32) + queries[:, 2] = (np.repeat(candidates[:, 2], num_beams).astype(np.float32) + + np.tile(self.downsampled_angles, n)) + buf = self._global_ranges_buf[:n * num_beams] + self.range_method.calc_range_many(queries, buf) + + table_width = self.sensor_model_table.shape[0] + obs_px = np.clip( + (np.tile(scan_ranges, n) / self.map_info.resolution).astype(int), + 0, table_width - 1) + rng_px = np.clip(buf.astype(int), 0, table_width - 1) + scores = self.sensor_model_table[obs_px, rng_px] + return scores.reshape(n, num_beams).sum(axis=1) + + def _pick_top_k(self, candidates, scores): + """G-3: greedy NMS — accept highest-scoring candidates spaced > min_dist apart.""" + order = np.argsort(scores)[::-1] + chosen = [] + for idx in order: + pos = candidates[idx, :2] + if all(np.linalg.norm(pos - candidates[c, :2]) > self.GLOBAL_LOC_MIN_DIST + for c in chosen): + chosen.append(idx) + if len(chosen) >= self.GLOBAL_LOC_TOP_K: + break + return candidates[chosen] + + def _hybrid_global_localize(self, scan_ranges): + """G-4: coarse search → score → NMS → seed particles around top-K hypotheses.""" + self.get_logger().info('Global localization: coarse search starting') + t0 = time.time() + candidates = self._build_candidate_poses() + scores = self._score_candidates(candidates, scan_ranges) + top_k = self._pick_top_k(candidates, scores) + self.get_logger().info( + f'Global localization: {len(candidates)} candidates scored in ' + f'{time.time() - t0:.2f}s; seeding around {len(top_k)} hypothesis(es)') + + n_each = self.MAX_PARTICLES // len(top_k) + theta_sigma = float(np.radians(self.GLOBAL_LOC_THETA_RES)) / 2.0 + xy_sigma = self.GLOBAL_LOC_COARSE_RES / 2.0 + with self.state_lock: + parts = [] + for hyp in top_k: + noise = np.random.randn(n_each, 3) * [xy_sigma, xy_sigma, theta_sigma] + parts.append(hyp + noise) + self.particles = np.vstack(parts)[:self.MAX_PARTICLES] + self.weights[:] = 1.0 / self.MAX_PARTICLES + self._pose_inited = True + + def _global_localize_srv_cb(self, _req, resp): + """G-6: service handler — trigger hybrid search on demand.""" + if self._laser_ready: + self._hybrid_global_localize(self.downsampled_ranges) + else: + self.get_logger().warn('Global localize called before first scan received') + return resp + + def _startup_global_loc_cb(self): + """G-7: one-shot timer — auto-trigger global search if no pose init yet.""" + self._startup_timer.cancel() + if not self._pose_inited: + self.get_logger().info( + f'No initial pose received after {self.GLOBAL_LOC_TIMEOUT:.1f}s; ' + 'triggering automatic global localization') + self._reset_particles(pose=None) + + # ── visualisation ───────────────────────────────────────────────────────── + + def visualize(self, stamp=None): + """Publish inferred pose, particle cloud, and simulated scan for RViz.""" + if not self.DO_VIZ: + return + # P-6: only publish every VIZ_THROTTLE MCL cycles (default ~10 Hz at 40 Hz MCL) + self._viz_counter = (self._viz_counter + 1) % self.VIZ_THROTTLE + if self._viz_counter != 0: + return + + stamp = self._get_stamp(stamp) + + if (self.pose_pub.get_subscription_count() > 0 + and isinstance(self.inferred_pose, np.ndarray)): + ps = PoseStamped() + ps.header.stamp = stamp + ps.header.frame_id = self.GLOBAL_FRAME_ID + ps.pose.position.x = self.inferred_pose[0] + ps.pose.position.y = self.inferred_pose[1] + ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) + self.pose_pub.publish(ps) + + if self.particle_pub.get_subscription_count() > 0: + if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: + idx = np.random.choice( + self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) + self._publish_particles(self.particles[idx], stamp) else: - self.state_lock.acquire() - self.timer.tick() - self.iters += 1 + self._publish_particles(self.particles, stamp) - t1 = time.time() - observation = np.copy(self.downsampled_ranges).astype(np.float32) - action = np.copy(self.odometry_data) - self.odometry_data = np.zeros(3) - - # run the MCL update algorithm - self.MCL(action, observation) - - # compute the expected value of the robot pose - self.inferred_pose = self.expected_pose() - self.state_lock.release() - t2 = time.time() - - # publish transformation frame based on inferred pose - self.publish_tf(self.inferred_pose, self.last_stamp) - - # this is for tracking particle filter speed - ips = 1.0 / (t2 - t1) - self.smoothing.append(ips) - if self.iters % 10 == 0: - self.get_logger().info(str(['iters per sec:', int(self.timer.fps()), ' possible:', int(self.smoothing.mean())])) - - self.visualize() - -# import argparse -# import sys -# parser = argparse.ArgumentParser(description='Particle filter.') -# parser.add_argument('--config', help='Path to yaml file containing config parameters. Helpful for calling node directly with Python for profiling.') - -# def load_params_from_yaml(fp): -# from yaml import load -# with open(fp, 'r') as infile: -# yaml_data = load(infile) -# for param in yaml_data: -# print 'param:', param, ':', yaml_data[param] -# rospy.set_param('~'+param, yaml_data[param]) - -# # this function can be used to generate flame graphs easily -# def make_flamegraph(filterx=None): -# import flamegraph, os -# perf_log_path = os.path.join(os.path.dirname(__file__), '../tmp/perf.log') -# flamegraph.start_profile_thread(fd=open(perf_log_path, 'w'), -# filter=filterx, -# interval=0.001) + if (self.pub_fake_scan.get_subscription_count() > 0 + and isinstance(self.ranges, np.ndarray)): + self.viz_queries[:, 0] = self.inferred_pose[0] + self.viz_queries[:, 1] = self.inferred_pose[1] + self.viz_queries[:, 2] = self.downsampled_angles + self.inferred_pose[2] + self.range_method.calc_range_many(self.viz_queries, self.viz_ranges) + self._publish_scan(self.downsampled_angles, self.viz_ranges, stamp=self.last_stamp) -def main(args=None): - rclpy.init(args=args) - pf = ParticleFiler() - rclpy.spin(pf) + def _publish_particles(self, particles, stamp=None): + stamp = self._get_stamp(stamp) + pa = PoseArray() + pa.header.stamp = stamp + pa.header.frame_id = self.GLOBAL_FRAME_ID + pa.poses = Utils.particles_to_poses(particles) + self.particle_pub.publish(pa) -if __name__ == '__main__': - main() + def _publish_scan(self, angles, ranges, stamp=None): + stamp = self._get_stamp(stamp) + ls = LaserScan() + ls.header.stamp = stamp + ls.header.frame_id = self.LASER_FRAME_ID + ls.angle_min = float(np.min(angles)) + ls.angle_max = float(np.max(angles)) + ls.angle_increment = float(np.abs(angles[0] - angles[1])) + ls.range_min = float(self._range_min) # E-5: from actual scan message + ls.range_max = float(np.max(ranges)) + ls.ranges = ranges.tolist() + self.pub_fake_scan.publish(ls) -# if __name__=='__main__': -# rospy.init_node('particle_filter') -# args,_ = parser.parse_known_args() -# if args.config: -# load_params_from_yaml(args.config) +def main(args=None): + rclpy.init(args=args) + pf = ParticleFilter() + # Fix 1: SingleThreadedExecutor sleeps at OS level (epoll) when idle; + # MultiThreadedExecutor(4) with GIL has threads polling in tight loops consuming ~300% CPU. + executor = SingleThreadedExecutor() + executor.add_node(pf) + executor.spin() -# # make_flamegraph(r'update') -# pf = ParticleFiler() -# rospy.spin() +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/particle_filter/utils.py b/particle_filter/utils.py index d7dcb7c..a309018 100644 --- a/particle_filter/utils.py +++ b/particle_filter/utils.py @@ -1,11 +1,38 @@ +import time +import math +from typing import Optional + import numpy as np +try: + import scipy + from scipy.spatial.transform import Rotation as R + SCIPY_INSTALLED = True + SCIPY_VERSION = scipy.__version__ +except ImportError: + SCIPY_INSTALLED = False + SCIPY_VERSION = '0.0.0' + +# Fix 5: cache once at import time; avoids repeated string parsing in hot-path functions +# (quaternion_to_angle is called in odomCB at 100+ Hz). +# True → scipy >= 1.14: use scalar_first=False kwarg +# False → scipy < 1.14: omit kwarg (not yet supported) +_SCIPY_SCALAR_FIRST = SCIPY_INSTALLED and int(SCIPY_VERSION.split('.')[1]) >= 14 + +import tf2_ros + +try: + import tf_transformations + from tf_transformations import quaternion_matrix + TF_TRANSFORMATIONS_INSTALLED = True +except ImportError: + TF_TRANSFORMATIONS_INSTALLED = False + from std_msgs.msg import Header from visualization_msgs.msg import Marker -from geometry_msgs.msg import Point, Pose, PoseStamped, PoseArray, Quaternion, PolygonStamped, Polygon, Point32, PoseWithCovarianceStamped, PointStamped -import tf_transformations -# import tf2_ros -import time +from geometry_msgs.msg import (Point, Pose, PoseStamped, TransformStamped, PoseArray, Quaternion, PolygonStamped, + Polygon, Point32, PoseWithCovarianceStamped, PointStamped) + class CircularArray(object): """ Simple implementation of a circular array. @@ -28,6 +55,7 @@ def mean(self): def median(self): return np.median(self.arr[:self.num_els]) + class Timer: """ Simple helper class to compute the rate at which something is called. @@ -49,9 +77,28 @@ def tick(self): def fps(self): return self.arr.mean() +def quaternion_from_euler(roll, pitch, yaw): + if SCIPY_INSTALLED: + if _SCIPY_SCALAR_FIRST: + return R.from_euler('xyz', [roll, pitch, yaw]).as_quat(scalar_first=False) + else: + return R.from_euler('xyz', [roll, pitch, yaw]).as_quat() # x, y, z, w + return tf_transformations.quaternion_from_euler(roll, pitch, yaw) + +def quaternion_from_matrix(mat): + if SCIPY_INSTALLED: + if _SCIPY_SCALAR_FIRST: + return R.from_matrix(mat).as_quat(scalar_first=False) + else: + return R.from_matrix(mat).as_quat() # x, y, z, w + if mat.shape != (4, 4): + mat = np.eye(4, dtype=np.float64) # dtype=mat.dtype + mat[:3, :3] = mat + return tf_transformations.quaternion_from_matrix(mat) + def angle_to_quaternion(angle): """Convert an angle in radians into a quaternion _message_.""" - q = tf_transformations.quaternion_from_euler(0, 0, angle) + q = quaternion_from_euler(0, 0, angle) q_out = Quaternion() q_out.x = q[0] q_out.y = q[1] @@ -60,47 +107,50 @@ def angle_to_quaternion(angle): return q_out def quaternion_to_angle(q): - """Convert a quaternion _message_ into an angle in radians. - The angle represents the yaw. - This is not just the z component of the quaternion.""" + """Convert a quaternion _message_ into an angle in radians (yaw / rotation about Z).""" x, y, z, w = q.x, q.y, q.z, q.w - roll, pitch, yaw = tf_transformations.euler_from_quaternion((x, y, z, w)) - return yaw + if SCIPY_INSTALLED: + if _SCIPY_SCALAR_FIRST: + return R.from_quat([x, y, z, w], scalar_first=False).as_euler('xyz')[2] + else: + return R.from_quat([x, y, z, w]).as_euler('xyz')[2] + return tf_transformations.euler_from_quaternion((x, y, z, w))[2] def rotation_matrix(theta): - ''' Creates a rotation matrix for the given angle in radians ''' + """Return a (2, 2) ndarray rotation matrix for the given angle in radians.""" c, s = np.cos(theta), np.sin(theta) - return np.matrix([[c, -s], [s, c]]) + return np.array([[c, -s], [s, c]]) -def particle_to_pose(particle): - ''' Converts a particle in the form [x, y, theta] into a Pose object ''' +def transpose_laser_to_base_link_xy(pose, laser_base_link_offset): + yaw = quaternion_to_angle(pose.orientation) + pose.position.x = pose.position.x - laser_base_link_offset[0] * math.cos(yaw) + pose.position.y = pose.position.y - laser_base_link_offset[0] * math.sin(yaw) + return pose + + +def particle_to_pose(particle, laser_base_link_offset=None): + """ Converts a particle in the form [x, y, theta] into a Pose object """ pose = Pose() pose.position.x = particle[0] pose.position.y = particle[1] pose.orientation = angle_to_quaternion(particle[2]) + if laser_base_link_offset: + pose = transpose_laser_to_base_link_xy(pose, laser_base_link_offset) return pose -def particles_to_poses(particles): - ''' Converts a two dimensional array of particles into an array of Poses. + +def particles_to_poses(particles, laser_base_link_offset=None): + """ Converts a two dimensional array of particles into an array of Poses. Particles can be a array like [[x0, y0, theta0], [x1, y1, theta1]...] - ''' + """ return list(map(particle_to_pose, particles)) -# DEPRECATED: should make the header inside the node now -# def make_header(frame_id, stamp=None): -# ''' Creates a Header object for stamped ROS objects ''' -# if stamp == None: -# stamp = rospy.Time.now() -# header = Header() -# header.stamp = stamp -# header.frame_id = frame_id -# return header - -def map_to_world_slow(x,y,t,map_info): - ''' Converts given (x,y,t) coordinates from the coordinate space of the map (pixels) into world coordinates (meters). + +def map_to_world_slow(x, y, t, map_info): + """ Converts given (x,y,t) coordinates from the coordinate space of the map (pixels) into world coordinates (meters). Provide the MapMetaData object from a map message to specify the change in coordinates. *** Logical, but slow implementation, when you need a lot of coordinate conversions, use the map_to_world function - ''' + """ scale = map_info.resolution angle = quaternion_to_angle(map_info.origin.orientation) rot = rotation_matrix(angle) @@ -111,10 +161,11 @@ def map_to_world_slow(x,y,t,map_info): [y]]) world = (rot*map_c) * scale + trans - return world[0,0],world[1,0],t+angle + return world[0, 0], world[1, 0], t+angle + def map_to_world(poses, map_info): - ''' Takes a two dimensional numpy array of poses: + """ Takes a two dimensional numpy array of poses: [[x0,y0,theta0], [x1,y1,theta1], [x2,y2,theta2], @@ -123,7 +174,7 @@ def map_to_world(poses, map_info): - Conversion is done in place, so this function does not return anything. - Provide the MapMetaData object from a map message to specify the change in coordinates. - This implements the same computation as map_to_world_slow but vectorized and inlined - ''' + """ scale = map_info.resolution angle = quaternion_to_angle(map_info.origin.orientation) @@ -131,20 +182,20 @@ def map_to_world(poses, map_info): # rotation c, s = np.cos(angle), np.sin(angle) # we need to store the x coordinates since they will be overwritten - temp = np.copy(poses[:,0]) - poses[:,0] = c*poses[:,0] - s*poses[:,1] - poses[:,1] = s*temp + c*poses[:,1] + temp = np.copy(poses[:, 0]) + poses[:, 0] = c*poses[:, 0] - s*poses[:, 1] + poses[:, 1] = s*temp + c*poses[:, 1] # scale - poses[:,:2] *= float(scale) + poses[:, :2] *= float(scale) # translate - poses[:,0] += map_info.origin.position.x - poses[:,1] += map_info.origin.position.y - poses[:,2] += angle + poses[:, 0] += map_info.origin.position.x + poses[:, 1] += map_info.origin.position.y + poses[:, 2] += angle def world_to_map(poses, map_info): - ''' Takes a two dimensional numpy array of poses: + """ Takes a two dimensional numpy array of poses: [[x0,y0,theta0], [x1,y1,theta1], [x2,y2,theta2], @@ -154,30 +205,31 @@ def world_to_map(poses, map_info): - Provide the MapMetaData object from a map message to specify the change in coordinates. - This implements the same computation as world_to_map_slow but vectorized and inlined - You may have to transpose the returned x and y coordinates to directly index a pixel array - ''' + """ scale = map_info.resolution angle = -quaternion_to_angle(map_info.origin.orientation) # translation - poses[:,0] -= map_info.origin.position.x - poses[:,1] -= map_info.origin.position.y + poses[:, 0] -= map_info.origin.position.x + poses[:, 1] -= map_info.origin.position.y # scale - poses[:,:2] *= (1.0/float(scale)) + poses[:, :2] *= (1.0/float(scale)) # rotation c, s = np.cos(angle), np.sin(angle) # we need to store the x coordinates since they will be overwritten - temp = np.copy(poses[:,0]) - poses[:,0] = c*poses[:,0] - s*poses[:,1] - poses[:,1] = s*temp + c*poses[:,1] - poses[:,2] += angle + temp = np.copy(poses[:, 0]) + poses[:, 0] = c*poses[:, 0] - s*poses[:, 1] + poses[:, 1] = s*temp + c*poses[:, 1] + poses[:, 2] += angle -def world_to_map_slow(x,y,t, map_info): - ''' Converts given (x,y,t) coordinates from the coordinate space of the world (meters) into map coordinates (pixels). + +def world_to_map_slow(x, y, t, map_info): + """ Converts given (x,y,t) coordinates from the coordinate space of the world (meters) into map coordinates (pixels). Provide the MapMetaData object from a map message to specify the change in coordinates. *** Logical, but slow implementation, when you need a lot of coordinate conversions, use the world_to_map function - ''' + """ scale = map_info.resolution angle = quaternion_to_angle(map_info.origin.orientation) rot = rotation_matrix(-angle) @@ -187,4 +239,116 @@ def world_to_map_slow(x,y,t, map_info): world = np.array([[x], [y]]) map_c = rot*((world - trans) / float(scale)) - return map_c[0,0],map_c[1,0],t-angle + return map_c[0, 0], map_c[1, 0], t-angle + +# ########################## +def normalize(z): + """Normalizes an angle to between [-pi, pi]""" + if -np.pi <= z <= np.pi: + return z + return np.arctan2(np.sin(z), np.cos(z)) + +def angle_diff(a, b): + """Computes the shortest distance between two angles""" + a = normalize(a) + b = normalize(b) + d1 = a - b + d2 = 2 * np.pi - np.abs(d1) + if(d1 > 0): + d2 *= -1.0 + if(np.abs(d1) < np.abs(d2)): + return d1 + else: + return d2 + + +def quat_trans_to_matrix(quat: list | np.ndarray, trans: list | np.ndarray, dtype=np.float64, + homogenous_matrix: Optional[np.ndarray] = None): + """ + Converts a quaternion (x,y,z,w) and translation (x,y,z) into a 4x4 numpy matrix. + If scipy is installed, it will use scipy.spatial.transform.Rotation to create the rotation matrix. + Otherwise, it will use tf_transformations to create the rotation matrix. + + Args: + quat (np.ndarray): Quaternion (x,y,z,w) + trans (np.ndarray): Translation (x,y,z) + """ + if homogenous_matrix is None: + homogenous_matrix = np.eye(4, dtype=dtype) + + if SCIPY_INSTALLED: + if _SCIPY_SCALAR_FIRST: + rotation_object = R.from_quat(quat, scalar_first=False) + else: + rotation_object = R.from_quat(quat) # x, y, z, w + + homogenous_matrix[:3, :3] = rotation_object.as_matrix() + homogenous_matrix[:3, 3] = trans + + elif TF_TRANSFORMATIONS_INSTALLED: + homogenous_matrix = quaternion_matrix(quat) + homogenous_matrix[:3, 3] = trans + + else: + raise ValueError("scipy or tf_transformations must be installed to use this function") + + # homogenous_matrix = tf_transformations.concatenate_matrices( + # tf_transformations.translation_matrix(trans), + # tf_transformations.quaternion_matrix(quat) + # ) + return homogenous_matrix + +def pose_msg_to_matrix(pose_msg): + """Converts a geometry_msgs/Pose into a 4x4 numpy matrix.""" + q = pose_msg.orientation + t = pose_msg.position + mat = quat_trans_to_matrix([q.x, q.y, q.z, q.w], [t.x, t.y, t.z]) + return mat + +def transformstamped_to_matrix(tr: TransformStamped): + """Converts a geometry_msgs/TransformStamped into a 4x4 numpy matrix. todo: remove since its a duplicate of pose_msg_to_matrix""" + q = tr.transform.rotation + t = tr.transform.translation + mat = quat_trans_to_matrix([q.x, q.y, q.z, q.w], [t.x, t.y, t.z]) + return mat + +def matrix_to_transformstamped(mat: np.ndarray, parent_frame: str, child_frame: str, stamp): + """Converts a 4x4 numpy matrix into a geometry_msgs/TransformStamped""" + tr = TransformStamped() + tr.header.stamp = stamp + tr.header.frame_id = parent_frame + tr.child_frame_id = child_frame + t = mat[:3, 3] + rot_mat = mat[:3, :3] + quat = quaternion_from_matrix(rot_mat) + tr.transform.translation.x = float(t[0]) + tr.transform.translation.y = float(t[1]) + tr.transform.translation.z = float(t[2]) + tr.transform.rotation.x = float(quat[0]) + tr.transform.rotation.y = float(quat[1]) + tr.transform.rotation.z = float(quat[2]) + tr.transform.rotation.w = float(quat[3]) + return tr + +def adjoint_from_matrix(T: np.ndarray): + """Computes the adjoint of a 4x4 numpy matrix""" + Rmat = T[:3, :3] + t = T[:3, 3] + # skew(t) + t_skew = np.array([[0, -t[2], t[1]], + [t[2], 0, -t[0]], + [-t[1], t[0], 0]], dtype=np.float64) + Ad = np.zeros((6, 6), dtype=np.float64) + Ad[:3, :3] = Rmat + Ad[3:, 3:] = Rmat + Ad[:3, 3:] = (t_skew @ Rmat) + return Ad + +def cov_list_to_mat(cov_list): + """Converts a list of 36 covariance values into a 6x6 numpy matrix""" + cov = np.array(cov_list, dtype=np.float64).reshape((6, 6)) + return cov + +def cov_mat_to_list(cov_mat): + """Converts a 6x6 numpy matrix into a list of 36 covariance values""" + return cov_mat.reshape(-1).astype(float).tolist() \ No newline at end of file From 89c86a1ab4cdb151d3f750996f093149ef510058 Mon Sep 17 00:00:00 2001 From: Boluwatife Olabiran Date: Wed, 27 May 2026 11:38:17 -0400 Subject: [PATCH 3/5] * promoted the max_range parameter to a double instead of an integer. * fixed covariance propagation bug --- config/localize.yaml | 2 +- config/ydlidar_x4.yaml | 2 +- particle_filter/particle_filter.py | 9 ++++++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/config/localize.yaml b/config/localize.yaml index 8728a6f..61e67da 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -25,7 +25,7 @@ range_method: 'rmgpu' theta_discretization: 112 # todo (test) 150 (ForzaETH) # range data filtering — YDLIDAR X4 max range (m) - max_range: 10 # YDLIDAR X4 + max_range: 10.0 # YDLIDAR X4 fine_timing: 0 publish_odom: 1 # MCL update rate in Hz; decoupled from odometry topic rate (P-2) diff --git a/config/ydlidar_x4.yaml b/config/ydlidar_x4.yaml index ad33964..5e4d3aa 100644 --- a/config/ydlidar_x4.yaml +++ b/config/ydlidar_x4.yaml @@ -10,7 +10,7 @@ /**: ros__parameters: # YDLIDAR X4 max range (m) - max_range: 10 + max_range: 10.0 # step=18 → ~35 beams per scan at 625 beams/rev angle_step: 18 diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index 8cfeb93..c2530fb 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -66,7 +66,7 @@ def __init__(self): self.declare_parameter('max_particles', 4000) self.declare_parameter('max_viz_particles', 60) self.declare_parameter('squash_factor', 2.2) - self.declare_parameter('max_range', 10) + self.declare_parameter('max_range', 10.0) self.declare_parameter('theta_discretization', 112) self.declare_parameter('range_method', 'rmgpu') self.declare_parameter('rangelib_variant', 2) @@ -845,6 +845,13 @@ def publish_tf(self, pose, stamp=None): odom.pose.pose.orientation = Quaternion( x=float(quat[0]), y=float(quat[1]), z=float(quat[2]), w=float(quat[3])) + # Map 3×3 (x, y, θ) particle covariance → 6×6 ROS pose covariance + # Row/col order: [x, y, z, roll, pitch, yaw]; θ lives at index 5. + cov_6x6 = np.zeros((6, 6)) + c = self.cov_3x3 + cov_6x6[0, 0] = c[0, 0]; cov_6x6[0, 1] = c[0, 1]; cov_6x6[0, 5] = c[0, 2] + cov_6x6[1, 0] = c[1, 0]; cov_6x6[1, 1] = c[1, 1]; cov_6x6[1, 5] = c[1, 2] + cov_6x6[5, 0] = c[2, 0]; cov_6x6[5, 1] = c[2, 1]; cov_6x6[5, 5] = c[2, 2] odom.pose.covariance = cov_6x6.flatten().tolist() odom.twist.twist.linear.x = self.current_speed self.odom_pub.publish(odom) From 4841536b49d62f1b79212053da90e449d3e59352 Mon Sep 17 00:00:00 2001 From: Boluwatife Olabiran Date: Wed, 27 May 2026 21:51:25 -0400 Subject: [PATCH 4/5] feat(mcl): stop publishing when odom goes stale after init MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add odom staleness guard in _mcl_timer_cb mirroring the existing scan staleness check. Once odom_initialized is set, _last_odom_stamp is updated each odomCB tick; if the gap exceeds odom_max_age (default 0.5s) the MCL cycle is skipped and a warning is logged — preventing the node from repeating a frozen pose when the odom publisher dies at runtime. Add odom_max_age param to localize.yaml alongside scan_max_age. --- config/localize.yaml | 2 ++ particle_filter/particle_filter.py | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/config/localize.yaml b/config/localize.yaml index 61e67da..58a3a89 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -32,6 +32,8 @@ mcl_hz: 40.0 # seconds; MCL skips cycle if scan is older than this (F-4) scan_max_age: 0.5 + # seconds; MCL skips cycle if odom is older than this (publisher died after init) + odom_max_age: 0.5 # sensor model constants (YDLIDAR X4 tuned values) z_short: 0.01 # YDLIDAR X4: low glass/crosstalk return probability z_max: 0.07 # YDLIDAR X4: occasional max-range misses diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index c2530fb..6aebdfe 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -87,6 +87,7 @@ def __init__(self): self.declare_parameter('odometry_topic', 'odom') self.declare_parameter('scan_qos_reliability', 'best_effort') self.declare_parameter('scan_max_age', 0.5) + self.declare_parameter('odom_max_age', 0.5) self.declare_parameter('mcl_hz', 40.0) self.declare_parameter('global_frame_id', 'map') self.declare_parameter('odom_frame_id', '') @@ -138,6 +139,7 @@ def __init__(self): self.tf_broadcast = self.get_parameter('tf_broadcast').value self.set_initial_pose = self.get_parameter('set_initial_pose').value self.SCAN_MAX_AGE_SEC = self.get_parameter('scan_max_age').value + self.ODOM_MAX_AGE_SEC = self.get_parameter('odom_max_age').value self.MCL_HZ = self.get_parameter('mcl_hz').value scan_qos_rel_str = self.get_parameter('scan_qos_reliability').value self.GLOBAL_LOC_COARSE_RES = self.get_parameter('global_loc_coarse_res').value @@ -187,6 +189,7 @@ def __init__(self): # F-4: scan staleness tracking self._last_scan_stamp = None + self._last_odom_stamp = None # E-5: range_min populated from each scan message self._range_min = 0.0 @@ -677,6 +680,7 @@ def odomCB(self, msg): self.odometry_data[2] += Utils.angle_diff(orientation, self.last_pose[2]) self.last_pose = pose self.last_stamp = msg.header.stamp + self._last_odom_stamp = msg.header.stamp self.odom_initialized = True else: self.get_logger().info('Received first Odometry message') @@ -715,6 +719,15 @@ def _mcl_timer_cb(self): f'Stale scan ({age:.2f}s > {self.SCAN_MAX_AGE_SEC}s); skipping MCL') return + # skip cycle if odom is stale (publisher died after init) + if self._last_odom_stamp is not None: + age = (self.get_clock().now() + - rclpy.time.Time.from_msg(self._last_odom_stamp)).nanoseconds * 1e-9 + if age > self.ODOM_MAX_AGE_SEC: + self.get_logger().warn( + f'Stale odom ({age:.2f}s > {self.ODOM_MAX_AGE_SEC}s); skipping MCL') + return + self.update() # ── update loop ─────────────────────────────────────────────────────────── From 3495f92ef9119f4acd91650693c22e6fccb78dec Mon Sep 17 00:00:00 2001 From: Boluwatife Olabiran Date: Tue, 2 Jun 2026 09:22:03 -0400 Subject: [PATCH 5/5] feat(mcl): add motion gate, circular yaw mean, and odom-paced TF publishing Stop a stationary robot from random-walking and fix yaw bias and TF spam: - Motion gate (update_min_d/update_min_a): consume the accumulated odom delta and run resample+motion-noise only once translation/rotation exceeds threshold, mirroring AMCL. First cycle is forced so the pose and TF establish at startup. 0.0 disables (update every tick). - expected_pose(): compute theta as a weighted *circular* mean (atan2 of weighted sin/cos) instead of a linear average that is wrong across the +/-pi wrap and biased for spread headings. - TF: republish when the odom stamp advances (tracks odom rate, not the MCL timer) to avoid TF_REPEATED_DATA when mcl_hz > odom rate; a tf_heartbeat_period re-stamps the last transform when odom is slow but alive. publish_tf() takes publish_odom_topic to skip np.cov on heartbeat/non-fix republishes. - Add odom_qos_reliability param and apply it (with depth=10) to the odom subscription. - Add anchor_to_odom_origin comparison aid: in map->base mode, freeze the map->odom correction at the first fix so pf/pose/odom starts coincident with raw odometry; warns if frame config selects another TF mode. Retune localize.yaml to match: angle_step 18->10, squash_factor 2.2->1.0, sigma_hit 4.0->2.0 (sharper yaw constraint), tighter motion_dispersion_*, plus the new gate/heartbeat/anchor/QoS keys. --- config/localize.yaml | 33 ++++-- particle_filter/particle_filter.py | 172 ++++++++++++++++++++++++----- 2 files changed, 172 insertions(+), 33 deletions(-) diff --git a/config/localize.yaml b/config/localize.yaml index 58a3a89..9685c62 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -9,13 +9,14 @@ # topic names scan_topic: 'scan' odometry_topic: 'odom' - # scan QoS: 'best_effort' for real hardware, 'reliable' for simulation (P-9) + # QoS reliability: 'best_effort' for real hardware, 'reliable' for simulation scan_qos_reliability: 'best_effort' + odom_qos_reliability: 'best_effort' # range data downsampling - # YDLIDAR X4: ~625 beams/rev; step=18 → ~35 beams per scan - angle_step: 18 + # YDLIDAR X4: ~625 beams/rev; step=10 → ~62 beams per scan + angle_step: 10 # was 18 (~35 beams); more beams → sharper yaw. Watch MCL iters/s on Jetson; 12 (~52) is a lighter middle ground max_particles: 2000 # reduced from 4000; halves all O(N) MCL work; validate N_eff in logs. 1500 (Autoware), 3000 (ForzaETH) - squash_factor: 2.2 + squash_factor: 1.0 # was 2.2; 1.0 = no flattening (exponent 1/squash). Lower squash → sharper weights # visualization viz: 1 max_viz_particles: 60 @@ -40,13 +41,17 @@ z_rand: 0.12 # YDLIDAR X4: random noise returns z_hit: 0.75 # YDLIDAR X4: probability of hitting the intended target # sigma_hit in pixels (map_resolution × metres); X4 ±2% at 5m ≈ 10cm ≈ 2px at 0.05m/px - sigma_hit: 4.0 # YDLIDAR X4 (pixels) + sigma_hit: 2.0 # YDLIDAR X4 (pixels); was 4.0 — sharpened for stronger yaw constraint # rate parameter for the z_short exponential beam component (Thrun beam model) lambda_short: 0.05 # YDLIDAR X4 # motion model dispersion constants - motion_dispersion_x: 0.05 - motion_dispersion_y: 0.025 - motion_dispersion_theta: 0.25 # todo (test): 0.20 (f1tenth jax) + motion_dispersion_x: 0.02 + motion_dispersion_y: 0.02 + motion_dispersion_theta: 0.05 + # motion gate (AMCL update_min_d/_a): skip resample+noise below these so a + # stationary robot does not random-walk from per-tick noise. 0.0 = update every tick. + update_min_d: 0.02 # meters of accumulated translation before an MCL update + update_min_a: 0.01 # radians of accumulated rotation before an MCL update # sensor model variant, variant 2 good for rmgpu, 3 doesn't work for rmgpu rangelib_variant: 2 # -1 = random RNG; set >= 0 for reproducible particle divergence/convergence (E-7) @@ -74,6 +79,18 @@ base_frame_id: 'base_link' static_laser_to_base_link: True transform_tolerance: 0.5 + # TF republish is driven by the odom rate (broadcast when the odom stamp advances), + # not the MCL timer — this avoids TF_REPEATED_DATA drops when mcl_hz > odom rate. + # The heartbeat re-stamps the last transform if this many seconds pass with no + # fresh odom, so map→odom does not expire while odom is slow but alive. 0.0 disables. + tf_heartbeat_period: 0.2 + # Comparison aid (default False, non-standard — AMCL and most nodes do not do this). When + # True in map→base mode, freeze the map→odom correction at the first fix and + # express the published pose relative to the odom origin, so pf/pose/odom starts + # coincident with raw odometry for direct odom-vs-PF trajectory comparison. + # Typical compare setup: odom_frame_id:'' (or ='map'), tf_broadcast:False, + # publish_odom:1, anchor_to_odom_origin:True. + anchor_to_odom_origin: False map_server: ros__parameters: diff --git a/particle_filter/particle_filter.py b/particle_filter/particle_filter.py index 6aebdfe..7f1e3ea 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -83,9 +83,15 @@ def __init__(self): self.declare_parameter('motion_dispersion_x', 0.05) self.declare_parameter('motion_dispersion_y', 0.025) self.declare_parameter('motion_dispersion_theta', 0.25) + # Motion gate (AMCL update_min_d/_a): skip resample+motion-noise when the + # accumulated odom delta is below these, so a stationary robot does not + # random-walk from per-tick noise. 0.0 disables the gate (update every tick). + self.declare_parameter('update_min_d', 0.02) + self.declare_parameter('update_min_a', 0.01) self.declare_parameter('scan_topic', 'scan') self.declare_parameter('odometry_topic', 'odom') self.declare_parameter('scan_qos_reliability', 'best_effort') + self.declare_parameter('odom_qos_reliability', 'reliable') self.declare_parameter('scan_max_age', 0.5) self.declare_parameter('odom_max_age', 0.5) self.declare_parameter('mcl_hz', 40.0) @@ -94,7 +100,20 @@ def __init__(self): self.declare_parameter('base_frame_id', 'base_link') self.declare_parameter('static_laser_to_base_link', True) self.declare_parameter('transform_tolerance', 0.5) + # TF republish is normally driven by the odom rate (we publish whenever the + # odom stamp advances), which avoids TF_REPEATED_DATA drops when the MCL + # timer runs faster than odom. The heartbeat re-stamps the last transform if + # this many seconds pass with no fresh odom, so map->odom does not expire + # while odom is slow (but still alive). 0.0 disables it. + self.declare_parameter('tf_heartbeat_period', 0.2) self.declare_parameter('tf_broadcast', True) + # Comparison aid (non-standard, default off): in map->base mode, freeze the + # map->odom correction at the first fix and express the published pose + # relative to the odom origin, so pf/pose/odom starts coincident with raw + # odometry and diverges only by the drift the PF corrects. AMCL never does + # this (it publishes a live map->odom TF); only enable for offline odom-vs-PF + # trajectory comparison. Requires base_frame_id set (map->base mode). + self.declare_parameter('anchor_to_odom_origin', False) self.declare_parameter('set_initial_pose', False) self.declare_parameter('initial_pose.x', 0.0) self.declare_parameter('initial_pose.y', 0.0) @@ -130,18 +149,23 @@ def __init__(self): self.MOTION_DISPERSION_X = self.get_parameter('motion_dispersion_x').value self.MOTION_DISPERSION_Y = self.get_parameter('motion_dispersion_y').value self.MOTION_DISPERSION_THETA = self.get_parameter('motion_dispersion_theta').value + self.UPDATE_MIN_D = self.get_parameter('update_min_d').value + self.UPDATE_MIN_A = self.get_parameter('update_min_a').value self.GLOBAL_FRAME_ID = self.get_parameter('global_frame_id').value self.ODOM_FRAME_ID = self.get_parameter('odom_frame_id').value self.BASE_FRAME_ID = self.get_parameter('base_frame_id').value self.LASER_FRAME_ID = '' self.STATIC_LASER_TO_BASE_LINK = self.get_parameter('static_laser_to_base_link').value self.TRANSFORM_TOLERANCE = self.get_parameter('transform_tolerance').value + self.TF_HEARTBEAT_PERIOD = self.get_parameter('tf_heartbeat_period').value self.tf_broadcast = self.get_parameter('tf_broadcast').value + self.ANCHOR_TO_ODOM_ORIGIN = self.get_parameter('anchor_to_odom_origin').value self.set_initial_pose = self.get_parameter('set_initial_pose').value self.SCAN_MAX_AGE_SEC = self.get_parameter('scan_max_age').value self.ODOM_MAX_AGE_SEC = self.get_parameter('odom_max_age').value self.MCL_HZ = self.get_parameter('mcl_hz').value scan_qos_rel_str = self.get_parameter('scan_qos_reliability').value + odom_qos_rel_str = self.get_parameter('odom_qos_reliability').value self.GLOBAL_LOC_COARSE_RES = self.get_parameter('global_loc_coarse_res').value self.GLOBAL_LOC_THETA_RES = self.get_parameter('global_loc_theta_res').value self.GLOBAL_LOC_TOP_K = self.get_parameter('global_loc_top_k').value @@ -149,6 +173,18 @@ def __init__(self): self.GLOBAL_LOC_MAX_CANDIDATES = self.get_parameter('global_loc_max_candidates').value self.GLOBAL_LOC_TIMEOUT = self.get_parameter('global_loc_timeout').value + # anchor_to_odom_origin only takes effect in map→base mode; warn if the + # configured frames will select map→odom or the map→laser fallback instead. + if self.ANCHOR_TO_ODOM_ORIGIN: + _will_be_base = bool(self.BASE_FRAME_ID) and not ( + self.ODOM_FRAME_ID and self.ODOM_FRAME_ID != self.GLOBAL_FRAME_ID) + if not _will_be_base: + self.get_logger().warn( + 'anchor_to_odom_origin is set but the frame config does not ' + 'select map→base mode (need base_frame_id set and ' + 'odom_frame_id empty/equal to global_frame_id); anchoring ' + 'will be ignored') + # E-7: reproducible RNG seed seed = self.get_parameter('seed').value if seed >= 0: @@ -179,6 +215,9 @@ def __init__(self): self.last_pose = None self.odom_pose = None self.current_speed = 0.0 + # Frozen inv(map->odom) correction for anchor_to_odom_origin; set once at + # the first map->base publish, then held constant. + self._odom_anchor_inv = None self.cov_3x3 = np.zeros((3, 3)) # F-3: readiness flags; MCL timer checks all before running @@ -191,6 +230,13 @@ def __init__(self): self._last_scan_stamp = None self._last_odom_stamp = None + # TF publish pacing: republish only when the odom stamp advances (tracks the + # odom rate, not the MCL timer) plus a low-rate heartbeat to keep map->odom + # alive when odom is slow. _last_tf_pub_time is wall/sim time of the last + # broadcast; None until the first publish. + self._last_published_stamp = None + self._last_tf_pub_time = None + # E-5: range_min populated from each scan message self._range_min = 0.0 @@ -277,10 +323,13 @@ def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - # ── scan QoS (P-9) ──────────────────────────────────────────────────── + # ── scan / odom QoS ─────────────────────────────────────────────────── qos_rel = (QoSReliabilityPolicy.BEST_EFFORT if scan_qos_rel_str.lower() == 'best_effort' else QoSReliabilityPolicy.RELIABLE) + odom_qos_rel = (QoSReliabilityPolicy.BEST_EFFORT + if odom_qos_rel_str.lower() == 'best_effort' + else QoSReliabilityPolicy.RELIABLE) # ── subscribers ─────────────────────────────────────────────────────── self.laser_sub = self.create_subscription( @@ -289,12 +338,12 @@ def __init__(self): self.lidarCB, QoSProfile(depth=1, reliability=qos_rel), callback_group=self._lidar_group) - # Fix 1: depth=10 so odom messages queued during ~25 ms MCL cycle are not dropped + # depth=10: buffers odom messages queued during ~25 ms MCL cycle self.odom_sub = self.create_subscription( Odometry, self.get_parameter('odometry_topic').value, self.odomCB, - 10, + QoSProfile(depth=10, reliability=odom_qos_rel), callback_group=self._odom_group) self.pose_sub = self.create_subscription( PoseWithCovarianceStamped, 'initialpose', self.clicked_pose, 1, @@ -598,7 +647,16 @@ def MCL(self, action, obs): 'filter may be degenerate') def expected_pose(self): - return np.dot(self.particles.T, self.weights) + # x, y: weighted linear mean. theta: weighted *circular* mean — a linear + # average is wrong across the ±pi wrap and biased whenever the cloud is + # spread in heading, which corrupts the published yaw. + pose = np.empty(3) + pose[0] = np.dot(self.particles[:, 0], self.weights) + pose[1] = np.dot(self.particles[:, 1], self.weights) + pose[2] = np.arctan2( + np.dot(np.sin(self.particles[:, 2]), self.weights), + np.dot(np.cos(self.particles[:, 2]), self.weights)) + return pose # ── callbacks ───────────────────────────────────────────────────────────── @@ -739,34 +797,94 @@ def update(self): return with self.state_lock: - self.timer.tick() - self.iters += 1 - t1 = time.time() - observation = np.copy(self.downsampled_ranges).astype(np.float32) with self._odom_lock: action = np.copy(self.odometry_data) - self.odometry_data[:] = 0.0 + # Motion gate: only consume the accumulated delta (and run MCL) once + # the robot has moved past update_min_d / update_min_a. Otherwise keep + # accumulating so no motion is lost, and skip resample+noise this tick. + # Force the first cycle so inferred_pose / TF get established at startup. + moved = (abs(action[0]) > self.UPDATE_MIN_D + or abs(action[1]) > self.UPDATE_MIN_D + or abs(action[2]) > self.UPDATE_MIN_A + or self.inferred_pose is None) + if moved: + self.odometry_data[:] = 0.0 last_stamp = self.last_stamp - self.MCL(action, observation) - self.inferred_pose = self.expected_pose() - t2 = time.time() - - self.publish_tf(self.inferred_pose, last_stamp) - - ips = 1.0 / (t2 - t1) - self.smoothing.append(ips) - if self.iters % 10 == 0: - self.get_logger().info( - f'MCL iters/s: {int(self.timer.fps())} ' - f'(possible: {int(self.smoothing.mean())})') + if moved: + self.timer.tick() + self.iters += 1 + t1 = time.time() + self.MCL(action, observation) + self.inferred_pose = self.expected_pose() + t2 = time.time() + + # Publish TF at the odom rate, not the MCL timer rate: broadcast whenever the + # odom stamp advances (so a 40 Hz timer over a 30 Hz odom does not re-send a + # stale stamp → TF_REPEATED_DATA). A heartbeat re-stamps the last transform if + # too long passes with no fresh odom, keeping map→odom from expiring while odom + # is slow but alive (the odom-staleness guard in the timer cb still stops + # publishing once odom dies). The odom topic is published only on a real fix. + if self.inferred_pose is not None: + now_time = self.get_clock().now() + stamp_changed = (last_stamp is not None + and last_stamp != self._last_published_stamp) + heartbeat_due = ( + self.TF_HEARTBEAT_PERIOD > 0.0 + and (self._last_tf_pub_time is None + or (now_time - self._last_tf_pub_time).nanoseconds * 1e-9 + >= self.TF_HEARTBEAT_PERIOD)) + if stamp_changed or heartbeat_due: + # On a pure heartbeat (no new odom) re-stamp with the current time so + # the forward-dated transform keeps advancing; otherwise use the odom + # stamp the pose is associated with. + pub_stamp = last_stamp if stamp_changed else now_time.to_msg() + self.publish_tf(self.inferred_pose, pub_stamp, + publish_odom_topic=moved) + self._last_published_stamp = last_stamp + self._last_tf_pub_time = now_time + + if moved: + ips = 1.0 / (t2 - t1) + self.smoothing.append(ips) + if self.iters % 10 == 0: + self.get_logger().info( + f'MCL iters/s: {int(self.timer.fps())} ' + f'(possible: {int(self.smoothing.mean())})') self.visualize() # ── TF / pose publishing ────────────────────────────────────────────────── - def publish_tf(self, pose, stamp=None): + def _anchor_to_odom_origin(self, T_map_base): + """ + Express a map→base_link pose relative to the odom origin (comparison aid). + + Freezes the map→odom correction C = T_map_base · inv(T_odom_base) at the + first valid fix, then returns inv(C) · T_map_base. At t0 this equals + T_odom_base exactly, so the published pose starts coincident with raw + odometry and afterwards diverges only by the drift the PF corrects. + C is held constant after the first call (a static rigid alignment, not a + feedback loop). Returns the input unchanged if odom is not available yet. + """ + if self._odom_anchor_inv is None: + if self.odom_pose is None: + return T_map_base # odom not ready; publish unanchored this once + T_odom_base0 = pose_msg_to_matrix(self.odom_pose) + # Analytical SE(3) inverse of the frozen T_map_base (R^T | -R^T·t); + # inv(C) = T_odom_base0 · inv(T_map_base0) = T_odom_map at t0. + _R = T_map_base[:3, :3] + inv_T_map_base0 = np.eye(4) + inv_T_map_base0[:3, :3] = _R.T + inv_T_map_base0[:3, 3] = -(_R.T @ T_map_base[:3, 3]) + self._odom_anchor_inv = T_odom_base0 @ inv_T_map_base0 + self.get_logger().info( + 'anchor_to_odom_origin: froze map→odom correction; pose now ' + 'expressed relative to the odom origin') + return self._odom_anchor_inv @ T_map_base + + def publish_tf(self, pose, stamp=None, publish_odom_topic=True): """ Publish the localisation result as a TF transform and (optionally) Odometry. @@ -784,8 +902,10 @@ def publish_tf(self, pose, stamp=None): else: stamp = rclpy.time.Time.from_msg(stamp) - # Fix 2: skip np.cov when nobody is subscribed to the odom topic - if self.PUBLISH_ODOM and self.odom_pub.get_subscription_count() > 0: + # Fix 2: skip np.cov when nobody is subscribed to the odom topic, or when this + # call is not publishing the odom topic (heartbeat / non-fix republish) + if (publish_odom_topic and self.PUBLISH_ODOM + and self.odom_pub.get_subscription_count() > 0): self.cov_3x3 = np.cov(self.particles, rowvar=False, ddof=0, aweights=self.weights) q_ms = quaternion_from_euler(0.0, 0.0, pose[2]) @@ -833,6 +953,8 @@ def publish_tf(self, pose, stamp=None): return T_publish = self.T_map_to_scan @ self.laser_to_base_frame_tf publish_child = self.BASE_FRAME_ID + if self.ANCHOR_TO_ODOM_ORIGIN: + T_publish = self._anchor_to_odom_origin(T_publish) else: # ── map → laser fallback ────────────────────────────────────────── @@ -846,7 +968,7 @@ def publish_tf(self, pose, stamp=None): matrix_to_transformstamped( T_publish, self.GLOBAL_FRAME_ID, publish_child, stamp_fwd)) - if self.PUBLISH_ODOM: + if publish_odom_topic and self.PUBLISH_ODOM: quat = quaternion_from_matrix(T_publish[:3, :3]) odom = Odometry() odom.header.stamp = stamp.to_msg()