static_layer.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  Copyright (c) 2015, Fetch Robotics, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
 
#include "nav2_costmap_2d/static_layer.hpp"
 
#include <algorithm>
#include <string>
 
#include "pluginlib/class_list_macros.hpp"
#include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_ros_common/validate_messages.hpp"
 
#define EPSILON 1e-5
 
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
using nav2_costmap_2d::NO_INFORMATION;
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using nav2_costmap_2d::FREE_SPACE;
using rcl_interfaces::msg::ParameterType;
 
namespace nav2_costmap_2d
{
 
StaticLayer::StaticLayer()
: map_buffer_(nullptr)
{
}
 
StaticLayer::~StaticLayer()
{
}
 
void
StaticLayer::onInitialize()
{
  global_frame_ = layered_costmap_->getGlobalFrameID();
 
  getParameters();
 
  rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS();  // initialize to default
  if (map_subscribe_transient_local_) {
    map_qos = nav2::qos::LatchedSubscriptionQoS(3);
  }
 
  RCLCPP_INFO(
    logger_,
    "Subscribing to the map topic (%s) with %s durability",
    map_topic_.c_str(),
    map_subscribe_transient_local_ ? "transient local" : "volatile");
 
  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
    map_topic_,
    std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1),
    map_qos);
 
  if (subscribe_to_updates_) {
    RCLCPP_INFO(logger_, "Subscribing to updates");
    map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
      map_topic_ + "_updates",
      std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
  }
}
 
void
StaticLayer::activate()
{
  auto node = node_.lock();
  // Add callback for dynamic parameters
  post_set_params_handler_ = node->add_post_set_parameters_callback(
    std::bind(
      &StaticLayer::updateParametersCallback,
      this, std::placeholders::_1));
  on_set_params_handler_ = node->add_on_set_parameters_callback(
    std::bind(
      &StaticLayer::validateParameterUpdatesCallback,
      this, std::placeholders::_1));
}
 
void
StaticLayer::deactivate()
{
  auto node = node_.lock();
  if (post_set_params_handler_ && node) {
    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
  }
  post_set_params_handler_.reset();
  if (on_set_params_handler_ && node) {
    node->remove_on_set_parameters_callback(on_set_params_handler_.get());
  }
  on_set_params_handler_.reset();
}
 
void
StaticLayer::reset()
{
  has_updated_data_ = true;
  setCurrent(false);
}
 
void
StaticLayer::getParameters()
{
  int temp_lethal_threshold = 0;
  double temp_tf_tol = 0.0;
 
  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
  subscribe_to_updates_ = node->declare_or_get_parameter(
    name_ + "." + "subscribe_to_updates", false);
  footprint_clearing_enabled_ = node->declare_or_get_parameter(
    name_ + "." + "footprint_clearing_enabled", false);
  restore_cleared_footprint_ = node->declare_or_get_parameter(
    name_ + "." + "restore_cleared_footprint", true);
  map_topic_ = node->declare_or_get_parameter(
    name_ + "." + "map_topic", std::string("map"));
  map_topic_ = joinWithParentNamespace(map_topic_);
  map_subscribe_transient_local_ = node->declare_or_get_parameter(
    name_ + "." + "map_subscribe_transient_local", true);
  node->get_parameter("track_unknown_space", track_unknown_space_);
  node->get_parameter("use_maximum", use_maximum_);
  node->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
  node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_);
  node->get_parameter("unknown_cost_value", unknown_cost_value_);
  node->get_parameter("trinary_costmap", trinary_costmap_);
  node->get_parameter("transform_tolerance", temp_tf_tol);
 
  // Enforce bounds
  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
  map_received_ = false;
  map_received_in_update_bounds_ = false;
 
  transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
}
 
void
StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
{
  RCLCPP_DEBUG(logger_, "StaticLayer: Process map");
 
  unsigned int size_x = new_map.info.width;
  unsigned int size_y = new_map.info.height;
 
  RCLCPP_DEBUG(
    logger_,
    "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
    new_map.info.resolution);
 
  // resize costmap if size, resolution or origin do not match
  Costmap2D * master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
    master->getSizeInCellsY() != size_y ||
    !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) ||
    !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) ||
    !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) ||
    !layered_costmap_->isSizeLocked()))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
 
    double fmod_x = std::fmod(new_map.info.origin.position.x, new_map.info.resolution);
    double fmod_y = std::fmod(new_map.info.origin.position.y, new_map.info.resolution);
 
    if (std::abs(fmod_x) > EPSILON || std::abs(fmod_y) > EPSILON) {
      RCLCPP_WARN(
        logger_,
        "StaticLayer: Costmap origin coordinates are not perfectly aligned with the resolution. "
        "This may cause misalignment aliasing between rolling and non-rolling costmaps.\n"
        "Map origin: (%.f, %.f) | Resolution: %.f",
        new_map.info.origin.position.x, new_map.info.origin.position.y,
        new_map.info.resolution);
    }
 
    layered_costmap_->resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x,
      new_map.info.origin.position.y,
      true);
  } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
    !isEqual(resolution_, new_map.info.resolution, EPSILON) ||
    !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
    !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
  {
    // only update the size of the costmap stored locally in this layer
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
    resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x, new_map.info.origin.position.y);
  }
 
  unsigned int index = 0;
 
  // we have a new map, update full size of map
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
 
  // initialize the costmap with static data
  for (unsigned int i = 0; i < size_y; ++i) {
    for (unsigned int j = 0; j < size_x; ++j) {
      unsigned char value = new_map.data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
 
  map_frame_ = new_map.header.frame_id;
 
  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  has_updated_data_ = true;
 
  setCurrent(true);
}
 
void
StaticLayer::matchSize()
{
  // If we are using rolling costmap, the static map size is
  //   unrelated to the size of the layered costmap
  if (!layered_costmap_->isRolling()) {
    Costmap2D * master = layered_costmap_->getCostmap();
    resizeMap(
      master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
      master->getOriginX(), master->getOriginY());
  }
}
 
unsigned char
StaticLayer::interpretValue(unsigned char value)
{
  // check if the static value is above the unknown or lethal thresholds
  if (track_unknown_space_ && value == unknown_cost_value_) {
    return NO_INFORMATION;
  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
    return FREE_SPACE;
  } else if (value == inscribed_obstacle_cost_value_) {
    return INSCRIBED_INFLATED_OBSTACLE;
  } else if (value >= lethal_threshold_) {
    return LETHAL_OBSTACLE;
  } else if (trinary_costmap_) {
    return FREE_SPACE;
  }
 
  double scale = static_cast<double>(value) / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}
 
void
StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & new_map)
{
  if (!nav2::validateMsg(*new_map)) {
    RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
    return;
  }
  if (!map_received_) {
    processMap(*new_map);
    map_received_ = true;
    return;
  }
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  map_buffer_ = new_map;
  setCurrent(false);
}
 
void
StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
{
  if (!nav2::validateMsg(*update)) {
    RCLCPP_ERROR(logger_, "Received map update is malformed. Rejecting.");
    return;
  }
 
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (update->y < static_cast<int32_t>(y_) ||
    y_ + height_ < update->y + update->height ||
    update->x < static_cast<int32_t>(x_) ||
    x_ + width_ < update->x + update->width)
  {
    RCLCPP_WARN(
      logger_,
      "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
      "Static layer origin: %d, %d   bounds: %d X %d\n"
      "Update origin: %d, %d   bounds: %d X %d",
      x_, y_, width_, height_, update->x, update->y, update->width,
      update->height);
    return;
  }
 
  if (update->header.frame_id != map_frame_) {
    RCLCPP_WARN(
      logger_,
      "StaticLayer: Map update ignored. Current map is in frame %s "
      "but update was in frame %s",
      map_frame_.c_str(), update->header.frame_id.c_str());
    return;
  }
 
  unsigned int di = 0;
  for (unsigned int y = 0; y < update->height; y++) {
    unsigned int index_base = (update->y + y) * size_x_;
    for (unsigned int x = 0; x < update->width; x++) {
      unsigned int index = index_base + x + update->x;
      costmap_[index] = interpretValue(update->data[di++]);
    }
  }
 
  has_updated_data_ = true;
}
 
 
void
StaticLayer::updateBounds(
  double robot_x, double robot_y, double robot_yaw, double * min_x,
  double * min_y,
  double * max_x,
  double * max_y)
{
  if (!map_received_) {
    map_received_in_update_bounds_ = false;
    return;
  }
  map_received_in_update_bounds_ = true;
 
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
 
  // If there is a new available map, load it.
  if (map_buffer_) {
    processMap(*map_buffer_);
    map_buffer_ = nullptr;
  }
 
  if (!layered_costmap_->isRolling() ) {
    if (!(has_updated_data_ || has_extra_bounds_)) {
      return;
    }
  }
 
  useExtraBounds(min_x, min_y, max_x, max_y);
 
  if (layered_costmap_->isRolling()) {
    // For rolling costmaps the global_frame (e.g. odom) differs from the
    // map frame.  mapToWorld() returns coordinates in the map frame, but
    // the layered costmap interprets bounds in its global_frame.  Report
    // bounds that cover the full rolling window using the robot pose,
    // which is already in the correct frame.  updateCosts() handles the
    // per-cell map↔odom transform itself.
    Costmap2D * master = layered_costmap_->getCostmap();
    double half_w = master->getSizeInMetersX() / 2.0;
    double half_h = master->getSizeInMetersY() / 2.0;
    *min_x = std::min(robot_x - half_w, *min_x);
    *min_y = std::min(robot_y - half_h, *min_y);
    *max_x = std::max(robot_x + half_w, *max_x);
    *max_y = std::max(robot_y + half_h, *max_y);
  } else {
    double wx, wy;
 
    mapToWorld(x_, y_, wx, wy);
    *min_x = std::min(wx, *min_x);
    *min_y = std::min(wy, *min_y);
 
    mapToWorld(x_ + width_, y_ + height_, wx, wy);
    *max_x = std::max(wx, *max_x);
    *max_y = std::max(wy, *max_y);
  }
 
  has_updated_data_ = false;
 
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
 
void
StaticLayer::updateFootprint(
  double robot_x, double robot_y, double robot_yaw,
  double * min_x, double * min_y,
  double * max_x,
  double * max_y)
{
  if (!footprint_clearing_enabled_) {return;}
 
  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
 
  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
    touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
  }
}
 
void
StaticLayer::updateCosts(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (!enabled_) {
    return;
  }
  if (!map_received_in_update_bounds_) {
    static int count = 0;
    // throttle warning down to only 1/10 message rate
    if (++count == 10) {
      RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
      count = 0;
    }
    return;
  }
 
  std::vector<MapLocation> map_region_to_restore;
  if (footprint_clearing_enabled_) {
    map_region_to_restore.reserve(100);
    getMapRegionOccupiedByPolygon(transformed_footprint_, map_region_to_restore);
    setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE);
  }
 
  if (!layered_costmap_->isRolling()) {
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_) {
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    } else {
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    }
  } else {
    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame
    geometry_msgs::msg::TransformStamped transform;
    try {
      transform = tf_->lookupTransform(
        map_frame_, global_frame_, tf2::TimePointZero,
        transform_tolerance_);
    } catch (tf2::TransformException & ex) {
      RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    tf2::Transform tf2_transform;
    tf2::fromMsg(transform.transform, tf2_transform);
 
    for (int i = min_i; i < max_i; ++i) {
      for (int j = min_j; j < max_j; ++j) {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf2::Vector3 p(wx, wy, 0);
        p = tf2_transform * p;
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my)) {
          if (!use_maximum_) {
            master_grid.setCost(i, j, getCost(mx, my));
          } else {
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
          }
        }
      }
    }
  }
 
  if (footprint_clearing_enabled_ && restore_cleared_footprint_) {
    // restore the map region occupied by the polygon using cached data
    restoreMapRegionOccupiedByPolygon(map_region_to_restore);
  }
  setCurrent(true);
}
 
bool StaticLayer::isEqual(double a, double b, double epsilon)
{
  return std::abs(a - b) < epsilon;
}
 
rcl_interfaces::msg::SetParametersResult StaticLayer::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(name_ + ".") != 0) {
      continue;
    }
 
    if (param_name == name_ + "." + "map_subscribe_transient_local" ||
      param_name == name_ + "." + "map_topic" ||
      param_name == name_ + "." + "subscribe_to_updates")
    {
      RCLCPP_WARN(
        logger_, "%s is not a dynamic parameter "
        "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
    } else if (param_type == ParameterType::PARAMETER_BOOL && // NOLINT
      param_name == name_ + "." + "restore_cleared_footprint")
    {
      if (!footprint_clearing_enabled_) {
        RCLCPP_WARN(
          logger_, "restore_cleared_footprint cannot be used "
          "when footprint_clearing_enabled is False. Rejecting parameter update.");
        result.successful = false;
      }
    }
  }
  return result;
}
 
void
StaticLayer::updateParametersCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
 
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find(name_ + ".") != 0) {
      continue;
    }
 
    if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
        enabled_ = parameter.as_bool();
 
        x_ = y_ = 0;
        width_ = size_x_;
        height_ = size_y_;
        has_updated_data_ = true;
        setCurrent(false);
      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
        footprint_clearing_enabled_ = parameter.as_bool();
      } else if (param_name == name_ + "." + "restore_cleared_footprint") {
        restore_cleared_footprint_ = parameter.as_bool();
      }
    }
  }
}
 
}  // namespace nav2_costmap_2d