[ROS 2] Configurable point cloud publishing. #779

alek-kam-robotec-ai · 2024-10-29T14:43:17Z

What does this PR do?

This PR aims to further improve ROS2 Lidar Component's configurability. It does so by adding message configuration for the ROS2 Lidar component. The message configuration is composed of fields which signify one or more actual PC2 message fields. The presence, order, padding and names of the published fields can now be configured through the ROS2 Lidar Component's editor interface.

Please describe any testing performed.

This feature was tested under various configurations on a simple scene.

List of features:

Added a new ROS2 Lidar Component field which allows for configurable PC2 message format,
- this feature introduced new fields (some fields are not currently supported and are filled with default values)
Improved Lidar performance
Added support for ray Ring IDs
Removes raycaster - side publishing

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.h

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp

PawelLiberadzki · 2024-10-30T11:26:11Z

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

+        static const Pc2MessageFormat DefaultMessageFormat = {
+            FieldFormat(FieldFlags::PositionXYZF32), FieldFormat(FieldFlags::Padding32),          FieldFormat(FieldFlags::RangeU32),
+            FieldFormat(FieldFlags::IntensityF32),   FieldFormat(FieldFlags::SegmentationData96),


Does not seem to work for me - I have an empty message, when I just add LiDAR component.

Why did you decide for this exact set of fields?

Should Lidar 2D also have some default message format? Or it does not use this feature (configurable ROS 2 messages) at all?

This seems to be related to an o3de bug where the vector element changes are not registered correctly as prefab changes.

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageBuilder.cpp

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h

adamdbrw · 2024-10-30T14:19:44Z

Note: performance impact testing is mandatory

Signed-off-by: Aleksander Kamiński <[email protected]>

alek-kam-robotec-ai · 2024-11-21T10:42:55Z

As @adamdbrw suggested, here are the benchmarks comparing both the previous version of the raycaster:
Note: The second histogram is incorrect (for the correct comparison, see newer comments)

as well as the one from this pr:

I've measured the times using the AZStd::chrono::high_resolution_clock. The lidar was configured to have 1024 x 1024 ray pattern (1048576 rays) and used the RGL implementation. In both cases compact was enabled (non hits were not processed).

We can see that the version with configurable publishing takes around 10ms longer. I will analyze and optimize this feature further but I believe that this may be a necessary cost for such a feature.

jhanca-robotecai · 2024-11-21T12:18:04Z

This feature was tested under various configurations on a simple scene. Please note that this is a draft pull request and is not yet ready for review.

Please use Draft option of github instead of text message to make it more clear.

alek-kam-robotec-ai · 2024-11-21T12:26:01Z

This feature was tested under various configurations on a simple scene. Please note that this is a draft pull request and is not yet ready for review.

Please use Draft option of github instead of text message to make it more clear.

This pr was a draft at first. As part of the review, new features were requested. I was unable to change it back to a draft and therefore it was not made a draft.

Signed-off-by: Aleksander Kamiński <[email protected]>

alek-kam-robotec-ai · 2024-11-21T15:16:57Z

After a bit of digging I've realized that I've used the wrong data for the histogram of frequency tick elapsed time (You can see it on the histogram as well, there would be no sufficient time to publish the message - publishing takes ~6ms and was not changed by this pr). I've also found a new optimization which further improved performance by about ~4ms (for 1mln rays).
Performance prior to configurable publishing (development branch):

and after introducing it (including the new performance optimization):

Signed-off-by: Aleksander Kamiński <[email protected]>

michalpelka · 2024-11-25T12:57:36Z

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h

-        //! @param includeMaxRange Should the raycaster add points at max range for rays that exceeded their range?
-        virtual void ConfigureMaxRangePointAddition([[maybe_unused]] bool addMaxRangePoints)
-        {
-            AZ_Assert(false, "This Lidar Implementation does not support Max range point addition configuration!");


Is this API breaking change is necessary?

I believe so. Although the new API call that was created in place of this one seems similar, it serves a different purpose.

Gems/ROS2/Code/Include/ROS2/Lidar/PC2PostProcessingBaseComponent.h

Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h

Signed-off-by: Aleksander Kamiński <[email protected]>

jhanca-robotecai · 2025-01-10T13:09:48Z

I understand that we agreed to move this code to a separate Gem, as the proposed functionality would make sense for very specific applications only while bringing the API-breaking changes and huge complications to Lidar component. Could you please confirm? If so, I will close the PR.

cc: @pijaro

jhanca-robotecai · 2025-01-10T13:10:51Z

I've also found a new optimization which further improved performance by about ~4ms (for 1mln rays).

@alek-kam-robotec-ai can this optimization be used within the current implementation?

alek-kam-robotec-ai · 2025-01-14T09:21:32Z

I understand that we agreed to move this code to a separate Gem, as the proposed functionality would make sense for very specific applications only while bringing the API-breaking changes and huge complications to Lidar component. Could you please confirm? If so, I will close the PR.

cc: @pijaro

Yes.

alek-kam-robotec-ai · 2025-01-14T09:23:18Z

I've also found a new optimization which further improved performance by about ~4ms (for 1mln rays).

@alek-kam-robotec-ai can this optimization be used within the current implementation?

This optimization is already implemented and was ported alongside other changes this PR made.

jhanca-robotecai · 2025-01-14T12:06:14Z

Closing the PR. Please reopen if anything changes.

alek-kam-robotec-ai mentioned this pull request Oct 29, 2024

Add configurable PC2 message RobotecAI/o3de-extras#59

Closed

5 tasks

alek-kam-robotec-ai force-pushed the feature/configurable-pc-message/alkam branch from 6807459 to 59fd0d1 Compare October 29, 2024 15:31

alek-kam-robotec-ai marked this pull request as ready for review October 29, 2024 15:31

alek-kam-robotec-ai requested review from a team as code owners October 29, 2024 15:31

byrcolin added the needs-sig Indicates an issue or PR lacks a `sig/foo` label and requires one. label Oct 29, 2024

jhanca-robotecai added sig/simulation Categorizes an issue or PR as relevant to SIG Simulation and removed needs-sig Indicates an issue or PR lacks a `sig/foo` label and requires one. labels Oct 30, 2024

PawelLiberadzki reviewed Oct 30, 2024

View reviewed changes

alek-kam-robotec-ai added 7 commits November 4, 2024 10:19

Add configurable PC2 message

ca7f258

Signed-off-by: Aleksander Kamiński <[email protected]>

Remove Raycaster-side publishing

14bff7e

Signed-off-by: Aleksander Kamiński <[email protected]>

Add default value handling

2a31113

Signed-off-by: Aleksander Kamiński <[email protected]>

Review changes

1f9a471

Signed-off-by: Aleksander Kamiński <[email protected]>

Add pointcloud ordering

6fa09cc

Signed-off-by: Aleksander Kamiński <[email protected]>

Transform pointcloud into lidar's local space

d9c0c0d

Signed-off-by: Aleksander Kamiński <[email protected]>

Add RingU8 field

8411ca8

Signed-off-by: Aleksander Kamiński <[email protected]>

alek-kam-robotec-ai force-pushed the feature/configurable-pc-message/alkam branch from 26e9584 to 8411ca8 Compare November 4, 2024 09:19

alek-kam-robotec-ai added 2 commits November 4, 2024 12:09

Add configurable dense and ordered point cloud parameters

c4be062

Signed-off-by: Aleksander Kamiński <[email protected]>

Add ring ID provider support

4b6907b

Signed-off-by: Aleksander Kamiński <[email protected]>

jhanca-robotecai requested a review from PawelLiberadzki November 21, 2024 12:21

Performance optimitzation

90e45cc

Signed-off-by: Aleksander Kamiński <[email protected]>

Review changes

18ea156

Signed-off-by: Aleksander Kamiński <[email protected]>

alek-kam-robotec-ai mentioned this pull request Nov 23, 2024

Update Lidar sensor documentation o3de/o3de.org#2632

Open

4 tasks

Add PC2 post processing

7cd650c

Signed-off-by: Aleksander Kamiński <[email protected]>

pijaro pushed a commit to RobotecAI/o3de-extras that referenced this pull request Nov 25, 2024

[ROS 2] Configurable point cloud publishing. (o3de#7797cd650c)

e7a4640

Signed-off-by: Aleksander Kamiński <[email protected]>

pijaro pushed a commit to RobotecAI/o3de-extras that referenced this pull request Nov 25, 2024

[ROS 2] Configurable point cloud publishing. (o3de#779 @ 7cd650c)

936124d

Signed-off-by: Aleksander Kamiński <[email protected]>

Add default message format generation

f572d53

Signed-off-by: Aleksander Kamiński <[email protected]>

pijaro mentioned this pull request Nov 25, 2024

Configurable point cloud publishing. (#779 @ 7cd650c --> Rosi/2409) RobotecAI/o3de-extras#61

Closed

michalpelka requested changes Nov 25, 2024

View reviewed changes

alek-kam-robotec-ai added 5 commits November 25, 2024 16:40

Review changes

2da2b63

Signed-off-by: Aleksander Kamiński <[email protected]>

Properly clear message before writing

8b41f39

Signed-off-by: Aleksander Kamiński <[email protected]>

Fix max points visualization and publishing

3e5b202

Signed-off-by: Aleksander Kamiński <[email protected]>

Correctly order the pointcloud

494b4f9

Signed-off-by: Aleksander Kamiński <[email protected]>

Add distance unit configuration

8b4f881

Signed-off-by: Aleksander Kamiński <[email protected]>

jhanca-robotecai closed this Jan 14, 2025

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[ROS 2] Configurable point cloud publishing. #779

[ROS 2] Configurable point cloud publishing. #779

alek-kam-robotec-ai commented Oct 29, 2024 •

edited

Loading

PawelLiberadzki Oct 30, 2024

alek-kam-robotec-ai Nov 22, 2024

adamdbrw commented Oct 30, 2024

alek-kam-robotec-ai commented Nov 21, 2024 •

edited

Loading

jhanca-robotecai commented Nov 21, 2024

alek-kam-robotec-ai commented Nov 21, 2024

alek-kam-robotec-ai commented Nov 21, 2024 •

edited

Loading

michalpelka Nov 25, 2024

alek-kam-robotec-ai Nov 25, 2024

jhanca-robotecai commented Jan 10, 2025

jhanca-robotecai commented Jan 10, 2025

alek-kam-robotec-ai commented Jan 14, 2025

alek-kam-robotec-ai commented Jan 14, 2025

jhanca-robotecai commented Jan 14, 2025

[ROS 2] Configurable point cloud publishing. #779

[ROS 2] Configurable point cloud publishing. #779

Conversation

alek-kam-robotec-ai commented Oct 29, 2024 • edited Loading

What does this PR do?

PawelLiberadzki Oct 30, 2024

Choose a reason for hiding this comment

alek-kam-robotec-ai Nov 22, 2024

Choose a reason for hiding this comment

adamdbrw commented Oct 30, 2024

alek-kam-robotec-ai commented Nov 21, 2024 • edited Loading

jhanca-robotecai commented Nov 21, 2024

alek-kam-robotec-ai commented Nov 21, 2024

alek-kam-robotec-ai commented Nov 21, 2024 • edited Loading

michalpelka Nov 25, 2024

Choose a reason for hiding this comment

alek-kam-robotec-ai Nov 25, 2024

Choose a reason for hiding this comment

jhanca-robotecai commented Jan 10, 2025

jhanca-robotecai commented Jan 10, 2025

alek-kam-robotec-ai commented Jan 14, 2025

alek-kam-robotec-ai commented Jan 14, 2025

jhanca-robotecai commented Jan 14, 2025

alek-kam-robotec-ai commented Oct 29, 2024 •

edited

Loading

alek-kam-robotec-ai commented Nov 21, 2024 •

edited

Loading

alek-kam-robotec-ai commented Nov 21, 2024 •

edited

Loading