costmap_2d_ros.cpp

/*********************************************************************
 *
 * 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 <cmath>
#include <limits>
#include <stdexcept>
#include <string>
#include <vector>
#include <utility>
 
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/execution_timer.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_ros_common/rate.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#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 rclcpp::NodeOptions & options)
: nav2::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;
  init();
}
 
rclcpp::NodeOptions getChildNodeOptions(
  const std::string & name,
  const std::string & parent_namespace,
  const bool & use_sim_time,
  const rclcpp::NodeOptions & parent_options)
{
  std::vector<std::string> new_arguments = parent_options.arguments();
  bool use_intra_process_comms = parent_options.use_intra_process_comms();
  nav2::replaceOrAddArgument(
    new_arguments, "-r", "__ns",
    "__ns:=" + nav2::add_namespaces(parent_namespace, name));
  nav2::replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name);
  nav2::replaceOrAddArgument(
    new_arguments, "-p", "use_sim_time",
    "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"));
  return rclcpp::NodeOptions().use_intra_process_comms(use_intra_process_comms).arguments(
    new_arguments);
}
 
Costmap2DROS::Costmap2DROS(
  const std::string & name,
  const std::string & parent_namespace,
  const bool & use_sim_time,
  const rclcpp::NodeOptions & options)
: nav2::LifecycleNode(name, "",
    getChildNodeOptions(name, parent_namespace, use_sim_time, options)
),
  name_(name),
  default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
  default_types_{
    "nav2_costmap_2d::StaticLayer",
    "nav2_costmap_2d::ObstacleLayer",
    "nav2_costmap_2d::InflationLayer"}
{
  init();
}
 
void Costmap2DROS::init()
{
  RCLCPP_INFO(get_logger(), "Creating Costmap");
  declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
  declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
  declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
  declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99));
  declare_parameter("use_maximum", rclcpp::ParameterValue(false));
}
 
Costmap2DROS::~Costmap2DROS()
{
}
 
nav2::CallbackReturn
Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Configuring");
  try {
    getParameters();
  } catch (const std::exception & e) {
    RCLCPP_ERROR(
      get_logger(), "Failed to configure costmap! %s.", e.what());
    return nav2::CallbackReturn::FAILURE;
  }
 
  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_ = nav2::create_transform_buffer(this, callback_group_);
  tf_listener_ = nav2::create_transform_listener(*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]);
 
    layered_costmap_->addPlugin(plugin);
 
    try {
      plugin->initialize(
        layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(),
        shared_from_this(), callback_group_);
    } catch (const std::exception & e) {
      RCLCPP_ERROR(
        get_logger(), "Failed to initialize costmap plugin %s! %s.",
        plugin_names_[i].c_str(), e.what());
      return nav2::CallbackReturn::FAILURE;
    }
 
    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]);
 
    layered_costmap_->addFilter(filter);
 
    filter->initialize(
      layered_costmap_.get(), filter_names_[i], tf_buffer_.get(),
      shared_from_this(), callback_group_);
 
    RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str());
  }
 
  // Create the publishers and subscribers
  if (subscribe_to_stamped_footprint_) {
    footprint_stamped_sub_ = create_subscription<geometry_msgs::msg::PolygonStamped>(
      "footprint", [this](const geometry_msgs::msg::PolygonStamped::ConstSharedPtr & footprint)
      {setRobotFootprintPolygon(footprint->polygon);});
  } else {
    footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
      "footprint", [this](const geometry_msgs::msg::Polygon::ConstSharedPtr & footprint)
      {setRobotFootprintPolygon(*footprint);});
  }
 
  footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
    "published_footprint");
 
  costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
    shared_from_this(),
    layered_costmap_->getCostmap(), global_frame_,
    "costmap", always_send_full_costmap_, map_vis_z_);
 
  auto layers = layered_costmap_->getPlugins();
 
  for (auto & layer : *layers) {
    auto costmap_layer = std::dynamic_pointer_cast<CostmapLayer>(layer);
    if (costmap_layer != nullptr) {
      layer_publishers_.emplace_back(
        std::make_unique<Costmap2DPublisher>(
          shared_from_this(),
          costmap_layer.get(), global_frame_,
          layer->getName(), always_send_full_costmap_, map_vis_z_)
      );
    }
  }
 
  // 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);
  }
 
  // Service to get the cost at a point
  get_cost_service_ = create_service<nav2_msgs::srv::GetCosts>(
    std::string("get_cost_") + get_name(),
    std::bind(
      &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
      std::placeholders::_3));
 
  // 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::NodeThread>(executor_);
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Activating");
 
  // 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);
  const auto initial_transform_timeout = rclcpp::Duration::from_seconds(
    initial_transform_timeout_);
  const auto initial_transform_timeout_point = now() + initial_transform_timeout;
  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());
 
    // Check timeout
    if (now() > initial_transform_timeout_point) {
      RCLCPP_ERROR(
        get_logger(),
        "Failed to activate %s because "
        "transform from %s to %s did not become available before timeout",
        get_name(), robot_base_frame_.c_str(), global_frame_.c_str());
 
      return nav2::CallbackReturn::FAILURE;
    }
 
    // 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();
  }
 
  // Activate publishers
  footprint_pub_->on_activate();
  costmap_publisher_->on_activate();
 
  for (auto & layer_pub : layer_publishers_) {
    layer_pub->on_activate();
  }
 
  // 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
  post_set_params_handler_ = this->add_post_set_parameters_callback(
    std::bind(
      &Costmap2DROS::updateParametersCallback,
      this, std::placeholders::_1));
  on_set_params_handler = this->add_on_set_parameters_callback(
    std::bind(&Costmap2DROS::validateParameterUpdatesCallback, this, _1));
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Deactivating");
 
  remove_post_set_parameters_callback(post_set_params_handler_.get());
  post_set_params_handler_.reset();
  remove_on_set_parameters_callback(on_set_params_handler.get());
  on_set_params_handler.reset();
 
  stop();
 
  // Map thread stuff
  map_update_thread_shutdown_ = true;
 
  if (map_update_thread_->joinable()) {
    map_update_thread_->join();
  }
 
  footprint_pub_->on_deactivate();
  costmap_publisher_->on_deactivate();
 
  for (auto & layer_pub : layer_publishers_) {
    layer_pub->on_deactivate();
  }
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Cleaning up");
  executor_thread_.reset();
  get_cost_service_.reset();
  costmap_publisher_.reset();
  clear_costmap_service_.reset();
 
  layer_publishers_.clear();
 
  layered_costmap_.reset();
 
  tf_listener_.reset();
  tf_buffer_.reset();
 
  footprint_sub_.reset();
  footprint_pub_.reset();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &)
{
  RCLCPP_INFO(get_logger(), "Shutting down");
  return nav2::CallbackReturn::SUCCESS;
}
 
void
Costmap2DROS::getParameters()
{
  RCLCPP_DEBUG(get_logger(), " getParameters");
 
  // Get all of the required parameters
  always_send_full_costmap_ = declare_or_get_parameter(
    "always_send_full_costmap", false);
  map_vis_z_ = declare_or_get_parameter("map_vis_z", 0.0);
  footprint_padding_ = declare_or_get_parameter("footprint_padding", 0.01f);
  footprint_ = declare_or_get_parameter(
    "footprint", std::string("[]"));
  global_frame_ = declare_or_get_parameter(
    "global_frame", std::string("map"));
  map_height_meters_ = declare_or_get_parameter(
    "height", 5);
  map_width_meters_ = declare_or_get_parameter(
    "width", 5);
  origin_x_ = declare_or_get_parameter(
    "origin_x", 0.0);
  origin_y_ = declare_or_get_parameter(
    "origin_y", 0.0);
  plugin_names_ = declare_or_get_parameter(
    "plugins", default_plugins_);
  filter_names_ = declare_or_get_parameter(
    "filters", std::vector<std::string>());
  map_publish_frequency_ = declare_or_get_parameter(
    "publish_frequency", 1.0);
  resolution_ = declare_or_get_parameter(
    "resolution", 0.1);
  robot_base_frame_ = declare_or_get_parameter(
    "robot_base_frame", std::string("base_link"));
  robot_radius_ = declare_or_get_parameter(
    "robot_radius", 0.1);
  rolling_window_ = declare_or_get_parameter(
    "rolling_window", false);
  track_unknown_space_ = declare_or_get_parameter(
    "track_unknown_space", false);
  transform_tolerance_ = declare_or_get_parameter(
    "transform_tolerance", 0.3);
  transform_staleness_threshold_ = declare_or_get_parameter(
    "transform_staleness_threshold", 0.0);
  initial_transform_timeout_ = declare_or_get_parameter(
    "initial_transform_timeout", 60.0);
  map_update_frequency_ = declare_or_get_parameter(
    "update_frequency", 5.0);
  subscribe_to_stamped_footprint_ = declare_or_get_parameter(
    "subscribe_to_stamped_footprint", false);
 
  auto node = shared_from_this();
 
  if (plugin_names_ == default_plugins_) {
    for (size_t i = 0; i < default_plugins_.size(); ++i) {
      nav2::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::get_plugin_type_param(node, plugin_names_[i]);
  }
  for (size_t i = 0; i < filter_names_.size(); ++i) {
    filter_types_[i] = nav2::get_plugin_type_param(node, filter_names_[i]);
  }
 
  // 2. The map publish frequency cannot be 0 (to avoid a divide-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_);
    }
  }
 
  // 4. The width, height, and resolution of map cannot be negative or 0
  // (to avoid abnormal memory usage)
  if (map_width_meters_ <= 0) {
    throw std::invalid_argument(
            "You try to set width of map to be negative or zero, "
            "this isn't allowed, please give a positive value.");
  }
  if (map_height_meters_ <= 0) {
    throw std::invalid_argument(
            "You try to set height of map to be negative or zero, "
            "this isn't allowed, please give a positive value.");
  }
  if (resolution_ <= 0.0) {
    throw std::invalid_argument(
            "Costmap resolution must be greater than zero.");
  }
 
  const double size_x = map_width_meters_ / resolution_;
  const double size_y = map_height_meters_ / resolution_;
  const double max_cells = std::numeric_limits<int>::max();
  if (size_x > max_cells / size_y) {
    throw std::invalid_argument("Costmap cell count exceeds the supported signed index range");
  }
}
 
void
Costmap2DROS::setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & points)
{
  if (points.empty()) {
    RCLCPP_ERROR(
      get_logger(), "You try to set an empty footprint"
      " this isn't allowed, a footprint must contain at least one point.");
    return;
  }
  auto padded = std::make_shared<std::vector<geometry_msgs::msg::Point>>(points);
  padFootprint(*padded, footprint_padding_);
 
#ifdef __cpp_lib_atomic_shared_ptr
  unpadded_footprint_.store(std::make_shared<std::vector<geometry_msgs::msg::Point>>(points));
  padded_footprint_.store(padded);
#else
  std::atomic_store(
    &unpadded_footprint_, std::make_shared<std::vector<geometry_msgs::msg::Point>>(points));
  std::atomic_store(&padded_footprint_, padded);
#endif
  layered_costmap_->setFootprint(*padded);
}
 
void
Costmap2DROS::setRobotFootprintPolygon(
  const geometry_msgs::msg::Polygon & 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);
#ifdef __cpp_lib_atomic_shared_ptr
  auto padded_footprint = padded_footprint_.load();
#else
  auto padded_footprint = std::atomic_load(&padded_footprint_);
#endif
  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");
 
  nav2::Rate r(this, 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);
 
        for (auto & layer_pub : layer_publishers_) {
          layer_pub->updateBounds(x0, xn, y0, yn);
        }
 
        auto current_time = now();
        const bool publish_due = last_publish_ + publish_cycle_ < current_time ||
          current_time < last_publish_;  // Time moved backwards, e.g. switching to sim_time.
        if (publish_due || costmap_publisher_->isRepublishRequested()) {
          RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
          costmap_publisher_->publishCostmap();
        }
 
        for (auto & layer_pub : layer_publishers_) {
          if (publish_due || layer_pub->isRepublishRequested()) {
            layer_pub->publishCostmap();
          }
        }
 
        // Subscriber requests do not postpone the regular publication schedule.
        if (publish_due) {
          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;
#ifdef __cpp_lib_atomic_shared_ptr
      auto padded_footprint = padded_footprint_.load();
#else
      auto padded_footprint = std::atomic_load(&padded_footprint_);
#endif
      transformFootprint(x, y, yaw, *padded_footprint, *footprint);
 
      RCLCPP_DEBUG(get_logger(), "Publishing footprint");
      footprint_pub_->publish(std::move(footprint));
      initialized_ = true;
    }
  }
}
 
void
Costmap2DROS::waitUntilCurrent(const rclcpp::Duration & timeout)
{
  rclcpp::Rate r(100);
  auto waiting_start = now();
  while (!isCurrent()) {
    if (now() - waiting_start > timeout) {
      throw std::runtime_error("Costmap timed out waiting for update");
    }
    r.sleep();
  }
}
 
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)
{
  if (!nav2_util::getFreshPose(
      *tf_buffer_, global_frame_, robot_base_frame_, now(),
      transform_staleness_threshold_, global_pose))
  {
    return false;
  }
  return true;
}
 
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::validateParameterUpdatesCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find('.') != std::string::npos) {
      continue;
    }
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (parameter.as_double() <= 0.0 &&
        (param_name == "resolution" || param_name == "publish_frequency"))
      {
        RCLCPP_WARN(
        get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
        "it should be >0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_double());
        result.successful = false;
      } else if (parameter.as_double() < 0.0 && // NOLINT
        (param_name != "origin_x" && param_name != "origin_y"))
      {
        RCLCPP_WARN(
        get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
        "it should be >0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_double());
        result.successful = false;
      }
    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
      if (parameter.as_int() <= 0.0) {
        RCLCPP_WARN(
        get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
        "it should be >0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_int());
        result.successful = false;
      }
    } else if (param_type == ParameterType::PARAMETER_STRING && param_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;
}
 
void
Costmap2DROS::updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters)
{
  bool resize_map = false;
  std::lock_guard<std::mutex> lock_reinit(_dynamic_parameter_mutex);
 
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find('.') != std::string::npos) {
      continue;
    }
 
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name == "robot_radius") {
        robot_radius_ = parameter.as_double();
        // Set the footprint
        if (use_radius_) {
          setRobotFootprint(makeFootprintFromRadius(robot_radius_));
        }
      } else if (param_name == "footprint_padding") {
        footprint_padding_ = parameter.as_double();
#ifdef __cpp_lib_atomic_shared_ptr
        auto padded = std::make_shared<std::vector<geometry_msgs::msg::Point>>(
          *unpadded_footprint_.load());
        padFootprint(*padded, footprint_padding_);
        padded_footprint_.store(padded);
#else
        auto padded = std::make_shared<std::vector<geometry_msgs::msg::Point>>(
          *std::atomic_load(&unpadded_footprint_));
        padFootprint(*padded, footprint_padding_);
        std::atomic_store(&padded_footprint_, padded);
#endif
        layered_costmap_->setFootprint(*padded);
      } else if (param_name == "transform_tolerance") {
        transform_tolerance_ = parameter.as_double();
      } else if (param_name == "transform_staleness_threshold") {
        transform_staleness_threshold_ = parameter.as_double();
      } else if (param_name == "publish_frequency") {
        map_publish_frequency_ = parameter.as_double();
        publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_);
      } else if (param_name == "resolution") {
        resize_map = true;
        resolution_ = parameter.as_double();
      } else if (param_name == "origin_x") {
        resize_map = true;
        origin_x_ = parameter.as_double();
      } else if (param_name == "origin_y") {
        resize_map = true;
        origin_y_ = parameter.as_double();
      }
    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
      if (param_name == "width") {
        resize_map = true;
        map_width_meters_ = parameter.as_int();
      } else if (param_name == "height") {
        resize_map = true;
        map_height_meters_ = parameter.as_int();
      }
    } else if (param_type == ParameterType::PARAMETER_STRING) {
      if (param_name == "footprint") {
        footprint_ = parameter.as_string();
        std::vector<geometry_msgs::msg::Point> new_footprint;
        if (makeFootprintFromString(footprint_, new_footprint)) {
          setRobotFootprint(new_footprint);
        }
      } else if (param_name == "robot_base_frame") {
        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();
  }
}
 
void Costmap2DROS::getCostsCallback(
  const std::shared_ptr<rmw_request_id_t>,
  const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
  const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
{
  unsigned int mx, my;
 
  Costmap2D * costmap = layered_costmap_->getCostmap();
  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
  response->success = true;
  for (const auto & pose : request->poses) {
    geometry_msgs::msg::PoseStamped pose_transformed;
    if (!transformPoseToGlobalFrame(pose, pose_transformed)) {
      RCLCPP_ERROR(
        get_logger(), "Failed to transform, cannot get cost for pose (%.2f, %.2f)",
        pose.pose.position.x, pose.pose.position.y);
      response->success = false;
      response->costs.push_back(NO_INFORMATION);
      continue;
    }
    double yaw = tf2::getYaw(pose_transformed.pose.orientation);
 
    if (request->use_footprint) {
      Footprint footprint = layered_costmap_->getFootprint();
      FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
 
      RCLCPP_DEBUG(
        get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
        pose_transformed.pose.position.x, pose_transformed.pose.position.y, yaw);
 
      response->costs.push_back(
        collision_checker.footprintCostAtPose(
          pose_transformed.pose.position.x,
          pose_transformed.pose.position.y, yaw, footprint));
    } else {
      RCLCPP_DEBUG(
        get_logger(), "Received request to get cost at point (%f, %f)",
        pose_transformed.pose.position.x,
        pose_transformed.pose.position.y);
 
      bool in_bounds = costmap->worldToMap(
        pose_transformed.pose.position.x,
        pose_transformed.pose.position.y, mx, my);
 
      if (!in_bounds) {
        response->success = false;
        response->costs.push_back(LETHAL_OBSTACLE);
        continue;
      }
      // Get the cost at the map coordinates
      response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));
    }
  }
}
 
}  // namespace nav2_costmap_2d