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 1db77f6..9685c62 100644 --- a/config/localize.yaml +++ b/config/localize.yaml @@ -1,34 +1,96 @@ -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' - odometry_topic: '/odom' + scan_topic: 'scan' + odometry_topic: 'odom' + # QoS reliability: 'best_effort' for real hardware, 'reliable' for simulation + scan_qos_reliability: 'best_effort' + odom_qos_reliability: 'best_effort' # range data downsampling - angle_step: 18 - max_particles: 4000 - squash_factor: 2.2 + # 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: 1.0 # was 2.2; 1.0 = no flattening (exponent 1/squash). Lower squash → sharper weights # 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.0 # 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 + # 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 + 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: 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 + 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) + 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' + 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/config/ydlidar_x4.yaml b/config/ydlidar_x4.yaml new file mode 100644 index 0000000..5e4d3aa --- /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.0 + + # 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 0f68cb8..7f1e3ea 100644 --- a/particle_filter/particle_filter.py +++ b/particle_filter/particle_filter.py @@ -1,6 +1,6 @@ # 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 @@ -20,34 +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 +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 tf2_ros import TransformBroadcaster -import tf_transformations +from std_srvs.srv import Empty +from tf2_ros import Buffer, TransformListener, TransformBroadcaster +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException -# 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 @@ -55,154 +55,356 @@ 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 - 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') - - # 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 + # ── parameters ──────────────────────────────────────────────────────── + 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.0) + 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('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', 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) + # 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) + self.declare_parameter('global_frame_id', 'map') + 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) + # 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) + 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 - - # various data containers used in the MCL algorithm + 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 + 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 + + # 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: + 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 + # 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 + 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 + 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 + + # 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) - - # these topics are to receive data from the racecar + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # ── 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( LaserScan, self.get_parameter('scan_topic').value, self.lidarCB, - 1) + QoSProfile(depth=1, reliability=qos_rel), + callback_group=self._lidar_group) + # 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, - 1) + QoSProfile(depth=10, reliability=odom_qos_rel), + 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 @@ -210,517 +412,753 @@ 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 == None: - stamp = self.get_clock().now().to_msg() - - t = TransformStamped() - # header - t.header.stamp = stamp - t.header.frame_id = '/map' - t.child_frame_id = '/laser' - # 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] - 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]) - 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 + # ── sensor model ────────────────────────────────────────────────────────── - def visualize(self): - ''' - Publish various visualization messages. - ''' - if not self.DO_VIZ: + 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)) + + 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.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 = '/map' - 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 = '/map' - 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 = '/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 - self.pub_fake_scan.publish(ls) + def expected_pose(self): + # 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 ───────────────────────────────────────────────────────────── 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 - 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. - ''' - position = np.array([ - msg.pose.pose.position.x, - msg.pose.pose.position.y]) + # 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): + """ + Accumulate car-local odometry delta between consecutive messages. + The MCL timer drives updates at fixed Hz; this callback only accumulates (P-2). + """ + self.odom_pose = msg.pose.pose + 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._last_odom_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() - - 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. - - 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)) - - 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)) - - # observed range is less than the predicted range - short reading - if r < d: - prob += 2.0 * z_short * (d - r) / float(d) - - # erroneous max range measurement - if int(r) == int(self.MAX_RANGE_PX): - prob += z_max - - # random measurement - if r < int(self.MAX_RANGE_PX): - prob += z_rand * 1.0/float(self.MAX_RANGE_PX) + 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 + + # 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 + + # 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 - norm += prob - self.sensor_model_table[int(r),int(d)] = prob + self.update() - # normalize - self.sensor_model_table[:,int(d)] /= norm + # ── update loop ─────────────────────────────────────────────────────────── - # 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) + 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 - 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) + with self.state_lock: + observation = np.copy(self.downsampled_ranges).astype(np.float32) + with self._odom_lock: + action = np.copy(self.odometry_data) + # 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 + + if moved: + self.timer.tick() + self.iters += 1 + t1 = time.time() + self.MCL(action, observation) + self.inferred_pose = self.expected_pose() + t2 = time.time() - 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) + # 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 _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. + + 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, 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]) + 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) + + if has_odom: + # ── map → odom mode ─────────────────────────────────────────────── + if self.odom_pose is None: + self.get_logger().warn('No odometry message yet; skipping TF publish') + return + + T_odom_base = pose_msg_to_matrix(self.odom_pose) + + 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.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.first_sensor_update = False + 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 + if self.ANCHOR_TO_ODOM_ORIGIN: + T_publish = self._anchor_to_odom_origin(T_publish) - 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) + else: + # ── map → laser fallback ────────────────────────────────────────── + T_publish = self.T_map_to_scan + publish_child = self.LASER_FRAME_ID + + 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)) + + if publish_odom_topic and 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])) + # 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) - # 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) - 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 + # ── 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().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 + 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 - 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)])) + stamp = self._get_stamp(stamp) - # 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.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) - 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') + 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) + 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) - # run the MCL update algorithm - self.MCL(action, observation) + 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) - # compute the expected value of the robot pose - self.inferred_pose = self.expected_pose() - self.state_lock.release() - t2 = time.time() + 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) - # 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) def main(args=None): rclpy.init(args=args) - pf = ParticleFiler() - rclpy.spin(pf) + 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() -if __name__ == '__main__': - main() - -# if __name__=='__main__': -# rospy.init_node('particle_filter') - -# args,_ = parser.parse_known_args() -# if args.config: -# load_params_from_yaml(args.config) -# # 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