Costmap Filters

doc/parameters/param_list.md

@@ -72,7 +72,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 
 ## static_layer plugin
 
-* `<static layer>`: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugin_names`, default value is `static_layer`
+* `<static layer>`: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugins`, default value is `static_layer`
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -84,7 +84,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 
 ## inflation_layer plugin
 
-* `<inflation layer>`: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugin_names`, default value is `inflation_layer`
+* `<inflation layer>`: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugins`, default value is `inflation_layer`
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -96,7 +96,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 
 ## obstacle_layer plugin
 
-* `<obstacle layer>`: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugin_names`, default value is `obstacle_layer`
+* `<obstacle layer>`: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugins`, default value is `obstacle_layer`
 * `<data source>`: Name of a source provided in ``<obstacle layer>`.observation_sources`
 
 | Parameter | Default | Description |
@@ -121,7 +121,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 
 ## range_sensor_layer plugin
 
-* `<range layer>`: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugin_names`.
+* `<range layer>`: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugins`.
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -137,10 +137,10 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 
 ## voxel_layer plugin
 
-* `<voxel layer>`: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugin_names`
+* `<voxel layer>`: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugins`
 * `<data source>`: Name of a source provided in `<voxel layer>`.observation_sources`
 
-*Note*: These parameters will only get declared if a `<voxel layer>` name such as `voxel_layer` is appended to `plugin_names` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to `plugin_types` parameter.
+*Note*: These parameters will only get declared if a `<voxel layer>` name such as `voxel_layer` is appended to `plugins` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to its `plugin` name parameter.
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
@@ -168,6 +168,15 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
 | `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | 
 
+## keepout filter
+
+* `<filter name>`: Name corresponding to the `nav2_costmap_2d::KeepoutFilter` plugin. This name gets defined in `plugins`.
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<filter name>`.enabled | true | Whether it is enabled |
+| `<filter name>`.filter_info_topic | N/A | Name of the CostmapFilterInfo topic having filter-related information |
+
 # controller_server
 
 | Parameter | Default | Description |
@@ -423,13 +432,14 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 
 # map_server
 
-## map_server
+## map_saver
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | save_map_timeout | 2000 | Timeout to attempt to save map with (ms) |
 | free_thresh_default | 0.25 | Free space maximum threshold for occupancy grid |
 | occupied_thresh_default | 0.65 | Occupied space minimum threshhold for occupancy grid |
+| map_subscribe_transient_local | true | Use transient local QoS profile for incoming map subscription |
 
 ## map_server
 

nav2_costmap_2d/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
   src/footprint_collision_checker.cpp
+  plugins/costmap_filters/costmap_filter.cpp
 )
 
 # prevent pluginlib from using boost
@@ -95,6 +96,16 @@ target_link_libraries(layers
   nav2_costmap_2d_core
 )
 
+add_library(filters SHARED
+  plugins/costmap_filters/keepout_filter.cpp
+)
+ament_target_dependencies(filters
+  ${dependencies}
+)
+target_link_libraries(filters
+  nav2_costmap_2d_core
+)
+
 add_library(nav2_costmap_2d_client SHARED
   src/footprint_subscriber.cpp
   src/costmap_subscriber.cpp
@@ -131,11 +142,13 @@ ament_target_dependencies(nav2_costmap_2d
 target_link_libraries(nav2_costmap_2d
   nav2_costmap_2d_core
   layers
+  filters
 )
 
 install(TARGETS
   nav2_costmap_2d_core
   layers
+  filters
   nav2_costmap_2d_client
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -169,7 +182,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client)
+ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client)
 ament_export_dependencies(${dependencies})
 pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
 ament_package()

nav2_costmap_2d/README.md

@@ -13,7 +13,7 @@ general ROS2 community. A proposal temporary replacement has been submitted as a
 ## How to configure using Voxel Layer plugin:
 By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic.
 
-The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used.
+The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugins``` and its ```plugin``` type should be correctly specified in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used.
 
 Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer.
 
@@ -24,9 +24,9 @@ Example param configuration snippet for enabling voxel layer in local costmap is
 local_costmap:
   local_costmap:
     ros__parameters:
-      plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"]
-      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
       obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
         observation_sources: scan
         scan:
@@ -36,6 +36,7 @@ local_costmap:
           marking: True
           data_type: "LaserScan"
       voxel_layer:
+        plugin: nav2_costmap_2d::VoxelLayer
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -74,9 +75,9 @@ For example, to add laser scan and pointcloud as two different sources of inputs
 local_costmap:
   local_costmap:
     ros__parameters:
-      plugin_names: ["obstacle_layer", "inflation_layer"]
-      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+      plugins: ["obstacle_layer", "inflation_layer"]
       obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
         observation_sources: scan pointcloud
         scan:
@@ -88,6 +89,12 @@ local_costmap:
 ```
 In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope.
 
+## Costmap Filters
+
+### Overview
+
+Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on navigation2 web-site: https://navigation.ros.org.
+
 ## Future Plans
 - Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like
 to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to

nav2_costmap_2d/costmap_plugins.xml

@@ -16,5 +16,11 @@
       <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
     </class>
   </library>
+
+  <library path="filters">
+    <class type="nav2_costmap_2d::KeepoutFilter"  base_class_type="nav2_costmap_2d::Layer">
+      <description>Prevents the robot from appearing in keepout zones marked on map.</description>
+    </class>
+  </library>
 </class_libraries>
 

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -48,6 +48,7 @@
 #include <queue>
 #include <mutex>
 #include "geometry_msgs/msg/point.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -87,6 +88,12 @@ class Costmap2D
    */
   Costmap2D(const Costmap2D & map);
 
+  /**
+   * @brief  Constructor for a costmap from an OccupancyGrid map
+   * @param  map The OccupancyGrid map to create costmap from
+   */
+  Costmap2D(const nav_msgs::msg::OccupancyGrid & map);
+
   /**
    * @brief  Overloaded assignment operator
    * @param  map The costmap to copy

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -0,0 +1,141 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020 Samsung Research Russia
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the <ORGANIZATION> nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Alexey Merzlyakov
+ *********************************************************************/
+
+#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_
+#define NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_
+
+#include <string>
+#include <mutex>
+
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+static constexpr double BASE_DEFAULT = 0.0;
+static constexpr double MULTIPLIER_DEFAULT = 1.0;
+
+/**
+ * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid
+ * hidden problems when the shared handling of costmap_ resource (PR #1936)
+ */
+class CostmapFilter : public Layer
+{
+public:
+  CostmapFilter();
+  ~CostmapFilter();
+
+  /**
+   * @brief: Provide a typedef to ease future code maintenance
+   */
+  typedef std::recursive_mutex mutex_t;
+  /**
+   * @brief: returns pointer to a mutex
+   */
+  mutex_t * getMutex()
+  {
+    return access_;
+  }
+
+  /** Layer API **/
+  virtual void onInitialize() final;
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw,
+    double * min_x, double * min_y, double * max_x, double * max_y) final;
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j) final;
+
+  virtual void activate() final;
+  virtual void deactivate() final;
+  virtual void reset() final;
+
+  /** CostmapFilter API **/
+  /**
+   * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics
+   * @param: Name of costmap filter info topic
+   */
+  virtual void initializeFilter(
+    const std::string & filter_info_topic) = 0;
+
+  /**
+   * @brief: An algorithm for how to use that map's information. Fills the Costmap2D with
+   *         calculated data and makes an action based on processed data
+   * @param: Reference to a master costmap2d
+   * @param: Low window map boundary OX
+   * @param: Low window map boundary OY
+   * @param: High window map boundary OX
+   * @param: High window map boundary OY
+   * @param: Robot 2D-pose
+   */
+  virtual void process(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j,
+    const geometry_msgs::msg::Pose2D & pose) = 0;
+
+  /**
+   * @brief: Resets costmap filter. Stops all subscriptions
+   */
+  virtual void resetFilter() = 0;
+
+protected:
+  /**
+   * @brief: Name of costmap filter info topic
+   */
+  std::string filter_info_topic_;
+
+  /**
+   * @brief: Name of filter mask topic
+   */
+  std::string mask_topic_;
+
+private:
+  /**
+   * @brief: Latest robot position
+   */
+  geometry_msgs::msg::Pose2D latest_pose_;
+
+  /**
+   * @brief: Mutex for locking filter's resources
+   */
+  mutex_t * access_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_