nav2_costmap_2d plugin container layer (#4781)

* updated CMakeLists.txt to include plugin_container_layer

Signed-off-by: alexander <alex@polymathrobotics.com>

* added plugin container layer to costmap_plugins.xml

Signed-off-by: alexander <alex@polymathrobotics.com>

* initial commit of plugin container layer

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed plugin namespace

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed message_filter

Signed-off-by: alexander <alex@polymathrobotics.com>

* linting

Signed-off-by: alexander <alex@polymathrobotics.com>

* modified addPlugin method to also take layer name

Signed-off-by: alexander <alex@polymathrobotics.com>

* reverted default plugins to be empty, free costmap_ during layer destruction, changed addPlugin to take layer name as an argument

Signed-off-by: alexander <alex@polymathrobotics.com>

* CMake changes to test plugin container layer

Signed-off-by: alexander <alex@polymathrobotics.com>

* added helper method to add plugin container layer

Signed-off-by: alexander <alex@polymathrobotics.com>

* added initial implementation of plugin container tests

Signed-off-by: alexander <alex@polymathrobotics.com>

* added enable to dynamic params, removed unecessary comments, removed unecessary members

Signed-off-by: alexander <alex@polymathrobotics.com>

* cleaned up and added additional tests

Signed-off-by: alexander <alex@polymathrobotics.com>

* added Apache copyrights

Signed-off-by: alexander <alex@polymathrobotics.com>

* linting for ament_cpplint

Signed-off-by: alexander <alex@polymathrobotics.com>

* added example file for plugin_container_layer to nav2_bringup

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed unused rolling_window_ member variable

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed default plugins and plugin types

Signed-off-by: alexander <alex@polymathrobotics.com>

* switched to using CombinationMethod enum, added updateWithMaxWithoutUnknownOverwrite case

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed combined_costmap_

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed layer naming and accomodating tests

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed nav2_params_plugin_container_layer.yaml

Signed-off-by: alexander <alex@polymathrobotics.com>

* added more comprehensive checks for checking if layers are clearable

Signed-off-by: alexander <alex@polymathrobotics.com>

* added dynamics parameter handling, fixed current_ setting, increased test coverage

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed unnecessary locks, added default value

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed unecessary resetMap

Signed-off-by: alexander <alex@polymathrobotics.com>

* added layer resetting when reset method is called

Signed-off-by: alexander <alex@polymathrobotics.com>

* swapped logic for isClearable

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed breaking tests, removed unnecessary combined_costmap_

Signed-off-by: alexander <alex@polymathrobotics.com>

* consolidated initialization for loops

Signed-off-by: alexander <alex@polymathrobotics.com>

* switched default_value_ to NO_INFORMATION

Signed-off-by: alexander <alex@polymathrobotics.com>

* added clearArea function

Signed-off-by: alexander <alex@polymathrobotics.com>

* added clearArea test

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed TODO message

Signed-off-by: alexander <alex@polymathrobotics.com>

* removed constructor and destructors since they do nothing

Signed-off-by: alexander <alex@polymathrobotics.com>

* added check on costmap layer to see if it is clearable first

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed tests for clearing functionality

Signed-off-by: alexander <alex@polymathrobotics.com>

* added try catch around initialization of plugins

Signed-off-by: alexander <alex@polymathrobotics.com>

* fixed indents

Signed-off-by: alexander <alex@polymathrobotics.com>

---------

Signed-off-by: alexander <alex@polymathrobotics.com>

nav2_costmap_2d/CMakeLists.txt

@@ -67,6 +67,7 @@ add_library(layers SHARED
   plugins/voxel_layer.cpp
   plugins/range_sensor_layer.cpp
   plugins/denoise_layer.cpp
+  plugins/plugin_container_layer.cpp
 )
 target_include_directories(layers
   PUBLIC

nav2_costmap_2d/costmap_plugins.xml

@@ -18,6 +18,9 @@
     <class type="nav2_costmap_2d::DenoiseLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>Filters noise-induced freestanding obstacles or small obstacles groups</description>
     </class>
+    <class type="nav2_costmap_2d::PluginContainerLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>Plugin to group different layers within the same costmap</description>
+    </class>
   </library>
 
   <library path="filters">

nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp

@@ -0,0 +1,128 @@
+// Copyright (c) 2024 Polymath Robotics, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_
+#define NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_costmap_2d/observation_buffer.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "tf2_ros/message_filter.h"
+#include "message_filters/subscriber.hpp"
+#include "pluginlib/class_loader.hpp"
+
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using nav2_costmap_2d::NO_INFORMATION;
+using rcl_interfaces::msg::ParameterType;
+namespace nav2_costmap_2d
+{
+/**
+ * @class PluginContainerLayer
+ * @brief Holds a list of plugins and applies them only to the specific layer
+ */
+class PluginContainerLayer : public CostmapLayer
+{
+public:
+  /**
+   * @brief Initialization process of layer on startup
+   */
+  virtual void onInitialize();
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update
+   *dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateBounds(
+    double robot_x,
+    double robot_y,
+    double robot_yaw,
+    double * min_x,
+    double * min_y,
+    double * max_x,
+    double * max_y);
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i,
+    int min_j,
+    int max_i,
+    int max_j);
+  virtual void onFootprintChanged();
+  /** @brief Update the footprint to match size of the parent costmap. */
+  virtual void matchSize();
+  /**
+   * @brief Deactivate the layer
+   */
+  virtual void deactivate();
+  /**
+   * @brief Activate the layer
+   */
+  virtual void activate();
+  /**
+   * @brief Reset this costmap
+   */
+  virtual void reset();
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
+  virtual bool isClearable();
+  /**
+   * @brief Clear an area in the constituent costmaps with the given dimension
+   * if invert, then clear everything except these dimensions
+   */
+  void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) override;
+
+  void addPlugin(std::shared_ptr<Layer> plugin, std::string layer_name);
+  pluginlib::ClassLoader<Layer> plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"};
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
+    std::vector<rclcpp::Parameter> parameters);
+
+private:
+  /// @brief Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+    dyn_params_handler_;
+
+  nav2_costmap_2d::CombinationMethod combination_method_;
+  std::vector<std::shared_ptr<Layer>> plugins_;
+  std::vector<std::string> plugin_names_;
+  std::vector<std::string> plugin_types_;
+};
+}  // namespace nav2_costmap_2d
+#endif  // NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_

nav2_costmap_2d/plugins/plugin_container_layer.cpp

@@ -0,0 +1,234 @@
+// Copyright (c) 2024 Polymath Robotics, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_costmap_2d/plugin_container_layer.hpp"
+
+#include "nav2_costmap_2d/costmap_math.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "rclcpp/parameter_events_filter.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::PluginContainerLayer, nav2_costmap_2d::Layer)
+
+using std::vector;
+
+namespace nav2_costmap_2d
+{
+
+void PluginContainerLayer::onInitialize()
+{
+  auto node = node_.lock();
+
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "enabled",
+      rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "plugins",
+      rclcpp::ParameterValue(std::vector<std::string>{}));
+  nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "combination_method",
+      rclcpp::ParameterValue(1));
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "plugins", plugin_names_);
+
+  int combination_method_param{};
+  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+  combination_method_ = combination_method_from_int(combination_method_param);
+
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &PluginContainerLayer::dynamicParametersCallback,
+      this,
+      std::placeholders::_1));
+
+  plugin_types_.resize(plugin_names_.size());
+
+  for (unsigned int i = 0; i < plugin_names_.size(); ++i) {
+    plugin_types_[i] = nav2_util::get_plugin_type_param(node, name_ + "." + plugin_names_[i]);
+    std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types_[i]);
+    addPlugin(plugin, plugin_names_[i]);
+  }
+
+  default_value_ = nav2_costmap_2d::NO_INFORMATION;
+
+  PluginContainerLayer::matchSize();
+  current_ = true;
+}
+
+void PluginContainerLayer::addPlugin(std::shared_ptr<Layer> plugin, std::string layer_name)
+{
+  plugins_.push_back(plugin);
+  auto node = node_.lock();
+  plugin->initialize(layered_costmap_, name_ + "." + layer_name, tf_, node, callback_group_);
+}
+
+void PluginContainerLayer::updateBounds(
+  double robot_x,
+  double robot_y,
+  double robot_yaw,
+  double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+  }
+}
+
+void PluginContainerLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & master_grid,
+  int min_i,
+  int min_j,
+  int max_i,
+  int max_j)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (!enabled_) {
+    return;
+  }
+
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->updateCosts(*this, min_i, min_j, max_i, max_j);
+  }
+
+  switch (combination_method_) {
+    case CombinationMethod::Overwrite:
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    case CombinationMethod::Max:
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    case CombinationMethod::MaxWithoutUnknownOverwrite:
+      updateWithMaxWithoutUnknownOverwrite(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    default:  // Nothing
+      break;
+  }
+
+  current_ = true;
+}
+
+void PluginContainerLayer::activate()
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->activate();
+  }
+}
+
+void PluginContainerLayer::deactivate()
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->deactivate();
+  }
+}
+
+void PluginContainerLayer::reset()
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->reset();
+  }
+  resetMaps();
+  current_ = false;
+}
+
+void PluginContainerLayer::onFootprintChanged()
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->onFootprintChanged();
+  }
+}
+
+void PluginContainerLayer::matchSize()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  Costmap2D * master = layered_costmap_->getCostmap();
+  resizeMap(
+    master->getSizeInCellsX(), master->getSizeInCellsY(),
+    master->getResolution(), master->getOriginX(), master->getOriginY());
+
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->matchSize();
+  }
+}
+
+bool PluginContainerLayer::isClearable()
+{
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    if((*plugin)->isClearable()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void PluginContainerLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert)
+{
+  CostmapLayer::clearArea(start_x, start_y, end_x, end_y, invert);
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
+    ++plugin)
+  {
+    auto costmap_layer = std::dynamic_pointer_cast<nav2_costmap_2d::CostmapLayer>(*plugin);
+    if ((*plugin)->isClearable() && costmap_layer != nullptr) {
+      costmap_layer->clearArea(start_x, start_y, end_x, end_y, invert);
+    }
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult PluginContainerLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "combination_method") {
+        combination_method_ = combination_method_from_int(parameter.as_int());
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+        current_ = false;
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_costmap_2d

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -177,9 +177,14 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
     layered_costmap_->addPlugin(plugin);
 
     // TODO(mjeronimo): instead of get(), use a shared ptr
-    plugin->initialize(
-      layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
-      shared_from_this(), callback_group_);
+    try {
+      plugin->initialize(layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
+          shared_from_this(), callback_group_);
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(get_logger(), "Failed to initialize costmap plugin %s! %s.",
+          plugin_names_[i].c_str(), e.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
 
     lock.unlock();