add humble-navigation2

navigation2/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp

@@ -0,0 +1,268 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022 Samsung R&D Institute 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
+ *********************************************************************/
+
+#include "nav2_costmap_2d/costmap_filters/binary_filter.hpp"
+
+#include <cmath>
+#include <utility>
+#include <memory>
+#include <string>
+
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+
+namespace nav2_costmap_2d
+{
+
+BinaryFilter::BinaryFilter()
+: filter_info_sub_(nullptr), mask_sub_(nullptr),
+  binary_state_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""),
+  default_state_(false), binary_state_(default_state_)
+{
+}
+
+void BinaryFilter::initializeFilter(
+  const std::string & filter_info_topic)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Declare parameters specific to BinaryFilter only
+  std::string binary_state_topic;
+  declareParameter("default_state", rclcpp::ParameterValue(false));
+  node->get_parameter(name_ + "." + "default_state", default_state_);
+  declareParameter("binary_state_topic", rclcpp::ParameterValue("binary_state"));
+  node->get_parameter(name_ + "." + "binary_state_topic", binary_state_topic);
+  declareParameter("flip_threshold", rclcpp::ParameterValue(50.0));
+  node->get_parameter(name_ + "." + "flip_threshold", flip_threshold_);
+
+  filter_info_topic_ = filter_info_topic;
+  // Setting new costmap filter info subscriber
+  RCLCPP_INFO(
+    logger_,
+    "BinaryFilter: Subscribing to \"%s\" topic for filter info...",
+    filter_info_topic_.c_str());
+  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
+    filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&BinaryFilter::filterInfoCallback, this, std::placeholders::_1));
+
+  // Get global frame required for binary state publisher
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  // Create new binary state publisher
+  binary_state_pub_ = node->create_publisher<std_msgs::msg::Bool>(
+    binary_state_topic, rclcpp::QoS(10));
+  binary_state_pub_->on_activate();
+
+  // Reset parameters
+  base_ = BASE_DEFAULT;
+  multiplier_ = MULTIPLIER_DEFAULT;
+
+  // Initialize state as "false" by-default
+  changeState(default_state_);
+}
+
+void BinaryFilter::filterInfoCallback(
+  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!mask_sub_) {
+    RCLCPP_INFO(
+      logger_,
+      "BinaryFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "BinaryFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
+      filter_info_topic_.c_str());
+    // Resetting previous subscriber each time when new costmap filter information arrives
+    mask_sub_.reset();
+  }
+
+  if (msg->type != BINARY_FILTER) {
+    RCLCPP_ERROR(logger_, "BinaryFilter: Mode %i is not supported", msg->type);
+    return;
+  }
+
+  // Set base_ and multiplier_
+  base_ = msg->base;
+  multiplier_ = msg->multiplier;
+  // Set topic name to receive filter mask from
+  mask_topic_ = msg->filter_mask_topic;
+
+  // Setting new filter mask subscriber
+  RCLCPP_INFO(
+    logger_,
+    "BinaryFilter: Subscribing to \"%s\" topic for filter mask...",
+    mask_topic_.c_str());
+  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&BinaryFilter::maskCallback, this, std::placeholders::_1));
+}
+
+void BinaryFilter::maskCallback(
+  const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (!filter_mask_) {
+    RCLCPP_INFO(
+      logger_,
+      "BinaryFilter: Received filter mask from %s topic.", mask_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "BinaryFilter: New filter mask arrived from %s topic. Updating old filter mask.",
+      mask_topic_.c_str());
+    filter_mask_.reset();
+  }
+
+  filter_mask_ = msg;
+  mask_frame_ = msg->header.frame_id;
+}
+
+void BinaryFilter::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)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (!filter_mask_) {
+    // Show warning message every 2 seconds to not litter an output
+    RCLCPP_WARN_THROTTLE(
+      logger_, *(clock_), 2000,
+      "BinaryFilter: Filter mask was not received");
+    return;
+  }
+
+  geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame
+
+  // Transforming robot pose from current layer frame to mask frame
+  if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) {
+    return;
+  }
+
+  // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
+  unsigned int mask_robot_i, mask_robot_j;
+  if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+    // Robot went out of mask range. Set "false" state by-default
+    RCLCPP_WARN(
+      logger_,
+      "BinaryFilter: Robot is outside of filter mask. Resetting binary state to default.");
+    changeState(default_state_);
+    return;
+  }
+
+  // Getting filter_mask data from cell where the robot placed
+  int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
+  if (mask_data == nav2_util::OCC_GRID_UNKNOWN) {
+    // Corresponding filter mask cell is unknown.
+    // Warn and do nothing.
+    RCLCPP_WARN_THROTTLE(
+      logger_, *(clock_), 2000,
+      "BinaryFilter: Filter mask [%i, %i] data is unknown. Do nothing.",
+      mask_robot_i, mask_robot_j);
+    return;
+  }
+  // Check and flip binary state, if necessary
+  if (base_ + mask_data * multiplier_ > flip_threshold_) {
+    if (binary_state_ == default_state_) {
+      changeState(!default_state_);
+    }
+  } else {
+    if (binary_state_ != default_state_) {
+      changeState(default_state_);
+    }
+  }
+}
+
+void BinaryFilter::resetFilter()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state");
+  changeState(default_state_);
+
+  filter_info_sub_.reset();
+  mask_sub_.reset();
+  if (binary_state_pub_) {
+    binary_state_pub_->on_deactivate();
+    binary_state_pub_.reset();
+  }
+}
+
+bool BinaryFilter::isActive()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (filter_mask_) {
+    return true;
+  }
+  return false;
+}
+
+void BinaryFilter::changeState(const bool state)
+{
+  binary_state_ = state;
+  if (state) {
+    RCLCPP_INFO(logger_, "BinaryFilter: Switched on");
+  } else {
+    RCLCPP_INFO(logger_, "BinaryFilter: Switched off");
+  }
+
+  // Forming and publishing new BinaryState message
+  std::unique_ptr<std_msgs::msg::Bool> msg =
+    std::make_unique<std_msgs::msg::Bool>();
+  msg->data = state;
+  binary_state_pub_->publish(std::move(msg));
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::BinaryFilter, nav2_costmap_2d::Layer)

navigation2/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -0,0 +1,212 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *         Alexey Merzlyakov
+ *********************************************************************/
+
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+
+#include <exception>
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+
+namespace nav2_costmap_2d
+{
+
+CostmapFilter::CostmapFilter()
+: filter_info_topic_(""), mask_topic_("")
+{
+  access_ = new mutex_t();
+}
+
+CostmapFilter::~CostmapFilter()
+{
+  delete access_;
+}
+
+void CostmapFilter::onInitialize()
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  try {
+    // Declare common for all costmap filters parameters
+    declareParameter("enabled", rclcpp::ParameterValue(true));
+    declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING);
+    declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
+
+    // Get parameters
+    node->get_parameter(name_ + "." + "enabled", enabled_);
+    filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string();
+    double transform_tolerance {};
+    node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
+    transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
+
+    // Costmap Filter enabling service
+    enable_service_ = node->create_service<std_srvs::srv::SetBool>(
+      name_ + "/toggle_filter",
+      std::bind(
+        &CostmapFilter::enableCallback, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what());
+    throw ex;
+  }
+}
+
+void CostmapFilter::activate()
+{
+  initializeFilter(filter_info_topic_);
+}
+
+void CostmapFilter::deactivate()
+{
+  resetFilter();
+}
+
+void CostmapFilter::reset()
+{
+  resetFilter();
+  initializeFilter(filter_info_topic_);
+  current_ = false;
+}
+
+void CostmapFilter::updateBounds(
+  double robot_x, double robot_y, double robot_yaw,
+  double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  latest_pose_.x = robot_x;
+  latest_pose_.y = robot_y;
+  latest_pose_.theta = robot_yaw;
+}
+
+void CostmapFilter::updateCosts(
+  nav2_costmap_2d::Costmap2D & master_grid,
+  int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  process(master_grid, min_i, min_j, max_i, max_j, latest_pose_);
+  current_ = true;
+}
+
+void CostmapFilter::enableCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+  std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+{
+  enabled_ = request->data;
+  response->success = true;
+  if (enabled_) {
+    response->message = "Enabled";
+  } else {
+    response->message = "Disabled";
+  }
+}
+
+bool CostmapFilter::transformPose(
+  const std::string global_frame,
+  const geometry_msgs::msg::Pose2D & global_pose,
+  const std::string mask_frame,
+  geometry_msgs::msg::Pose2D & mask_pose) const
+{
+  if (mask_frame != global_frame) {
+    // Filter mask and current layer are in different frames:
+    // Transform (global_pose.x, global_pose.y) point from current layer frame (global_frame)
+    // to mask_pose point in mask_frame
+    geometry_msgs::msg::TransformStamped transform;
+    geometry_msgs::msg::PointStamped in, out;
+    in.header.stamp = clock_->now();
+    in.header.frame_id = global_frame;
+    in.point.x = global_pose.x;
+    in.point.y = global_pose.y;
+    in.point.z = 0;
+
+    try {
+      tf_->transform(in, out, mask_frame, transform_tolerance_);
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR(
+        logger_,
+        "CostmapFilter: failed to get costmap frame (%s) "
+        "transformation to mask frame (%s) with error: %s",
+        global_frame.c_str(), mask_frame.c_str(), ex.what());
+      return false;
+    }
+    mask_pose.x = out.point.x;
+    mask_pose.y = out.point.y;
+  } else {
+    // Filter mask and current layer are in the same frame:
+    // Just use global_pose coordinates
+    mask_pose = global_pose;
+  }
+
+  return true;
+}
+
+bool CostmapFilter::worldToMask(
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
+  double wx, double wy, unsigned int & mx, unsigned int & my) const
+{
+  const double origin_x = filter_mask->info.origin.position.x;
+  const double origin_y = filter_mask->info.origin.position.y;
+  const double resolution = filter_mask->info.resolution;
+  const unsigned int size_x = filter_mask->info.width;
+  const unsigned int size_y = filter_mask->info.height;
+
+  if (wx < origin_x || wy < origin_y) {
+    return false;
+  }
+
+  mx = static_cast<unsigned int>((wx - origin_x) / resolution);
+  my = static_cast<unsigned int>((wy - origin_y) / resolution);
+  if (mx >= size_x || my >= size_y) {
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -0,0 +1,315 @@
+/*********************************************************************
+ *
+ * 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
+ *********************************************************************/
+
+#include <string>
+#include <memory>
+#include <algorithm>
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+
+namespace nav2_costmap_2d
+{
+
+KeepoutFilter::KeepoutFilter()
+: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr),
+  mask_frame_(""), global_frame_("")
+{
+}
+
+void KeepoutFilter::initializeFilter(
+  const std::string & filter_info_topic)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  filter_info_topic_ = filter_info_topic;
+  // Setting new costmap filter info subscriber
+  RCLCPP_INFO(
+    logger_,
+    "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
+    filter_info_topic_.c_str());
+  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
+    filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
+
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+}
+
+void KeepoutFilter::filterInfoCallback(
+  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!mask_sub_) {
+    RCLCPP_INFO(
+      logger_,
+      "KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
+      filter_info_topic_.c_str());
+    // Resetting previous subscriber each time when new costmap filter information arrives
+    mask_sub_.reset();
+  }
+
+  // Checking that base and multiplier are set to their default values
+  if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) {
+    RCLCPP_ERROR(
+      logger_,
+      "KeepoutFilter: For proper use of keepout filter base and multiplier"
+      " in CostmapFilterInfo message should be set to their default values (%f and %f)",
+      BASE_DEFAULT, MULTIPLIER_DEFAULT);
+  }
+
+  mask_topic_ = msg->filter_mask_topic;
+
+  // Setting new filter mask subscriber
+  RCLCPP_INFO(
+    logger_,
+    "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
+    mask_topic_.c_str());
+  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1));
+}
+
+void KeepoutFilter::maskCallback(
+  const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!mask_costmap_) {
+    RCLCPP_INFO(
+      logger_,
+      "KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
+      mask_topic_.c_str());
+    mask_costmap_.reset();
+  }
+
+  // Making a new mask_costmap_
+  mask_costmap_ = std::make_unique<Costmap2D>(*msg);
+  mask_frame_ = msg->header.frame_id;
+}
+
+void KeepoutFilter::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*/)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (!mask_costmap_) {
+    // Show warning message every 2 seconds to not litter an output
+    RCLCPP_WARN_THROTTLE(
+      logger_, *(clock_), 2000,
+      "KeepoutFilter: Filter mask was not received");
+    return;
+  }
+
+  tf2::Transform tf2_transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+  int mg_min_x, mg_min_y;  // masger_grid indexes of bottom-left window corner
+  int mg_max_x, mg_max_y;  // masger_grid indexes of top-right window corner
+
+  if (mask_frame_ != global_frame_) {
+    // Filter mask and current layer are in different frames:
+    // prepare frame transformation if mask_frame_ != global_frame_
+    geometry_msgs::msg::TransformStamped transform;
+    try {
+      transform = tf_->lookupTransform(
+        mask_frame_, global_frame_, tf2::TimePointZero,
+        transform_tolerance_);
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR(
+        logger_,
+        "KeepoutFilter: Failed to get costmap frame (%s) "
+        "transformation to mask frame (%s) with error: %s",
+        global_frame_.c_str(), mask_frame_.c_str(), ex.what());
+      return;
+    }
+    tf2::fromMsg(transform.transform, tf2_transform);
+
+    mg_min_x = min_i;
+    mg_min_y = min_j;
+    mg_max_x = max_i;
+    mg_max_y = max_j;
+  } else {
+    // Filter mask and current layer are in the same frame:
+    // apply the following optimization - iterate only in overlapped
+    // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area.
+    //
+    //           mask_costmap_
+    //       *----------------------------*
+    //       |                            |
+    //       |                            |
+    //       |      (2)                   |
+    // *-----+-------*                    |
+    // |     |///////|<- overlapped area  |
+    // |     |///////|   to iterate in    |
+    // |     *-------+--------------------*
+    // |    (1)      |
+    // |             |
+    // *-------------*
+    //  master_grid (min_i, min_j)..(max_i, max_j) window
+    //
+    // ToDo: after costmap rotation will be added, this should be re-worked.
+
+    double wx, wy;  // world coordinates
+
+    // Calculating bounds corresponding to bottom-left overlapping (1) corner
+    // mask_costmap_ -> master_grid intexes conversion
+    const double half_cell_size = 0.5 * mask_costmap_->getResolution();
+    wx = mask_costmap_->getOriginX() + half_cell_size;
+    wy = mask_costmap_->getOriginY() + half_cell_size;
+    master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y);
+    // Calculation of (1) corner bounds
+    if (mg_min_x >= max_i || mg_min_y >= max_j) {
+      // There is no overlapping. Do nothing.
+      return;
+    }
+    mg_min_x = std::max(min_i, mg_min_x);
+    mg_min_y = std::max(min_j, mg_min_y);
+
+    // Calculating bounds corresponding to top-right window (2) corner
+    // mask_costmap_ -> master_grid intexes conversion
+    wx = mask_costmap_->getOriginX() +
+      mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size;
+    wy = mask_costmap_->getOriginY() +
+      mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size;
+    master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y);
+    // Calculation of (2) corner bounds
+    if (mg_max_x <= min_i || mg_max_y <= min_j) {
+      // There is no overlapping. Do nothing.
+      return;
+    }
+    mg_max_x = std::min(max_i, mg_max_x);
+    mg_max_y = std::min(max_j, mg_max_y);
+  }
+
+  // unsigned<-signed conversions.
+  unsigned const int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
+  unsigned const int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
+  unsigned const int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
+  unsigned const int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
+
+  unsigned int i, j;  // master_grid iterators
+  unsigned int index;  // corresponding index of master_grid
+  double gl_wx, gl_wy;  // world coordinates in a global_frame_
+  double msk_wx, msk_wy;  // world coordinates in a mask_frame_
+  unsigned int mx, my;  // mask_costmap_ coordinates
+  unsigned char data, old_data;  // master_grid element data
+
+  // Main master_grid updating loop
+  // Iterate in costmap window by master_grid indexes
+  unsigned char * master_array = master_grid.getCharMap();
+  for (i = mg_min_x_u; i < mg_max_x_u; i++) {
+    for (j = mg_min_y_u; j < mg_max_y_u; j++) {
+      index = master_grid.getIndex(i, j);
+      old_data = master_array[index];
+      // Calculating corresponding to (i, j) point at mask_costmap_:
+      // Get world coordinates in global_frame_
+      master_grid.mapToWorld(i, j, gl_wx, gl_wy);
+      if (mask_frame_ != global_frame_) {
+        // Transform (i, j) point from global_frame_ to mask_frame_
+        tf2::Vector3 point(gl_wx, gl_wy, 0);
+        point = tf2_transform * point;
+        msk_wx = point.x();
+        msk_wy = point.y();
+      } else {
+        // In this case master_grid and filter-mask are in the same frame
+        msk_wx = gl_wx;
+        msk_wy = gl_wy;
+      }
+      // Get mask coordinates corresponding to (i, j) point at mask_costmap_
+      if (mask_costmap_->worldToMap(msk_wx, msk_wy, mx, my)) {
+        data = mask_costmap_->getCost(mx, my);
+        // Update if mask_ data is valid and greater than existing master_grid's one
+        if (data == NO_INFORMATION) {
+          continue;
+        }
+        if (data > old_data || old_data == NO_INFORMATION) {
+          master_array[index] = data;
+        }
+      }
+    }
+  }
+}
+
+void KeepoutFilter::resetFilter()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  filter_info_sub_.reset();
+  mask_sub_.reset();
+}
+
+bool KeepoutFilter::isActive()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (mask_costmap_) {
+    return true;
+  }
+  return false;
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer)

navigation2/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -0,0 +1,288 @@
+/*********************************************************************
+ *
+ * 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
+ *********************************************************************/
+
+#include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
+
+#include <cmath>
+#include <utility>
+#include <memory>
+#include <string>
+
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+
+namespace nav2_costmap_2d
+{
+
+SpeedFilter::SpeedFilter()
+: filter_info_sub_(nullptr), mask_sub_(nullptr),
+  speed_limit_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""),
+  speed_limit_(NO_SPEED_LIMIT), speed_limit_prev_(NO_SPEED_LIMIT)
+{
+}
+
+void SpeedFilter::initializeFilter(
+  const std::string & filter_info_topic)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Declare "speed_limit_topic" parameter specific to SpeedFilter only
+  std::string speed_limit_topic;
+  declareParameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
+  node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic);
+
+  filter_info_topic_ = filter_info_topic;
+  // Setting new costmap filter info subscriber
+  RCLCPP_INFO(
+    logger_,
+    "SpeedFilter: Subscribing to \"%s\" topic for filter info...",
+    filter_info_topic_.c_str());
+  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
+    filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1));
+
+  // Get global frame required for speed limit publisher
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  // Create new speed limit publisher
+  speed_limit_pub_ = node->create_publisher<nav2_msgs::msg::SpeedLimit>(
+    speed_limit_topic, rclcpp::QoS(10));
+  speed_limit_pub_->on_activate();
+
+  // Reset speed conversion states
+  base_ = BASE_DEFAULT;
+  multiplier_ = MULTIPLIER_DEFAULT;
+  percentage_ = false;
+}
+
+void SpeedFilter::filterInfoCallback(
+  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!mask_sub_) {
+    RCLCPP_INFO(
+      logger_,
+      "SpeedFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
+      filter_info_topic_.c_str());
+    // Resetting previous subscriber each time when new costmap filter information arrives
+    mask_sub_.reset();
+  }
+
+  // Set base_/multiplier_ or use speed limit in % of maximum speed
+  base_ = msg->base;
+  multiplier_ = msg->multiplier;
+  if (msg->type == SPEED_FILTER_PERCENT) {
+    // Using speed limit in % of maximum speed
+    percentage_ = true;
+    RCLCPP_INFO(
+      logger_,
+      "SpeedFilter: Using expressed in a percent from maximum speed"
+      "speed_limit = %f + filter_mask_data * %f",
+      base_, multiplier_);
+  } else if (msg->type == SPEED_FILTER_ABSOLUTE) {
+    // Using speed limit in m/s
+    percentage_ = false;
+    RCLCPP_INFO(
+      logger_,
+      "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
+      base_, multiplier_);
+  } else {
+    RCLCPP_ERROR(logger_, "SpeedFilter: Mode is not supported");
+    return;
+  }
+
+  mask_topic_ = msg->filter_mask_topic;
+
+  // Setting new filter mask subscriber
+  RCLCPP_INFO(
+    logger_,
+    "SpeedFilter: Subscribing to \"%s\" topic for filter mask...",
+    mask_topic_.c_str());
+  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1));
+}
+
+void SpeedFilter::maskCallback(
+  const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (!filter_mask_) {
+    RCLCPP_INFO(
+      logger_,
+      "SpeedFilter: Received filter mask from %s topic.", mask_topic_.c_str());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.",
+      mask_topic_.c_str());
+    filter_mask_.reset();
+  }
+
+  filter_mask_ = msg;
+  mask_frame_ = msg->header.frame_id;
+}
+
+void SpeedFilter::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)
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (!filter_mask_) {
+    // Show warning message every 2 seconds to not litter an output
+    RCLCPP_WARN_THROTTLE(
+      logger_, *(clock_), 2000,
+      "SpeedFilter: Filter mask was not received");
+    return;
+  }
+
+  geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame
+
+  // Transforming robot pose from current layer frame to mask frame
+  if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) {
+    return;
+  }
+
+  // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
+  unsigned int mask_robot_i, mask_robot_j;
+  if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+    return;
+  }
+
+  // Getting filter_mask data from cell where the robot placed and
+  // calculating speed limit value
+  int8_t speed_mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
+  if (speed_mask_data == SPEED_MASK_NO_LIMIT) {
+    // Corresponding filter mask cell is free.
+    // Setting no speed limit there.
+    speed_limit_ = NO_SPEED_LIMIT;
+  } else if (speed_mask_data == SPEED_MASK_UNKNOWN) {
+    // Corresponding filter mask cell is unknown.
+    // Do nothing.
+    RCLCPP_ERROR(
+      logger_,
+      "SpeedFilter: Found unknown cell in filter_mask[%i, %i], "
+      "which is invalid for this kind of filter",
+      mask_robot_i, mask_robot_j);
+    return;
+  } else {
+    // Normal case: speed_mask_data in range of [1..100]
+    speed_limit_ = speed_mask_data * multiplier_ + base_;
+    if (percentage_) {
+      if (speed_limit_ < 0.0 || speed_limit_ > 100.0) {
+        RCLCPP_WARN(
+          logger_,
+          "SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, "
+          "out of bounds of [0, 100]. Setting it to no-limit value.",
+          mask_robot_i, mask_robot_j, speed_limit_);
+        speed_limit_ = NO_SPEED_LIMIT;
+      }
+    } else {
+      if (speed_limit_ < 0.0) {
+        RCLCPP_WARN(
+          logger_,
+          "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, "
+          "which can not be true. Setting it to no-limit value.",
+          mask_robot_i, mask_robot_j);
+        speed_limit_ = NO_SPEED_LIMIT;
+      }
+    }
+  }
+
+  if (speed_limit_ != speed_limit_prev_) {
+    if (speed_limit_ != NO_SPEED_LIMIT) {
+      RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_);
+    } else {
+      RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value");
+    }
+
+    // Forming and publishing new SpeedLimit message
+    std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
+      std::make_unique<nav2_msgs::msg::SpeedLimit>();
+    msg->header.frame_id = global_frame_;
+    msg->header.stamp = clock_->now();
+    msg->percentage = percentage_;
+    msg->speed_limit = speed_limit_;
+    speed_limit_pub_->publish(std::move(msg));
+
+    speed_limit_prev_ = speed_limit_;
+  }
+}
+
+void SpeedFilter::resetFilter()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  filter_info_sub_.reset();
+  mask_sub_.reset();
+  if (speed_limit_pub_) {
+    speed_limit_pub_->on_deactivate();
+    speed_limit_pub_.reset();
+  }
+}
+
+bool SpeedFilter::isActive()
+{
+  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
+  if (filter_mask_) {
+    return true;
+  }
+  return false;
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::SpeedFilter, nav2_costmap_2d::Layer)

navigation2/nav2_costmap_2d/plugins/denoise_layer.cpp

@@ -0,0 +1,211 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// 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/denoise_layer.hpp"
+
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_costmap_2d
+{
+
+void
+DenoiseLayer::onInitialize()
+{
+  // Enable/disable plugin
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  // Smaller groups should be filtered
+  declareParameter("minimal_group_size", rclcpp::ParameterValue(2));
+  // Pixels connectivity type
+  declareParameter("group_connectivity_type", rclcpp::ParameterValue(8));
+
+  const auto node = node_.lock();
+
+  if (!node) {
+    throw std::runtime_error("DenoiseLayer::onInitialize: Failed to lock node");
+  }
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+
+  auto getInt = [&](const std::string & parameter_name) {
+      int param{};
+      node->get_parameter(name_ + "." + parameter_name, param);
+      return param;
+    };
+
+  const int minimal_group_size_param = getInt("minimal_group_size");
+
+  if (minimal_group_size_param <= 1) {
+    RCLCPP_WARN(
+      logger_,
+      "DenoiseLayer::onInitialize(): param minimal_group_size: %i."
+      " A value of 1 or less means that all map cells will be left as they are.",
+      minimal_group_size_param);
+    minimal_group_size_ = 1;
+  } else {
+    minimal_group_size_ = static_cast<size_t>(minimal_group_size_param);
+  }
+
+  const int group_connectivity_type_param = getInt("group_connectivity_type");
+
+  if (group_connectivity_type_param == 4) {
+    group_connectivity_type_ = ConnectivityType::Way4;
+  } else {
+    group_connectivity_type_ = ConnectivityType::Way8;
+
+    if (group_connectivity_type_param != 8) {
+      RCLCPP_WARN(
+        logger_, "DenoiseLayer::onInitialize(): param group_connectivity_type: %i."
+        " Possible values are  4 (neighbors pixels are connected horizontally and vertically) "
+        "or 8 (neighbors pixels are connected horizontally, vertically and diagonally)."
+        "The default value 8 will be used",
+        group_connectivity_type_param);
+    }
+  }
+
+  current_ = true;
+}
+
+void
+DenoiseLayer::reset()
+{
+  current_ = false;
+}
+
+bool
+DenoiseLayer::isClearable()
+{
+  return false;
+}
+
+void
+DenoiseLayer::updateBounds(
+  double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/,
+  double * /*min_x*/, double * /*min_y*/,
+  double * /*max_x*/, double * /*max_y*/) {}
+
+void
+DenoiseLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & master_grid, int min_x, int min_y, int max_x, int max_y)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  if (min_x >= max_x || min_y >= max_y) {
+    return;
+  }
+  no_information_is_obstacle_ = master_grid.getDefaultValue() != NO_INFORMATION;
+
+  // wrap roi_image over existing costmap2d buffer
+  unsigned char * master_array = master_grid.getCharMap();
+  const int step = static_cast<int>(master_grid.getSizeInCellsX());
+
+  const size_t width = max_x - min_x;
+  const size_t height = max_y - min_y;
+  Image<uint8_t> roi_image(height, width, master_array + min_y * step + min_x, step);
+
+  try {
+    denoise(roi_image);
+  } catch (std::exception & ex) {
+    RCLCPP_ERROR(logger_, "%s", (std::string("Inner error: ") + ex.what()).c_str());
+  }
+
+  current_ = true;
+}
+
+void
+DenoiseLayer::denoise(Image<uint8_t> & image) const
+{
+  if (image.empty()) {
+    return;
+  }
+
+  if (minimal_group_size_ <= 1) {
+    return;  // A smaller group cannot exist. No one pixel will be changed
+  }
+
+  if (minimal_group_size_ == 2) {
+    // Performs fast filtration based on erosion function
+    removeSinglePixels(image);
+  } else {
+    // Performs a slower segmentation-based operation
+    removeGroups(image);
+  }
+}
+
+void
+DenoiseLayer::removeGroups(Image<uint8_t> & image) const
+{
+  groups_remover_.removeGroups(
+    image, buffer_, group_connectivity_type_, minimal_group_size_,
+    [this](uint8_t pixel) {return isBackground(pixel);});
+}
+
+void
+DenoiseLayer::removeSinglePixels(Image<uint8_t> & image) const
+{
+  // Building a map of 4 or 8-connected neighbors.
+  // The pixel of the map is 255 if there is an obstacle nearby
+  uint8_t * buf = buffer_.get<uint8_t>(image.rows() * image.columns());
+  Image<uint8_t> max_neighbors_image(image.rows(), image.columns(), buf, image.columns());
+
+  // If NO_INFORMATION (=255) isn't obstacle, we can't use a simple max() to check
+  // any obstacle nearby. In this case, we interpret NO_INFORMATION as an empty space.
+  if (!no_information_is_obstacle_) {
+    auto replace_to_free = [](uint8_t v) {
+        return v == NO_INFORMATION ? FREE_SPACE : v;
+      };
+    auto max = [&](const std::initializer_list<uint8_t> lst) {
+        std::array<uint8_t, 3> buf = {
+          replace_to_free(*lst.begin()),
+          replace_to_free(*(lst.begin() + 1)),
+          replace_to_free(*(lst.begin() + 2))
+        };
+        return *std::max_element(buf.begin(), buf.end());
+      };
+    dilate(image, max_neighbors_image, group_connectivity_type_, max);
+  } else {
+    auto max = [](const std::initializer_list<uint8_t> lst) {
+        return std::max(lst);
+      };
+    dilate(image, max_neighbors_image, group_connectivity_type_, max);
+  }
+
+  max_neighbors_image.convert(
+    image, [this](uint8_t maxNeighbor, uint8_t & img) {
+      if (!isBackground(img) && isBackground(maxNeighbor)) {
+        img = FREE_SPACE;
+      }
+    });
+}
+
+bool DenoiseLayer::isBackground(uint8_t pixel) const
+{
+  bool is_obstacle =
+    pixel == LETHAL_OBSTACLE ||
+    pixel == INSCRIBED_INFLATED_OBSTACLE ||
+    (pixel == NO_INFORMATION && no_information_is_obstacle_);
+  return !is_obstacle;
+}
+
+}  // namespace nav2_costmap_2d
+
+// This is the macro allowing a DenoiseLayer class
+// to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer.
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::DenoiseLayer, nav2_costmap_2d::Layer)

navigation2/nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -0,0 +1,488 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#include "nav2_costmap_2d/inflation_layer.hpp"
+
+#include <limits>
+#include <map>
+#include <vector>
+#include <algorithm>
+#include <utility>
+
+#include "nav2_costmap_2d/costmap_math.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "rclcpp/parameter_events_filter.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer)
+
+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
+{
+
+InflationLayer::InflationLayer()
+: inflation_radius_(0),
+  inscribed_radius_(0),
+  cost_scaling_factor_(0),
+  inflate_unknown_(false),
+  inflate_around_unknown_(false),
+  cell_inflation_radius_(0),
+  cached_cell_inflation_radius_(0),
+  resolution_(0),
+  cache_length_(0),
+  last_min_x_(std::numeric_limits<double>::lowest()),
+  last_min_y_(std::numeric_limits<double>::lowest()),
+  last_max_x_(std::numeric_limits<double>::max()),
+  last_max_y_(std::numeric_limits<double>::max())
+{
+  access_ = new mutex_t();
+}
+
+InflationLayer::~InflationLayer()
+{
+  dyn_params_handler_.reset();
+  delete access_;
+}
+
+void
+InflationLayer::onInitialize()
+{
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
+  declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
+  declareParameter("inflate_unknown", rclcpp::ParameterValue(false));
+  declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false));
+
+  {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+    node->get_parameter(name_ + "." + "enabled", enabled_);
+    node->get_parameter(name_ + "." + "inflation_radius", inflation_radius_);
+    node->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_);
+    node->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_);
+    node->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_);
+
+    dyn_params_handler_ = node->add_on_set_parameters_callback(
+      std::bind(
+        &InflationLayer::dynamicParametersCallback,
+        this, std::placeholders::_1));
+  }
+
+  current_ = true;
+  seen_.clear();
+  cached_distances_.clear();
+  cached_costs_.clear();
+  need_reinflation_ = false;
+  cell_inflation_radius_ = cellDistance(inflation_radius_);
+  matchSize();
+}
+
+void
+InflationLayer::matchSize()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap();
+  resolution_ = costmap->getResolution();
+  cell_inflation_radius_ = cellDistance(inflation_radius_);
+  computeCaches();
+  seen_ = std::vector<bool>(costmap->getSizeInCellsX() * costmap->getSizeInCellsY(), false);
+}
+
+void
+InflationLayer::updateBounds(
+  double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
+  double * min_y, double * max_x, double * max_y)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (need_reinflation_) {
+    last_min_x_ = *min_x;
+    last_min_y_ = *min_y;
+    last_max_x_ = *max_x;
+    last_max_y_ = *max_y;
+
+    *min_x = std::numeric_limits<double>::lowest();
+    *min_y = std::numeric_limits<double>::lowest();
+    *max_x = std::numeric_limits<double>::max();
+    *max_y = std::numeric_limits<double>::max();
+    need_reinflation_ = false;
+  } else {
+    double tmp_min_x = last_min_x_;
+    double tmp_min_y = last_min_y_;
+    double tmp_max_x = last_max_x_;
+    double tmp_max_y = last_max_y_;
+    last_min_x_ = *min_x;
+    last_min_y_ = *min_y;
+    last_max_x_ = *max_x;
+    last_max_y_ = *max_y;
+    *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
+    *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
+    *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
+    *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
+  }
+}
+
+void
+InflationLayer::onFootprintChanged()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  inscribed_radius_ = layered_costmap_->getInscribedRadius();
+  cell_inflation_radius_ = cellDistance(inflation_radius_);
+  computeCaches();
+  need_reinflation_ = true;
+
+  if (inflation_radius_ < inscribed_radius_) {
+    RCLCPP_ERROR(
+      logger_,
+      "The configured inflation radius (%.3f) is smaller than "
+      "the computed inscribed radius (%.3f) of your footprint, "
+      "it is highly recommended to set inflation radius to be at "
+      "least as big as the inscribed radius to avoid collisions",
+      inflation_radius_, inscribed_radius_);
+  }
+
+  RCLCPP_DEBUG(
+    logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu,"
+    " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
+    layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
+}
+
+void
+InflationLayer::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_ || (cell_inflation_radius_ == 0)) {
+    return;
+  }
+
+  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
+  for (auto & dist : inflation_cells_) {
+    RCLCPP_FATAL_EXPRESSION(
+      logger_,
+      !dist.empty(), "The inflation list must be empty at the beginning of inflation");
+  }
+
+  unsigned char * master_array = master_grid.getCharMap();
+  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
+
+  if (seen_.size() != size_x * size_y) {
+    RCLCPP_WARN(
+      logger_, "InflationLayer::updateCosts(): seen_ vector size is wrong");
+    seen_ = std::vector<bool>(size_x * size_y, false);
+  }
+
+  std::fill(begin(seen_), end(seen_), false);
+
+  // We need to include in the inflation cells outside the bounding
+  // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
+  // up to that distance outside the box can still influence the costs
+  // stored in cells inside the box.
+  const int base_min_i = min_i;
+  const int base_min_j = min_j;
+  const int base_max_i = max_i;
+  const int base_max_j = max_j;
+  min_i -= static_cast<int>(cell_inflation_radius_);
+  min_j -= static_cast<int>(cell_inflation_radius_);
+  max_i += static_cast<int>(cell_inflation_radius_);
+  max_j += static_cast<int>(cell_inflation_radius_);
+
+  min_i = std::max(0, min_i);
+  min_j = std::max(0, min_j);
+  max_i = std::min(static_cast<int>(size_x), max_i);
+  max_j = std::min(static_cast<int>(size_y), max_j);
+
+  // Inflation list; we append cells to visit in a list associated with
+  // its distance to the nearest obstacle
+  // We use a map<distance, list> to emulate the priority queue used before,
+  // with a notable performance boost
+
+  // Start with lethal obstacles: by definition distance is 0.0
+  auto & obs_bin = inflation_cells_[0];
+  for (int j = min_j; j < max_j; j++) {
+    for (int i = min_i; i < max_i; i++) {
+      int index = static_cast<int>(master_grid.getIndex(i, j));
+      unsigned char cost = master_array[index];
+      if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) {
+        obs_bin.emplace_back(index, i, j, i, j);
+      }
+    }
+  }
+
+  // Process cells by increasing distance; new cells are appended to the
+  // corresponding distance bin, so they
+  // can overtake previously inserted but farther away cells
+  for (const auto & dist_bin : inflation_cells_) {
+    for (std::size_t i = 0; i < dist_bin.size(); ++i) {
+      // Do not use iterator or for-range based loops to
+      // iterate though dist_bin, since it's size might
+      // change when a new cell is enqueued, invalidating all iterators
+      unsigned int index = dist_bin[i].index_;
+
+      // ignore if already visited
+      if (seen_[index]) {
+        continue;
+      }
+
+      seen_[index] = true;
+
+      unsigned int mx = dist_bin[i].x_;
+      unsigned int my = dist_bin[i].y_;
+      unsigned int sx = dist_bin[i].src_x_;
+      unsigned int sy = dist_bin[i].src_y_;
+
+      // assign the cost associated with the distance from an obstacle to the cell
+      unsigned char cost = costLookup(mx, my, sx, sy);
+      unsigned char old_cost = master_array[index];
+      // In order to avoid artifacts appeared out of boundary areas
+      // when some layer is going after inflation_layer,
+      // we need to apply inflation_layer only to inside of given bounds
+      if (static_cast<int>(mx) >= base_min_i &&
+        static_cast<int>(my) >= base_min_j &&
+        static_cast<int>(mx) < base_max_i &&
+        static_cast<int>(my) < base_max_j)
+      {
+        if (old_cost == NO_INFORMATION &&
+          (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
+        {
+          master_array[index] = cost;
+        } else {
+          master_array[index] = std::max(old_cost, cost);
+        }
+      }
+
+      // attempt to put the neighbors of the current cell onto the inflation list
+      if (mx > 0) {
+        enqueue(index - 1, mx - 1, my, sx, sy);
+      }
+      if (my > 0) {
+        enqueue(index - size_x, mx, my - 1, sx, sy);
+      }
+      if (mx < size_x - 1) {
+        enqueue(index + 1, mx + 1, my, sx, sy);
+      }
+      if (my < size_y - 1) {
+        enqueue(index + size_x, mx, my + 1, sx, sy);
+      }
+    }
+  }
+
+  for (auto & dist : inflation_cells_) {
+    dist.clear();
+    dist.reserve(200);
+  }
+
+  current_ = true;
+}
+
+/**
+ * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
+ * @param  grid The costmap
+ * @param  index The index of the cell
+ * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
+ * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
+ * @param  src_x The x index of the obstacle point inflation started at
+ * @param  src_y The y index of the obstacle point inflation started at
+ */
+void
+InflationLayer::enqueue(
+  unsigned int index, unsigned int mx, unsigned int my,
+  unsigned int src_x, unsigned int src_y)
+{
+  if (!seen_[index]) {
+    // we compute our distance table one cell further than the
+    // inflation radius dictates so we can make the check below
+    double distance = distanceLookup(mx, my, src_x, src_y);
+
+    // we only want to put the cell in the list if it is within
+    // the inflation radius of the obstacle point
+    if (distance > cell_inflation_radius_) {
+      return;
+    }
+
+    const unsigned int r = cell_inflation_radius_ + 2;
+
+    // push the cell data onto the inflation list and mark
+    inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
+      index, mx, my, src_x, src_y);
+  }
+}
+
+void
+InflationLayer::computeCaches()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (cell_inflation_radius_ == 0) {
+    return;
+  }
+
+  cache_length_ = cell_inflation_radius_ + 2;
+
+  // based on the inflation radius... compute distance and cost caches
+  if (cell_inflation_radius_ != cached_cell_inflation_radius_) {
+    cached_costs_.resize(cache_length_ * cache_length_);
+    cached_distances_.resize(cache_length_ * cache_length_);
+
+    for (unsigned int i = 0; i < cache_length_; ++i) {
+      for (unsigned int j = 0; j < cache_length_; ++j) {
+        cached_distances_[i * cache_length_ + j] = hypot(i, j);
+      }
+    }
+
+    cached_cell_inflation_radius_ = cell_inflation_radius_;
+  }
+
+  for (unsigned int i = 0; i < cache_length_; ++i) {
+    for (unsigned int j = 0; j < cache_length_; ++j) {
+      cached_costs_[i * cache_length_ + j] = computeCost(cached_distances_[i * cache_length_ + j]);
+    }
+  }
+
+  int max_dist = generateIntegerDistances();
+  inflation_cells_.clear();
+  inflation_cells_.resize(max_dist + 1);
+  for (auto & dist : inflation_cells_) {
+    dist.reserve(200);
+  }
+}
+
+int
+InflationLayer::generateIntegerDistances()
+{
+  const int r = cell_inflation_radius_ + 2;
+  const int size = r * 2 + 1;
+
+  std::vector<std::pair<int, int>> points;
+
+  for (int y = -r; y <= r; y++) {
+    for (int x = -r; x <= r; x++) {
+      if (x * x + y * y <= r * r) {
+        points.emplace_back(x, y);
+      }
+    }
+  }
+
+  std::sort(
+    points.begin(), points.end(),
+    [](const std::pair<int, int> & a, const std::pair<int, int> & b) -> bool {
+      return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second;
+    }
+  );
+
+  std::vector<std::vector<int>> distance_matrix(size, std::vector<int>(size, 0));
+  std::pair<int, int> last = {0, 0};
+  int level = 0;
+  for (auto const & p : points) {
+    if (p.first * p.first + p.second * p.second !=
+      last.first * last.first + last.second * last.second)
+    {
+      level++;
+    }
+    distance_matrix[p.first + r][p.second + r] = level;
+    last = p;
+  }
+
+  distance_matrix_ = distance_matrix;
+  return level;
+}
+
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+InflationLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+
+  bool need_cache_recompute = false;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "inflation_radius" &&
+        inflation_radius_ != parameter.as_double())
+      {
+        inflation_radius_ = parameter.as_double();
+        need_reinflation_ = true;
+        need_cache_recompute = true;
+      } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT
+        cost_scaling_factor_ != parameter.as_double())
+      {
+        cost_scaling_factor_ = parameter.as_double();
+        need_reinflation_ = true;
+        need_cache_recompute = true;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+        need_reinflation_ = true;
+        current_ = false;
+      } else if (param_name == name_ + "." + "inflate_unknown" && // NOLINT
+        inflate_unknown_ != parameter.as_bool())
+      {
+        inflate_unknown_ = parameter.as_bool();
+        need_reinflation_ = true;
+      } else if (param_name == name_ + "." + "inflate_around_unknown" && // NOLINT
+        inflate_around_unknown_ != parameter.as_bool())
+      {
+        inflate_around_unknown_ = parameter.as_bool();
+        need_reinflation_ = true;
+      }
+    }
+  }
+
+  if (need_cache_recompute) {
+    matchSize();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -0,0 +1,766 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *         Steve Macenski
+ *********************************************************************/
+#include "nav2_costmap_2d/obstacle_layer.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "nav2_costmap_2d/costmap_math.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::NO_INFORMATION;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::FREE_SPACE;
+
+using nav2_costmap_2d::ObservationBuffer;
+using nav2_costmap_2d::Observation;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+
+ObstacleLayer::~ObstacleLayer()
+{
+  dyn_params_handler_.reset();
+  for (auto & notifier : observation_notifiers_) {
+    notifier.reset();
+  }
+}
+
+void ObstacleLayer::onInitialize()
+{
+  bool track_unknown_space;
+  double transform_tolerance;
+
+  // The topics that we'll subscribe to from the parameter server
+  std::string topics_string;
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
+  declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
+  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
+  node->get_parameter(name_ + "." + "combination_method", combination_method_);
+  node->get_parameter("track_unknown_space", track_unknown_space);
+  node->get_parameter("transform_tolerance", transform_tolerance);
+  node->get_parameter(name_ + "." + "observation_sources", topics_string);
+
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ObstacleLayer::dynamicParametersCallback,
+      this,
+      std::placeholders::_1));
+
+  RCLCPP_INFO(
+    logger_,
+    "Subscribed to Topics: %s", topics_string.c_str());
+
+  rolling_window_ = layered_costmap_->isRolling();
+
+  if (track_unknown_space) {
+    default_value_ = NO_INFORMATION;
+  } else {
+    default_value_ = FREE_SPACE;
+  }
+
+  ObstacleLayer::matchSize();
+  current_ = true;
+  was_reset_ = false;
+
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+
+  // now we need to split the topics based on whitespace which we can use a stringstream for
+  std::stringstream ss(topics_string);
+
+  std::string source;
+  while (ss >> source) {
+    // get the parameters for the specific topic
+    double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
+    std::string topic, sensor_frame, data_type;
+    bool inf_is_valid, clearing, marking;
+
+    declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
+    declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
+    declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));
+    declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));
+    declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "obstacle_max_range", rclcpp::ParameterValue(2.5));
+    declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
+    declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));
+
+    node->get_parameter(name_ + "." + source + "." + "topic", topic);
+    node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
+    node->get_parameter(
+      name_ + "." + source + "." + "observation_persistence",
+      observation_keep_time);
+    node->get_parameter(
+      name_ + "." + source + "." + "expected_update_rate",
+      expected_update_rate);
+    node->get_parameter(name_ + "." + source + "." + "data_type", data_type);
+    node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
+    node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
+    node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
+    node->get_parameter(name_ + "." + source + "." + "marking", marking);
+    node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
+
+    if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
+      RCLCPP_FATAL(
+        logger_,
+        "Only topics that use point cloud2s or laser scans are currently supported");
+      throw std::runtime_error(
+              "Only topics that use point cloud2s or laser scans are currently supported");
+    }
+
+    // get the obstacle range for the sensor
+    double obstacle_max_range, obstacle_min_range;
+    node->get_parameter(name_ + "." + source + "." + "obstacle_max_range", obstacle_max_range);
+    node->get_parameter(name_ + "." + source + "." + "obstacle_min_range", obstacle_min_range);
+
+    // get the raytrace ranges for the sensor
+    double raytrace_max_range, raytrace_min_range;
+    node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range);
+    node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range);
+
+
+    RCLCPP_DEBUG(
+      logger_,
+      "Creating an observation buffer for source %s, topic %s, frame %s",
+      source.c_str(), topic.c_str(),
+      sensor_frame.c_str());
+
+    // create an observation buffer
+    observation_buffers_.push_back(
+      std::shared_ptr<ObservationBuffer
+      >(
+        new ObservationBuffer(
+          node, topic, observation_keep_time, expected_update_rate,
+          min_obstacle_height,
+          max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range,
+          raytrace_min_range, *tf_,
+          global_frame_,
+          sensor_frame, tf2::durationFromSec(transform_tolerance))));
+
+    // check if we'll add this buffer to our marking observation buffers
+    if (marking) {
+      marking_buffers_.push_back(observation_buffers_.back());
+    }
+
+    // check if we'll also add this buffer to our clearing observation buffers
+    if (clearing) {
+      clearing_buffers_.push_back(observation_buffers_.back());
+    }
+
+    RCLCPP_DEBUG(
+      logger_,
+      "Created an observation buffer for source %s, topic %s, global frame: %s, "
+      "expected update rate: %.2f, observation persistence: %.2f",
+      source.c_str(), topic.c_str(),
+      global_frame_.c_str(), expected_update_rate, observation_keep_time);
+
+    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
+    custom_qos_profile.depth = 50;
+
+    // create a callback for the topic
+    if (data_type == "LaserScan") {
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
+      sub->unsubscribe();
+
+      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
+        *sub, *tf_, global_frame_, 50,
+        node->get_node_logging_interface(),
+        node->get_node_clock_interface(),
+        tf2::durationFromSec(transform_tolerance));
+
+      if (inf_is_valid) {
+        filter->registerCallback(
+          std::bind(
+            &ObstacleLayer::laserScanValidInfCallback, this, std::placeholders::_1,
+            observation_buffers_.back()));
+
+      } else {
+        filter->registerCallback(
+          std::bind(
+            &ObstacleLayer::laserScanCallback, this, std::placeholders::_1,
+            observation_buffers_.back()));
+      }
+
+      observation_subscribers_.push_back(sub);
+
+      observation_notifiers_.push_back(filter);
+      observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05));
+
+    } else {
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
+      sub->unsubscribe();
+
+      if (inf_is_valid) {
+        RCLCPP_WARN(
+          logger_,
+          "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+      }
+
+      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(
+        *sub, *tf_, global_frame_, 50,
+        node->get_node_logging_interface(),
+        node->get_node_clock_interface(),
+        tf2::durationFromSec(transform_tolerance));
+
+      filter->registerCallback(
+        std::bind(
+          &ObstacleLayer::pointCloud2Callback, this, std::placeholders::_1,
+          observation_buffers_.back()));
+
+      observation_subscribers_.push_back(sub);
+      observation_notifiers_.push_back(filter);
+    }
+
+    if (sensor_frame != "") {
+      std::vector<std::string> target_frames;
+      target_frames.push_back(global_frame_);
+      target_frames.push_back(sensor_frame);
+      observation_notifiers_.back()->setTargetFrames(target_frames);
+    }
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult
+ObstacleLayer::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_DOUBLE) {
+      if (param_name == name_ + "." + "min_obstacle_height") {
+        min_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+        if (enabled_) {
+          current_ = false;
+        }
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      }
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "combination_method") {
+        combination_method_ = parameter.as_int();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+void
+ObstacleLayer::laserScanCallback(
+  sensor_msgs::msg::LaserScan::ConstSharedPtr message,
+  const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer)
+{
+  // project the laser into a point cloud
+  sensor_msgs::msg::PointCloud2 cloud;
+  cloud.header = message->header;
+
+  // project the scan into a point cloud
+  try {
+    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "High fidelity enabled, but TF returned a transform exception to frame %s: %s",
+      global_frame_.c_str(),
+      ex.what());
+    projector_.projectLaser(*message, cloud);
+  } catch (std::runtime_error & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "transformLaserScanToPointCloud error, it seems the message from laser is malformed."
+      " Ignore this message. what(): %s",
+      ex.what());
+    return;
+  }
+
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(cloud);
+  buffer->unlock();
+}
+
+void
+ObstacleLayer::laserScanValidInfCallback(
+  sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message,
+  const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer)
+{
+  // Filter positive infinities ("Inf"s) to max_range.
+  float epsilon = 0.0001;  // a tenth of a millimeter
+  sensor_msgs::msg::LaserScan message = *raw_message;
+  for (size_t i = 0; i < message.ranges.size(); i++) {
+    float range = message.ranges[i];
+    if (!std::isfinite(range) && range > 0) {
+      message.ranges[i] = message.range_max - epsilon;
+    }
+  }
+
+  // project the laser into a point cloud
+  sensor_msgs::msg::PointCloud2 cloud;
+  cloud.header = message.header;
+
+  // project the scan into a point cloud
+  try {
+    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "High fidelity enabled, but TF returned a transform exception to frame %s: %s",
+      global_frame_.c_str(), ex.what());
+    projector_.projectLaser(message, cloud);
+  } catch (std::runtime_error & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "transformLaserScanToPointCloud error, it seems the message from laser is malformed."
+      " Ignore this message. what(): %s",
+      ex.what());
+    return;
+  }
+
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(cloud);
+  buffer->unlock();
+}
+
+void
+ObstacleLayer::pointCloud2Callback(
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr message,
+  const std::shared_ptr<ObservationBuffer> & buffer)
+{
+  // buffer the point cloud
+  buffer->lock();
+  buffer->bufferCloud(*message);
+  buffer->unlock();
+}
+
+void
+ObstacleLayer::updateBounds(
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
+  double * min_y, double * max_x, double * max_y)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (rolling_window_) {
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  }
+  if (!enabled_) {
+    return;
+  }
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = current && getMarkingObservations(observations);
+
+  // get the clearing observations
+  current = current && getClearingObservations(clearing_observations);
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<Observation>::const_iterator it = observations.begin();
+    it != observations.end(); ++it)
+  {
+    const Observation & obs = *it;
+
+    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+
+    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
+    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+      double px = *iter_x, py = *iter_y, pz = *iter_z;
+
+      // if the obstacle is too low, we won't add it
+      if (pz < min_obstacle_height_) {
+        RCLCPP_DEBUG(logger_, "The point is too low");
+        continue;
+      }
+
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (pz > max_obstacle_height_) {
+        RCLCPP_DEBUG(logger_, "The point is too high");
+        continue;
+      }
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist =
+        (px -
+        obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) +
+        (pz - obs.origin_.z) * (pz - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_max_range) {
+        RCLCPP_DEBUG(logger_, "The point is too far away");
+        continue;
+      }
+
+      // if the point is too close, do not conisder it
+      if (sq_dist < sq_obstacle_min_range) {
+        RCLCPP_DEBUG(logger_, "The point is too close");
+        continue;
+      }
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my;
+      if (!worldToMap(px, py, mx, my)) {
+        RCLCPP_DEBUG(logger_, "Computing map coords failed");
+        continue;
+      }
+
+      unsigned int index = getIndex(mx, my);
+      costmap_[index] = LETHAL_OBSTACLE;
+      touch(px, py, min_x, min_y, max_x, max_y);
+    }
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void
+ObstacleLayer::updateFootprint(
+  double robot_x, double robot_y, double robot_yaw,
+  double * min_x, double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  if (!footprint_clearing_enabled_) {return;}
+  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+
+  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
+    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
+  }
+}
+
+void
+ObstacleLayer::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;
+  }
+
+  // if not current due to reset, set current now after clearing
+  if (!current_ && was_reset_) {
+    was_reset_ = false;
+    current_ = true;
+  }
+
+  if (footprint_clearing_enabled_) {
+    setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
+  }
+
+  switch (combination_method_) {
+    case 0:  // Overwrite
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    case 1:  // Maximum
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+      break;
+    default:  // Nothing
+      break;
+  }
+}
+
+void
+ObstacleLayer::addStaticObservation(
+  nav2_costmap_2d::Observation & obs,
+  bool marking, bool clearing)
+{
+  if (marking) {
+    static_marking_observations_.push_back(obs);
+  }
+  if (clearing) {
+    static_clearing_observations_.push_back(obs);
+  }
+}
+
+void
+ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
+{
+  if (marking) {
+    static_marking_observations_.clear();
+  }
+  if (clearing) {
+    static_clearing_observations_.clear();
+  }
+}
+
+bool
+ObstacleLayer::getMarkingObservations(std::vector<Observation> & marking_observations) const
+{
+  bool current = true;
+  // get the marking observations
+  for (unsigned int i = 0; i < marking_buffers_.size(); ++i) {
+    marking_buffers_[i]->lock();
+    marking_buffers_[i]->getObservations(marking_observations);
+    current = marking_buffers_[i]->isCurrent() && current;
+    marking_buffers_[i]->unlock();
+  }
+  marking_observations.insert(
+    marking_observations.end(),
+    static_marking_observations_.begin(), static_marking_observations_.end());
+  return current;
+}
+
+bool
+ObstacleLayer::getClearingObservations(std::vector<Observation> & clearing_observations) const
+{
+  bool current = true;
+  // get the clearing observations
+  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) {
+    clearing_buffers_[i]->lock();
+    clearing_buffers_[i]->getObservations(clearing_observations);
+    current = clearing_buffers_[i]->isCurrent() && current;
+    clearing_buffers_[i]->unlock();
+  }
+  clearing_observations.insert(
+    clearing_observations.end(),
+    static_clearing_observations_.begin(), static_clearing_observations_.end());
+  return current;
+}
+
+void
+ObstacleLayer::raytraceFreespace(
+  const Observation & clearing_observation, double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  const sensor_msgs::msg::PointCloud2 & cloud = *(clearing_observation.cloud_);
+
+  // get the map coordinates of the origin of the sensor
+  unsigned int x0, y0;
+  if (!worldToMap(ox, oy, x0, y0)) {
+    RCLCPP_WARN(
+      logger_,
+      "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f, %.2f) to (%.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy,
+      origin_x_, origin_y_,
+      origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY());
+    return;
+  }
+
+  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+  double origin_x = origin_x_, origin_y = origin_y_;
+  double map_end_x = origin_x + size_x_ * resolution_;
+  double map_end_y = origin_y + size_y_ * resolution_;
+
+
+  touch(ox, oy, min_x, min_y, max_x, max_y);
+
+  // for each point in the cloud, we want to trace a line from the origin
+  // and clear obstacles along it
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) {
+    double wx = *iter_x;
+    double wy = *iter_y;
+
+    // now we also need to make sure that the enpoint we're raytracing
+    // to isn't off the costmap and scale if necessary
+    double a = wx - ox;
+    double b = wy - oy;
+
+    // the minimum value to raytrace from is the origin
+    if (wx < origin_x) {
+      double t = (origin_x - ox) / a;
+      wx = origin_x;
+      wy = oy + b * t;
+    }
+    if (wy < origin_y) {
+      double t = (origin_y - oy) / b;
+      wx = ox + a * t;
+      wy = origin_y;
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wx > map_end_x) {
+      double t = (map_end_x - ox) / a;
+      wx = map_end_x - .001;
+      wy = oy + b * t;
+    }
+    if (wy > map_end_y) {
+      double t = (map_end_y - oy) / b;
+      wx = ox + a * t;
+      wy = map_end_y - .001;
+    }
+
+    // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
+    unsigned int x1, y1;
+
+    // check for legality just in case
+    if (!worldToMap(wx, wy, x1, y1)) {
+      continue;
+    }
+
+    unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_);
+    unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
+    MarkCell marker(costmap_, FREE_SPACE);
+    // and finally... we can execute our trace to clear obstacles along that line
+    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range);
+
+    updateRaytraceBounds(
+      ox, oy, wx, wy, clearing_observation.raytrace_max_range_,
+      clearing_observation.raytrace_min_range_, min_x, min_y, max_x,
+      max_y);
+  }
+}
+
+void
+ObstacleLayer::activate()
+{
+  for (auto & notifier : observation_notifiers_) {
+    notifier->clear();
+  }
+
+  // if we're stopped we need to re-subscribe to topics
+  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) {
+    if (observation_subscribers_[i] != NULL) {
+      observation_subscribers_[i]->subscribe();
+    }
+  }
+  resetBuffersLastUpdated();
+}
+
+void
+ObstacleLayer::deactivate()
+{
+  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) {
+    if (observation_subscribers_[i] != NULL) {
+      observation_subscribers_[i]->unsubscribe();
+    }
+  }
+}
+
+void
+ObstacleLayer::updateRaytraceBounds(
+  double ox, double oy, double wx, double wy, double max_range, double min_range,
+  double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  double dx = wx - ox, dy = wy - oy;
+  double full_distance = hypot(dx, dy);
+  if (full_distance < min_range) {
+    return;
+  }
+  double scale = std::min(1.0, max_range / full_distance);
+  double ex = ox + dx * scale, ey = oy + dy * scale;
+  touch(ex, ey, min_x, min_y, max_x, max_y);
+}
+
+void
+ObstacleLayer::reset()
+{
+  resetMaps();
+  resetBuffersLastUpdated();
+  current_ = false;
+  was_reset_ = true;
+}
+
+void
+ObstacleLayer::resetBuffersLastUpdated()
+{
+  for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
+    if (observation_buffers_[i]) {
+      observation_buffers_[i]->resetLastUpdated();
+    }
+  }
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/range_sensor_layer.cpp

@@ -0,0 +1,542 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 David V. Lu!!
+ *  Copyright (c) 2020, Bytes Robotics
+ *  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 copyright holder 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 HOLDER 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.
+ */
+
+#include <angles/angles.h>
+#include <algorithm>
+#include <list>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "nav2_costmap_2d/range_sensor_layer.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using nav2_costmap_2d::NO_INFORMATION;
+
+using namespace std::literals::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+RangeSensorLayer::RangeSensorLayer() {}
+
+void RangeSensorLayer::onInitialize()
+{
+  current_ = true;
+  was_reset_ = false;
+  buffered_readings_ = 0;
+  last_reading_time_ = clock_->now();
+  default_value_ = to_cost(0.5);
+
+  matchSize();
+  resetRange();
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  declareParameter("phi", rclcpp::ParameterValue(1.2));
+  node->get_parameter(name_ + "." + "phi", phi_v_);
+  declareParameter("inflate_cone", rclcpp::ParameterValue(1.0));
+  node->get_parameter(name_ + "." + "inflate_cone", inflate_cone_);
+  declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0));
+  node->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_);
+  declareParameter("clear_threshold", rclcpp::ParameterValue(0.2));
+  node->get_parameter(name_ + "." + "clear_threshold", clear_threshold_);
+  declareParameter("mark_threshold", rclcpp::ParameterValue(0.8));
+  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
+  declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false));
+  node->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_);
+
+  double temp_tf_tol = 0.0;
+  node->get_parameter("transform_tolerance", temp_tf_tol);
+  transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
+
+  std::vector<std::string> topic_names{};
+  declareParameter("topics", rclcpp::ParameterValue(topic_names));
+  node->get_parameter(name_ + "." + "topics", topic_names);
+
+  InputSensorType input_sensor_type = InputSensorType::ALL;
+  std::string sensor_type_name;
+  declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL"));
+  node->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name);
+
+  std::transform(
+    sensor_type_name.begin(), sensor_type_name.end(),
+    sensor_type_name.begin(), ::toupper);
+  RCLCPP_INFO(
+    logger_, "%s: %s as input_sensor_type given",
+    name_.c_str(), sensor_type_name.c_str());
+
+  if (sensor_type_name == "VARIABLE") {
+    input_sensor_type = InputSensorType::VARIABLE;
+  } else if (sensor_type_name == "FIXED") {
+    input_sensor_type = InputSensorType::FIXED;
+  } else if (sensor_type_name == "ALL") {
+    input_sensor_type = InputSensorType::ALL;
+  } else {
+    RCLCPP_ERROR(
+      logger_, "%s: Invalid input sensor type: %s. Defaulting to ALL.",
+      name_.c_str(), sensor_type_name.c_str());
+  }
+
+  // Validate topic names list: it must be a (normally non-empty) list of strings
+  if (topic_names.empty()) {
+    RCLCPP_FATAL(
+      logger_, "Invalid topic names list: it must"
+      "be a non-empty list of strings");
+    return;
+  }
+
+  // Traverse the topic names list subscribing to all of them with the same callback method
+  for (auto & topic_name : topic_names) {
+    if (input_sensor_type == InputSensorType::VARIABLE) {
+      processRangeMessageFunc_ = std::bind(
+        &RangeSensorLayer::processVariableRangeMsg, this,
+        std::placeholders::_1);
+    } else if (input_sensor_type == InputSensorType::FIXED) {
+      processRangeMessageFunc_ = std::bind(
+        &RangeSensorLayer::processFixedRangeMsg, this,
+        std::placeholders::_1);
+    } else if (input_sensor_type == InputSensorType::ALL) {
+      processRangeMessageFunc_ = std::bind(
+        &RangeSensorLayer::processRangeMsg, this,
+        std::placeholders::_1);
+    } else {
+      RCLCPP_ERROR(
+        logger_,
+        "%s: Invalid input sensor type: %s. Did you make a new type"
+        "and forgot to choose the subscriber for it?",
+        name_.c_str(), sensor_type_name.c_str());
+    }
+    range_subs_.push_back(
+      node->create_subscription<sensor_msgs::msg::Range>(
+        topic_name, rclcpp::SensorDataQoS(), std::bind(
+          &RangeSensorLayer::bufferIncomingRangeMsg, this,
+          std::placeholders::_1)));
+
+    RCLCPP_INFO(
+      logger_, "RangeSensorLayer: subscribed to "
+      "topic %s", range_subs_.back()->get_topic_name());
+  }
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+}
+
+
+double RangeSensorLayer::gamma(double theta)
+{
+  if (fabs(theta) > max_angle_) {
+    return 0.0;
+  } else {
+    return 1 - pow(theta / max_angle_, 2);
+  }
+}
+
+double RangeSensorLayer::delta(double phi)
+{
+  return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2;
+}
+
+void RangeSensorLayer::get_deltas(double angle, double * dx, double * dy)
+{
+  double ta = tan(angle);
+  if (ta == 0) {
+    *dx = 0;
+  } else {
+    *dx = resolution_ / ta;
+  }
+
+  *dx = copysign(*dx, cos(angle));
+  *dy = copysign(resolution_, sin(angle));
+}
+
+double RangeSensorLayer::sensor_model(double r, double phi, double theta)
+{
+  double lbda = delta(phi) * gamma(theta);
+
+  double delta = resolution_;
+
+  if (phi >= 0.0 && phi < r - 2 * delta * r) {
+    return (1 - lbda) * (0.5);
+  } else if (phi < r - delta * r) {
+    return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) +
+           (1 - lbda) * .5;
+  } else if (phi < r + delta * r) {
+    double J = (r - phi) / (delta * r);
+    return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5;
+  } else {
+    return 0.5;
+  }
+}
+
+void RangeSensorLayer::bufferIncomingRangeMsg(
+  const sensor_msgs::msg::Range::SharedPtr range_message)
+{
+  range_message_mutex_.lock();
+  range_msgs_buffer_.push_back(*range_message);
+  range_message_mutex_.unlock();
+}
+
+void RangeSensorLayer::updateCostmap()
+{
+  std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
+
+  range_message_mutex_.lock();
+  range_msgs_buffer_copy = std::list<sensor_msgs::msg::Range>(range_msgs_buffer_);
+  range_msgs_buffer_.clear();
+  range_message_mutex_.unlock();
+
+  for (auto & range_msgs_it : range_msgs_buffer_copy) {
+    processRangeMessageFunc_(range_msgs_it);
+  }
+}
+
+void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+  if (range_message.min_range == range_message.max_range) {
+    processFixedRangeMsg(range_message);
+  } else {
+    processVariableRangeMsg(range_message);
+  }
+}
+
+void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+  if (!std::isinf(range_message.range)) {
+    RCLCPP_ERROR(
+      logger_,
+      "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. "
+      "Only -Inf (== object detected) and Inf (== no object detected) are valid.",
+      range_message.header.frame_id.c_str());
+    return;
+  }
+
+  bool clear_sensor_cone = false;
+
+  if (range_message.range > 0) {  // +inf
+    if (!clear_on_max_reading_) {
+      return;  // no clearing at all
+    }
+    clear_sensor_cone = true;
+  }
+
+  range_message.range = range_message.min_range;
+
+  updateCostmap(range_message, clear_sensor_cone);
+}
+
+void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message)
+{
+  if (range_message.range < range_message.min_range || range_message.range >
+    range_message.max_range)
+  {
+    return;
+  }
+
+  bool clear_sensor_cone = false;
+
+  if (range_message.range >= range_message.max_range && clear_on_max_reading_) {
+    clear_sensor_cone = true;
+  }
+
+  updateCostmap(range_message, clear_sensor_cone);
+}
+
+void RangeSensorLayer::updateCostmap(
+  sensor_msgs::msg::Range & range_message,
+  bool clear_sensor_cone)
+{
+  max_angle_ = range_message.field_of_view / 2;
+
+  geometry_msgs::msg::PointStamped in, out;
+  in.header.stamp = range_message.header.stamp;
+  in.header.frame_id = range_message.header.frame_id;
+
+  if (!tf_->canTransform(
+      in.header.frame_id, global_frame_,
+      tf2_ros::fromMsg(in.header.stamp),
+      tf2_ros::fromRclcpp(transform_tolerance_)))
+  {
+    RCLCPP_INFO(
+      logger_, "Range sensor layer can't transform from %s to %s",
+      global_frame_.c_str(), in.header.frame_id.c_str());
+    return;
+  }
+
+  tf_->transform(in, out, global_frame_, transform_tolerance_);
+
+  double ox = out.point.x, oy = out.point.y;
+
+  in.point.x = range_message.range;
+
+  tf_->transform(in, out, global_frame_, transform_tolerance_);
+
+  double tx = out.point.x, ty = out.point.y;
+
+  // calculate target props
+  double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy);
+
+  // Integer Bounds of Update
+  int bx0, by0, bx1, by1;
+
+  // Triangle that will be really updated; the other cells within bounds are ignored
+  // This triangle is formed by the origin and left and right sides of sonar cone
+  int Ox, Oy, Ax, Ay, Bx, By;
+
+  // Bounds includes the origin
+  worldToMapNoBounds(ox, oy, Ox, Oy);
+  bx1 = bx0 = Ox;
+  by1 = by0 = Oy;
+  touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
+
+  // Update Map with Target Point
+  unsigned int aa, ab;
+  if (worldToMap(tx, ty, aa, ab)) {
+    setCost(aa, ab, 233);
+    touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
+  }
+
+  double mx, my;
+
+  // Update left side of sonar cone
+  mx = ox + cos(theta - max_angle_) * d * 1.2;
+  my = oy + sin(theta - max_angle_) * d * 1.2;
+  worldToMapNoBounds(mx, my, Ax, Ay);
+  bx0 = std::min(bx0, Ax);
+  bx1 = std::max(bx1, Ax);
+  by0 = std::min(by0, Ay);
+  by1 = std::max(by1, Ay);
+  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
+
+  // Update right side of sonar cone
+  mx = ox + cos(theta + max_angle_) * d * 1.2;
+  my = oy + sin(theta + max_angle_) * d * 1.2;
+
+  worldToMapNoBounds(mx, my, Bx, By);
+  bx0 = std::min(bx0, Bx);
+  bx1 = std::max(bx1, Bx);
+  by0 = std::min(by0, By);
+  by1 = std::max(by1, By);
+  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
+
+  // Limit Bounds to Grid
+  bx0 = std::max(0, bx0);
+  by0 = std::max(0, by0);
+  bx1 = std::min(static_cast<int>(size_x_), bx1);
+  by1 = std::min(static_cast<int>(size_y_), by1);
+
+  for (unsigned int x = bx0; x <= (unsigned int)bx1; x++) {
+    for (unsigned int y = by0; y <= (unsigned int)by1; y++) {
+      bool update_xy_cell = true;
+
+      // Unless inflate_cone_ is set to 100 %, we update cells only within the
+      // (partially inflated) sensor cone, projected on the costmap as a triangle.
+      // 0 % corresponds to just the triangle, but if your sensor fov is very
+      // narrow, the covered area can become zero due to cell discretization.
+      // See wiki description for more details
+      if (inflate_cone_ < 1.0) {
+        // Determine barycentric coordinates
+        int w0 = orient2d(Ax, Ay, Bx, By, x, y);
+        int w1 = orient2d(Bx, By, Ox, Oy, x, y);
+        int w2 = orient2d(Ox, Oy, Ax, Ay, x, y);
+
+        // Barycentric coordinates inside area threshold; this is not mathematically
+        // sound at all, but it works!
+        float bcciath = -static_cast<float>(inflate_cone_) * area(Ax, Ay, Bx, By, Ox, Oy);
+        update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
+      }
+
+      if (update_xy_cell) {
+        double wx, wy;
+        mapToWorld(x, y, wx, wy);
+        update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
+      }
+    }
+  }
+
+  buffered_readings_++;
+  last_reading_time_ = clock_->now();
+}
+
+void RangeSensorLayer::update_cell(
+  double ox, double oy, double ot, double r,
+  double nx, double ny, bool clear)
+{
+  unsigned int x, y;
+  if (worldToMap(nx, ny, x, y)) {
+    double dx = nx - ox, dy = ny - oy;
+    double theta = atan2(dy, dx) - ot;
+    theta = angles::normalize_angle(theta);
+    double phi = sqrt(dx * dx + dy * dy);
+    double sensor = 0.0;
+    if (!clear) {
+      sensor = sensor_model(r, phi, theta);
+    }
+    double prior = to_prob(getCost(x, y));
+    double prob_occ = sensor * prior;
+    double prob_not = (1 - sensor) * (1 - prior);
+    double new_prob = prob_occ / (prob_occ + prob_not);
+
+    RCLCPP_DEBUG(
+      logger_,
+      "%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
+    RCLCPP_DEBUG(
+      logger_,
+      "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
+    unsigned char c = to_cost(new_prob);
+    setCost(x, y, c);
+  }
+}
+
+void RangeSensorLayer::resetRange()
+{
+  min_x_ = min_y_ = std::numeric_limits<double>::max();
+  max_x_ = max_y_ = -std::numeric_limits<double>::max();
+}
+
+void RangeSensorLayer::updateBounds(
+  double robot_x, double robot_y,
+  double robot_yaw, double * min_x, double * min_y,
+  double * max_x, double * max_y)
+{
+  robot_yaw = 0 + robot_yaw;  // Avoid error if variable not in use
+  if (layered_costmap_->isRolling()) {
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  }
+
+  updateCostmap();
+
+  *min_x = std::min(*min_x, min_x_);
+  *min_y = std::min(*min_y, min_y_);
+  *max_x = std::max(*max_x, max_x_);
+  *max_y = std::max(*max_y, max_y_);
+
+  resetRange();
+
+  if (!enabled_) {
+    current_ = true;
+    return;
+  }
+
+  if (buffered_readings_ == 0) {
+    if (no_readings_timeout_ > 0.0 &&
+      (clock_->now() - last_reading_time_).seconds() >
+      no_readings_timeout_)
+    {
+      RCLCPP_WARN(
+        logger_,
+        "No range readings received for %.2f seconds, while expected at least every %.2f seconds.",
+        (clock_->now() - last_reading_time_).seconds(),
+        no_readings_timeout_);
+      current_ = false;
+    }
+  }
+}
+
+void RangeSensorLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & master_grid,
+  int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  unsigned char * master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+  unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
+
+  for (int j = min_j; j < max_j; j++) {
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++) {
+      unsigned char prob = costmap_[it];
+      unsigned char current;
+      if (prob == nav2_costmap_2d::NO_INFORMATION) {
+        it++;
+        continue;
+      } else if (prob > mark) {
+        current = nav2_costmap_2d::LETHAL_OBSTACLE;
+      } else if (prob < clear) {
+        current = nav2_costmap_2d::FREE_SPACE;
+      } else {
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+
+      if (old_cost == NO_INFORMATION || old_cost < current) {
+        master_array[it] = current;
+      }
+      it++;
+    }
+  }
+
+  buffered_readings_ = 0;
+
+  // if not current due to reset, set current now after clearing
+  if (!current_ && was_reset_) {
+    was_reset_ = false;
+    current_ = true;
+  }
+}
+
+void RangeSensorLayer::reset()
+{
+  RCLCPP_DEBUG(logger_, "Reseting range sensor layer...");
+  deactivate();
+  resetMaps();
+  was_reset_ = true;
+  activate();
+}
+
+void RangeSensorLayer::deactivate()
+{
+  range_msgs_buffer_.clear();
+}
+
+void RangeSensorLayer::activate()
+{
+  range_msgs_buffer_.clear();
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/static_layer.cpp

@@ -0,0 +1,512 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2015, Fetch Robotics, Inc.
+ *  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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+#include "nav2_costmap_2d/static_layer.hpp"
+
+#include <algorithm>
+#include <string>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/validate_messages.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::NO_INFORMATION;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+
+StaticLayer::StaticLayer()
+: map_buffer_(nullptr)
+{
+}
+
+StaticLayer::~StaticLayer()
+{
+}
+
+void
+StaticLayer::onInitialize()
+{
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  getParameters();
+
+  rclcpp::QoS map_qos(10);  // initialize to default
+  if (map_subscribe_transient_local_) {
+    map_qos.transient_local();
+    map_qos.reliable();
+    map_qos.keep_last(1);
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "Subscribing to the map topic (%s) with %s durability",
+    map_topic_.c_str(),
+    map_subscribe_transient_local_ ? "transient local" : "volatile");
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    map_topic_, map_qos,
+    std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
+
+  if (subscribe_to_updates_) {
+    RCLCPP_INFO(logger_, "Subscribing to updates");
+    map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
+      map_topic_ + "_updates",
+      rclcpp::SystemDefaultsQoS(),
+      std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
+  }
+}
+
+void
+StaticLayer::activate()
+{
+}
+
+void
+StaticLayer::deactivate()
+{
+  dyn_params_handler_.reset();
+}
+
+void
+StaticLayer::reset()
+{
+  has_updated_data_ = true;
+  current_ = false;
+}
+
+void
+StaticLayer::getParameters()
+{
+  int temp_lethal_threshold = 0;
+  double temp_tf_tol = 0.0;
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
+  declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
+  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
+  declareParameter("map_topic", rclcpp::ParameterValue(""));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false));
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  std::string private_map_topic, global_map_topic;
+  node->get_parameter(name_ + "." + "map_topic", private_map_topic);
+  node->get_parameter("map_topic", global_map_topic);
+  if (!private_map_topic.empty()) {
+    map_topic_ = private_map_topic;
+  } else {
+    map_topic_ = global_map_topic;
+  }
+  node->get_parameter(
+    name_ + "." + "map_subscribe_transient_local",
+    map_subscribe_transient_local_);
+  node->get_parameter("track_unknown_space", track_unknown_space_);
+  node->get_parameter("use_maximum", use_maximum_);
+  node->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
+  node->get_parameter("unknown_cost_value", unknown_cost_value_);
+  node->get_parameter("trinary_costmap", trinary_costmap_);
+  node->get_parameter("transform_tolerance", temp_tf_tol);
+
+  // Enforce bounds
+  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
+  map_received_ = false;
+  map_received_in_update_bounds_ = false;
+
+  transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &StaticLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
+}
+
+void
+StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
+{
+  RCLCPP_DEBUG(logger_, "StaticLayer: Process map");
+
+  unsigned int size_x = new_map.info.width;
+  unsigned int size_y = new_map.info.height;
+
+  RCLCPP_DEBUG(
+    logger_,
+    "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
+    new_map.info.resolution);
+
+  // resize costmap if size, resolution or origin do not match
+  Costmap2D * master = layered_costmap_->getCostmap();
+  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
+    master->getSizeInCellsY() != size_y ||
+    master->getResolution() != new_map.info.resolution ||
+    master->getOriginX() != new_map.info.origin.position.x ||
+    master->getOriginY() != new_map.info.origin.position.y ||
+    !layered_costmap_->isSizeLocked()))
+  {
+    // Update the size of the layered costmap (and all layers, including this one)
+    RCLCPP_INFO(
+      logger_,
+      "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
+      new_map.info.resolution);
+    layered_costmap_->resizeMap(
+      size_x, size_y, new_map.info.resolution,
+      new_map.info.origin.position.x,
+      new_map.info.origin.position.y,
+      true);
+  } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
+    resolution_ != new_map.info.resolution ||
+    origin_x_ != new_map.info.origin.position.x ||
+    origin_y_ != new_map.info.origin.position.y)
+  {
+    // only update the size of the costmap stored locally in this layer
+    RCLCPP_INFO(
+      logger_,
+      "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
+      new_map.info.resolution);
+    resizeMap(
+      size_x, size_y, new_map.info.resolution,
+      new_map.info.origin.position.x, new_map.info.origin.position.y);
+  }
+
+  unsigned int index = 0;
+
+  // we have a new map, update full size of map
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  // initialize the costmap with static data
+  for (unsigned int i = 0; i < size_y; ++i) {
+    for (unsigned int j = 0; j < size_x; ++j) {
+      unsigned char value = new_map.data[index];
+      costmap_[index] = interpretValue(value);
+      ++index;
+    }
+  }
+
+  map_frame_ = new_map.header.frame_id;
+
+  x_ = y_ = 0;
+  width_ = size_x_;
+  height_ = size_y_;
+  has_updated_data_ = true;
+
+  current_ = true;
+}
+
+void
+StaticLayer::matchSize()
+{
+  // If we are using rolling costmap, the static map size is
+  //   unrelated to the size of the layered costmap
+  if (!layered_costmap_->isRolling()) {
+    Costmap2D * master = layered_costmap_->getCostmap();
+    resizeMap(
+      master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+      master->getOriginX(), master->getOriginY());
+  }
+}
+
+unsigned char
+StaticLayer::interpretValue(unsigned char value)
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if (track_unknown_space_ && value == unknown_cost_value_) {
+    return NO_INFORMATION;
+  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
+    return FREE_SPACE;
+  } else if (value >= lethal_threshold_) {
+    return LETHAL_OBSTACLE;
+  } else if (trinary_costmap_) {
+    return FREE_SPACE;
+  }
+
+  double scale = static_cast<double>(value) / lethal_threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+void
+StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
+{
+  if (!nav2_util::validateMsg(*new_map)) {
+    RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
+    return;
+  }
+  if (!map_received_) {
+    processMap(*new_map);
+    map_received_ = true;
+    return;
+  }
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  map_buffer_ = new_map;
+}
+
+void
+StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (update->y < static_cast<int32_t>(y_) ||
+    y_ + height_ < update->y + update->height ||
+    update->x < static_cast<int32_t>(x_) ||
+    x_ + width_ < update->x + update->width)
+  {
+    RCLCPP_WARN(
+      logger_,
+      "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
+      "Static layer origin: %d, %d   bounds: %d X %d\n"
+      "Update origin: %d, %d   bounds: %d X %d",
+      x_, y_, width_, height_, update->x, update->y, update->width,
+      update->height);
+    return;
+  }
+
+  if (update->header.frame_id != map_frame_) {
+    RCLCPP_WARN(
+      logger_,
+      "StaticLayer: Map update ignored. Current map is in frame %s "
+      "but update was in frame %s",
+      map_frame_.c_str(), update->header.frame_id.c_str());
+    return;
+  }
+
+  unsigned int di = 0;
+  for (unsigned int y = 0; y < update->height; y++) {
+    unsigned int index_base = (update->y + y) * size_x_;
+    for (unsigned int x = 0; x < update->width; x++) {
+      unsigned int index = index_base + x + update->x;
+      costmap_[index] = interpretValue(update->data[di++]);
+    }
+  }
+
+  has_updated_data_ = true;
+}
+
+
+void
+StaticLayer::updateBounds(
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  if (!map_received_) {
+    map_received_in_update_bounds_ = false;
+    return;
+  }
+  map_received_in_update_bounds_ = true;
+
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  // If there is a new available map, load it.
+  if (map_buffer_) {
+    processMap(*map_buffer_);
+    map_buffer_ = nullptr;
+  }
+
+  if (!layered_costmap_->isRolling() ) {
+    if (!(has_updated_data_ || has_extra_bounds_)) {
+      return;
+    }
+  }
+
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  double wx, wy;
+
+  mapToWorld(x_, y_, wx, wy);
+  *min_x = std::min(wx, *min_x);
+  *min_y = std::min(wy, *min_y);
+
+  mapToWorld(x_ + width_, y_ + height_, wx, wy);
+  *max_x = std::max(wx, *max_x);
+  *max_y = std::max(wy, *max_y);
+
+  has_updated_data_ = false;
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void
+StaticLayer::updateFootprint(
+  double robot_x, double robot_y, double robot_yaw,
+  double * min_x, double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  if (!footprint_clearing_enabled_) {return;}
+
+  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+
+  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
+    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
+  }
+}
+
+void
+StaticLayer::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;
+  }
+  if (!map_received_in_update_bounds_) {
+    static int count = 0;
+    // throttle warning down to only 1/10 message rate
+    if (++count == 10) {
+      RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
+      count = 0;
+    }
+    return;
+  }
+
+  if (footprint_clearing_enabled_) {
+    setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
+  }
+
+  if (!layered_costmap_->isRolling()) {
+    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
+    if (!use_maximum_) {
+      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
+    } else {
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+    }
+  } else {
+    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
+    unsigned int mx, my;
+    double wx, wy;
+    // Might even be in a different frame
+    geometry_msgs::msg::TransformStamped transform;
+    try {
+      transform = tf_->lookupTransform(
+        map_frame_, global_frame_, tf2::TimePointZero,
+        transform_tolerance_);
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
+      return;
+    }
+    // Copy map data given proper transformations
+    tf2::Transform tf2_transform;
+    tf2::fromMsg(transform.transform, tf2_transform);
+
+    for (int i = min_i; i < max_i; ++i) {
+      for (int j = min_j; j < max_j; ++j) {
+        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
+        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
+        // Transform from global_frame_ to map_frame_
+        tf2::Vector3 p(wx, wy, 0);
+        p = tf2_transform * p;
+        // Set master_grid with cell from map
+        if (worldToMap(p.x(), p.y(), mx, my)) {
+          if (!use_maximum_) {
+            master_grid.setCost(i, j, getCost(mx, my));
+          } else {
+            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
+          }
+        }
+      }
+    }
+  }
+  current_ = true;
+}
+
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+StaticLayer::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_name == name_ + "." + "map_subscribe_transient_local" ||
+      param_name == name_ + "." + "map_topic" ||
+      param_name == name_ + "." + "subscribe_to_updates")
+    {
+      RCLCPP_WARN(
+        logger_, "%s is not a dynamic parameter "
+        "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
+    } else if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "transform_tolerance") {
+        transform_tolerance_ = tf2::durationFromSec(parameter.as_double());
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+
+        x_ = y_ = 0;
+        width_ = size_x_;
+        height_ = size_y_;
+        has_updated_data_ = true;
+        current_ = false;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -0,0 +1,538 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+#include "nav2_costmap_2d/voxel_layer.hpp"
+
+#include <algorithm>
+#include <cassert>
+#include <vector>
+#include <memory>
+#include <utility>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::NO_INFORMATION;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+
+void VoxelLayer::onInitialize()
+{
+  ObstacleLayer::onInitialize();
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("z_voxels", rclcpp::ParameterValue(10));
+  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
+  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
+  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
+  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
+  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
+  node->get_parameter(name_ + "." + "z_voxels", size_z_);
+  node->get_parameter(name_ + "." + "origin_z", origin_z_);
+  node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
+  node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
+  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
+  node->get_parameter(name_ + "." + "combination_method", combination_method_);
+  node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
+
+  auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
+
+  if (publish_voxel_) {
+    voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(
+      "voxel_grid", custom_qos);
+    voxel_pub_->on_activate();
+  }
+
+  clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "clearing_endpoints", custom_qos);
+  clearing_endpoints_pub_->on_activate();
+
+  unknown_threshold_ += (VOXEL_BITS - size_z_);
+  matchSize();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &VoxelLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
+}
+
+VoxelLayer::~VoxelLayer()
+{
+  dyn_params_handler_.reset();
+}
+
+void VoxelLayer::matchSize()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  ObstacleLayer::matchSize();
+  voxel_grid_.resize(size_x_, size_y_, size_z_);
+  assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
+}
+
+void VoxelLayer::reset()
+{
+  // Call the base class method before adding our own functionality
+  ObstacleLayer::reset();
+  resetMaps();
+}
+
+void VoxelLayer::resetMaps()
+{
+  // Call the base class method before adding our own functionality
+  // Note: at the time this was written, ObstacleLayer doesn't implement
+  // resetMaps so this goes to the next layer down Costmap2DLayer which also
+  // doesn't implement this, so it actually goes all the way to Costmap2D
+  ObstacleLayer::resetMaps();
+  voxel_grid_.reset();
+}
+
+void VoxelLayer::updateBounds(
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
+  double * min_y, double * max_x, double * max_y)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  if (rolling_window_) {
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  }
+  if (!enabled_) {
+    return;
+  }
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = getMarkingObservations(observations) && current;
+
+  // get the clearing observations
+  current = getClearingObservations(clearing_observations) && current;
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end();
+    ++it)
+  {
+    const Observation & obs = *it;
+
+    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+
+    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
+    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (*iter_z > max_obstacle_height_) {
+        continue;
+      }
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x) +
+        (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) +
+        (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_max_range) {
+        continue;
+      }
+
+      // If the point is too close, do not consider it
+      if (sq_dist < sq_obstacle_min_range) {
+        continue;
+      }
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my, mz;
+      if (*iter_z < origin_z_) {
+        if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz)) {
+          continue;
+        }
+      } else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) {
+        continue;
+      }
+
+      // mark the cell in the voxel grid and check if we should also mark it in the costmap
+      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) {
+        unsigned int index = getIndex(mx, my);
+
+        costmap_[index] = LETHAL_OBSTACLE;
+        touch(
+          static_cast<double>(*iter_x), static_cast<double>(*iter_y),
+          min_x, min_y, max_x, max_y);
+      }
+    }
+  }
+
+  if (publish_voxel_) {
+    auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>();
+    unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
+    grid_msg->size_x = voxel_grid_.sizeX();
+    grid_msg->size_y = voxel_grid_.sizeY();
+    grid_msg->size_z = voxel_grid_.sizeZ();
+    grid_msg->data.resize(size);
+    memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg->origin.x = origin_x_;
+    grid_msg->origin.y = origin_y_;
+    grid_msg->origin.z = origin_z_;
+
+    grid_msg->resolutions.x = resolution_;
+    grid_msg->resolutions.y = resolution_;
+    grid_msg->resolutions.z = z_resolution_;
+    grid_msg->header.frame_id = global_frame_;
+    grid_msg->header.stamp = clock_->now();
+
+    voxel_pub_->publish(std::move(grid_msg));
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void VoxelLayer::raytraceFreespace(
+  const Observation & clearing_observation, double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+    return;
+  }
+
+  double sensor_x, sensor_y, sensor_z;
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  double oz = clearing_observation.origin_.z;
+
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) {
+    RCLCPP_WARN(
+      logger_,
+      "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy, oz,
+      origin_x_, origin_y_, origin_z_,
+      origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(),
+      origin_z_ + getSizeInMetersZ());
+
+    return;
+  }
+
+  bool publish_clearing_points;
+
+  {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+    publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0);
+  }
+
+  clearing_endpoints_->data.clear();
+  clearing_endpoints_->width = clearing_observation.cloud_->width;
+  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->is_dense = true;
+  clearing_endpoints_->is_bigendian = false;
+
+  sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
+  modifier.setPointCloud2Fields(
+    3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(*clearing_endpoints_, "x");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(*clearing_endpoints_, "y");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(*clearing_endpoints_, "z");
+
+  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+  double map_end_x = origin_x_ + getSizeInMetersX();
+  double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + getSizeInMetersZ();
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    double wpx = *iter_x;
+    double wpy = *iter_y;
+    double wpz = *iter_z;
+
+    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
+    double scaling_fact = 1.0;
+    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
+    wpx = scaling_fact * (wpx - ox) + ox;
+    wpy = scaling_fact * (wpy - oy) + oy;
+    wpz = scaling_fact * (wpz - oz) + oz;
+
+    double a = wpx - ox;
+    double b = wpy - oy;
+    double c = wpz - oz;
+    double t = 1.0;
+
+    // we can only raytrace to a maximum z height
+    if (wpz > map_end_z) {
+      // we know we want the vector's z value to be max_z
+      t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
+    } else if (wpz < origin_z_) {
+      // and we can only raytrace down to the floor
+      // we know we want the vector's z value to be 0.0
+      t = std::min(t, (origin_z_ - oz) / c);
+    }
+
+    // the minimum value to raytrace from is the origin
+    if (wpx < origin_x_) {
+      t = std::min(t, (origin_x_ - ox) / a);
+    }
+    if (wpy < origin_y_) {
+      t = std::min(t, (origin_y_ - oy) / b);
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wpx > map_end_x) {
+      t = std::min(t, (map_end_x - ox) / a);
+    }
+    if (wpy > map_end_y) {
+      t = std::min(t, (map_end_y - oy) / b);
+    }
+
+    wpx = ox + a * t;
+    wpy = oy + b * t;
+    wpz = oz + c * t;
+
+    double point_x, point_y, point_z;
+    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) {
+      unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_);
+      unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
+
+
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      voxel_grid_.clearVoxelLineInMap(
+        sensor_x, sensor_y, sensor_z, point_x, point_y, point_z,
+        costmap_,
+        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+        cell_raytrace_max_range, cell_raytrace_min_range);
+
+      updateRaytraceBounds(
+        ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_,
+        clearing_observation.raytrace_min_range_, min_x, min_y,
+        max_x,
+        max_y);
+
+      if (publish_clearing_points) {
+        *clearing_endpoints_iter_x = wpx;
+        *clearing_endpoints_iter_y = wpy;
+        *clearing_endpoints_iter_z = wpz;
+
+        ++clearing_endpoints_iter_x;
+        ++clearing_endpoints_iter_y;
+        ++clearing_endpoints_iter_z;
+      }
+    }
+  }
+
+  if (publish_clearing_points) {
+    clearing_endpoints_->header.frame_id = global_frame_;
+    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+
+    clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
+  }
+}
+
+void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  // project the new origin into the grid
+  int cell_ox, cell_oy;
+  cell_ox = static_cast<int>((new_origin_x - origin_x_) / resolution_);
+  cell_oy = static_cast<int>((new_origin_y - origin_y_) / resolution_);
+
+  // compute the associated world coordinates for the origin cell
+  // beacuase we want to keep things grid-aligned
+  double new_grid_ox, new_grid_oy;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
+
+  // To save casting from unsigned int to int a bunch of times
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // we need to compute the overlap of the new and existing windows
+  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+
+  unsigned int cell_size_x = upper_right_x - lower_left_x;
+  unsigned int cell_size_y = upper_right_y - lower_left_y;
+
+  // we need a map to store the obstacles in the window temporarily
+  unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
+  unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int * voxel_map = voxel_grid_.getData();
+
+  // copy the local window in the costmap to the local map
+  copyMapRegion(
+    costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+  copyMapRegion(
+    voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+
+  // we'll reset our maps to unknown space if appropriate
+  resetMaps();
+
+  // update the origin with the appropriate world coordinates
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+
+  // compute the starting cell location for copying data back in
+  int start_x = lower_left_x - cell_ox;
+  int start_y = lower_left_y - cell_oy;
+
+  // now we want to copy the overlapping information back into the map, but in its new location
+  copyMapRegion(
+    local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x,
+    cell_size_y);
+  copyMapRegion(
+    local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_,
+    cell_size_x,
+    cell_size_y);
+
+  // make sure to clean up
+  delete[] local_map;
+  delete[] local_voxel_map;
+}
+
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+VoxelLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+  bool resize_map_needed = false;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "origin_z") {
+        origin_z_ = parameter.as_double();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "z_resolution") {
+        z_resolution_ = parameter.as_double();
+        resize_map_needed = true;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+        current_ = false;
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "publish_voxel_map") {
+        RCLCPP_WARN(
+          logger_, "publish voxel map is not a dynamic parameter "
+          "cannot be changed while running. Rejecting parameter update.");
+        continue;
+      }
+
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "z_voxels") {
+        size_z_ = parameter.as_int();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "unknown_threshold") {
+        unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_);
+      } else if (param_name == name_ + "." + "mark_threshold") {
+        mark_threshold_ = parameter.as_int();
+      } else if (param_name == name_ + "." + "combination_method") {
+        combination_method_ = parameter.as_int();
+      }
+    }
+  }
+
+  if (resize_map_needed) {
+    matchSize();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_costmap_2d