add humble-navigation2

navigation2/nav2_costmap_2d/src/array_parser.cpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2012, 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 the 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: Dave Hershberger
+ */
+
+#include <cstdio>  // for EOF
+#include <string>
+#include <sstream>
+#include <vector>
+
+namespace nav2_costmap_2d
+{
+
+/** @brief Parse a vector of vector of floats from a string.
+ * @param input
+ * @param error_return
+ * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
+std::vector<std::vector<float>> parseVVF(const std::string & input, std::string & error_return)
+{
+  std::vector<std::vector<float>> result;
+
+  std::stringstream input_ss(input);
+  int depth = 0;
+  std::vector<float> current_vector;
+  while (!!input_ss && !input_ss.eof()) {
+    switch (input_ss.peek()) {
+      case EOF:
+        break;
+      case '[':
+        depth++;
+        if (depth > 2) {
+          error_return = "Array depth greater than 2";
+          return result;
+        }
+        input_ss.get();
+        current_vector.clear();
+        break;
+      case ']':
+        depth--;
+        if (depth < 0) {
+          error_return = "More close ] than open [";
+          return result;
+        }
+        input_ss.get();
+        if (depth == 1) {
+          result.push_back(current_vector);
+        }
+        break;
+      case ',':
+      case ' ':
+      case '\t':
+        input_ss.get();
+        break;
+      default:  // All other characters should be part of the numbers.
+        if (depth != 2) {
+          std::stringstream err_ss;
+          err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
+          error_return = err_ss.str();
+          return result;
+        }
+        float value;
+        input_ss >> value;
+        if (!!input_ss) {
+          current_vector.push_back(value);
+        }
+        break;
+    }
+  }
+
+  if (depth != 0) {
+    error_return = "Unterminated vector string.";
+  } else {
+    error_return = "";
+  }
+
+  return result;
+}
+
+}  // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/clear_costmap_service.cpp

@@ -0,0 +1,167 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 <vector>
+#include <string>
+#include <algorithm>
+#include <memory>
+
+#include "nav2_costmap_2d/clear_costmap_service.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+namespace nav2_costmap_2d
+{
+
+using std::vector;
+using std::string;
+using std::shared_ptr;
+using std::any_of;
+using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
+using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
+using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
+
+ClearCostmapService::ClearCostmapService(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  Costmap2DROS & costmap)
+: costmap_(costmap)
+{
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  reset_value_ = costmap_.getCostmap()->getDefaultValue();
+
+  clear_except_service_ = node->create_service<ClearExceptRegion>(
+    "clear_except_" + costmap_.getName(),
+    std::bind(
+      &ClearCostmapService::clearExceptRegionCallback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+  clear_around_service_ = node->create_service<ClearAroundRobot>(
+    "clear_around_" + costmap.getName(),
+    std::bind(
+      &ClearCostmapService::clearAroundRobotCallback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+  clear_entire_service_ = node->create_service<ClearEntirely>(
+    "clear_entirely_" + costmap_.getName(),
+    std::bind(
+      &ClearCostmapService::clearEntireCallback, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+}
+
+ClearCostmapService::~ClearCostmapService()
+{
+  // make sure services shutdown.
+  clear_except_service_.reset();
+  clear_around_service_.reset();
+  clear_entire_service_.reset();
+}
+
+void ClearCostmapService::clearExceptRegionCallback(
+  const shared_ptr<rmw_request_id_t>/*request_header*/,
+  const shared_ptr<ClearExceptRegion::Request> request,
+  const shared_ptr<ClearExceptRegion::Response>/*response*/)
+{
+  RCLCPP_INFO(
+    logger_, "%s",
+    ("Received request to clear except a region the " + costmap_.getName()).c_str());
+
+  clearRegion(request->reset_distance, true);
+}
+
+void ClearCostmapService::clearAroundRobotCallback(
+  const shared_ptr<rmw_request_id_t>/*request_header*/,
+  const shared_ptr<ClearAroundRobot::Request> request,
+  const shared_ptr<ClearAroundRobot::Response>/*response*/)
+{
+  clearRegion(request->reset_distance, false);
+}
+
+void ClearCostmapService::clearEntireCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<ClearEntirely::Request>/*request*/,
+  const std::shared_ptr<ClearEntirely::Response>/*response*/)
+{
+  RCLCPP_INFO(
+    logger_, "%s",
+    ("Received request to clear entirely the " + costmap_.getName()).c_str());
+
+  clearEntirely();
+}
+
+void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
+{
+  double x, y;
+
+  if (!getPosition(x, y)) {
+    RCLCPP_ERROR(
+      logger_, "%s",
+      "Cannot clear map because robot pose cannot be retrieved.");
+    return;
+  }
+
+  auto layers = costmap_.getLayeredCostmap()->getPlugins();
+
+  for (auto & layer : *layers) {
+    if (layer->isClearable()) {
+      auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
+      clearLayerRegion(costmap_layer, x, y, reset_distance, invert);
+    }
+  }
+
+  // AlexeyMerzlyakov: No need to clear layer region for costmap filters
+  // as they are always supposed to be not clearable.
+}
+
+void ClearCostmapService::clearLayerRegion(
+  shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
+  bool invert)
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
+  double start_point_x = pose_x - reset_distance / 2;
+  double start_point_y = pose_y - reset_distance / 2;
+  double end_point_x = start_point_x + reset_distance;
+  double end_point_y = start_point_y + reset_distance;
+
+  int start_x, start_y, end_x, end_y;
+  costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y);
+  costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y);
+
+  costmap->clearArea(start_x, start_y, end_x, end_y, invert);
+
+  double ox = costmap->getOriginX(), oy = costmap->getOriginY();
+  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
+  costmap->addExtraBounds(ox, oy, ox + width, oy + height);
+}
+
+void ClearCostmapService::clearEntirely()
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getCostmap()->getMutex()));
+  costmap_.resetLayers();
+}
+
+bool ClearCostmapService::getPosition(double & x, double & y) const
+{
+  geometry_msgs::msg::PoseStamped pose;
+  if (!costmap_.getRobotPose(pose)) {
+    return false;
+  }
+
+  x = pose.pose.position.x;
+  y = pose.pose.position.y;
+
+  return true;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d.cpp

@@ -0,0 +1,556 @@
+/*********************************************************************
+ *
+ * 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/costmap_2d.hpp"
+
+#include <algorithm>
+#include <cstdio>
+#include <string>
+#include <vector>
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+
+namespace nav2_costmap_2d
+{
+Costmap2D::Costmap2D(
+  unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
+  double origin_x, double origin_y, unsigned char default_value)
+: size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
+  origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
+{
+  access_ = new mutex_t();
+
+  // create the costmap
+  initMaps(size_x_, size_y_);
+  resetMaps();
+}
+
+Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map)
+: default_value_(FREE_SPACE)
+{
+  access_ = new mutex_t();
+
+  // fill local variables
+  size_x_ = map.info.width;
+  size_y_ = map.info.height;
+  resolution_ = map.info.resolution;
+  origin_x_ = map.info.origin.position.x;
+  origin_y_ = map.info.origin.position.y;
+
+  // create the costmap
+  costmap_ = new unsigned char[size_x_ * size_y_];
+
+  // fill the costmap with a data
+  int8_t data;
+  for (unsigned int it = 0; it < size_x_ * size_y_; it++) {
+    data = map.data[it];
+    if (data == nav2_util::OCC_GRID_UNKNOWN) {
+      costmap_[it] = NO_INFORMATION;
+    } else {
+      // Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED]
+      // to costmap data range [FREE_SPACE..LETHAL_OBSTACLE]
+      costmap_[it] = std::round(
+        static_cast<double>(data) * (LETHAL_OBSTACLE - FREE_SPACE) /
+        (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE));
+    }
+  }
+}
+
+void Costmap2D::deleteMaps()
+{
+  // clean up data
+  std::unique_lock<mutex_t> lock(*access_);
+  delete[] costmap_;
+  costmap_ = NULL;
+}
+
+void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
+{
+  std::unique_lock<mutex_t> lock(*access_);
+  delete[] costmap_;
+  costmap_ = new unsigned char[size_x * size_y];
+}
+
+void Costmap2D::resizeMap(
+  unsigned int size_x, unsigned int size_y, double resolution,
+  double origin_x, double origin_y)
+{
+  size_x_ = size_x;
+  size_y_ = size_y;
+  resolution_ = resolution;
+  origin_x_ = origin_x;
+  origin_y_ = origin_y;
+
+  initMaps(size_x, size_y);
+
+  // reset our maps to have no information
+  resetMaps();
+}
+
+void Costmap2D::resetMaps()
+{
+  std::unique_lock<mutex_t> lock(*access_);
+  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
+}
+
+void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
+{
+  resetMapToValue(x0, y0, xn, yn, default_value_);
+}
+
+void Costmap2D::resetMapToValue(
+  unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value)
+{
+  std::unique_lock<mutex_t> lock(*(access_));
+  unsigned int len = xn - x0;
+  for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) {
+    memset(costmap_ + y, value, len * sizeof(unsigned char));
+  }
+}
+
+bool Costmap2D::copyCostmapWindow(
+  const Costmap2D & map, double win_origin_x, double win_origin_y,
+  double win_size_x,
+  double win_size_y)
+{
+  // check for self windowing
+  if (this == &map) {
+    // ROS_ERROR("Cannot convert this costmap into a window of itself");
+    return false;
+  }
+
+  // clean up old data
+  deleteMaps();
+
+  // compute the bounds of our new map
+  unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) ||
+    !map.worldToMap(
+      win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x,
+      upper_right_y))
+  {
+    // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
+    return false;
+  }
+
+  size_x_ = upper_right_x - lower_left_x;
+  size_y_ = upper_right_y - lower_left_y;
+  resolution_ = map.resolution_;
+  origin_x_ = win_origin_x;
+  origin_y_ = win_origin_y;
+
+  // initialize our various maps and reset markers for inflation
+  initMaps(size_x_, size_y_);
+
+  // copy the window of the static map and the costmap that we're taking
+  copyMapRegion(
+    map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_,
+    size_x_,
+    size_y_);
+  return true;
+}
+
+bool Costmap2D::copyWindow(
+  const Costmap2D & source,
+  unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn,
+  unsigned int dx0, unsigned int dy0)
+{
+  const unsigned int sz_x = sxn - sx0;
+  const unsigned int sz_y = syn - sy0;
+
+  if (sxn > source.getSizeInCellsX() || syn > source.getSizeInCellsY()) {
+    return false;
+  }
+
+  if (dx0 + sz_x > size_x_ || dy0 + sz_y > size_y_) {
+    return false;
+  }
+
+  copyMapRegion(
+    source.costmap_, sx0, sy0, source.size_x_,
+    costmap_, dx0, dy0, size_x_,
+    sz_x, sz_y);
+  return true;
+}
+
+Costmap2D & Costmap2D::operator=(const Costmap2D & map)
+{
+  // check for self assignement
+  if (this == &map) {
+    return *this;
+  }
+
+  // clean up old data
+  deleteMaps();
+
+  size_x_ = map.size_x_;
+  size_y_ = map.size_y_;
+  resolution_ = map.resolution_;
+  origin_x_ = map.origin_x_;
+  origin_y_ = map.origin_y_;
+
+  // initialize our various maps
+  initMaps(size_x_, size_y_);
+
+  // copy the cost map
+  memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
+
+  return *this;
+}
+
+Costmap2D::Costmap2D(const Costmap2D & map)
+: costmap_(NULL)
+{
+  access_ = new mutex_t();
+  *this = map;
+}
+
+// just initialize everything to NULL by default
+Costmap2D::Costmap2D()
+: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
+{
+  access_ = new mutex_t();
+}
+
+Costmap2D::~Costmap2D()
+{
+  deleteMaps();
+  delete access_;
+}
+
+unsigned int Costmap2D::cellDistance(double world_dist)
+{
+  double cells_dist = std::max(0.0, ceil(world_dist / resolution_));
+  return (unsigned int)cells_dist;
+}
+
+unsigned char * Costmap2D::getCharMap() const
+{
+  return costmap_;
+}
+
+unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+{
+  return costmap_[getIndex(mx, my)];
+}
+
+unsigned char Costmap2D::getCost(unsigned int undex) const
+{
+  return costmap_[undex];
+}
+
+void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
+{
+  costmap_[getIndex(mx, my)] = cost;
+}
+
+void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const
+{
+  wx = origin_x_ + (mx + 0.5) * resolution_;
+  wy = origin_y_ + (my + 0.5) * resolution_;
+}
+
+bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const
+{
+  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 true;
+  }
+  return false;
+}
+
+void Costmap2D::worldToMapNoBounds(double wx, double wy, int & mx, int & my) const
+{
+  mx = static_cast<int>((wx - origin_x_) / resolution_);
+  my = static_cast<int>((wy - origin_y_) / resolution_);
+}
+
+void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int & mx, int & my) const
+{
+  // Here we avoid doing any math to wx,wy before comparing them to
+  // the bounds, so their values can go out to the max and min values
+  // of double floating point.
+  if (wx < origin_x_) {
+    mx = 0;
+  } else if (wx > resolution_ * size_x_ + origin_x_) {
+    mx = size_x_ - 1;
+  } else {
+    mx = static_cast<int>((wx - origin_x_) / resolution_);
+  }
+
+  if (wy < origin_y_) {
+    my = 0;
+  } else if (wy > resolution_ * size_y_ + origin_y_) {
+    my = size_y_ - 1;
+  } else {
+    my = static_cast<int>((wy - origin_y_) / resolution_);
+  }
+}
+
+void Costmap2D::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
+  // because 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];
+
+  // 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);
+
+  // now we'll set the costmap to be completely unknown if we track unknown space
+  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);
+
+  // make sure to clean up
+  delete[] local_map;
+}
+
+bool Costmap2D::setConvexPolygonCost(
+  const std::vector<geometry_msgs::msg::Point> & polygon,
+  unsigned char cost_value)
+{
+  // we assume the polygon is given in the global_frame...
+  // we need to transform it to map coordinates
+  std::vector<MapLocation> map_polygon;
+  for (unsigned int i = 0; i < polygon.size(); ++i) {
+    MapLocation loc;
+    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
+      // ("Polygon lies outside map bounds, so we can't fill it");
+      return false;
+    }
+    map_polygon.push_back(loc);
+  }
+
+  std::vector<MapLocation> polygon_cells;
+
+  // get the cells that fill the polygon
+  convexFillCells(map_polygon, polygon_cells);
+
+  // set the cost of those cells
+  for (unsigned int i = 0; i < polygon_cells.size(); ++i) {
+    unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
+    costmap_[index] = cost_value;
+  }
+  return true;
+}
+
+void Costmap2D::polygonOutlineCells(
+  const std::vector<MapLocation> & polygon,
+  std::vector<MapLocation> & polygon_cells)
+{
+  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
+  for (unsigned int i = 0; i < polygon.size() - 1; ++i) {
+    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
+  }
+  if (!polygon.empty()) {
+    unsigned int last_index = polygon.size() - 1;
+    // we also need to close the polygon by going from the last point to the first
+    raytraceLine(
+      cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x,
+      polygon[0].y);
+  }
+}
+
+void Costmap2D::convexFillCells(
+  const std::vector<MapLocation> & polygon,
+  std::vector<MapLocation> & polygon_cells)
+{
+  // we need a minimum polygon of a triangle
+  if (polygon.size() < 3) {
+    return;
+  }
+
+  // first get the cells that make up the outline of the polygon
+  polygonOutlineCells(polygon, polygon_cells);
+
+  // quick bubble sort to sort points by x
+  MapLocation swap;
+  unsigned int i = 0;
+  while (i < polygon_cells.size() - 1) {
+    if (polygon_cells[i].x > polygon_cells[i + 1].x) {
+      swap = polygon_cells[i];
+      polygon_cells[i] = polygon_cells[i + 1];
+      polygon_cells[i + 1] = swap;
+
+      if (i > 0) {
+        --i;
+      }
+    } else {
+      ++i;
+    }
+  }
+
+  i = 0;
+  MapLocation min_pt;
+  MapLocation max_pt;
+  unsigned int min_x = polygon_cells[0].x;
+  unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
+
+  // walk through each column and mark cells inside the polygon
+  for (unsigned int x = min_x; x <= max_x; ++x) {
+    if (i >= polygon_cells.size() - 1) {
+      break;
+    }
+
+    if (polygon_cells[i].y < polygon_cells[i + 1].y) {
+      min_pt = polygon_cells[i];
+      max_pt = polygon_cells[i + 1];
+    } else {
+      min_pt = polygon_cells[i + 1];
+      max_pt = polygon_cells[i];
+    }
+
+    i += 2;
+    while (i < polygon_cells.size() && polygon_cells[i].x == x) {
+      if (polygon_cells[i].y < min_pt.y) {
+        min_pt = polygon_cells[i];
+      } else if (polygon_cells[i].y > max_pt.y) {
+        max_pt = polygon_cells[i];
+      }
+      ++i;
+    }
+
+    MapLocation pt;
+    // loop though cells in the column
+    for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) {
+      pt.x = x;
+      pt.y = y;
+      polygon_cells.push_back(pt);
+    }
+  }
+}
+
+unsigned int Costmap2D::getSizeInCellsX() const
+{
+  return size_x_;
+}
+
+unsigned int Costmap2D::getSizeInCellsY() const
+{
+  return size_y_;
+}
+
+double Costmap2D::getSizeInMetersX() const
+{
+  return (size_x_ - 1 + 0.5) * resolution_;
+}
+
+double Costmap2D::getSizeInMetersY() const
+{
+  return (size_y_ - 1 + 0.5) * resolution_;
+}
+
+double Costmap2D::getOriginX() const
+{
+  return origin_x_;
+}
+
+double Costmap2D::getOriginY() const
+{
+  return origin_y_;
+}
+
+double Costmap2D::getResolution() const
+{
+  return resolution_;
+}
+
+bool Costmap2D::saveMap(std::string file_name)
+{
+  FILE * fp = fopen(file_name.c_str(), "w");
+
+  if (!fp) {
+    return false;
+  }
+
+  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
+  for (unsigned int iy = 0; iy < size_y_; iy++) {
+    for (unsigned int ix = 0; ix < size_x_; ix++) {
+      unsigned char cost = getCost(ix, iy);
+      fprintf(fp, "%d ", cost);
+    }
+    fprintf(fp, "\n");
+  }
+  fclose(fp);
+  return true;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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.
+ */
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "nav2_voxel_grid/voxel_grid.hpp"
+#include "nav2_msgs/msg/voxel_grid.hpp"
+#include "nav2_util/execution_timer.hpp"
+
+static inline void mapToWorld3D(
+  const unsigned int mx,
+  const unsigned int my, const unsigned int mz,
+  const double origin_x, const double origin_y, const double origin_z,
+  const double x_resolution, const double y_resolution,
+  const double z_resolution,
+  double & wx, double & wy, double & wz)
+{
+  // returns the center point of the cell
+  wx = origin_x + (mx + 0.5) * x_resolution;
+  wy = origin_y + (my + 0.5) * y_resolution;
+  wz = origin_z + (mz + 0.5) * z_resolution;
+}
+
+struct Cell
+{
+  double x;
+  double y;
+  double z;
+  nav2_voxel_grid::VoxelStatus status;
+};
+typedef std::vector<Cell> V_Cell;
+
+float g_colors_r[] = {0.0f, 0.0f, 1.0f};
+float g_colors_g[] = {0.0f, 0.0f, 0.0f};
+float g_colors_b[] = {0.0f, 1.0f, 0.0f};
+float g_colors_a[] = {0.0f, 0.5f, 1.0f};
+
+V_Cell g_marked;
+V_Cell g_unknown;
+
+rclcpp::Node::SharedPtr g_node;
+
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
+
+/**
+ * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
+ * @param cloud PointCloud2 Ptr which needs to be filled
+ * @param num_channels Represents the total number of points that are going to be filled
+ * @param header Carries the header information that needs to be assigned to PointCloud2 header
+ * @param g_cells contains the x, y, z values that needs to be added to the PointCloud2
+ */
+void pointCloud2Helper(
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,
+  uint32_t num_channels,
+  std_msgs::msg::Header header,
+  V_Cell & g_cells)
+{
+  cloud->header = header;
+  cloud->width = num_channels;
+  cloud->height = 1;
+  cloud->is_dense = true;
+  cloud->is_bigendian = false;
+  sensor_msgs::PointCloud2Modifier modifier(*cloud);
+
+  modifier.setPointCloud2Fields(
+    6, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "r", 1, sensor_msgs::msg::PointField::UINT8,
+    "g", 1, sensor_msgs::msg::PointField::UINT8,
+    "b", 1, sensor_msgs::msg::PointField::UINT8);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud, "z");
+
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud, "r");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud, "g");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud, "b");
+
+  for (uint32_t i = 0; i < num_channels; ++i) {
+    Cell & c = g_cells[i];
+    // assigning value to the point cloud2's iterator
+    *iter_x = c.x;
+    *iter_y = c.y;
+    *iter_z = c.z;
+    *iter_r = g_colors_r[c.status] * 255.0;
+    *iter_g = g_colors_g[c.status] * 255.0;
+    *iter_b = g_colors_b[c.status] * 255.0;
+
+    ++iter_x;
+    ++iter_y;
+    ++iter_z;
+    ++iter_r;
+    ++iter_g;
+    ++iter_b;
+  }
+}
+
+void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
+{
+  if (grid->data.empty()) {
+    RCLCPP_ERROR(g_node->get_logger(), "Received empty voxel grid");
+    return;
+  }
+
+  nav2_util::ExecutionTimer timer;
+  timer.start();
+
+  RCLCPP_DEBUG(g_node->get_logger(), "Received voxel grid");
+  const std::string frame_id = grid->header.frame_id;
+  const rclcpp::Time stamp = grid->header.stamp;
+  const uint32_t * data = &grid->data.front();
+  const double x_origin = grid->origin.x;
+  const double y_origin = grid->origin.y;
+  const double z_origin = grid->origin.z;
+  const double x_res = grid->resolutions.x;
+  const double y_res = grid->resolutions.y;
+  const double z_res = grid->resolutions.z;
+  const uint32_t x_size = grid->size_x;
+  const uint32_t y_size = grid->size_y;
+  const uint32_t z_size = grid->size_z;
+
+  g_marked.clear();
+  g_unknown.clear();
+  uint32_t num_marked = 0;
+  uint32_t num_unknown = 0;
+  for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) {
+    for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) {
+      for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) {
+        nav2_voxel_grid::VoxelStatus status =
+          nav2_voxel_grid::VoxelGrid::getVoxel(
+          x_grid, y_grid,
+          z_grid, x_size, y_size, z_size, data);
+        if (status == nav2_voxel_grid::UNKNOWN) {
+          Cell c;
+          c.status = status;
+          mapToWorld3D(
+            x_grid, y_grid, z_grid, x_origin, y_origin,
+            z_origin, x_res, y_res, z_res, c.x, c.y, c.z);
+
+          g_unknown.push_back(c);
+
+          ++num_unknown;
+        } else if (status == nav2_voxel_grid::MARKED) {
+          Cell c;
+          c.status = status;
+          mapToWorld3D(
+            x_grid, y_grid, z_grid, x_origin, y_origin,
+            z_origin, x_res, y_res, z_res, c.x, c.y, c.z);
+
+          g_marked.push_back(c);
+
+          ++num_marked;
+        }
+      }
+    }
+  }
+
+  std_msgs::msg::Header pcl_header;
+  pcl_header.frame_id = frame_id;
+  pcl_header.stamp = stamp;
+
+  {
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pointCloud2Helper(cloud, num_marked, pcl_header, g_marked);
+    pub_marked->publish(std::move(cloud));
+  }
+
+  {
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown);
+    pub_unknown->publish(std::move(cloud));
+  }
+
+  timer.end();
+  RCLCPP_DEBUG(
+    g_node->get_logger(), "Published %d points in %f seconds",
+    num_marked + num_unknown, timer.elapsed_time_in_seconds());
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  g_node = rclcpp::Node::make_shared("costmap_2d_cloud");
+
+  RCLCPP_DEBUG(g_node->get_logger(), "Starting up costmap_2d_cloud");
+
+  pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "voxel_marked_cloud", 1);
+  pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "voxel_unknown_cloud", 1);
+  auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
+    "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
+
+  rclcpp::spin(g_node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_costmap_2d/src/costmap_2d_markers.cpp

@@ -0,0 +1,162 @@
+/*********************************************************************
+ *
+ * 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 <string>
+#include <vector>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "nav2_msgs/msg/voxel_grid.hpp"
+#include "nav2_voxel_grid/voxel_grid.hpp"
+#include "nav2_util/execution_timer.hpp"
+
+struct Cell
+{
+  double x;
+  double y;
+  double z;
+  nav2_voxel_grid::VoxelStatus status;
+};
+typedef std::vector<Cell> V_Cell;
+
+float g_colors_r[] = {0.0f, 0.0f, 1.0f};
+float g_colors_g[] = {0.0f, 0.0f, 0.0f};
+float g_colors_b[] = {0.0f, 1.0f, 0.0f};
+float g_colors_a[] = {0.0f, 0.5f, 1.0f};
+
+V_Cell g_cells;
+rclcpp::Node::SharedPtr g_node;
+rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub;
+
+void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
+{
+  if (grid->data.empty()) {
+    RCLCPP_ERROR(g_node->get_logger(), "Received voxel grid");
+    return;
+  }
+
+  nav2_util::ExecutionTimer timer;
+  timer.start();
+
+  RCLCPP_DEBUG(g_node->get_logger(), "Received voxel grid");
+
+  const std::string frame_id = grid->header.frame_id;
+  const rclcpp::Time stamp = grid->header.stamp;
+  const uint32_t * data = &grid->data.front();
+  const double x_origin = grid->origin.x;
+  const double y_origin = grid->origin.y;
+  const double z_origin = grid->origin.z;
+  const double x_res = grid->resolutions.x;
+  const double y_res = grid->resolutions.y;
+  const double z_res = grid->resolutions.z;
+  const uint32_t x_size = grid->size_x;
+  const uint32_t y_size = grid->size_y;
+  const uint32_t z_size = grid->size_z;
+
+  g_cells.clear();
+  uint32_t num_markers = 0;
+  for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) {
+    for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) {
+      for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) {
+        nav2_voxel_grid::VoxelStatus status =
+          nav2_voxel_grid::VoxelGrid::getVoxel(
+          x_grid, y_grid,
+          z_grid, x_size, y_size, z_size, data);
+        if (status == nav2_voxel_grid::MARKED) {
+          Cell c;
+          c.status = status;
+          c.x = x_origin + (x_grid + 0.5) * x_res;
+          c.y = y_origin + (y_grid + 0.5) * y_res;
+          c.z = z_origin + (z_grid + 0.5) * z_res;
+          g_cells.push_back(c);
+
+          ++num_markers;
+        }
+      }
+    }
+  }
+
+  auto m = std::make_unique<visualization_msgs::msg::Marker>();
+  m->header.frame_id = frame_id;
+  m->header.stamp = stamp;
+  m->ns = g_node->get_namespace();
+  m->id = 0;
+  m->type = visualization_msgs::msg::Marker::CUBE_LIST;
+  m->action = visualization_msgs::msg::Marker::ADD;
+  m->pose.orientation.w = 1.0;
+  m->scale.x = x_res;
+  m->scale.y = y_res;
+  m->scale.z = z_res;
+  m->color.r = g_colors_r[nav2_voxel_grid::MARKED];
+  m->color.g = g_colors_g[nav2_voxel_grid::MARKED];
+  m->color.b = g_colors_b[nav2_voxel_grid::MARKED];
+  m->color.a = g_colors_a[nav2_voxel_grid::MARKED];
+  m->points.resize(num_markers);
+  for (uint32_t i = 0; i < num_markers; ++i) {
+    Cell & c = g_cells[i];
+    geometry_msgs::msg::Point & p = m->points[i];
+    p.x = c.x;
+    p.y = c.y;
+    p.z = c.z;
+  }
+
+  pub->publish(std::move(m));
+
+  timer.end();
+  RCLCPP_INFO(
+    g_node->get_logger(), "Published %d markers in %f seconds",
+    num_markers, timer.elapsed_time_in_seconds());
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  g_node = rclcpp::Node::make_shared("costmap_2d_marker");
+
+  RCLCPP_DEBUG(g_node->get_logger(), "Starting costmap_2d_marker");
+
+  pub = g_node->create_publisher<visualization_msgs::msg::Marker>(
+    "visualization_marker", 1);
+
+  auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
+    "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
+
+  rclcpp::spin(g_node->get_node_base_interface());
+}

navigation2/nav2_costmap_2d/src/costmap_2d_node.cpp

@@ -0,0 +1,51 @@
+/*********************************************************************
+ *
+ * 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 <memory>
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap");
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -0,0 +1,264 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2019, Samsung Research America, 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/costmap_2d_publisher.hpp"
+
+#include <string>
+#include <memory>
+#include <utility>
+
+#include "nav2_costmap_2d/cost_values.hpp"
+
+namespace nav2_costmap_2d
+{
+
+char * Costmap2DPublisher::cost_translation_table_ = NULL;
+
+Costmap2DPublisher::Costmap2DPublisher(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  Costmap2D * costmap,
+  std::string global_frame,
+  std::string topic_name,
+  bool always_send_full_costmap)
+: costmap_(costmap),
+  global_frame_(global_frame),
+  topic_name_(topic_name),
+  active_(false),
+  always_send_full_costmap_(always_send_full_costmap)
+{
+  auto node = parent.lock();
+  clock_ = node->get_clock();
+  logger_ = node->get_logger();
+
+  auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
+
+  // TODO(bpwilcox): port onNewSubscription functionality for publisher
+  costmap_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
+    topic_name,
+    custom_qos);
+  costmap_raw_pub_ = node->create_publisher<nav2_msgs::msg::Costmap>(
+    topic_name + "_raw",
+    custom_qos);
+  costmap_update_pub_ = node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
+    topic_name + "_updates", custom_qos);
+
+  // Create a service that will use the callback function to handle requests.
+  costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(
+    "get_costmap", std::bind(
+      &Costmap2DPublisher::costmap_service_callback,
+      this, std::placeholders::_1, std::placeholders::_2,
+      std::placeholders::_3));
+
+  if (cost_translation_table_ == NULL) {
+    cost_translation_table_ = new char[256];
+
+    // special values:
+    cost_translation_table_[0] = 0;  // NO obstacle
+    cost_translation_table_[253] = 99;  // INSCRIBED obstacle
+    cost_translation_table_[254] = 100;  // LETHAL obstacle
+    cost_translation_table_[255] = -1;  // UNKNOWN
+
+    // regular cost values scale the range 1 to 252 (inclusive) to fit
+    // into 1 to 98 (inclusive).
+    for (int i = 1; i < 253; i++) {
+      cost_translation_table_[i] = static_cast<char>(1 + (97 * (i - 1)) / 251);
+    }
+  }
+
+  xn_ = yn_ = 0;
+  x0_ = costmap_->getSizeInCellsX();
+  y0_ = costmap_->getSizeInCellsY();
+}
+
+Costmap2DPublisher::~Costmap2DPublisher() {}
+
+// TODO(bpwilcox): find equivalent/workaround to ros::SingleSubscriberPublishr
+/*
+void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub)
+{
+  prepareGrid();
+  pub.publish(grid_);
+} */
+
+// prepare grid_ message for publication.
+void Costmap2DPublisher::prepareGrid()
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  grid_resolution = costmap_->getResolution();
+  grid_width = costmap_->getSizeInCellsX();
+  grid_height = costmap_->getSizeInCellsY();
+
+  grid_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+  grid_->header.frame_id = global_frame_;
+  grid_->header.stamp = clock_->now();
+
+  grid_->info.resolution = grid_resolution;
+
+  grid_->info.width = grid_width;
+  grid_->info.height = grid_height;
+
+  double wx, wy;
+  costmap_->mapToWorld(0, 0, wx, wy);
+  grid_->info.origin.position.x = wx - grid_resolution / 2;
+  grid_->info.origin.position.y = wy - grid_resolution / 2;
+  grid_->info.origin.position.z = 0.0;
+  grid_->info.origin.orientation.w = 1.0;
+  saved_origin_x_ = costmap_->getOriginX();
+  saved_origin_y_ = costmap_->getOriginY();
+
+  grid_->data.resize(grid_->info.width * grid_->info.height);
+
+  unsigned char * data = costmap_->getCharMap();
+  for (unsigned int i = 0; i < grid_->data.size(); i++) {
+    grid_->data[i] = cost_translation_table_[data[i]];
+  }
+}
+
+void Costmap2DPublisher::prepareCostmap()
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  double resolution = costmap_->getResolution();
+
+  costmap_raw_ = std::make_unique<nav2_msgs::msg::Costmap>();
+
+  costmap_raw_->header.frame_id = global_frame_;
+  costmap_raw_->header.stamp = clock_->now();
+
+  costmap_raw_->metadata.layer = "master";
+  costmap_raw_->metadata.resolution = resolution;
+
+  costmap_raw_->metadata.size_x = costmap_->getSizeInCellsX();
+  costmap_raw_->metadata.size_y = costmap_->getSizeInCellsY();
+
+  double wx, wy;
+  costmap_->mapToWorld(0, 0, wx, wy);
+  costmap_raw_->metadata.origin.position.x = wx - resolution / 2;
+  costmap_raw_->metadata.origin.position.y = wy - resolution / 2;
+  costmap_raw_->metadata.origin.position.z = 0.0;
+  costmap_raw_->metadata.origin.orientation.w = 1.0;
+
+  costmap_raw_->data.resize(costmap_raw_->metadata.size_x * costmap_raw_->metadata.size_y);
+
+  unsigned char * data = costmap_->getCharMap();
+  for (unsigned int i = 0; i < costmap_raw_->data.size(); i++) {
+    costmap_raw_->data[i] = data[i];
+  }
+}
+
+void Costmap2DPublisher::publishCostmap()
+{
+  if (costmap_raw_pub_->get_subscription_count() > 0) {
+    prepareCostmap();
+    costmap_raw_pub_->publish(std::move(costmap_raw_));
+  }
+  float resolution = costmap_->getResolution();
+
+  if (always_send_full_costmap_ || grid_resolution != resolution ||
+    grid_width != costmap_->getSizeInCellsX() ||
+    grid_height != costmap_->getSizeInCellsY() ||
+    saved_origin_x_ != costmap_->getOriginX() ||
+    saved_origin_y_ != costmap_->getOriginY())
+  {
+    if (costmap_pub_->get_subscription_count() > 0) {
+      prepareGrid();
+      costmap_pub_->publish(std::move(grid_));
+    }
+  } else if (x0_ < xn_) {
+    if (costmap_update_pub_->get_subscription_count() > 0) {
+      std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+      // Publish Just an Update
+      auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>();
+      update->header.stamp = rclcpp::Time();
+      update->header.frame_id = global_frame_;
+      update->x = x0_;
+      update->y = y0_;
+      update->width = xn_ - x0_;
+      update->height = yn_ - y0_;
+      update->data.resize(update->width * update->height);
+      unsigned int i = 0;
+      for (unsigned int y = y0_; y < yn_; y++) {
+        for (unsigned int x = x0_; x < xn_; x++) {
+          unsigned char cost = costmap_->getCost(x, y);
+          update->data[i++] = cost_translation_table_[cost];
+        }
+      }
+      costmap_update_pub_->publish(std::move(update));
+    }
+  }
+
+  xn_ = yn_ = 0;
+  x0_ = costmap_->getSizeInCellsX();
+  y0_ = costmap_->getSizeInCellsY();
+}
+
+void
+Costmap2DPublisher::costmap_service_callback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,
+  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response)
+{
+  RCLCPP_DEBUG(logger_, "Received costmap service request");
+
+  // TODO(bpwilcox): Grab correct orientation information
+  tf2::Quaternion quaternion;
+  quaternion.setRPY(0.0, 0.0, 0.0);
+
+  auto size_x = costmap_->getSizeInCellsX();
+  auto size_y = costmap_->getSizeInCellsY();
+  auto data_length = size_x * size_y;
+  unsigned char * data = costmap_->getCharMap();
+  auto current_time = clock_->now();
+
+  response->map.header.stamp = current_time;
+  response->map.header.frame_id = global_frame_;
+  response->map.metadata.size_x = size_x;
+  response->map.metadata.size_y = size_y;
+  response->map.metadata.resolution = costmap_->getResolution();
+  response->map.metadata.layer = "master";
+  response->map.metadata.map_load_time = current_time;
+  response->map.metadata.update_time = current_time;
+  response->map.metadata.origin.position.x = costmap_->getOriginX();
+  response->map.metadata.origin.position.y = costmap_->getOriginY();
+  response->map.metadata.origin.position.z = 0.0;
+  response->map.metadata.origin.orientation = tf2::toMsg(quaternion);
+  response->map.data.resize(data_length);
+  response->map.data.assign(data, data + data_length);
+}
+
+}  // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -0,0 +1,769 @@
+/*********************************************************************
+ *
+ * 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/costmap_2d_ros.hpp"
+
+#include <memory>
+#include <chrono>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_util/execution_timer.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/create_timer_ros.h"
+#include "nav2_util/robot_utils.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+Costmap2DROS::Costmap2DROS(const std::string & name)
+: Costmap2DROS(name, "/", name) {}
+
+Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("costmap", "", options),
+  name_("costmap"),
+  default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
+  default_types_{
+    "nav2_costmap_2d::StaticLayer",
+    "nav2_costmap_2d::ObstacleLayer",
+    "nav2_costmap_2d::InflationLayer"}
+{
+  is_lifecycle_follower_ = false;
+  
+  RCLCPP_INFO(get_logger(), "Creating Costmap");
+
+  declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
+  declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
+  declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
+  declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
+  declare_parameter("height", rclcpp::ParameterValue(5));
+  declare_parameter("width", rclcpp::ParameterValue(5));
+  declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+  declare_parameter(
+    "map_topic", rclcpp::ParameterValue(
+      (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map")));
+  declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+  declare_parameter("origin_x", rclcpp::ParameterValue(0.0));
+  declare_parameter("origin_y", rclcpp::ParameterValue(0.0));
+  declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_));
+  declare_parameter("filters", rclcpp::ParameterValue(std::vector<std::string>()));
+  declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0));
+  declare_parameter("resolution", rclcpp::ParameterValue(0.1));
+  declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
+  declare_parameter("robot_radius", rclcpp::ParameterValue(0.1));
+  declare_parameter("rolling_window", rclcpp::ParameterValue(false));
+  declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+  declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+  declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+  declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+  declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
+  declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+}
+
+Costmap2DROS::Costmap2DROS(
+  const std::string & name,
+  const std::string & parent_namespace,
+  const std::string & local_namespace)
+: nav2_util::LifecycleNode(name, "",
+    // NodeOption arguments take precedence over the ones provided on the command line
+    // use this to make sure the node is placed on the provided namespace
+    // TODO(orduno) Pass a sub-node instead of creating a new node for better handling
+    //              of the namespaces
+    rclcpp::NodeOptions().arguments({
+    "--ros-args", "-r", std::string("__ns:=") +
+    nav2_util::add_namespaces(parent_namespace, local_namespace),
+    "--ros-args", "-r", name + ":" + std::string("__node:=") + name
+  })),
+  name_(name),
+  parent_namespace_(parent_namespace),
+  default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
+  default_types_{
+    "nav2_costmap_2d::StaticLayer",
+    "nav2_costmap_2d::ObstacleLayer",
+    "nav2_costmap_2d::InflationLayer"}
+{
+  RCLCPP_INFO(get_logger(), "Creating Costmap");
+
+  declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false));
+  declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
+  declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
+  declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
+  declare_parameter("height", rclcpp::ParameterValue(5));
+  declare_parameter("width", rclcpp::ParameterValue(5));
+  declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+  declare_parameter(
+    "map_topic", rclcpp::ParameterValue(
+      (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map")));
+  declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+  declare_parameter("origin_x", rclcpp::ParameterValue(0.0));
+  declare_parameter("origin_y", rclcpp::ParameterValue(0.0));
+  declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_));
+  declare_parameter("filters", rclcpp::ParameterValue(std::vector<std::string>()));
+  declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0));
+  declare_parameter("resolution", rclcpp::ParameterValue(0.1));
+  declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
+  declare_parameter("robot_radius", rclcpp::ParameterValue(0.1));
+  declare_parameter("rolling_window", rclcpp::ParameterValue(false));
+  declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+  declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+  declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+  declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+  declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
+  declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+}
+
+Costmap2DROS::~Costmap2DROS()
+{
+}
+
+nav2_util::CallbackReturn
+Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+  getParameters();
+
+  callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+
+  // Create the costmap itself
+  layered_costmap_ = std::make_unique<LayeredCostmap>(
+    global_frame_, rolling_window_, track_unknown_space_);
+
+  if (!layered_costmap_->isSizeLocked()) {
+    layered_costmap_->resizeMap(
+      (unsigned int)(map_width_meters_ / resolution_),
+      (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_);
+  }
+
+  // Create the transform-related objects
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+    get_node_base_interface(),
+    get_node_timers_interface(),
+    callback_group_);
+  tf_buffer_->setCreateTimerInterface(timer_interface);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  // Then load and add the plug-ins to the costmap
+  for (unsigned int i = 0; i < plugin_names_.size(); ++i) {
+    RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str());
+
+    std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types_[i]);
+
+    // lock the costmap because no update is allowed until the plugin is initialized
+    std::unique_lock<Costmap2D::mutex_t> lock(*(layered_costmap_->getCostmap()->getMutex()));
+
+    layered_costmap_->addPlugin(plugin);
+
+    // TODO(mjeronimo): instead of get(), use a shared ptr
+    plugin->initialize(
+      layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
+      shared_from_this(), callback_group_);
+
+    lock.unlock();
+
+    RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
+  }
+  // and costmap filters as well
+  for (unsigned int i = 0; i < filter_names_.size(); ++i) {
+    RCLCPP_INFO(get_logger(), "Using costmap filter \"%s\"", filter_names_[i].c_str());
+
+    std::shared_ptr<Layer> filter = plugin_loader_.createSharedInstance(filter_types_[i]);
+
+    // lock the costmap because no update is allowed until the filter is initialized
+    std::unique_lock<Costmap2D::mutex_t> lock(*(layered_costmap_->getCostmap()->getMutex()));
+
+    layered_costmap_->addFilter(filter);
+
+    filter->initialize(
+      layered_costmap_.get(), filter_names_[i], tf_buffer_.get(),
+      shared_from_this(), callback_group_);
+
+    lock.unlock();
+
+    RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str());
+  }
+
+  // Create the publishers and subscribers
+  footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
+    "footprint",
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1));
+
+  footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
+    "published_footprint", rclcpp::SystemDefaultsQoS());
+
+  costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
+    shared_from_this(),
+    layered_costmap_->getCostmap(), global_frame_,
+    "costmap", always_send_full_costmap_);
+
+  // Set the footprint
+  if (use_radius_) {
+    setRobotFootprint(makeFootprintFromRadius(robot_radius_));
+  } else {
+    std::vector<geometry_msgs::msg::Point> new_footprint;
+    makeFootprintFromString(footprint_, new_footprint);
+    setRobotFootprint(new_footprint);
+  }
+
+  // Add cleaning service
+  clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
+
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  executor_->add_callback_group(callback_group_, get_node_base_interface());
+  executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  costmap_publisher_->on_activate();
+  footprint_pub_->on_activate();
+
+  // First, make sure that the transform between the robot base frame
+  // and the global frame is available
+
+  std::string tf_error;
+
+  RCLCPP_INFO(get_logger(), "Checking transform");
+  rclcpp::Rate r(2);
+  while (rclcpp::ok() &&
+    !tf_buffer_->canTransform(
+      global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error))
+  {
+    RCLCPP_INFO(
+      get_logger(), "Timed out waiting for transform from %s to %s"
+      " to become available, tf error: %s",
+      robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
+
+    // The error string will accumulate and errors will typically be the same, so the last
+    // will do for the warning above. Reset the string here to avoid accumulation
+    tf_error.clear();
+    r.sleep();
+  }
+
+  // Create a thread to handle updating the map
+  stopped_ = true;  // to active plugins
+  stop_updates_ = false;
+  map_update_thread_shutdown_ = false;
+  map_update_thread_ = std::make_unique<std::thread>(
+    std::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency_));
+
+  start();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler = this->add_on_set_parameters_callback(
+    std::bind(&Costmap2DROS::dynamicParametersCallback, this, _1));
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  dyn_params_handler.reset();
+
+  stop();
+
+  // Map thread stuff
+  map_update_thread_shutdown_ = true;
+
+  if (map_update_thread_->joinable()) {
+    map_update_thread_->join();
+  }
+
+  costmap_publisher_->on_deactivate();
+  footprint_pub_->on_deactivate();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  layered_costmap_.reset();
+
+  tf_listener_.reset();
+  tf_buffer_.reset();
+
+  footprint_sub_.reset();
+  footprint_pub_.reset();
+
+  costmap_publisher_.reset();
+  clear_costmap_service_.reset();
+
+  executor_thread_.reset();
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void
+Costmap2DROS::getParameters()
+{
+  RCLCPP_DEBUG(get_logger(), " getParameters");
+
+  // Get all of the required parameters
+  get_parameter("always_send_full_costmap", always_send_full_costmap_);
+  get_parameter("footprint", footprint_);
+  get_parameter("footprint_padding", footprint_padding_);
+  get_parameter("global_frame", global_frame_);
+  get_parameter("height", map_height_meters_);
+  get_parameter("origin_x", origin_x_);
+  get_parameter("origin_y", origin_y_);
+  get_parameter("publish_frequency", map_publish_frequency_);
+  get_parameter("resolution", resolution_);
+  get_parameter("robot_base_frame", robot_base_frame_);
+  get_parameter("robot_radius", robot_radius_);
+  get_parameter("rolling_window", rolling_window_);
+  get_parameter("track_unknown_space", track_unknown_space_);
+  get_parameter("transform_tolerance", transform_tolerance_);
+  get_parameter("update_frequency", map_update_frequency_);
+  get_parameter("width", map_width_meters_);
+  get_parameter("plugins", plugin_names_);
+  get_parameter("filters", filter_names_);
+
+  auto node = shared_from_this();
+
+  if (plugin_names_ == default_plugins_) {
+    for (size_t i = 0; i < default_plugins_.size(); ++i) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, default_plugins_[i] + ".plugin", rclcpp::ParameterValue(default_types_[i]));
+    }
+  }
+  plugin_types_.resize(plugin_names_.size());
+  filter_types_.resize(filter_names_.size());
+
+  // 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type
+  for (size_t i = 0; i < plugin_names_.size(); ++i) {
+    plugin_types_[i] = nav2_util::get_plugin_type_param(node, plugin_names_[i]);
+  }
+  for (size_t i = 0; i < filter_names_.size(); ++i) {
+    filter_types_[i] = nav2_util::get_plugin_type_param(node, filter_names_[i]);
+  }
+
+  // 2. The map publish frequency cannot be 0 (to avoid a divde-by-zero)
+  if (map_publish_frequency_ > 0) {
+    publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_);
+  } else {
+    publish_cycle_ = rclcpp::Duration(-1s);
+  }
+
+  // 3. If the footprint has been specified, it must be in the correct format
+  use_radius_ = true;
+
+  if (footprint_ != "" && footprint_ != "[]") {
+    // Footprint parameter has been specified, try to convert it
+    std::vector<geometry_msgs::msg::Point> new_footprint;
+    if (makeFootprintFromString(footprint_, new_footprint)) {
+      // The specified footprint is valid, so we'll use that instead of the radius
+      use_radius_ = false;
+    } else {
+      // Footprint provided but invalid, so stay with the radius
+      RCLCPP_ERROR(
+        get_logger(), "The footprint parameter is invalid: \"%s\", using radius (%lf) instead",
+        footprint_.c_str(), robot_radius_);
+    }
+  }
+}
+
+void
+Costmap2DROS::setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & points)
+{
+  unpadded_footprint_ = points;
+  padded_footprint_ = points;
+  padFootprint(padded_footprint_, footprint_padding_);
+  layered_costmap_->setFootprint(padded_footprint_);
+}
+
+void
+Costmap2DROS::setRobotFootprintPolygon(
+  const geometry_msgs::msg::Polygon::SharedPtr footprint)
+{
+  setRobotFootprint(toPointVector(footprint));
+}
+
+void
+Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::msg::Point> & oriented_footprint)
+{
+  geometry_msgs::msg::PoseStamped global_pose;
+  if (!getRobotPose(global_pose)) {
+    return;
+  }
+
+  double yaw = tf2::getYaw(global_pose.pose.orientation);
+  transformFootprint(
+    global_pose.pose.position.x, global_pose.pose.position.y, yaw,
+    padded_footprint_, oriented_footprint);
+}
+
+void
+Costmap2DROS::mapUpdateLoop(double frequency)
+{
+  RCLCPP_DEBUG(get_logger(), "mapUpdateLoop frequency: %lf", frequency);
+
+  // the user might not want to run the loop every cycle
+  if (frequency == 0.0) {
+    return;
+  }
+
+  RCLCPP_DEBUG(get_logger(), "Entering loop");
+
+  rclcpp::WallRate r(frequency);    // 200ms by default
+
+  while (rclcpp::ok() && !map_update_thread_shutdown_) {
+    nav2_util::ExecutionTimer timer;
+
+    // Execute after start() will complete plugins activation
+    if (!stopped_) {
+      // Lock while modifying layered costmap and publishing values
+      std::scoped_lock<std::mutex> lock(_dynamic_parameter_mutex);
+
+      // Measure the execution time of the updateMap method
+      timer.start();
+      updateMap();
+      timer.end();
+
+      RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds());
+      if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) {
+        unsigned int x0, y0, xn, yn;
+        layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
+        costmap_publisher_->updateBounds(x0, xn, y0, yn);
+
+        auto current_time = now();
+        if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
+          (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
+        {
+          RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
+          costmap_publisher_->publishCostmap();
+          last_publish_ = current_time;
+        }
+      }
+    }
+
+    // Make sure to sleep for the remainder of our cycle time
+    r.sleep();
+
+#if 0
+    // TODO(bpwilcox): find ROS2 equivalent or port for r.cycletime()
+    if (r.period() > tf2::durationFromSec(1 / frequency)) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Costmap2DROS: Map update loop missed its desired rate of %.4fHz... "
+        "the loop actually took %.4f seconds", frequency, r.period());
+    }
+#endif
+  }
+}
+
+void
+Costmap2DROS::updateMap()
+{
+  RCLCPP_DEBUG(get_logger(), "Updating map...");
+
+  if (!stop_updates_) {
+    // get global pose
+    geometry_msgs::msg::PoseStamped pose;
+    if (getRobotPose(pose)) {
+      const double & x = pose.pose.position.x;
+      const double & y = pose.pose.position.y;
+      const double yaw = tf2::getYaw(pose.pose.orientation);
+      layered_costmap_->updateMap(x, y, yaw);
+
+      auto footprint = std::make_unique<geometry_msgs::msg::PolygonStamped>();
+      footprint->header = pose.header;
+      transformFootprint(x, y, yaw, padded_footprint_, *footprint);
+
+      RCLCPP_DEBUG(get_logger(), "Publishing footprint");
+      footprint_pub_->publish(std::move(footprint));
+      initialized_ = true;
+    }
+  }
+}
+
+void
+Costmap2DROS::start()
+{
+  RCLCPP_INFO(get_logger(), "start");
+  std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
+  std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
+
+  // check if we're stopped or just paused
+  if (stopped_) {
+    // if we're stopped we need to re-subscribe to topics
+    for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
+      plugin != plugins->end();
+      ++plugin)
+    {
+      (*plugin)->activate();
+    }
+    for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
+      filter != filters->end();
+      ++filter)
+    {
+      (*filter)->activate();
+    }
+    stopped_ = false;
+  }
+  stop_updates_ = false;
+
+  // block until the costmap is re-initialized.. meaning one update cycle has run
+  rclcpp::Rate r(20.0);
+  while (rclcpp::ok() && !initialized_) {
+    RCLCPP_DEBUG(get_logger(), "Sleeping, waiting for initialized_");
+    r.sleep();
+  }
+}
+
+void
+Costmap2DROS::stop()
+{
+  stop_updates_ = true;
+  // layered_costmap_ is set only if on_configure has been called
+  if (layered_costmap_) {
+    std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
+    std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
+
+    // unsubscribe from topics
+    for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
+      plugin != plugins->end(); ++plugin)
+    {
+      (*plugin)->deactivate();
+    }
+    for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
+      filter != filters->end(); ++filter)
+    {
+      (*filter)->deactivate();
+    }
+  }
+  initialized_ = false;
+  stopped_ = true;
+}
+
+void
+Costmap2DROS::pause()
+{
+  stop_updates_ = true;
+  initialized_ = false;
+}
+
+void
+Costmap2DROS::resume()
+{
+  stop_updates_ = false;
+
+  // block until the costmap is re-initialized.. meaning one update cycle has run
+  rclcpp::Rate r(100.0);
+  while (!initialized_) {
+    r.sleep();
+  }
+}
+
+void
+Costmap2DROS::resetLayers()
+{
+  Costmap2D * top = layered_costmap_->getCostmap();
+  top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
+
+  // Reset each of the plugins
+  std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
+  std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
+  for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
+    plugin != plugins->end(); ++plugin)
+  {
+    (*plugin)->reset();
+  }
+  for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
+    filter != filters->end(); ++filter)
+  {
+    (*filter)->reset();
+  }
+}
+
+bool
+Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
+{
+  return nav2_util::getCurrentPose(
+    global_pose, *tf_buffer_,
+    global_frame_, robot_base_frame_, transform_tolerance_);
+}
+
+bool
+Costmap2DROS::transformPoseToGlobalFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose)
+{
+  if (input_pose.header.frame_id == global_frame_) {
+    transformed_pose = input_pose;
+    return true;
+  } else {
+    return nav2_util::transformPoseInTargetFrame(
+      input_pose, transformed_pose, *tf_buffer_,
+      global_frame_, transform_tolerance_);
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult
+Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  auto result = rcl_interfaces::msg::SetParametersResult();
+  bool resize_map = false;
+  std::lock_guard<std::mutex> lock_reinit(_dynamic_parameter_mutex);
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == "robot_radius") {
+        robot_radius_ = parameter.as_double();
+        // Set the footprint
+        if (use_radius_) {
+          setRobotFootprint(makeFootprintFromRadius(robot_radius_));
+        }
+      } else if (name == "footprint_padding") {
+        footprint_padding_ = parameter.as_double();
+        padded_footprint_ = unpadded_footprint_;
+        padFootprint(padded_footprint_, footprint_padding_);
+        layered_costmap_->setFootprint(padded_footprint_);
+      } else if (name == "transform_tolerance") {
+        transform_tolerance_ = parameter.as_double();
+      } else if (name == "publish_frequency") {
+        map_publish_frequency_ = parameter.as_double();
+        if (map_publish_frequency_ > 0) {
+          publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_);
+        } else {
+          publish_cycle_ = rclcpp::Duration(-1s);
+        }
+      } else if (name == "resolution") {
+        resize_map = true;
+        resolution_ = parameter.as_double();
+      } else if (name == "origin_x") {
+        resize_map = true;
+        origin_x_ = parameter.as_double();
+      } else if (name == "origin_y") {
+        resize_map = true;
+        origin_y_ = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == "width") {
+        if (parameter.as_int() > 0) {
+          resize_map = true;
+          map_width_meters_ = parameter.as_int();
+        } else {
+          RCLCPP_ERROR(
+            get_logger(), "You try to set width of map to be negative or zero,"
+            " this isn't allowed, please give a positive value.");
+          result.successful = false;
+          return result;
+        }
+      } else if (name == "height") {
+        if (parameter.as_int() > 0) {
+          resize_map = true;
+          map_height_meters_ = parameter.as_int();
+        } else {
+          RCLCPP_ERROR(
+            get_logger(), "You try to set height of map to be negative or zero,"
+            " this isn't allowed, please give a positive value.");
+          result.successful = false;
+          return result;
+        }
+      }
+    } else if (type == ParameterType::PARAMETER_STRING) {
+      if (name == "footprint") {
+        footprint_ = parameter.as_string();
+        std::vector<geometry_msgs::msg::Point> new_footprint;
+        if (makeFootprintFromString(footprint_, new_footprint)) {
+          setRobotFootprint(new_footprint);
+        }
+      } else if (name == "robot_base_frame") {
+        // First, make sure that the transform between the robot base frame
+        // and the global frame is available
+        std::string tf_error;
+        RCLCPP_INFO(get_logger(), "Checking transform");
+        if (!tf_buffer_->canTransform(
+            global_frame_, parameter.as_string(), tf2::TimePointZero,
+            tf2::durationFromSec(1.0), &tf_error))
+        {
+          RCLCPP_WARN(
+            get_logger(), "Timed out waiting for transform from %s to %s"
+            " to become available, tf error: %s",
+            parameter.as_string().c_str(), global_frame_.c_str(), tf_error.c_str());
+          RCLCPP_WARN(
+            get_logger(), "Rejecting robot_base_frame change to %s , leaving it to its original"
+            " value of %s", parameter.as_string().c_str(), robot_base_frame_.c_str());
+          result.successful = false;
+          return result;
+        }
+        robot_base_frame_ = parameter.as_string();
+      }
+    }
+  }
+
+  if (resize_map && !layered_costmap_->isSizeLocked()) {
+    layered_costmap_->resizeMap(
+      (unsigned int)(map_width_meters_ / resolution_),
+      (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_);
+    updateMap();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_layer.cpp

@@ -0,0 +1,218 @@
+/*********************************************************************
+ *
+ * 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/costmap_layer.hpp>
+#include <stdexcept>
+#include <algorithm>
+
+namespace nav2_costmap_2d
+{
+
+void CostmapLayer::touch(
+  double x, double y, double * min_x, double * min_y, double * max_x,
+  double * max_y)
+{
+  *min_x = std::min(x, *min_x);
+  *min_y = std::min(y, *min_y);
+  *max_x = std::max(x, *max_x);
+  *max_y = std::max(y, *max_y);
+}
+
+void CostmapLayer::matchSize()
+{
+  Costmap2D * master = layered_costmap_->getCostmap();
+  resizeMap(
+    master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+    master->getOriginX(), master->getOriginY());
+}
+
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert)
+{
+  current_ = false;
+  unsigned char * grid = getCharMap();
+  for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
+    bool xrange = x > start_x && x < end_x;
+
+    for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
+      if ((xrange && y > start_y && y < end_y) == invert) {
+        continue;
+      }
+      int index = getIndex(x, y);
+      if (grid[index] != NO_INFORMATION) {
+        grid[index] = NO_INFORMATION;
+      }
+    }
+  }
+}
+
+void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
+{
+  extra_min_x_ = std::min(mx0, extra_min_x_);
+  extra_max_x_ = std::max(mx1, extra_max_x_);
+  extra_min_y_ = std::min(my0, extra_min_y_);
+  extra_max_y_ = std::max(my1, extra_max_y_);
+  has_extra_bounds_ = true;
+}
+
+void CostmapLayer::useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  if (!has_extra_bounds_) {
+    return;
+  }
+
+  *min_x = std::min(extra_min_x_, *min_x);
+  *min_y = std::min(extra_min_y_, *min_y);
+  *max_x = std::max(extra_max_x_, *max_x);
+  *max_y = std::max(extra_max_y_, *max_y);
+  extra_min_x_ = 1e6;
+  extra_min_y_ = 1e6;
+  extra_max_x_ = -1e6;
+  extra_max_y_ = -1e6;
+  has_extra_bounds_ = false;
+}
+
+void CostmapLayer::updateWithMax(
+  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();
+
+  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++) {
+      if (costmap_[it] == NO_INFORMATION) {
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION || old_cost < costmap_[it]) {
+        master_array[it] = costmap_[it];
+      }
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithTrueOverwrite(
+  nav2_costmap_2d::Costmap2D & master_grid, int min_i,
+  int min_j,
+  int max_i,
+  int max_j)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  if (costmap_ == nullptr) {
+    throw std::runtime_error("Can't update costmap layer: It has't been initialized yet!");
+  }
+
+  unsigned char * master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++) {
+    unsigned int it = span * j + min_i;
+    for (int i = min_i; i < max_i; i++) {
+      master[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithOverwrite(
+  nav2_costmap_2d::Costmap2D & master_grid,
+  int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_) {
+    return;
+  }
+  unsigned char * master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++) {
+    unsigned int it = span * j + min_i;
+    for (int i = min_i; i < max_i; i++) {
+      if (costmap_[it] != NO_INFORMATION) {
+        master[it] = costmap_[it];
+      }
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithAddition(
+  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();
+
+  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++) {
+      if (costmap_[it] == NO_INFORMATION) {
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION) {
+        master_array[it] = costmap_[it];
+      } else {
+        int sum = old_cost + costmap_[it];
+        if (sum >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+          master_array[it] = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
+        } else {
+          master_array[it] = sum;
+        }
+      }
+      it++;
+    }
+  }
+}
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_math.cpp

@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 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 the 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.
+ */
+
+#include <nav2_costmap_2d/costmap_math.hpp>
+
+#include <vector>
+
+double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
+{
+  double A = pX - x0;
+  double B = pY - y0;
+  double C = x1 - x0;
+  double D = y1 - y0;
+
+  double dot = A * C + B * D;
+  double len_sq = C * C + D * D;
+  double param = dot / len_sq;
+
+  double xx, yy;
+
+  if (param < 0) {
+    xx = x0;
+    yy = y0;
+  } else if (param > 1) {
+    xx = x1;
+    yy = y1;
+  } else {
+    xx = x0 + param * C;
+    yy = y0 + param * D;
+  }
+
+  return distance(pX, pY, xx, yy);
+}

navigation2/nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -0,0 +1,97 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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 <string>
+#include <memory>
+
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+
+namespace nav2_costmap_2d
+{
+
+CostmapSubscriber::CostmapSubscriber(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  const std::string & topic_name)
+: topic_name_(topic_name)
+{
+  auto node = parent.lock();
+  costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
+    topic_name_,
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+}
+
+CostmapSubscriber::CostmapSubscriber(
+  const rclcpp::Node::WeakPtr & parent,
+  const std::string & topic_name)
+: topic_name_(topic_name)
+{
+  auto node = parent.lock();
+  costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
+    topic_name_,
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
+}
+
+std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
+{
+  if (!costmap_received_) {
+    throw std::runtime_error("Costmap is not available");
+  }
+  toCostmap2D();
+  return costmap_;
+}
+
+void CostmapSubscriber::toCostmap2D()
+{
+  auto current_costmap_msg = std::atomic_load(&costmap_msg_);
+
+  if (costmap_ == nullptr) {
+    costmap_ = std::make_shared<Costmap2D>(
+      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
+      current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x,
+      current_costmap_msg->metadata.origin.position.y);
+  } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x ||  // NOLINT
+    costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y ||
+    costmap_->getResolution() != current_costmap_msg->metadata.resolution ||
+    costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x ||
+    costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y)
+  {
+    // Update the size of the costmap
+    costmap_->resizeMap(
+      current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
+      current_costmap_msg->metadata.resolution,
+      current_costmap_msg->metadata.origin.position.x,
+      current_costmap_msg->metadata.origin.position.y);
+  }
+
+  unsigned char * master_array = costmap_->getCharMap();
+  unsigned int index = 0;
+  for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) {
+    for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) {
+      master_array[index] = current_costmap_msg->data[index];
+      ++index;
+    }
+  }
+}
+
+void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+{
+  std::atomic_store(&costmap_msg_, msg);
+  if (!costmap_received_) {
+    costmap_received_ = true;
+  }
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -0,0 +1,103 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+//
+// Modified by: Shivang Patel (shivaan14@gmail.com)
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <iostream>
+
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
+
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/exceptions.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
+  CostmapSubscriber & costmap_sub,
+  FootprintSubscriber & footprint_sub,
+  std::string name)
+: name_(name),
+  costmap_sub_(costmap_sub),
+  footprint_sub_(footprint_sub),
+  collision_checker_(nullptr)
+{}
+
+bool CostmapTopicCollisionChecker::isCollisionFree(
+  const geometry_msgs::msg::Pose2D & pose,
+  bool fetch_costmap_and_footprint)
+{
+  try {
+    if (scorePose(pose, fetch_costmap_and_footprint) >= LETHAL_OBSTACLE) {
+      return false;
+    }
+    return true;
+  } catch (const IllegalPoseException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what());
+    return false;
+  } catch (const CollisionCheckerException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what());
+    return false;
+  } catch (...) {
+    RCLCPP_ERROR(rclcpp::get_logger(name_), "Failed to check pose score!");
+    return false;
+  }
+}
+
+double CostmapTopicCollisionChecker::scorePose(
+  const geometry_msgs::msg::Pose2D & pose,
+  bool fetch_costmap_and_footprint)
+{
+  if (fetch_costmap_and_footprint) {
+    try {
+      collision_checker_.setCostmap(costmap_sub_.getCostmap());
+    } catch (const std::runtime_error & e) {
+      throw CollisionCheckerException(e.what());
+    }
+  }
+
+  unsigned int cell_x, cell_y;
+  if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y);
+    throw IllegalPoseException(name_, "Pose Goes Off Grid.");
+  }
+
+  return collision_checker_.footprintCost(getFootprint(pose, fetch_costmap_and_footprint));
+}
+
+Footprint CostmapTopicCollisionChecker::getFootprint(
+  const geometry_msgs::msg::Pose2D & pose,
+  bool fetch_latest_footprint)
+{
+  if (fetch_latest_footprint) {
+    std_msgs::msg::Header header;
+    if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
+      throw CollisionCheckerException("Current footprint not available.");
+    }
+  }
+  Footprint footprint;
+  transformFootprint(pose.x, pose.y, pose.theta, footprint_, footprint);
+
+  return footprint;
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint.cpp

@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) 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 the 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.
+ */
+#include "nav2_costmap_2d/footprint.hpp"
+
+#include <algorithm>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/point32.hpp"
+#include "nav2_costmap_2d/array_parser.hpp"
+#include "nav2_costmap_2d/costmap_math.hpp"
+
+namespace nav2_costmap_2d
+{
+
+void calculateMinAndMaxDistances(
+  const std::vector<geometry_msgs::msg::Point> & footprint,
+  double & min_dist, double & max_dist)
+{
+  min_dist = std::numeric_limits<double>::max();
+  max_dist = 0.0;
+
+  if (footprint.size() <= 2) {
+    return;
+  }
+
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
+    // check the distance from the robot center point to the first vertex
+    double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
+    double edge_dist = distanceToLine(
+      0.0, 0.0, footprint[i].x, footprint[i].y,
+      footprint[i + 1].x, footprint[i + 1].y);
+    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+  }
+
+  // we also need to do the last vertex and the first vertex
+  double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
+  double edge_dist = distanceToLine(
+    0.0, 0.0, footprint.back().x, footprint.back().y,
+    footprint.front().x, footprint.front().y);
+  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+}
+
+geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt)
+{
+  geometry_msgs::msg::Point32 point32;
+  point32.x = pt.x;
+  point32.y = pt.y;
+  point32.z = pt.z;
+  return point32;
+}
+
+geometry_msgs::msg::Point toPoint(geometry_msgs::msg::Point32 pt)
+{
+  geometry_msgs::msg::Point point;
+  point.x = pt.x;
+  point.y = pt.y;
+  point.z = pt.z;
+  return point;
+}
+
+geometry_msgs::msg::Polygon toPolygon(std::vector<geometry_msgs::msg::Point> pts)
+{
+  geometry_msgs::msg::Polygon polygon;
+  for (unsigned int i = 0; i < pts.size(); i++) {
+    polygon.points.push_back(toPoint32(pts[i]));
+  }
+  return polygon;
+}
+
+std::vector<geometry_msgs::msg::Point> toPointVector(geometry_msgs::msg::Polygon::SharedPtr polygon)
+{
+  std::vector<geometry_msgs::msg::Point> pts;
+  for (unsigned int i = 0; i < polygon->points.size(); i++) {
+    pts.push_back(toPoint(polygon->points[i]));
+  }
+  return pts;
+}
+
+void transformFootprint(
+  double x, double y, double theta,
+  const std::vector<geometry_msgs::msg::Point> & footprint_spec,
+  std::vector<geometry_msgs::msg::Point> & oriented_footprint)
+{
+  // build the oriented footprint at a given location
+  oriented_footprint.resize(footprint_spec.size());
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
+    double new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    double new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
+    new_pt.x = new_x;
+    new_pt.y = new_y;
+  }
+}
+
+void transformFootprint(
+  double x, double y, double theta,
+  const std::vector<geometry_msgs::msg::Point> & footprint_spec,
+  geometry_msgs::msg::PolygonStamped & oriented_footprint)
+{
+  // build the oriented footprint at a given location
+  oriented_footprint.polygon.points.clear();
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
+    geometry_msgs::msg::Point32 new_pt;
+    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    oriented_footprint.polygon.points.push_back(new_pt);
+  }
+}
+
+void padFootprint(std::vector<geometry_msgs::msg::Point> & footprint, double padding)
+{
+  // pad footprint in place
+  for (unsigned int i = 0; i < footprint.size(); i++) {
+    geometry_msgs::msg::Point & pt = footprint[i];
+    pt.x += sign0(pt.x) * padding;
+    pt.y += sign0(pt.y) * padding;
+  }
+}
+
+
+std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(double radius)
+{
+  std::vector<geometry_msgs::msg::Point> points;
+
+  // Loop over 16 angles around a circle making a point each time
+  int N = 16;
+  geometry_msgs::msg::Point pt;
+  for (int i = 0; i < N; ++i) {
+    double angle = i * 2 * M_PI / N;
+    pt.x = cos(angle) * radius;
+    pt.y = sin(angle) * radius;
+
+    points.push_back(pt);
+  }
+
+  return points;
+}
+
+
+bool makeFootprintFromString(
+  const std::string & footprint_string,
+  std::vector<geometry_msgs::msg::Point> & footprint)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf = parseVVF(footprint_string, error);
+
+  if (error != "") {
+    RCLCPP_ERROR(
+      rclcpp::get_logger(
+        "nav2_costmap_2d"), "Error parsing footprint parameter: '%s'", error.c_str());
+    RCLCPP_ERROR(
+      rclcpp::get_logger(
+        "nav2_costmap_2d"), "  Footprint string was '%s'.", footprint_string.c_str());
+    return false;
+  }
+
+  // convert vvf into points.
+  if (vvf.size() < 3) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger(
+        "nav2_costmap_2d"),
+      "You must specify at least three points for the robot footprint, reverting to previous footprint."); //NOLINT
+    return false;
+  }
+  footprint.reserve(vvf.size());
+  for (unsigned int i = 0; i < vvf.size(); i++) {
+    if (vvf[i].size() == 2) {
+      geometry_msgs::msg::Point point;
+      point.x = vvf[i][0];
+      point.y = vvf[i][1];
+      point.z = 0;
+      footprint.push_back(point);
+    } else {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(
+          "nav2_costmap_2d"),
+        "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT
+        static_cast<int>(vvf[i].size()));
+      return false;
+    }
+  }
+
+  return true;
+}
+
+}  // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -0,0 +1,149 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <algorithm>
+
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/exceptions.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker()
+: costmap_(nullptr)
+{
+}
+
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
+  CostmapT costmap)
+: costmap_(costmap)
+{
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footprint)
+{
+  // now we really have to lay down the footprint in the costmap_ grid
+  unsigned int x0, x1, y0, y1;
+  double footprint_cost = 0.0;
+
+  // get the cell coord of the first point
+  if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  // cache the start to eliminate a worldToMap call
+  unsigned int xstart = x0;
+  unsigned int ystart = y0;
+
+  // we need to rasterize each line in the footprint
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
+    // get the cell coord of the second point
+    if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
+      return static_cast<double>(LETHAL_OBSTACLE);
+    }
+
+    footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
+
+    // the second point is next iteration's first point
+    x0 = x1;
+    y0 = y1;
+
+    // if in collision, no need to continue
+    if (footprint_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+      return footprint_cost;
+    }
+  }
+
+  // we also need to connect the first point in the footprint to the last point
+  // the last iteration's x1, y1 are the last footprint point's coordinates
+  return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int y1) const
+{
+  double line_cost = 0.0;
+  double point_cost = -1.0;
+
+  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
+    point_cost = pointCost(line.getX(), line.getY());   // Score the current point
+
+    // if in collision, no need to continue
+    if (point_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+      return point_cost;
+    }
+
+    if (line_cost < point_cost) {
+      line_cost = point_cost;
+    }
+  }
+
+  return line_cost;
+}
+
+template<typename CostmapT>
+bool FootprintCollisionChecker<CostmapT>::worldToMap(
+  double wx, double wy, unsigned int & mx, unsigned int & my)
+{
+  return costmap_->worldToMap(wx, wy, mx, my);
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::pointCost(int x, int y) const
+{
+  return costmap_->getCost(x, y);
+}
+
+template<typename CostmapT>
+void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
+{
+  costmap_ = costmap;
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
+  double x, double y, double theta, const Footprint footprint)
+{
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  Footprint oriented_footprint;
+  for (unsigned int i = 0; i < footprint.size(); ++i) {
+    geometry_msgs::msg::Point new_pt;
+    new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th);
+    new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th);
+    oriented_footprint.push_back(new_pt);
+  }
+
+  return footprintCost(oriented_footprint);
+}
+
+// declare our valid template parameters
+template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
+template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -0,0 +1,117 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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 <string>
+#include <vector>
+#include <memory>
+
+#include "nav2_costmap_2d/footprint_subscriber.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+
+namespace nav2_costmap_2d
+{
+
+FootprintSubscriber::FootprintSubscriber(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  const std::string & topic_name,
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
+: tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
+{
+  auto node = parent.lock();
+  footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
+    topic_name, rclcpp::SystemDefaultsQoS(),
+    std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
+}
+
+FootprintSubscriber::FootprintSubscriber(
+  const rclcpp::Node::WeakPtr & parent,
+  const std::string & topic_name,
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
+: tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
+{
+  auto node = parent.lock();
+  footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
+    topic_name, rclcpp::SystemDefaultsQoS(),
+    std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
+}
+
+bool
+FootprintSubscriber::getFootprintRaw(
+  std::vector<geometry_msgs::msg::Point> & footprint,
+  std_msgs::msg::Header & footprint_header)
+{
+  if (!footprint_received_) {
+    return false;
+  }
+
+  auto current_footprint = std::atomic_load(&footprint_);
+  footprint = toPointVector(
+    std::make_shared<geometry_msgs::msg::Polygon>(current_footprint->polygon));
+  footprint_header = current_footprint->header;
+
+  return true;
+}
+
+bool
+FootprintSubscriber::getFootprintInRobotFrame(
+  std::vector<geometry_msgs::msg::Point> & footprint,
+  std_msgs::msg::Header & footprint_header)
+{
+  if (!getFootprintRaw(footprint, footprint_header)) {
+    return false;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, tf_, footprint_header.frame_id, robot_base_frame_,
+      transform_tolerance_, footprint_header.stamp))
+  {
+    return false;
+  }
+
+  double x = current_pose.pose.position.x;
+  double y = current_pose.pose.position.y;
+  double theta = tf2::getYaw(current_pose.pose.orientation);
+
+  std::vector<geometry_msgs::msg::Point> temp;
+  transformFootprint(-x, -y, 0, footprint, temp);
+  transformFootprint(0, 0, -theta, temp, footprint);
+
+  footprint_header.frame_id = robot_base_frame_;
+  footprint_header.stamp = current_pose.header.stamp;
+
+  return true;
+}
+
+void
+FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg)
+{
+  std::atomic_store(&footprint_, msg);
+  if (!footprint_received_) {
+    footprint_received_ = true;
+  }
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/layer.cpp

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 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 the 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.
+ */
+
+#include "nav2_costmap_2d/layer.hpp"
+
+#include <string>
+#include <vector>
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_costmap_2d
+{
+
+Layer::Layer()
+: layered_costmap_(nullptr),
+  name_(),
+  tf_(nullptr),
+  current_(false),
+  enabled_(false)
+{}
+
+void
+Layer::initialize(
+  LayeredCostmap * parent,
+  std::string name,
+  tf2_ros::Buffer * tf,
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  rclcpp::CallbackGroup::SharedPtr callback_group)
+{
+  layered_costmap_ = parent;
+  name_ = name;
+  tf_ = tf;
+  node_ = node;
+  callback_group_ = callback_group;
+  {
+    auto node_shared_ptr = node_.lock();
+    logger_ = node_shared_ptr->get_logger();
+    clock_ = node_shared_ptr->get_clock();
+  }
+
+  onInitialize();
+}
+
+const std::vector<geometry_msgs::msg::Point> &
+Layer::getFootprint() const
+{
+  return layered_costmap_->getFootprint();
+}
+
+void
+Layer::declareParameter(
+  const std::string & param_name,
+  const rclcpp::ParameterValue & value)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  local_params_.insert(param_name);
+  nav2_util::declare_parameter_if_not_declared(
+    node, getFullName(param_name), value);
+}
+
+void
+Layer::declareParameter(
+  const std::string & param_name,
+  const rclcpp::ParameterType & param_type)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  local_params_.insert(param_name);
+  nav2_util::declare_parameter_if_not_declared(
+    node, getFullName(param_name), param_type);
+}
+
+bool
+Layer::hasParameter(const std::string & param_name)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  return node->has_parameter(getFullName(param_name));
+}
+
+std::string
+Layer::getFullName(const std::string & param_name)
+{
+  return std::string(name_ + "." + param_name);
+}
+
+}  // end namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/layered_costmap.cpp

@@ -0,0 +1,296 @@
+/*********************************************************************
+ *
+ * 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/layered_costmap.hpp"
+
+#include <algorithm>
+#include <cstdio>
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "nav2_costmap_2d/footprint.hpp"
+
+
+using std::vector;
+
+namespace nav2_costmap_2d
+{
+
+LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
+: primary_costmap_(), combined_costmap_(),
+  global_frame_(global_frame),
+  rolling_window_(rolling_window),
+  current_(false),
+  minx_(0.0),
+  miny_(0.0),
+  maxx_(0.0),
+  maxy_(0.0),
+  bx0_(0),
+  bxn_(0),
+  by0_(0),
+  byn_(0),
+  initialized_(false),
+  size_locked_(false),
+  circumscribed_radius_(1.0),
+  inscribed_radius_(0.1)
+{
+  if (track_unknown) {
+    primary_costmap_.setDefaultValue(255);
+    combined_costmap_.setDefaultValue(255);
+  } else {
+    primary_costmap_.setDefaultValue(0);
+    combined_costmap_.setDefaultValue(0);
+  }
+}
+
+LayeredCostmap::~LayeredCostmap()
+{
+  while (plugins_.size() > 0) {
+    plugins_.pop_back();
+  }
+  while (filters_.size() > 0) {
+    filters_.pop_back();
+  }
+}
+
+void LayeredCostmap::addPlugin(std::shared_ptr<Layer> plugin)
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
+  plugins_.push_back(plugin);
+}
+
+void LayeredCostmap::addFilter(std::shared_ptr<Layer> filter)
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
+  filters_.push_back(filter);
+}
+
+void LayeredCostmap::resizeMap(
+  unsigned int size_x, unsigned int size_y, double resolution,
+  double origin_x,
+  double origin_y,
+  bool size_locked)
+{
+  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
+  size_locked_ = size_locked;
+  primary_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
+  combined_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+    plugin != plugins_.end(); ++plugin)
+  {
+    (*plugin)->matchSize();
+  }
+  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+    filter != filters_.end(); ++filter)
+  {
+    (*filter)->matchSize();
+  }
+}
+
+bool LayeredCostmap::isOutofBounds(double robot_x, double robot_y)
+{
+  unsigned int mx, my;
+  return !combined_costmap_.worldToMap(robot_x, robot_y, mx, my);
+}
+
+void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
+{
+  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
+  // implement thread unsafe updateBounds() functions.
+  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
+
+  // if we're using a rolling buffer costmap...
+  // we need to update the origin using the robot's position
+  if (rolling_window_) {
+    double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX() / 2;
+    double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY() / 2;
+    primary_costmap_.updateOrigin(new_origin_x, new_origin_y);
+    combined_costmap_.updateOrigin(new_origin_x, new_origin_y);
+  }
+
+  if (isOutofBounds(robot_x, robot_y)) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("nav2_costmap_2d"),
+      "Robot is out of bounds of the costmap!");
+  }
+
+  if (plugins_.size() == 0 && filters_.size() == 0) {
+    return;
+  }
+
+  minx_ = miny_ = std::numeric_limits<double>::max();
+  maxx_ = maxy_ = std::numeric_limits<double>::lowest();
+
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+    plugin != plugins_.end(); ++plugin)
+  {
+    double prev_minx = minx_;
+    double prev_miny = miny_;
+    double prev_maxx = maxx_;
+    double prev_maxy = maxy_;
+    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
+      RCLCPP_WARN(
+        rclcpp::get_logger(
+          "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
+        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
+        prev_minx, prev_miny, prev_maxx, prev_maxy,
+        minx_, miny_, maxx_, maxy_,
+        (*plugin)->getName().c_str());
+    }
+  }
+  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+    filter != filters_.end(); ++filter)
+  {
+    double prev_minx = minx_;
+    double prev_miny = miny_;
+    double prev_maxx = maxx_;
+    double prev_maxy = maxy_;
+    (*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
+      RCLCPP_WARN(
+        rclcpp::get_logger(
+          "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
+        "is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s",
+        prev_minx, prev_miny, prev_maxx, prev_maxy,
+        minx_, miny_, maxx_, maxy_,
+        (*filter)->getName().c_str());
+    }
+  }
+
+  int x0, xn, y0, yn;
+  combined_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
+  combined_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
+
+  x0 = std::max(0, x0);
+  xn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsX()), xn + 1);
+  y0 = std::max(0, y0);
+  yn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsY()), yn + 1);
+
+  RCLCPP_DEBUG(
+    rclcpp::get_logger(
+      "nav2_costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
+
+  if (xn < x0 || yn < y0) {
+    return;
+  }
+
+  if (filters_.size() == 0) {
+    // If there are no filters enabled just update costmap sequentially by each plugin
+    combined_costmap_.resetMap(x0, y0, xn, yn);
+    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+      plugin != plugins_.end(); ++plugin)
+    {
+      (*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn);
+    }
+  } else {
+    // Costmap Filters enabled
+    // 1. Update costmap by plugins
+    primary_costmap_.resetMap(x0, y0, xn, yn);
+    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+      plugin != plugins_.end(); ++plugin)
+    {
+      (*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn);
+    }
+
+    // 2. Copy processed costmap window to a final costmap.
+    // primary_costmap_ remain to be untouched for further usage by plugins.
+    if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("nav2_costmap_2d"),
+        "Can not copy costmap (%i,%i)..(%i,%i) window",
+        x0, y0, xn, yn);
+      throw std::runtime_error{"Can not copy costmap"};
+    }
+
+    // 3. Apply filters over the plugins in order to make filters' work
+    // not being considered by plugins on next updateMap() calls
+    for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+      filter != filters_.end(); ++filter)
+    {
+      (*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn);
+    }
+  }
+
+  bx0_ = x0;
+  bxn_ = xn;
+  by0_ = y0;
+  byn_ = yn;
+
+  initialized_ = true;
+}
+
+bool LayeredCostmap::isCurrent()
+{
+  current_ = true;
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+    plugin != plugins_.end(); ++plugin)
+  {
+    current_ = current_ && ((*plugin)->isCurrent() || !(*plugin)->isEnabled());
+  }
+  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+    filter != filters_.end(); ++filter)
+  {
+    current_ = current_ && ((*filter)->isCurrent() || !(*filter)->isEnabled());
+  }
+  return current_;
+}
+
+void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec)
+{
+  footprint_ = footprint_spec;
+  nav2_costmap_2d::calculateMinAndMaxDistances(
+    footprint_spec,
+    inscribed_radius_, circumscribed_radius_);
+
+  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+    plugin != plugins_.end();
+    ++plugin)
+  {
+    (*plugin)->onFootprintChanged();
+  }
+  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+    filter != filters_.end();
+    ++filter)
+  {
+    (*filter)->onFootprintChanged();
+  }
+}
+
+}  // namespace nav2_costmap_2d

navigation2/nav2_costmap_2d/src/observation_buffer.cpp

@@ -0,0 +1,236 @@
+/*********************************************************************
+ *
+ * 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
+ *********************************************************************/
+#include "nav2_costmap_2d/observation_buffer.hpp"
+
+#include <algorithm>
+#include <list>
+#include <string>
+#include <vector>
+#include <chrono>
+
+#include "tf2/convert.h"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+ObservationBuffer::ObservationBuffer(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  std::string topic_name,
+  double observation_keep_time,
+  double expected_update_rate,
+  double min_obstacle_height, double max_obstacle_height, double obstacle_max_range,
+  double obstacle_min_range,
+  double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer,
+  std::string global_frame,
+  std::string sensor_frame,
+  tf2::Duration tf_tolerance)
+: tf2_buffer_(tf2_buffer),
+  observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)),
+  expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)),
+  global_frame_(global_frame),
+  sensor_frame_(sensor_frame),
+  topic_name_(topic_name),
+  min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
+  obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range),
+  raytrace_max_range_(raytrace_max_range), raytrace_min_range_(
+    raytrace_min_range), tf_tolerance_(tf_tolerance)
+{
+  auto node = parent.lock();
+  clock_ = node->get_clock();
+  logger_ = node->get_logger();
+  last_updated_ = node->now();
+}
+
+ObservationBuffer::~ObservationBuffer()
+{
+}
+
+void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
+{
+  geometry_msgs::msg::PointStamped global_origin;
+
+  // create a new observation on the list to be populated
+  observation_list_.push_front(Observation());
+
+  // check whether the origin frame has been set explicitly
+  // or whether we should get it from the cloud
+  std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
+
+  try {
+    // given these observations come from sensors...
+    // we'll need to store the origin pt of the sensor
+    geometry_msgs::msg::PointStamped local_origin;
+    local_origin.header.stamp = cloud.header.stamp;
+    local_origin.header.frame_id = origin_frame;
+    local_origin.point.x = 0;
+    local_origin.point.y = 0;
+    local_origin.point.z = 0;
+    tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_);
+    tf2::convert(global_origin.point, observation_list_.front().origin_);
+
+    // make sure to pass on the raytrace/obstacle range
+    // of the observation buffer to the observations
+    observation_list_.front().raytrace_max_range_ = raytrace_max_range_;
+    observation_list_.front().raytrace_min_range_ = raytrace_min_range_;
+    observation_list_.front().obstacle_max_range_ = obstacle_max_range_;
+    observation_list_.front().obstacle_min_range_ = obstacle_min_range_;
+
+    sensor_msgs::msg::PointCloud2 global_frame_cloud;
+
+    // transform the point cloud
+    tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_);
+    global_frame_cloud.header.stamp = cloud.header.stamp;
+
+    // now we need to remove observations from the cloud that are below
+    // or above our height thresholds
+    sensor_msgs::msg::PointCloud2 & observation_cloud = *(observation_list_.front().cloud_);
+    observation_cloud.height = global_frame_cloud.height;
+    observation_cloud.width = global_frame_cloud.width;
+    observation_cloud.fields = global_frame_cloud.fields;
+    observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
+    observation_cloud.point_step = global_frame_cloud.point_step;
+    observation_cloud.row_step = global_frame_cloud.row_step;
+    observation_cloud.is_dense = global_frame_cloud.is_dense;
+
+    unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width;
+    sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
+    modifier.resize(cloud_size);
+    unsigned int point_count = 0;
+
+    // copy over the points that are within our height bounds
+    sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
+    std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(),
+      iter_global_end = global_frame_cloud.data.end();
+    std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
+    for (; iter_global != iter_global_end; ++iter_z, iter_global +=
+      global_frame_cloud.point_step)
+    {
+      if ((*iter_z) <= max_obstacle_height_ &&
+        (*iter_z) >= min_obstacle_height_)
+      {
+        std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
+        iter_obs += global_frame_cloud.point_step;
+        ++point_count;
+      }
+    }
+
+    // resize the cloud for the number of legal points
+    modifier.resize(point_count);
+    observation_cloud.header.stamp = cloud.header.stamp;
+    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
+  } catch (tf2::TransformException & ex) {
+    // if an exception occurs, we need to remove the empty observation from the list
+    observation_list_.pop_front();
+    RCLCPP_ERROR(
+      logger_,
+      "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
+      sensor_frame_.c_str(),
+      cloud.header.frame_id.c_str(), ex.what());
+    return;
+  }
+
+  // if the update was successful, we want to update the last updated time
+  last_updated_ = clock_->now();
+
+  // we'll also remove any stale observations from the list
+  purgeStaleObservations();
+}
+
+// returns a copy of the observations
+void ObservationBuffer::getObservations(std::vector<Observation> & observations)
+{
+  // first... let's make sure that we don't have any stale observations
+  purgeStaleObservations();
+
+  // now we'll just copy the observations for the caller
+  std::list<Observation>::iterator obs_it;
+  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) {
+    observations.push_back(*obs_it);
+  }
+}
+
+void ObservationBuffer::purgeStaleObservations()
+{
+  if (!observation_list_.empty()) {
+    std::list<Observation>::iterator obs_it = observation_list_.begin();
+    // if we're keeping observations for no time... then we'll only keep one observation
+    if (observation_keep_time_ == rclcpp::Duration(0.0s)) {
+      observation_list_.erase(++obs_it, observation_list_.end());
+      return;
+    }
+
+    // otherwise... we'll have to loop through the observations to see which ones are stale
+    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) {
+      Observation & obs = *obs_it;
+      // check if the observation is out of date... and if it is,
+      // remove it and those that follow from the list
+      if ((clock_->now() - obs.cloud_->header.stamp) >
+        observation_keep_time_)
+      {
+        observation_list_.erase(obs_it, observation_list_.end());
+        return;
+      }
+    }
+  }
+}
+
+bool ObservationBuffer::isCurrent() const
+{
+  if (expected_update_rate_ == rclcpp::Duration(0.0s)) {
+    return true;
+  }
+
+  bool current = (clock_->now() - last_updated_) <=
+    expected_update_rate_;
+  if (!current) {
+    RCLCPP_WARN(
+      logger_,
+      "The %s observation buffer has not been updated for %.2f seconds, "
+      "and it should be updated every %.2f seconds.",
+      topic_name_.c_str(),
+      (clock_->now() - last_updated_).seconds(),
+      expected_update_rate_.seconds());
+  }
+  return current;
+}
+
+void ObservationBuffer::resetLastUpdated()
+{
+  last_updated_ = clock_->now();
+}
+}  // namespace nav2_costmap_2d