diff --git a/configuration/index.rst b/configuration/index.rst index 50f00ed7a..3894f4c7d 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -34,3 +34,4 @@ the best navigation performance. packages/configuring-velocity-smoother.rst packages/configuring-collision-monitor.rst packages/configuring-waypoint-follower.rst + packages/configuring-vector-object-server.rst diff --git a/configuration/packages/configuring-vector-object-server.rst b/configuration/packages/configuring-vector-object-server.rst new file mode 100644 index 000000000..5092ff3d1 --- /dev/null +++ b/configuration/packages/configuring-vector-object-server.rst @@ -0,0 +1,323 @@ +.. _configuring_vector_object_server: + +Vector Object Server +#################### + +Vector Object server - is a node that puts vector objects (such as polygons and circles) on OccupancyGrid raster map. The output raster map is being published by Vector Object server, and it could be used anywhere in the Nav2 stack or outside it. + +The main application of Vector Object server - is to combine output raster maps with existing costmaps of environment, targeting for robot navigation purposes (for example for dynamic obstacles simulation/highlighting, sensors noise removal, black-out areas on maps, synthetic testing purposes, and much more). +Typical set-up model for this - is Nav2 stack with enabled Costmap Filters in it, which is operating along with Vector Object server producing vectorized OccupancyGrid maps as an input mask for Costmap Filters: + +.. image:: images/vector_object_server/vo_design.png + :width: 1000px + :align: center + +These vector shapes could be added by using input ROS-parameters as well as being handled by the following service calls: ``AddShapes.srv`` which adds new or modifies existing shapes, ``RemoveShapes.srv`` that removes any or all shapes from map and ``GetShapes.srv`` which reports back all shapes on map. + +Each vector shape is being handled by its UUID which is of ``unique_identifier_msgs/UUID`` type. Final developer could choose whether to specify it manually for a new shape, or it will be generated automatically by the Vector Object server. The UUID can be always obtained by making a ``GetShapes.srv`` request and getting the response with all shapes' UUID-s and their properties. + +During its work, Vector Object server puts shapes on map. Each vector object has its own value in the range from ``{-1}, [0..100]`` that is meeting OccupancyGrid values. Vector objects could be overlapped with each other by using one of the global overlapping rules: sequential overlay in the same order as vector objects have been arrived to server, or taking the maximum / minimum value from all vector objects and the map background (if it is known). + +This page describes all configuration parameters of the Vector Object server. For more information how to navigate with your own Vector Object server, please refer to the :ref:`navigation2_with_vector_objects` tutorial. + +Features +******** + +- The following vector shapes are currently supported to be put on map: + + - Polygons + - Circles + +- Polygons could be filled with any value or drawn as polygonal chain, if it is not supposed to be a closed shape: + +.. image:: images/vector_object_server/polygon_closed.png + :width: 400px + :height: 200px + :align: center + +- Circles could be filled with some value or drawn without any fill (only circle border being put on map): + +.. image:: images/vector_object_server/circle_fill.png + :width: 400px + :height: 200px + :align: center + +- Vector shapes could be set once during the Vector Object server startup as ROS-parameters, and added/modified/removed over the time using the following service calls: + + - ``AddShapes.srv`` - adds new or modifies existing shapes + - ``RemoveShapes.srv`` - removes any or all shapes from map + - ``GetShapes.srv`` - obtains all shapes and their properties + +- Vector shapes are being identified by their UUID (``unique_identifier_msgs/UUID``), which is generating automatically for a new shape, or could be given manually by developer +- Vector shapes could be set in any frame: + + - If at least one of the shape is set in different than map's frame, dynamic update model to be enabled: this shape can move over the time, output map will be published dynamically with a given rate. + - If all shapes are set in the same as map frame, map is being published/updated once: during Vector Object server startup and per each shape changing call (``AddShapes.srv`` or ``RemoveShapes.srv``). + +Covered use-cases +***************** + +Using Vector Object server publishing an output map as input mask to :ref:`Costmap Filters ` allows covering following (not restricted only to) use-cases: + +- No-access zone +- Speed-restriction areas +- Virtual obstacles on costmap +- Flying zone for UAV-s with outer boundary and inner virtual obstacles +- Robot footprint (or any other moving objects) replacement +- Hiding some areas from costmap +- Sensors noise removal +- Dynamic objects simulation/highlighting +- Other testing purposes + +Parameters +********** + +:map_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "vo_map" + ============== ============================= + + Description: + Output topic, publishing an OccupancyGrid map with vector objects put on it. + +:global_frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "map" + ============== ============================= + + Description: + The name of the coordinate frame where the map is being published at. + +:resolution: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description: + Output map resolution in meters. + +:default_value: + + ============== ============================= + Type Default + -------------- ----------------------------- + int -1 (unknown) + ============== ============================= + + Description: + Default OccupancyGrid value to fill the background of output map with. + +:overlay_type: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 0 + ============== ============================= + + Description: + How one vector object to be overlaid with other and the map. + The following values are supported: + + - 0 (``OVERLAY_SEQ``): Vector objects are superimposed in the order in which they have arrived. + - 1 (``OVERLAY_MAX``): Maximum value from vector objects and map is being chosen. + - 2 (``OVERLAY_MIN``): Minimum value from vector objects and map is being chosen. Unknown OccupancyGrid value is always being overrode, when it is possible. + +:update_frequency: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description: + Output map update frequency (when dynamic update model is switched-on). + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description: + Transform tolerance for the case when any of the shapes are placed in different than map's frame. + +:shapes: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector {} + ============== ============================= + + Description: + List of vector objects (polygons and circles). Empty by-default. + +Shapes parameters +================= + +```` - is the corresponding shape name string selected for this vector object. + +:````.type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Type of vector object shape. Available values are ``polygon`` and ``circle``. Causes an error, if not specialized. + +:````.uuid: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + UUID of the shape specified in ``12345678-9abc-def0-1234-56789abcdef0`` format. Parameter is optional and could be skipped: if not specialized, Vector Object server will automatically generate a new one for the shape. + +:````.frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "" + ============== ============================= + + Description: + Frame ID of the given shape. Empty value is being treated as map's global frame. + +:````.value: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 100 (occupied) + ============== ============================= + + Description: + Shape's value to be put on map with. + +Parameters applicable for polygons only +--------------------------------------- + +:````.points: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}`` for the square). Minimum 3 points for a triangle polygon. Causes an error, if not specialized incorrectly (less than 6 or odd number of items in the vector) or not specialized. + +:````.closed: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description: + Whether the polygon is closed (and filled), or drawn as polygonal chain otherwise. + +Parameters applicable for circles only +-------------------------------------- + +:````.center: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + Center of the circle, listed in ``{center.x, center.y}`` format (e.g. ``{0.2, 0.3}``). Should contain exactly 2 items: X and Y coordinate of the circle's center in a given frame. Otherwise, causes an error. + +:````.radius: + + ============== ============================= + Type Default + -------------- ----------------------------- + double N/A + ============== ============================= + + Description: + Circle radius. Causes an error, if less than zero or not specialized. + +:````.fill: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description: + Whether the circle to be filled with a given value, or drawn only circle's border otherwise. + +Example +******* + +Here is an example of configuration YAML for the Vector Object server: + +.. code-block:: yaml + + vector_object_server: + ros__parameters: + map_topic: "vo_map" + global_frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "CircleA", "CircleB"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + CircleA: + type: "circle" + frame_id: "map" + fill: True + value: 10 + center: [3.0, 3.0] + radius: 0.5 + uuid: "7b3f3d7d-135c-4b6c-aca1-7a84d1050505" + CircleB: + type: "circle" + frame_id: "map" + fill: False + value: 90 + center: [3.5, 3.5] + radius: 1.5 + +For this, Vector Object server will produce the following map: + + +.. image:: images/vector_object_server/vo_config_map.png + :width: 636px + :height: 638px + :align: center diff --git a/configuration/packages/images/vector_object_server/circle_fill.png b/configuration/packages/images/vector_object_server/circle_fill.png new file mode 100644 index 000000000..1902d02f2 Binary files /dev/null and b/configuration/packages/images/vector_object_server/circle_fill.png differ diff --git a/configuration/packages/images/vector_object_server/polygon_closed.png b/configuration/packages/images/vector_object_server/polygon_closed.png new file mode 100644 index 000000000..f80f9e14e Binary files /dev/null and b/configuration/packages/images/vector_object_server/polygon_closed.png differ diff --git a/configuration/packages/images/vector_object_server/vo_config_map.png b/configuration/packages/images/vector_object_server/vo_config_map.png new file mode 100644 index 000000000..0ab6bf315 Binary files /dev/null and b/configuration/packages/images/vector_object_server/vo_config_map.png differ diff --git a/configuration/packages/images/vector_object_server/vo_design.png b/configuration/packages/images/vector_object_server/vo_design.png new file mode 100644 index 000000000..c6604e516 Binary files /dev/null and b/configuration/packages/images/vector_object_server/vo_design.png differ diff --git a/migration/Iron.rst b/migration/Iron.rst index 9354fa018..20bda2959 100644 --- a/migration/Iron.rst +++ b/migration/Iron.rst @@ -146,7 +146,7 @@ New features ``allow_primitive_interpolation`` which allows for more primitives New node in nav2_collision_monitor: Collision Detector ****************************************************** -In this `PR #3693 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. +In this `PR #3500 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the ``collision_detector_state`` topic that might be used by any external module (e.g. switching LED or sound alarm in case of collision). Dynamic enabling/disabling of sources/polygons in Collision Monitor/Detector @@ -254,3 +254,16 @@ In the GIF, it can be seen that there are two controller_ids namely, `FollowPath In this case, the `FollowPath` is the default controller_id. The difference between the two controller_ids is that HighSpeedFollowPath has a higher max velocity compared to the FollowPath. This difference can be well noted in the GIF. .. attention:: If a server is unavailable, then the combo box or the drop down list of the particular component will be empty. + +Vector Objects were Supported for Raster Maps +********************************************* + +`PR #3930 `_ adds new Vector Object server into ``nav2_map_server`` package. +It reads vector objects (polygons and polygonal chains as ``PolygonObject.msg``; and circles as ``CircleObject.msg``) from input parameters, handles them by service calls (``AddShapes.srv``/``GetShapes.srv``/``RemoveShapes.srv``) and finally puts them on output raster OccupancyGrid map. +This map is typically used with costmaps by acting as an input mask for Costmap Filters. +This allows to cover such use-cases as: +adding virtual obstacles on maps, dynamic objects simulation/highlighting, hiding some areas or sticking-out robot parts, sensors noise removal, blacking-out areas on maps, adding keep-out or maximum speed restricted areas on vector basis, synthetic testing purposes, and much more. + +To run Vector Object server a new ``vector_object_server.launch.py`` launch-file is being supplied. +:ref:`navigation2_with_vector_objects` tutorial explains how launch Vector Object server and navigate with vector objects added to raster costmaps. +The information about Vector Object server parameters set-up could be found at :ref:`configuring_vector_object_server` configuration guide. diff --git a/tutorials/docs/images/Vector_Object_server/vector_objects_avoidance.png b/tutorials/docs/images/Vector_Object_server/vector_objects_avoidance.png new file mode 100644 index 000000000..3e82a28f6 Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_avoidance.png differ diff --git a/tutorials/docs/images/Vector_Object_server/vector_objects_demo.gif b/tutorials/docs/images/Vector_Object_server/vector_objects_demo.gif new file mode 100644 index 000000000..9f32aa2b0 Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_demo.gif differ diff --git a/tutorials/docs/images/Vector_Object_server/vector_objects_on_costmap.png b/tutorials/docs/images/Vector_Object_server/vector_objects_on_costmap.png new file mode 100644 index 000000000..684d54094 Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_on_costmap.png differ diff --git a/tutorials/docs/images/Vector_Object_server/vector_objects_removed.png b/tutorials/docs/images/Vector_Object_server/vector_objects_removed.png new file mode 100644 index 000000000..91516a512 Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_removed.png differ diff --git a/tutorials/docs/images/Vector_Object_server/vector_objects_updated.png b/tutorials/docs/images/Vector_Object_server/vector_objects_updated.png new file mode 100644 index 000000000..2e1916795 Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_updated.png differ diff --git a/tutorials/docs/navigation2_with_keepout_filter.rst b/tutorials/docs/navigation2_with_keepout_filter.rst index 1f65ba1e3..33a453ce7 100644 --- a/tutorials/docs/navigation2_with_keepout_filter.rst +++ b/tutorials/docs/navigation2_with_keepout_filter.rst @@ -284,7 +284,7 @@ After Costmap Filter Info Publisher Server and Map Server were launched and Keep .. code-block:: bash - ros2 launch nav2_bringup tb3_simulation_launch.py + ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False And check that filter is working properly as in the pictures below (first picture shows keepout filter enabled for the global costmap, second - differently-sized ``keepout_mask.pgm`` filter mask): diff --git a/tutorials/docs/navigation2_with_speed_filter.rst b/tutorials/docs/navigation2_with_speed_filter.rst index fc1dc44af..6285e4dba 100644 --- a/tutorials/docs/navigation2_with_speed_filter.rst +++ b/tutorials/docs/navigation2_with_speed_filter.rst @@ -296,7 +296,7 @@ After Costmap Filter Info Publisher Server and Map Server were launched and Spee .. code-block:: bash - ros2 launch nav2_bringup tb3_simulation_launch.py + ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False For better visualization of speed filter mask, in RViz in the left ``Displays`` pane unfold ``Map`` and change ``Topic`` from ``/map`` -> to ``/speed_filter_mask``. Set the goal behind the speed restriction areas and check that the filter is working properly: robot should slow down when going through a speed restricting areas. Below is how it might look (first picture shows speed filter enabled for the global costmap, second - ``speed_mask.pgm`` filter mask): diff --git a/tutorials/docs/navigation2_with_vector_objects.rst b/tutorials/docs/navigation2_with_vector_objects.rst new file mode 100644 index 000000000..1986a4d23 --- /dev/null +++ b/tutorials/docs/navigation2_with_vector_objects.rst @@ -0,0 +1,219 @@ +.. _navigation2_with_vector_objects: + + +Navigating with Vector Objects +****************************** + +- `Overview`_ +- `Requirements`_ +- `Configuring Vector Object Server`_ +- `Preparing Nav2 stack`_ +- `Demo Execution`_ +- `Working with Vector Objects`_ + +.. image:: images/Vector_Object_server/vector_objects_demo.gif + +Overview +======== + +This tutorial shows how to navigate with vector objects added to raster costmaps. +It could be made for different purposes, e.g. for hiding some areas or sticking-out robot parts, for adding virtual obstacles on maps, or like Costmap Filters do - adding keep-out or maximum speed restricted areas, but on vector basis. +In current tutorial added vector objects will be treated as obstacles in costmaps. +For that, we need to prepare Navigation2 stack with enabled Keepout Filter in it, operating along with Vector Object server which publishing an OccupancyGrid map with rasterized vector objects as an input mask for Keepout Filter. +Other use-cases are utilizing the similar principles and could be trivially adjusted after this tutorial completion. + +.. note:: + + Using with Keepout Filter is the choice for adding virtual obstacles or removing some areas from costmaps. However, Vector Object server is not restricted to be operated with Keepout Filter only. It also could be used with different Costmap Filters for another use-cases. For example, for having speed restriction area defined by vector shape, one could choose Speed Filter; or for polygon-defined room where camera to be switched-off, Vector Object server could be operated with Binary Filter. + +Requirements +============ + +It is assumed ROS2 and Nav2 dependent packages are installed or built locally. +Please make sure that Nav2 project is also built locally as it was made in :ref:`build-instructions`. +For the best understanding how Keepout Filter works (which is the part of current configuration), it is also recommended to pass through the :ref:`navigation2_with_keepout_filter` tutorial. + + +Configuring Vector Object Server +================================ + +Vector Object server has its own ``vector_object_server.launch.py`` launch-file and preset parameters in the ``vector_object_server_params.yaml`` file for demonstration, though its trivial to add this to Nav2's main launch file if being used in practice. + +In this tutorial, we are focusing on the application how to utilize the simple setup allowing to add virtual obstacles on costmaps. +For demonstration of Vector Object server capabilities, let's specify two obstacle shapes: triangle polygon and circle filled with "occupied" value, in order to don't allow the robot to go through them. The YAML-part for polygon and circle will look as follows: + +.. code-block:: yaml + + shapes: ["Poly", "Circle"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: + type: "circle" + frame_id: "map" + fill: True + value: 100 + center: [1.5, 0.5] + radius: 0.7 + +Where the triangle polygon is specified by ``{0.3, 0.5}, {-0.4, 1.2}, {-0.4, -0.2}`` 3-point shape and the circle has ``{1.5, 0.5}`` coordinate of its center with ``0.7`` meter radius in the ``map`` frame. +``closed`` true-value for the polygon and ``fill`` for the circle mean that both shapes to be filled the with specified ``value``. +This value is equal to ``100`` which means "occupied" in OccupancyGrid format. + +.. note:: + + The frame for vector objects were specified the same as map's global frame. It was chosen for the simplicity to have static objects on map. However, it is possible to specify the shape in any frame different from global map's one. For this case, Vector Object server will use dynamic output map processing & publishing, suitable for moving objects. + +.. note:: + + Each shape is being addressed by UUID, which could be specified manually in a string format. In the demonstration, it was skipped to specify UUID of the shapes in the parameters, so Vector Object server will automatically generate a new one for each shape. The list of UUID could be obtained later by calling ``GetShapes.srv`` service. + +Costmap Filters are required ``CostmapFilterInfo.msg`` message to be published along with filter mask (rasterized map with vector shapes). +Costmap Filter Info message is being published by Costmap Filter Info server, which is also launching by ``vector_object_server.launch.py`` script. + +Overall ``vector_object_server_params.yaml`` YAML-file for the demonstration to be look as follows: + +.. code-block:: yaml + + vector_object_server: + ros__parameters: + map_topic: "vo_map" + global_frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "Circle"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: + type: "circle" + frame_id: "map" + fill: True + value: 100 + center: [1.5, 0.5] + radius: 0.7 + costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "costmap_filter_info" + mask_topic: "vo_map" + base: 0.0 + multiplier: 1.0 + +More detailed information about each Vector Object server parameter and its operating principle could be found on :ref:`configuring_vector_object_server` configuration guide page. Costmap Filter Info server parameters description could be found at :ref:`configuring_map_server` page. + +After Vector Objects and Costmap Filters Info servers were configured, launch them by command from below. +Robot should bypass vector obstacles. For the demonstration purposes it is enough to avoid path planning through them. +Thus, vector objects should be added to global costmap, so we need to choose ``global_costmap`` namespace for correct work: + +.. code-block:: bash + + ros2 launch nav2_map_server vector_object_server.launch.py namespace:=global_costmap + +Preparing Nav2 stack +==================== + +Vector Object server puts shapes to OccupacyGrid map and publishes it in a topic, which is used as an input mask for enabled in Nav2 Keepout Filter. +Enabling of Keeput Filter in Nav2 stack principles are similar as written in :ref:`navigation2_with_keepout_filter` tutorial. +Since vector objects are being enabled in global costmaps, Keepout Filter called as "vector_object_layer", should be added to the global costmap section of the ``nav2_params.yaml`` standard Nav2 configuration as follows: + +.. code-block:: yaml + + global_costmap: + ros__parameters: + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["vector_object_layer"] + ... + vector_object_layer: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "costmap_filter_info" + +Demo Execution +============== + +After Vector Object server was launched and Vector Object layer was enabled for the global costmap, run Nav2 stack as written in :ref:`getting_started`: + +.. code-block:: bash + + ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False + +We are using composable nodes technology, so in the console where Vector Object server run the following message should appear: + +.. code-block:: text + + [leha@leha-PC nav2_ws]$ ros2 launch nav2_map_server vector_object_server.launch.py namespace:=global_costmap + [INFO] [launch]: All log files can be found below /home/leha/.ros/log/2023-11-24-13-18-42-257011-leha-PC-18207 + [INFO] [launch]: Default logging verbosity is set to INFO + [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/global_costmap/lifecycle_manager_vo_server' in container 'nav2_container' + [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/global_costmap/vector_object_server' in container 'nav2_container' + [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/global_costmap/costmap_filter_info_server' in container 'nav2_container' + +The last lines mean that all three nodes: Vector Object server, Costmap Filter Info server and Lifecycle Manager handling them, were successfully loaded into running Nav2 container ``nav2_container``. + +Set the initial pose for the robot, and check that vector objects were appeared on global costmap: + + .. image:: images/Vector_Object_server/vector_objects_on_costmap.png + :width: 860px + +As well as for the Keepout Filter, robot will consider vector objects as obstacles on costmaps and will avoid them: + + .. image:: images/Vector_Object_server/vector_objects_avoidance.png + :width: 860px + +Working with Vector Objects +=========================== + +During the operation, vector objects can be changed, added or removed. +Let's change triangle shape to the bar. + +As was mentioned above, each shape is handled by its own UUID, which is being generated by Vector Object server if it is not specified explicitly in parameters. +To obtain shapes UUID, run the ``GetShapes.srv`` service request from the console: + +.. code-block:: bash + + ros2 service call /global_costmap/vector_object_server/get_shapes nav2_msgs/srv/GetShapes + +The output is expected to be the as follows: + +.. code-block:: text + + requester: making request: nav2_msgs.srv.GetShapes_Request() + + response: + nav2_msgs.srv.GetShapes_Response(circles=[nav2_msgs.msg.CircleObject(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='map'), uuid=unique_identifier_msgs.msg.UUID(uuid=array([73, 141, 241, 249, 116, 24, 69, 81, 178, 153, 159, 19, 245, 152, 28, 29], dtype=uint8)), center=geometry_msgs.msg.Point32(x=1.5, y=0.5, z=0.0), radius=0.699999988079071, fill=True, value=100)], polygons=[nav2_msgs.msg.PolygonObject(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='map'), uuid=unique_identifier_msgs.msg.UUID(uuid=array([153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39], dtype=uint8)), points=[geometry_msgs.msg.Point32(x=0.30000001192092896, y=0.5, z=0.0), geometry_msgs.msg.Point32(x=-0.4000000059604645, y=1.2000000476837158, z=0.0), geometry_msgs.msg.Point32(x=-0.4000000059604645, y=-0.20000000298023224, z=0.0)], closed=True, value=100)]) + +In our case, UUID for triangle polygon will be ``[153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39]``. + +Calling ``AddShapes.srv`` service will add new shape if no UUID was specified, or given UUID was not found. +If UUID is already existing, the shape will be updated. +The triangle to be changed to the bar polygon with 4 points. +Call the following service request with obtained UUID to do this: + +.. code-block:: bash + + ros2 service call /global_costmap/vector_object_server/add_shapes nav2_msgs/srv/AddShapes "polygons: [{points: [{x: 0.0, y: 1.0}, {x: -0.5, y: 1.0}, {x: -0.5, y: 0.0}, {x: 0.0, y: 0.0}], closed: true, value: 100, uuid: {uuid: [153, 128, 30, 121, 241, 60, 76, 15, 140, 187, 58, 60, 164, 241, 97, 39]}}]" + +The polygon shape in Vector Object server will be changed, ``vo_map`` will be updated and resulting costmap will look as follows - triangle obstacle was updated to bar: + + .. image:: images/Vector_Object_server/vector_objects_updated.png + +Finally, remove all shapes from map. +To remove any existing shape, there is ``RemoveShapes.srv`` service supported. It has array of UUID-s to specify which shapes to remove or just ``all_objects`` option for the case if we want to remove all shapes at once. Let's do the latter: + +.. code-block:: bash + + ros2 service call /global_costmap/vector_object_server/remove_shapes nav2_msgs/srv/RemoveShapes "all_objects: true" + +As a result, all vector shapes were disappeared from global costmap: + + .. image:: images/Vector_Object_server/vector_objects_removed.png diff --git a/tutorials/index.rst b/tutorials/index.rst index d1ce3cd60..87ebd3cbb 100644 --- a/tutorials/index.rst +++ b/tutorials/index.rst @@ -25,4 +25,5 @@ Navigation2 Tutorials docs/camera_calibration.rst docs/get_backtrace.rst docs/get_profile.rst - docs/docker_dev.rst \ No newline at end of file + docs/docker_dev.rst + docs/navigation2_with_vector_objects.rst