Feature/vector object server by Sushant-Chavan · Pull Request #5479 · ros-navigation/navigation2

Sushant-Chavan · 2025-08-22T11:18:37Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3017
Primary OS tested on	Ubuntu
Robotic platform tested on	Proprietary (and also in simulation)
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

This PR continues from the open but non-active PR #4680. The following additional updates were performed:

Fix linter issues
Include .hpp instead of .h tf2_ros headers
Fixed a race condition when publishing/subscribing the map published by the vector object server, when testing the feature by following the tutorial
Minor cleanup

Description of documentation updates required from your changes

The PR for the corresponding documentation can be found at: ros-navigation/docs.nav2.org#766

Description of how this change was tested

Tested that the sim-based tutorial works as expected
Tested with additional boiler-plate code on a proprietary robot with keepout and speed-limit filter masks generated by the vector object server.

Future work that may be required in bullet points

N/A

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
Should this be backported to current distributions? If so, tag with backport-*.

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Alexey Merzlyakov <[email protected]>

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Alexey Merzlyakov <[email protected]>

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

* Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

* Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov <[email protected]>

Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Sushant Chavan <[email protected]>

…t_server

…ct_server

Signed-off-by: Sushant Chavan <[email protected]>

…ct_server

Signed-off-by: Sushant Chavan <[email protected]>

…ct_server

codecov · 2025-08-22T13:06:47Z

Codecov Report

❌ Patch coverage is 97.38318% with 14 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
..._map_server/src/vo_server/vector_object_shapes.cpp	96.81%	7 Missing ⚠️
nav2_map_server/src/vo_server/main.cpp	0.00%	4 Missing ⚠️
..._map_server/src/vo_server/vector_object_server.cpp	99.08%	2 Missing ⚠️
...tmap_2d/plugins/costmap_filters/keepout_filter.cpp	50.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_collision_monitor/src/polygon.cpp	`97.29% <100.00%> (-0.12%)`	⬇️
..._costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp	`100.00% <ø> (ø)`
...nav2_costmap_2d/costmap_filters/costmap_filter.hpp	`66.66% <ø> (ø)`
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	`94.66% <100.00%> (ø)`
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp	`88.05% <ø> (-1.56%)`	⬇️
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp	`95.12% <100.00%> (ø)`
nav2_costmap_2d/plugins/obstacle_layer.cpp	`82.10% <100.00%> (ø)`
nav2_costmap_2d/src/costmap_2d.cpp	`88.20% <100.00%> (+0.05%)`	⬆️
...er/include/nav2_map_server/vector_object_utils.hpp	`100.00% <100.00%> (ø)`
nav2_util/include/nav2_util/geometry_utils.hpp	`96.22% <100.00%> (+1.10%)`	⬆️
... and 6 more

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

SteveMacenski

I did a medium level review, knowing that some of the details were written by Alexey and I can do a detailed review of that level in the next round. I wanted to get things I thought you should fix and will verify functionality in the next one after these are resolved.

Generally looks good to me though. I don't think I'm going to find much I object to.

…ct_server

Signed-off-by: Sushant Chavan <[email protected]>

Copilot

Pull Request Overview

This PR implements a Vector Object Server feature that allows dynamic addition, removal, and management of vector shapes (polygons and circles) on occupancy grid maps. The main purpose is to enable runtime manipulation of keepout zones, speed limit areas, and other regions on navigation maps using geometric primitives instead of static map files.

Adds VectorObjectServer lifecycle node for managing vector shapes on maps
Implements polygon and circle geometric primitives with transformation support
Provides service interfaces for adding, removing, and querying vector objects

Reviewed Changes

Copilot reviewed 39 out of 39 changed files in this pull request and generated 4 comments.

File	Description
nav2_msgs/	Adds message definitions and service interfaces for vector objects
nav2_util/	Implements utility functions for raytracing, occupancy grid operations, and geometric calculations
nav2_map_server/	Core implementation of vector object server with shape management

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Co-authored-by: Copilot <[email protected]> Signed-off-by: Steve Macenski <[email protected]>

SteveMacenski

Small changes for logging and otherwise happy with this! This should be mergable immediately on these updates

…ct_server

Signed-off-by: Sushant Chavan <[email protected]>

…ct_server

Signed-off-by: Sushant Chavan <[email protected]>

SteveMacenski · 2025-09-11T16:20:41Z

Thanks @Sushant-Chavan for testing and refining for your application, @ajtudela for doing the hard work of continuing this from @AlexeyMerzlyakov - and of course Alexey for his core contributions here!

ajtudela · 2025-09-11T18:09:49Z

Great! I'm glad this has finally been merged.

* Add Vector Object server Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Meet review comments Signed-off-by: Alexey Merzlyakov <[email protected]> * Simplify shapes param configuring Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Rename getROSParameter() to getParameter() Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Return back getMaskData() to nav2_costmap_2d Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Add composition node support Signed-off-by: Alexey Merzlyakov <[email protected]> * Remove redundant methods Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Update nav2_map_server/src/vo_server/vector_object_server.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Avoid shapes clearing Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Optimize switchMapUpdate() method Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Switch to vector of shapes Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Minor fixes Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Meet review comments Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Move isPointInside algorithm to nav2_util Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Testcases covering new functionality Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Fix linting issues Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Adjust for Vector Objects demonstration Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Code clean-up * Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> * Additional code facelift * Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov <[email protected]> * Minor fixing after rebase Signed-off-by: Alberto Tudela <[email protected]> * Rename vector object server Signed-off-by: Alberto Tudela <[email protected]> * Minor changes Signed-off-by: Alberto Tudela <[email protected]> * Update tests Signed-off-by: Alberto Tudela <[email protected]> * Merge branch 'main' into feature/vector_object_server Signed-off-by: Sushant Chavan <[email protected]> * Fix merge issues and pre-commit checks Signed-off-by: Sushant Chavan <[email protected]> * Change tf2_ros headers from `.h` to `.hpp` Signed-off-by: Sushant Chavan <[email protected]> * Fix race condition in pub-sub of VO map Signed-off-by: Sushant Chavan <[email protected]> * Cleanup Signed-off-by: Sushant Chavan <[email protected]> * Remove use of ament_target_dependencies Signed-off-by: Sushant Chavan <[email protected]> * Fix review comments Signed-off-by: Sushant Chavan <[email protected]> * Fix linter errors Signed-off-by: Sushant Chavan <[email protected]> * Fix exception handling Signed-off-by: Sushant Chavan <[email protected]> * Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp Co-authored-by: Copilot <[email protected]> Signed-off-by: Steve Macenski <[email protected]> * Update nav2_util/include/nav2_util/raytrace_line_2d.hpp Co-authored-by: Copilot <[email protected]> Signed-off-by: Steve Macenski <[email protected]> * Add error logs Signed-off-by: Sushant Chavan <[email protected]> * Fix cpplint Signed-off-by: Sushant Chavan <[email protected]> --------- Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> Signed-off-by: Sushant Chavan <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Alberto Tudela <[email protected]> Co-authored-by: Copilot <[email protected]>

AlexeyMerzlyakov and others added 30 commits September 17, 2024 14:27

Add Vector Object server

4b8c7eb

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Meet review comments

f02a925

Signed-off-by: Alexey Merzlyakov <[email protected]>

Simplify shapes param configuring

8a506bb

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Rename getROSParameter() to getParameter()

64c63ce

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Return back getMaskData() to nav2_costmap_2d

75ccc4c

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Add composition node support

4467b38

Signed-off-by: Alexey Merzlyakov <[email protected]>

Remove redundant methods

718a5b3

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Update nav2_map_server/src/vo_server/vector_object_server.cpp

4465258

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Avoid shapes clearing

79bae1c

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Optimize switchMapUpdate() method

24a87c9

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Switch to vector of shapes

b4b303f

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Minor fixes

724d848

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Meet review comments

3a02878

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Move isPointInside algorithm to nav2_util

1067a56

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Testcases covering new functionality

ebfa990

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Fix linting issues

b33d88a

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Adjust for Vector Objects demonstration

c0025ec

Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Code clean-up

d579516

* Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov <[email protected]> Signed-off-by: Alberto Tudela <[email protected]>

Additional code facelift

9a05aad

* Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov <[email protected]>

Minor fixing after rebase

323410a

Signed-off-by: Alberto Tudela <[email protected]>

Rename vector object server

1a8d07f

Signed-off-by: Alberto Tudela <[email protected]>

Minor changes

906ed7c

Signed-off-by: Alberto Tudela <[email protected]>

Update tests

d101b1e

Signed-off-by: Alberto Tudela <[email protected]>

Merge branch 'main' into feature/vector_object_server

f8a37ed

Signed-off-by: Sushant Chavan <[email protected]>

Merge branch 'main' into feature/vector_object_server

1737522

Fix merge issues and pre-commit checks

df50d78

Signed-off-by: Sushant Chavan <[email protected]>

Merge remote-tracking branch 'sushant/main' into feature/vector_objec…

8210f64

…t_server

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

98ecea1

…ct_server

Change tf2_ros headers from .h to .hpp

cce1189

Signed-off-by: Sushant Chavan <[email protected]>

Fix race condition in pub-sub of VO map

189ee11

Signed-off-by: Sushant Chavan <[email protected]>

Sushant-Chavan added 4 commits August 21, 2025 16:54

Cleanup

7531c47

Signed-off-by: Sushant Chavan <[email protected]>

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

bd4c505

…ct_server

Remove use of ament_target_dependencies

5ad9e94

Signed-off-by: Sushant Chavan <[email protected]>

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

e3796c3

…ct_server

Sushant-Chavan mentioned this pull request Aug 22, 2025

Feature/vector object server Sushant-Chavan/navigation2#2

Closed

8 tasks

SteveMacenski requested changes Aug 27, 2025

View reviewed changes

Sushant-Chavan added 4 commits August 29, 2025 08:01

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

dc68e2a

…ct_server

Fix review comments

0350f77

Signed-off-by: Sushant Chavan <[email protected]>

Fix linter errors

94c3dac

Signed-off-by: Sushant Chavan <[email protected]>

Fix exception handling

08a87fd

Signed-off-by: Sushant Chavan <[email protected]>

Sushant-Chavan requested a review from SteveMacenski August 29, 2025 14:43

SteveMacenski requested a review from Copilot September 3, 2025 20:55

Copilot AI reviewed Sep 3, 2025

View reviewed changes

Comment thread nav2_util/include/nav2_util/raytrace_line_2d.hpp Outdated

Comment thread nav2_util/include/nav2_util/occ_grid_utils.hpp

Comment thread nav2_map_server/src/vo_server/vector_object_shapes.cpp

Comment thread nav2_map_server/include/nav2_map_server/vector_object_utils.hpp Outdated

SteveMacenski and others added 2 commits September 3, 2025 13:57

Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp

131aad6

Co-authored-by: Copilot <[email protected]> Signed-off-by: Steve Macenski <[email protected]>

Update nav2_util/include/nav2_util/raytrace_line_2d.hpp

47321dd

Co-authored-by: Copilot <[email protected]> Signed-off-by: Steve Macenski <[email protected]>

SteveMacenski requested changes Sep 3, 2025

View reviewed changes

Sushant-Chavan added 4 commits September 4, 2025 11:29

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

bd551de

…ct_server

Add error logs

6f8cbd8

Signed-off-by: Sushant Chavan <[email protected]>

Merge remote-tracking branch 'upstream/main' into feature/vector_obje…

3f553bd

…ct_server

Fix cpplint

d34351a

Signed-off-by: Sushant Chavan <[email protected]>

Sushant-Chavan requested a review from SteveMacenski September 8, 2025 13:59

SteveMacenski merged commit 97de6f2 into ros-navigation:main Sep 11, 2025
13 of 14 checks passed

SteveMacenski mentioned this pull request Sep 11, 2025

Add Vector Object server #4680

Closed

7 tasks

Conversation

Sushant-Chavan commented Aug 22, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

SteveMacenski 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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski 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!

SteveMacenski commented Sep 11, 2025

Uh oh!

ajtudela commented Sep 11, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

codecov Bot commented Aug 22, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading