diff --git a/configuration/packages/configuring-map-server.rst b/configuration/packages/configuring-map-server.rst index 86fdf26348..2bdea07ba7 100644 --- a/configuration/packages/configuring-map-server.rst +++ b/configuration/packages/configuring-map-server.rst @@ -1,218 +1,18 @@ .. _configuring_map_server: -Map Server / Saver +Map Server ################## Source code on Github_. .. _Github: https://github.com/ros-navigation/navigation2/tree/main/nav2_map_server -The Map Server implements the server for handling the map load requests for the stack and host a map topic. -It also implements a map saver server which will run in the background and save maps based on service requests. There exists a map saver CLI similar to ROS 1 as well for a single map save. +The Map server package implements various components for handling grid maps, including loading, saving, and publishing maps and their metadata. Currently the following components are supported in Nav2: -Map Saver Parameters -******************** +.. toctree:: + :maxdepth: 1 -:save_map_timeout: - - ============== ======= - Type Default - -------------- ------- - int 2.0 - ============== ======= - - Description - Timeout to attempt saving the map (seconds). - -:free_thresh_default: - - ============== ============== - Type Default - -------------- -------------- - double 0.25 - ============== ============== - - Description - Free space maximum probability threshold value for occupancy grid. - -:occupied_thresh_default: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.65 - ============== ============================= - - Description - Occupied space minimum probability threshold value for occupancy grid. - -:introspection_mode: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "disabled" - ============== ============================= - - Description - The introspection mode for services and actions. Options are "disabled", "metadata", "contents". - -Map Server Parameters -********************* - -:yaml_filename: - - ============== ============================= - Type Default - -------------- ----------------------------- - string N/A - ============== ============================= - - Description - Path to map yaml file. Note from Rolling + Iron-Turtle forward: This parameter can set either from the yaml file or using the launch configuration parameter ``map``. If we set it on launch commandline / launch configuration default, we override the yaml default. If you would like the specify your map file in yaml, remove the launch default so it is not overridden in Nav2's default launch files. Before Iron: ``yaml_filename`` must be set in the yaml (even if a bogus value) so that our launch scripts can overwrite it with launch values. - -:topic_name: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "map" - ============== ============================= - - Description - Topic to publish loaded map to. - -:frame_id: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "map" - ============== ============================= - - Description - Frame to publish loaded map in. - -:introspection_mode: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "disabled" - ============== ============================= - - Description - The introspection mode for services and actions. Options are "disabled", "metadata", "contents". - -Costmap Filter Info Server Parameters -************************************* - -:type: - - ============== ============================= - Type Default - -------------- ----------------------------- - int 0 - ============== ============================= - - Description - Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: - - - 0: keepout zones / preferred lanes filter - - 1: speed filter, speed limit is specified in % of maximum speed - - 2: speed filter, speed limit is specified in absolute value (m/s) - - 3: binary filter - -:filter_info_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "costmap_filter_info" - ============== ============================= - - Description - Topic to publish costmap filter information to. - -:mask_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "filter_mask" - ============== ============================= - - Description - Topic to publish filter mask to. The value of this parameter should be in accordance with ``topic_name`` parameter of Map Server tuned to filter mask publishing. - -:base: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.0 - ============== ============================= - - Description - Base of ``OccupancyGrid`` mask value -> filter space value linear conversion which is being proceeded as: - ``filter_space_value = base + multiplier * mask_value`` - -:multiplier: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.0 - ============== ============================= - - Description - Multiplier of ``OccupancyGrid`` mask value -> filter space value linear conversion which is being proceeded as: - ``filter_space_value = base + multiplier * mask_value`` - -:bond_heartbeat_period: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description - The lifecycle node bond mechanism publishing period (on the /bond topic). Disabled if inferior or equal to 0.0. - -:allow_parameter_qos_overrides: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool true - ============== ============================= - - Description - Whether to allow QoS profiles to be overwritten with parameterized values. - -Example -******* -.. code-block:: yaml - - map_server: - ros__parameters: - yaml_filename: "turtlebot3_world.yaml" - topic_name: "map" - frame_id: "map" - introspection_mode: "disabled" - - map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - introspection_mode: "disabled" - - costmap_filter_info_server: - ros__parameters: - type: 1 - filter_info_topic: "costmap_filter_info" - mask_topic: "filter_mask" - base: 0.0 - multiplier: 0.25 + map_server/configuring-map-server.rst + map_server/configuring-map-saver.rst + map_server/configuring-costmap-filter-info-server.rst + map_server/configuring-vector-object-server.rst diff --git a/configuration/packages/map_server/configuring-costmap-filter-info-server.rst b/configuration/packages/map_server/configuring-costmap-filter-info-server.rst new file mode 100644 index 0000000000..1360973422 --- /dev/null +++ b/configuration/packages/map_server/configuring-costmap-filter-info-server.rst @@ -0,0 +1,119 @@ +.. _configuring_costmap_filter_info_server: + +Costmap Filter Info Server +########################### + +The costmap filter info server is responsible for providing information about the :ref:`Costmap Filters ` being used in the navigation stack. It publishes costmap filter mask specific metadata on a configured topic. This metadata is used by other components in the system to interpret the values in the costmap filter masks. + +Costmap Filter Info Server Parameters +************************************* + +:type: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 0 + ============== ============================= + + Description + Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: + + - 0: keepout zones / preferred lanes filter + - 1: speed filter, speed limit is specified in % of maximum speed + - 2: speed filter, speed limit is specified in absolute value (m/s) + - 3: binary filter + +:filter_info_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "costmap_filter_info" + ============== ============================= + + Description + Topic to publish costmap filter information to. + +:mask_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "filter_mask" + ============== ============================= + + Description + Topic to publish filter mask to. The value of this parameter should be in accordance with ``topic_name`` parameter of Map Server tuned to filter mask publishing. + +:base: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.0 + ============== ============================= + + Description + Base of ``OccupancyGrid`` mask value -> filter space value linear conversion which is being proceeded as: + ``filter_space_value = base + multiplier * mask_value`` + +:multiplier: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description + Multiplier of ``OccupancyGrid`` mask value -> filter space value linear conversion which is being proceeded as: + ``filter_space_value = base + multiplier * mask_value`` + +:bond_heartbeat_period: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + The lifecycle node bond mechanism publishing period (on the /bond topic). Disabled if inferior or equal to 0.0. + +:allow_parameter_qos_overrides: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description + Whether to allow QoS profiles to be overwritten with parameterized values. + +Example +******* +.. code-block:: yaml + + map_server: + ros__parameters: + yaml_filename: "turtlebot3_world.yaml" + topic_name: "map" + frame_id: "map" + introspection_mode: "disabled" + + map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + introspection_mode: "disabled" + + costmap_filter_info_server: + ros__parameters: + type: 1 + filter_info_topic: "costmap_filter_info" + mask_topic: "filter_mask" + base: 0.0 + multiplier: 0.25 diff --git a/configuration/packages/map_server/configuring-map-saver.rst b/configuration/packages/map_server/configuring-map-saver.rst new file mode 100644 index 0000000000..c12ca78dd4 --- /dev/null +++ b/configuration/packages/map_server/configuring-map-saver.rst @@ -0,0 +1,53 @@ +.. _configuring_map_saver: + +Map Saver +######### + +The map saver server runs in the background and saves maps based on service requests. There exists a map saver CLI similar to ROS 1 as well for a single map save. + +Map Saver Parameters +******************** + +:save_map_timeout: + + ============== ======= + Type Default + -------------- ------- + int 2.0 + ============== ======= + + Description + Timeout to attempt saving the map (seconds). + +:free_thresh_default: + + ============== ============== + Type Default + -------------- -------------- + double 0.25 + ============== ============== + + Description + Free space maximum probability threshold value for occupancy grid. + +:occupied_thresh_default: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.65 + ============== ============================= + + Description + Occupied space minimum probability threshold value for occupancy grid. + +:introspection_mode: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "disabled" + ============== ============================= + + Description + The introspection mode for services and actions. Options are "disabled", "metadata", "contents". diff --git a/configuration/packages/map_server/configuring-map-server.rst b/configuration/packages/map_server/configuring-map-server.rst new file mode 100644 index 0000000000..61f5b83bc4 --- /dev/null +++ b/configuration/packages/map_server/configuring-map-server.rst @@ -0,0 +1,53 @@ +.. _configuring_map_server_params: + +Map Server +########## + +The Map Server implements the server for handling the map load requests for the stack and hosts a map topic. + +Map Server Parameters +********************* + +:yaml_filename: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description + Path to map yaml file. Note from Rolling + Iron-Turtle forward: This parameter can set either from the yaml file or using the launch configuration parameter ``map``. If we set it on launch commandline / launch configuration default, we override the yaml default. If you would like the specify your map file in yaml, remove the launch default so it is not overridden in Nav2's default launch files. Before Iron: ``yaml_filename`` must be set in the yaml (even if a bogus value) so that our launch scripts can overwrite it with launch values. + +:topic_name: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "map" + ============== ============================= + + Description + Topic to publish loaded map to. + +:frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "map" + ============== ============================= + + Description + Frame to publish loaded map in. + +:introspection_mode: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "disabled" + ============== ============================= + + Description + The introspection mode for services and actions. Options are "disabled", "metadata", "contents". diff --git a/configuration/packages/map_server/configuring-vector-object-server.rst b/configuration/packages/map_server/configuring-vector-object-server.rst new file mode 100644 index 0000000000..449aae5997 --- /dev/null +++ b/configuration/packages/map_server/configuring-vector-object-server.rst @@ -0,0 +1,325 @@ +.. _configuring_vector_object_server: + +Vector Object Server +#################### + +The Vector Object Server implements a server 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 output raster maps of polygon objects or regions 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). +Rather than rastering vector objects each iteration or at run-time, it is done one-time on startup, and served to costmap layers or other consumers improving performance. +A typical setup model for this is a Nav2 stack with Costmap Filters enabled, running in conjunction with a Vector Object server, which produces vectorised OccupancyGrid maps as input masks for the Costmap Filters: + +.. image:: images/vector_object_server/vo_design.png + :width: 1000px + :align: center + +These vector shapes could be added by using ROS input parameters as well as being handled by the following service calls: ``AddShapes.srv`` which adds new shapes or modifies existing ones, ``RemoveShapes.srv`` which removes any or all shapes from the map ``GetShapes.srv`` which returns all shapes on the map. + +Each vector shape is being handled by its ``unique_identifier_msgs/UUID``. Developers can choose whether to specify it manually for a new shape, or have it generated automatically by the Vector Object server. The UUID can always be obtained by making a ``GetShapes.srv`` request and getting the response with all shapes' UUIDs and their properties. + +The Vector Object server places shapes on the map. Each vector object has its own value in the range from ``{-1}, [0..100]``, which matches the OccupancyGrid values. Vector objects can be overlapped with each other by using one of the global overlapping rules: (a) **sequential overlapping** in the same order as vector objects arrived on the server, or (b) taking the **maximum / minimum value** from all vector objects and the map background (if it is known). + +This page describes all the configuration parameters of the Vector Object server. For more information on 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 for placing on a map: + + - Polygons + - Circles + +- Polygons can be filled with any value or drawn as a 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 can be filled with any value or drawn without any fill (only the circle boundary is placed on the 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 shapes or modifies existing ones + - ``RemoveShapes.srv``: removes any or all shapes from the map + - ``GetShapes.srv``: gets all shapes and their properties + +- Vector shapes are being identified by their UUID (``unique_identifier_msgs/UUID``), which is generated automatically for a new shape, or could be given manually by the developer. + +- Vector shapes can be placed in any frame: + + - If at least one of the shapes is set in a different frame than the map, a dynamic update model will 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 frame as map, map will be published/updated once: at startup of Vector Object server and on each call of ``AddShapes.srv`` or ``RemoveShapes.srv`` to change the shape. + +Covered use-cases +================= + +Using Vector Object server publishing an output map as input mask to :ref:`Costmap Filters ` allows the following example use-cases using polygon, vector representations of areas rather than rastered masks: + +- No-access zone +- Speed-restriction areas +- Virtual obstacles on costmap +- Geofence / 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 vertices, 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/map_server/images/vector_object_server/circle_fill.png b/configuration/packages/map_server/images/vector_object_server/circle_fill.png new file mode 100644 index 0000000000..1902d02f28 Binary files /dev/null and b/configuration/packages/map_server/images/vector_object_server/circle_fill.png differ diff --git a/configuration/packages/map_server/images/vector_object_server/polygon_closed.png b/configuration/packages/map_server/images/vector_object_server/polygon_closed.png new file mode 100644 index 0000000000..f80f9e14e2 Binary files /dev/null and b/configuration/packages/map_server/images/vector_object_server/polygon_closed.png differ diff --git a/configuration/packages/map_server/images/vector_object_server/vo_config_map.png b/configuration/packages/map_server/images/vector_object_server/vo_config_map.png new file mode 100644 index 0000000000..0ab6bf3152 Binary files /dev/null and b/configuration/packages/map_server/images/vector_object_server/vo_config_map.png differ diff --git a/configuration/packages/map_server/images/vector_object_server/vo_design.png b/configuration/packages/map_server/images/vector_object_server/vo_design.png new file mode 100644 index 0000000000..c6604e5161 Binary files /dev/null and b/configuration/packages/map_server/images/vector_object_server/vo_design.png differ diff --git a/migration/Foxy.rst b/migration/Foxy.rst index ea049875ab..2597b1d5cd 100644 --- a/migration/Foxy.rst +++ b/migration/Foxy.rst @@ -137,6 +137,8 @@ Original GitHub tickets: - `PhotoAtWaypoint `_ - `InputAtWaypoint `_ +.. _costmap_filters: + Costmap Filters *************** diff --git a/migration/Kilted.rst b/migration/Kilted.rst index fddcb16ad7..e0d9e4e44c 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -358,3 +358,17 @@ Default value: - window_size: 7 - poly_order: 3 + + +Vector Objects were Supported for Raster Maps +--------------------------------------------- + +`PR #5479 `_ 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 0000000000..3e82a28f67 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 0000000000..9f32aa2b02 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 0000000000..684d540941 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 0000000000..91516a5129 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 0000000000..2e1916795c Binary files /dev/null and b/tutorials/docs/images/Vector_Object_server/vector_objects_updated.png differ diff --git a/tutorials/docs/navigation2_with_vector_objects.rst b/tutorials/docs/navigation2_with_vector_objects.rst new file mode 100644 index 0000000000..11d2ca1716 --- /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. +They can be used for various purposes, such as programmatically generating complete occupancy grid maps for navigation, adding virtual static/dynamic obstacles on an existing map, or like :ref:`Costmap Filters ` do with rastered maps, but on a vector (polygon or shape) basis rather than annotating a map-sized mask. + +In this tutorial, the added vector objects will be treated as obstacles in costmaps using a Keepout Filter. +To do this, we need to prepare the Nav2 stack with the Keepout Filter enabled, along with the Vector Object server which publishes an ``OccupancyGrid`` map with the rasterized vector objects as an input mask for the Keepout Filter. +Other use cases use similar principles and could be easily adapted after finishing this tutorial. + +.. note:: + + Using with Keepout Filter is a good choice for adding virtual obstacles or removing some areas from costmaps. However, the Vector Object server is not restricted to this application. It can be paired with different Costmap Filters for other use cases or even other applications entirely. For example, to represent polygonal speed restriction areas, a polygon-defined room where the camera is to be turned off using the Binary Filter, or using custom spatial / polygon applications. + +Requirements +============ + +It is assumed ROS 2 and Nav2 dependent packages are installed or built locally. +Please follow the instructions 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 purposes, let's specify two obstacle shapes: triangle polygon and circle filled with "occupied" value, in order to prevent 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 require ``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 launched by the ``vector_object_server.launch.py`` script. + +The complete ``vector_object_server_params.yaml`` YAML-file for the demonstration looks 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: "vo_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_costmap_filter_info_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. + +.. code-block:: bash + + ros2 launch nav2_map_server vector_object_server.launch.py + +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: ["keepout_filter", "speed_filter", "vector_object_layer"] + ... + vector_object_layer: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "vo_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 + [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 '/lifecycle_manager_vo_server' in container 'nav2_container' + [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/vector_object_server' in container 'nav2_container' + [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/costmap_filter_info_server' in container 'nav2_container' + +The last lines mean that all three nodes: Vector Object server, Costmap Filter Info server, and the Lifecycle Manager handling them, were successfully loaded into the 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 /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 /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 /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 bb6766ba10..d440fcd695 100644 --- a/tutorials/index.rst +++ b/tutorials/index.rst @@ -28,3 +28,4 @@ Nav2 Tutorials docs/get_profile.rst docs/docker_dev.rst docs/route_server_tools.rst + docs/navigation2_with_vector_objects.rst