Add support for point clouds in beluga_amcl by LaBruma · Pull Request #491 · Ekumen-OS/beluga

LaBruma · 2025-08-01T19:34:51Z

Proposed changes

Add support to handle for point clouds in beluga_amcl (callback, subscriptions, parameters, etc.)
Provide testing for the added code

Type of change

🐛 Bugfix (change which fixes an issue)
🚀 Feature (change which adds functionality)
📚 Documentation (change which fixes or extends documentation)

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

Lint and unit tests (if any) pass locally with my changes
I have added tests that prove my fix is effective or that my feature works
I have added necessary documentation (if appropriate)
All commits have been signed for DCO

Additional comments

hidmic

First pass! Nevermind the comments, this is solid C++ code @LaBruma 👏

beluga_amcl/include/beluga_amcl/amcl_node.hpp

beluga_amcl/src/amcl_node.cpp

beluga_amcl/src/ndt_amcl_node.cpp

beluga_amcl/src/amcl_node.cpp

beluga_amcl/test/test_amcl_node.cpp

beluga_ros/include/beluga_ros/amcl.hpp

beluga_amcl/src/amcl_node.cpp

…iled, not tested) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Added subscription logic for scan_topic and point_cloud_topic Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

…ser_callback, point_cloud_callback, etc.) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Default for laser_scan subscription changed from "scan" to "" - Minor fix on ndt_amcl_node to subscribe to scan if parameter scan_topic is empty Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Added tests for point cloud handling (sensor_callback on amcl_node.cpp and update on amcl.cpp) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

…oints) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Topic subscription logic - Restrict types on beluga_amcl sensor_callback - SparsePointCloud3 type change, double -> float - Added size() method for LaserScan - Naming: wrapper -> measurement - Fixed bug on TrowsIfBothSensorTopicsAreSet (test_amcl_node) - Tidy up on documentation (amcl.hpp) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

…e cast implemented at points() method level. - Deduplicated lines in AmclNode::do_cleanup - Ran pre-commit Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

…ode.cpp) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic · 2025-08-26T21:44:57Z

This is ready to go. Let's see what CI says.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

xaru8145 · 2025-08-28T14:14:59Z

I tried to use scan_topic parameter for the pointcloud and of course, it failed as there is now have a dedicated parameter point_cloud_topic. The way of failing was a bit misleading though (I am using autostart=true) as after some time the lifecycle bond fails and there is no pose and tf output:

[amcl_node-2] [ERROR] [1756390281.419729963] [beluga_amcl]: The bond connection to the lifecycle manager has been broken

Not sure if there is a way to check or assert a LaserScan message when using scan_topic and a PointCloud2 message when using point_cloud_topic so that the user identifies the issue quicker.

hidmic · 2025-08-28T14:30:15Z

The way of failing was a bit misleading though (I am using autostart=true) as after some time the lifecycle bond fails and there is no pose and tf output

Not sure if there is a way to check or assert a LaserScan message when using scan_topic and a PointCloud2 message when using point_cloud_topic so that the user identifies the issue quicker.

Hmm, this one's tricky. Topic types are not guaranteed to be unique in ROS 2 (for reasons) and rclcpp can't fail fast on a missing topic name and type tuple because it may be there, just not yet discovered. This is not an issue in Beluga per se but a quirk of ROS 2 that can happen with any node. It would've failed the same if you had used a gps topic for a scan.

Maybe we can send throttled logs if after a while we are still not seeing any messages, but I'd do that in a follow-up.

glpuga

Nice. The projection thing is worrisome, though.

beluga_amcl/src/amcl_node.cpp

beluga_ros/src/amcl.cpp

beluga_ros/test/include/beluga_ros/test/point_cloud_utils.hpp

beluga_ros/test/test_sparse_point_cloud.cpp

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

beluga_amcl/src/amcl_node.cpp

glpuga

Mostly minor comments, but I still think we should remove the PointCloud3 struct if we don't use it.

beluga_amcl/README.md

beluga_amcl/src/amcl_node.cpp

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

glpuga

I still think we should not include beluga_ros/include/beluga_ros/point_cloud.hpp since we are not using it, but I won't hold this PR because of that.

hidmic · 2025-09-11T20:13:14Z

I still think we should not include beluga_ros/include/beluga_ros/point_cloud.hpp since we are not using it, but I won't hold this PR for that.

I know. We have benchmarks. I'll double check what would we be leaving on the table if we dropped it and if it's not significant I'll yank it.

hidmic

Self-approving because I'm not the author.

LaBruma requested a review from hidmic August 1, 2025 19:34

LaBruma force-pushed the LaBruma/support_for_point_clouds branch from 479536b to d4d772e Compare August 5, 2025 13:44

LaBruma changed the title ~~Added support for point clouds in beluga_amcl~~ Add support for point clouds in beluga_amcl Aug 8, 2025

hidmic reviewed Aug 8, 2025

View reviewed changes

hidmic reviewed Aug 13, 2025

View reviewed changes

beluga_amcl/src/amcl_node.cpp Outdated Show resolved Hide resolved

beluga_amcl/src/amcl_node.cpp Outdated Show resolved Hide resolved

beluga_amcl/src/amcl_node.cpp Outdated Show resolved Hide resolved

hidmic marked this pull request as ready for review August 26, 2025 21:41

LaBruma and others added 15 commits August 26, 2025 18:42

Added support for point clouds in beluga_amcl - First iteration (comp…

aa0525f

…iled, not tested) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Solved a minor rebasing issue

211703a

- Added subscription logic for scan_topic and point_cloud_topic Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Template introduced for sensor_callback to avoid repeating code (la…

c5f70d9

…ser_callback, point_cloud_callback, etc.) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Fix on do_deactivate and do_cleanup on amcl_node.cpp

7e17015

- Default for laser_scan subscription changed from "scan" to "" - Minor fix on ndt_amcl_node to subscribe to scan if parameter scan_topic is empty Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Solved a lint issue

6237516

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Fix on Clang-tidy issue (using else after throw)

5485cc8

- Added tests for point cloud handling (sensor_callback on amcl_node.cpp and update on amcl.cpp) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Fix on "update" method for PointClouds (Transformation to Base of p…

1b6d38e

…oints) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Fix some clang-tidy error diagnostics

38ec571

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Minor fix after pre-commit issue

4a0ca83

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Another clang-tidy fix

bd15875

Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Class SparsePointCloud3 back compatible for strict type check + typ…

64afa14

…e cast implemented at points() method level. - Deduplicated lines in AmclNode::do_cleanup - Ran pre-commit Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

- Fixed declaration redefinition in AmclNode::sensor_callback (amcl_n…

051fd87

…ode.cpp) Signed-off-by: Bruma <andres.brumovsky@ekumenlabs.com>

Polishes AMCL point cloud support

6e0e0bf

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Complete Beluga's point cloud support

bbb6cfe

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic force-pushed the LaBruma/support_for_point_clouds branch from ec9f047 to bbb6cfe Compare August 26, 2025 21:43

hidmic requested review from glpuga and xaru8145 August 26, 2025 21:44

Please linters

24e9132

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic added 4 commits August 26, 2025 20:31

Please clang-tidy

db6d025

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Please doxygen

02fcf83

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Please clang-tidy (take 2)

462f6ea

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Ensure code coverage

bc25f2d

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Merge branch 'main' into LaBruma/support_for_point_clouds

453b689

glpuga requested changes Aug 29, 2025

View reviewed changes

hidmic added 3 commits September 3, 2025 00:10

Address peer review comments

4dd4e03

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Update docs

cbc350f

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

Fix warning for non-planar point clouds

dbcca65

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic requested a review from glpuga September 3, 2025 12:14

glpuga reviewed Sep 3, 2025

View reviewed changes

beluga_amcl/src/amcl_node.cpp Show resolved Hide resolved

glpuga reviewed Sep 3, 2025

View reviewed changes

beluga_amcl/README.md Outdated Show resolved Hide resolved

beluga_amcl/README.md Outdated Show resolved Hide resolved

beluga_amcl/src/amcl_node.cpp Show resolved Hide resolved

beluga_amcl/src/amcl_node.cpp Show resolved Hide resolved

Address peer review comments

b203957

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic mentioned this pull request Sep 8, 2025

Standardize assumptions about sensor pre-processing #500

Open

Please codecov

57d11e0

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic requested a review from glpuga September 11, 2025 17:48

glpuga approved these changes Sep 11, 2025

View reviewed changes

hidmic approved these changes Sep 11, 2025

View reviewed changes

hidmic merged commit 7bd5344 into main Sep 11, 2025
10 checks passed

hidmic deleted the LaBruma/support_for_point_clouds branch September 11, 2025 20:15

hidmic mentioned this pull request Sep 11, 2025

Add support for point clouds to beluga_amcl #490

Closed

Conversation

LaBruma commented Aug 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Proposed changes

Type of change

Checklist

Additional comments

Uh oh!

hidmic left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

hidmic commented Aug 26, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

xaru8145 commented Aug 28, 2025

Uh oh!

hidmic commented Aug 28, 2025

Uh oh!

glpuga left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

glpuga left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

glpuga left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hidmic commented Sep 11, 2025

Uh oh!

hidmic left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

LaBruma commented Aug 1, 2025 •

edited

Loading

hidmic commented Aug 26, 2025 •

edited

Loading

glpuga left a comment •

edited

Loading