Add Vector Object server #3930

AlexeyMerzlyakov · 2023-11-02T13:32:52Z

Adding Vector Objects support to costmaps (and OccupancyGrid maps).

This is done by new Vector Object server, working as a separate node. In this server, Vector Objects are being putted into OccupancyGrid map. OccupancyMap is being published via output map topic (the performance question of map publishing was discussed in the original ticket). VO-server have an ability to support moving objects working dynamically with a given rate. If at least one vector object is being published in separate than "map" frame, resulting OccupancyGrid is being updated dynamically with this rate.

Support of vector objects in costmaps could be enabled by switching published by VO-server map as an input mask for Keepout Costmap Filter (it can't be done via Static Layer as it reads input map once and thus can not support moving objects; transfers input OccupancyGrid map to costmap w/o objects overlay on initial background; works with fixed-size input maps while VO-server can change its origin and dimensions).
Also, they are being able to work with other Costmap Filters (Speed Filter), to have a support for speed restricting areas specified by polygons).

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3017
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	TB3 simulation in Gazebo

Description of contribution in a few bullet points

Added new Vector Object server into nav2_map_server
Supported polygons and circles vector objects for now
New messages: PolygonVO.msg and CircleVO.msg for them
New services to work with Vector Objects: AddShapes.srv, GetShapes.srv and RemoveShapes.srv
raytraceLine(), bresenham2D() algorithms were moved into common nav2_utils part
Such functions as maskToWorld()/worldToMask(), getMaskData() were moved to nav2_util/occ_grid_utils.hpp common module working OccupancyGrid-s

Use-cases to be covered by this feature (from the #3017 discussions):

Testing/virtual objects
No-access-zone
Speed restriction areas
Robot footprint (or any other moving objects) replacement
Areas restriction for UAVs to define a flying zone (outer / boundary) with obstacles (inner rings / holes)
Remove some area from costmap (need KeepoutFilter modification: new "always overlay" overlay parameter)

Description of documentation updates required from your changes

Chapter in Iron to Jazzy migration guide
Parameters page on Configuration guide
Demo tutorial of using the VO-server in costmaps

Future work that may be required in bullet points

Consider to move point-in-polygon implementation (which is currently written twice in the code: initially for Collision Monitor, and for VO-server) to common nav2_utils function
Consider to move Bresenham3D algorithm into common nav2_utils part as well as it was made for Bresenham2D one
Testcase coverage once design will be agreed

For Maintainers:

Check that any new parameters added are updated in navigation.ros.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

AlexeyMerzlyakov · 2023-11-02T13:36:45Z

nav2_map_server/src/vo_server/vector_object_shapes.cpp

+  }
+}
+
+bool Polygon::isPointInside(const double px, const double py) const


This need to be considered move to common part (e.g. to nav2_util/include/nav2_util/vector_object_utils.hpp), as Collision Monitor has also the similar routine

I leave that to your discretion

Finally moved to nav2_util in 6d04b15 to avoid code duplication (fortunately, it could be easily combined with existing code by using structure templating in isPointInsidePolygon() function)

nav2_util/include/nav2_util/raytrace_line_2d.hpp

SteveMacenski

This is alot of code to review, I really didn't have a chance yet to go through the cpp files in much detail, but I didn't want to delay the few comments I have now another few days while I continue to review the rest (its a big PR).

Higher level comment that this will really need alot of documentation on Navigation.ros.org to support (e.g. how to use tutorial / demo) and its missing test coverage

nav2_map_server/CMakeLists.txt

nav2_util/include/nav2_util/occ_grid_utils.hpp

nav2_util/include/nav2_util/raytrace_line_2d.hpp

nav2_msgs/msg/PolygonVO.msg

nav2_msgs/msg/CircleVO.msg

nav2_msgs/srv/RemoveShapes.srv

nav2_map_server/src/vo_server/vector_object_server.cpp

nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp

nav2_map_server/src/vo_server/vector_object_server.cpp

nav2_map_server/src/vo_server/vector_object_shapes.cpp

AlexeyMerzlyakov · 2023-11-07T18:25:52Z

@SteveMacenski, thank you for having the time to promptly review the PR, which became to be really large change. I'll prepare the fixes to meet the review comments soon. Only one thing: moving from OccGrid maps -> to Costmap2D is still doubtful for me, so I've left the responds for a few review items from above.

SteveMacenski

Needs tests / docs 😄

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

nav2_map_server/src/vo_server/vector_object_server.cpp

SteveMacenski · 2023-11-13T17:44:53Z

nav2_map_server/params/vector_object_server_params.yaml

+      value: 10
+      center: [3.0, 3.0]
+      radius: 0.5
+      uuid: "7b3f3d7d-135c-4b6c-aca1-7a84d1050505"


I didn't think about this before, but if we're asking users to specify shapes in a params file, UUID might not be the best option. That would require them to offline generate some UUIDs to put into the params file. If we instead just made it an id field of unsigned int then it would be easier to enumerate them in a config file.

When its generated via code, UUID makes sense, but not when manually requiring annotation

BTW, uuid ROS-param is optional, it could be skipped by developer: in this case it will be generated to a new shape automatically. In this case, does it still have a sense to have an ID?

I'm asking, because when we have an ID, we do not need to support UUID anymore: any shape could be uniquely addressed by its ID, it also could be easily generated if not specified w/o any UUID dependency. So we could just skip it.

What do you think, maybe it is better to totally refuse from UUID usage in favor of ID?
Another question that logically raises here: how about to use ID of string type, which could be called "name"? It is even much more user-friendly, than unsigned ints, I believe; and we do not need to worry about ID-s/UUID-s in ROS-parameters: name of each shape is already specified in its parameters path and shapes array.

However, from my point of view, using UUID is still the nice way for UX:

We could optionally specify it in ROS-parameters

We could optionally specify it in AddShapes.srv service request when adding a new shape

And we to specify it on mandatory basis when modifying existing shape while calling AddShapes.srv and removing some specific shape in RemoveShapes.srv

uuid ROS-param is optional, it could be skipped by developer

Ah, ok! I'd just make sure to document that in the param guide that its optional except if you might want to remove it, in which case specifying it is important.

nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp

nav2_map_server/src/vo_server/vector_object_server.cpp

nav2_map_server/src/vo_server/vector_object_shapes.cpp

AlexeyMerzlyakov · 2023-11-16T12:33:56Z

Regarding #3930 (comment), I've missed it initially, but currently during the testcase development it was discovered that switching to std::unordered_mapbroke the OVERALY_SEQ updating mechanism, where the shapes are put on map in the same order, as they arrived (independently on their values). This sequential updating mechanism is important to support some use-cases, such as outer ring with inner objects inside this ring, which was reported in the initial ticket.

To do this, there is a way to add unsigned int seq field to the Polygon and Circle classes, but I think it will overcomplicate the overall structure.
So, unfortunately, better way - is roll-back the usage of std::unordered_map in favor of std::vector-s where we could save the order of shapes arrived.

SteveMacenski · 2023-11-18T00:19:52Z

where the shapes are put on map in the same order, as they arrived (independently on their values).

Ohhhhhhhhhh yeah, that sounds like a problem :( Sorry about that bad suggestion making extra work!

AlexeyMerzlyakov · 2023-11-20T17:39:18Z

Not a problem at all. This is some kind of working process while making better contribution. Restored back vectors usage in 0ddb73c.

There are also tests covering new functionality were added in 355b95b. What is need ToGo - it is documentation (MG, Configuration pages, tutorial).

AlexeyMerzlyakov · 2023-12-01T11:44:44Z

Everything is set to be completed for today:

All review comments were met
Testcases developed. Code coverage for Vector Object server is around ~97% which is fine:

And aligns with overall Map Server code coverage:
Documentation update made in Add Vector Object server documentation docs.nav2.org#496

mergify · 2023-12-23T01:59:48Z

This pull request is in conflict. Could you fix it @AlexeyMerzlyakov?

AlexeyMerzlyakov · 2024-01-26T08:45:49Z

Force-pushed the branch resolving conflicts with latest main. @SteveMacenski, do we plan to merge this PR or this feature is no more necessary?

PranavShevkar · 2024-01-26T16:49:19Z

Tested on Rolling. Looks good and works fine

mergify · 2024-02-13T22:38:28Z

This pull request is in conflict. Could you fix it @AlexeyMerzlyakov?

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

AlexeyMerzlyakov · 2024-02-22T10:05:15Z

Resolved small merge conflict and force-pushed development branch.
@SteveMacenski, are we ready to merge this change? (from my side, everything: change, tests, docs - are ready)

mergify · 2024-04-23T01:04:11Z

This pull request is in conflict. Could you fix it @AlexeyMerzlyakov?

ajtudela · 2024-05-12T21:00:59Z

Bumping this!

This server would be very beneficial for one our current use cases. Let me know if I can help to wrap this up.

SteveMacenski · 2024-05-13T20:34:29Z

This is functionally on permanent hold as Alexey is no longer working on this project in a professional capacity. If someone wants to pick this up, test it, and resubmit a new PR with fixes made, we're happy to review!

AlexeyMerzlyakov commented Nov 2, 2023

View reviewed changes

SteveMacenski reviewed Nov 7, 2023

View reviewed changes

SteveMacenski requested changes Nov 13, 2023

View reviewed changes

SteveMacenski reviewed Nov 15, 2023

View reviewed changes

nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp Outdated Show resolved Hide resolved

nav2_map_server/src/vo_server/vector_object_server.cpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Nov 15, 2023

View reviewed changes

nav2_map_server/src/vo_server/vector_object_shapes.cpp Outdated Show resolved Hide resolved

AlexeyMerzlyakov mentioned this pull request Nov 29, 2023

Add Vector Object server documentation ros-navigation/docs.nav2.org#496

Open

AlexeyMerzlyakov force-pushed the vo_server_dev_pr branch from 2a46ab5 to 1665d84 Compare January 26, 2024 08:41

AlexeyMerzlyakov force-pushed the vo_server_dev_pr branch from 1665d84 to 60f647b Compare January 26, 2024 08:47

AlexeyMerzlyakov and others added 12 commits February 22, 2024 13:01

Add Vector Object server

d2cabdf

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Meet review comments

726eac8

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Simplify shapes param configuring

a225fce

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Rename getROSParameter() to getParameter()

1ba3920

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Return back getMaskData() to nav2_costmap_2d

8bd61a3

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Add composition node support

2c0322f

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Remove redundant methods

b6ea965

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Update nav2_map_server/src/vo_server/vector_object_server.cpp

c987eb4

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Avoid shapes clearing

3cade88

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Optimize switchMapUpdate() method

0ffb216

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Switch to vector of shapes

50d4cac

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Minor fixes

2ece7c2

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

AlexeyMerzlyakov added 7 commits February 22, 2024 13:01

Meet review comments

43bbeca

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Move isPointInside algorithm to nav2_util

d3a832e

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Testcases covering new functionality

2b3bf84

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Fix linting issues

2bf3fb7

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Adjust for Vector Objects demonstration

3197856

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Code clean-up

e2883b6

* Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

Additional code facelift

67a16b0

* Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

AlexeyMerzlyakov force-pushed the vo_server_dev_pr branch from 60f647b to 67a16b0 Compare February 22, 2024 10:03

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add Vector Object server #3930

Add Vector Object server #3930

AlexeyMerzlyakov commented Nov 2, 2023 •

edited

AlexeyMerzlyakov Nov 2, 2023

SteveMacenski Nov 13, 2023

AlexeyMerzlyakov Dec 1, 2023

SteveMacenski left a comment

AlexeyMerzlyakov commented Nov 7, 2023 •

edited

SteveMacenski left a comment

SteveMacenski Nov 13, 2023

AlexeyMerzlyakov Nov 14, 2023

AlexeyMerzlyakov Nov 14, 2023 •

edited

SteveMacenski Nov 15, 2023

AlexeyMerzlyakov commented Nov 16, 2023 •

edited

SteveMacenski commented Nov 18, 2023 •

edited

AlexeyMerzlyakov commented Nov 20, 2023 •

edited

AlexeyMerzlyakov commented Dec 1, 2023

mergify bot commented Dec 23, 2023

AlexeyMerzlyakov commented Jan 26, 2024

PranavShevkar commented Jan 26, 2024

mergify bot commented Feb 13, 2024

AlexeyMerzlyakov commented Feb 22, 2024 •

edited

mergify bot commented Apr 23, 2024

ajtudela commented May 12, 2024 •

edited

SteveMacenski commented May 13, 2024

Add Vector Object server #3930

Are you sure you want to change the base?

Add Vector Object server #3930

Conversation

AlexeyMerzlyakov commented Nov 2, 2023 • edited

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

AlexeyMerzlyakov Nov 2, 2023

Choose a reason for hiding this comment

SteveMacenski Nov 13, 2023

Choose a reason for hiding this comment

AlexeyMerzlyakov Dec 1, 2023

Choose a reason for hiding this comment

SteveMacenski left a comment

Choose a reason for hiding this comment

AlexeyMerzlyakov commented Nov 7, 2023 • edited

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Nov 13, 2023

Choose a reason for hiding this comment

AlexeyMerzlyakov Nov 14, 2023

Choose a reason for hiding this comment

AlexeyMerzlyakov Nov 14, 2023 • edited

Choose a reason for hiding this comment

SteveMacenski Nov 15, 2023

Choose a reason for hiding this comment

AlexeyMerzlyakov commented Nov 16, 2023 • edited

SteveMacenski commented Nov 18, 2023 • edited

AlexeyMerzlyakov commented Nov 20, 2023 • edited

AlexeyMerzlyakov commented Dec 1, 2023

mergify bot commented Dec 23, 2023

AlexeyMerzlyakov commented Jan 26, 2024

PranavShevkar commented Jan 26, 2024

mergify bot commented Feb 13, 2024

AlexeyMerzlyakov commented Feb 22, 2024 • edited

mergify bot commented Apr 23, 2024

ajtudela commented May 12, 2024 • edited

SteveMacenski commented May 13, 2024

AlexeyMerzlyakov commented Nov 2, 2023 •

edited

AlexeyMerzlyakov commented Nov 7, 2023 •

edited

AlexeyMerzlyakov Nov 14, 2023 •

edited

AlexeyMerzlyakov commented Nov 16, 2023 •

edited

SteveMacenski commented Nov 18, 2023 •

edited

AlexeyMerzlyakov commented Nov 20, 2023 •

edited

AlexeyMerzlyakov commented Feb 22, 2024 •

edited

ajtudela commented May 12, 2024 •

edited