keepout_filter.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020 Samsung Research Russia
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the <ORGANIZATION> nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Alexey Merzlyakov
 *********************************************************************/
 
#include <string>
#include <memory>
#include <algorithm>
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
namespace nav2_costmap_2d
{
 
KeepoutFilter::KeepoutFilter()
: filter_info_sub_(nullptr), mask_sub_(nullptr), filter_mask_(nullptr),
  global_frame_("")
{
}
 
void KeepoutFilter::initializeFilter(
  const std::string & filter_info_topic)
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  filter_info_topic_ = filter_info_topic;
  // Setting new costmap filter info subscriber
  RCLCPP_INFO(
    logger_,
    "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
    filter_info_topic_.c_str());
  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
    filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
    std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
 
  global_frame_ = layered_costmap_->getGlobalFrameID();
 
  declareParameter("override_lethal_cost", rclcpp::ParameterValue(false));
  node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_);
  declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE));
  node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_);
 
  // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
  lethal_override_cost_ = \
    std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
}
 
void KeepoutFilter::filterInfoCallback(
  const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  if (!mask_sub_) {
    RCLCPP_INFO(
      logger_,
      "KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
  } else {
    RCLCPP_WARN(
      logger_,
      "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
      filter_info_topic_.c_str());
    // Resetting previous subscriber each time when new costmap filter information arrives
    mask_sub_.reset();
  }
 
  // Checking that base and multiplier are set to their default values
  if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) {
    RCLCPP_ERROR(
      logger_,
      "KeepoutFilter: For proper use of keepout filter base and multiplier"
      " in CostmapFilterInfo message should be set to their default values (%f and %f)",
      BASE_DEFAULT, MULTIPLIER_DEFAULT);
  }
 
  mask_topic_ = msg->filter_mask_topic;
 
  // Setting new filter mask subscriber
  RCLCPP_INFO(
    logger_,
    "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
    mask_topic_.c_str());
  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
    mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
    std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1));
}
 
void KeepoutFilter::maskCallback(
  const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  if (!filter_mask_) {
    RCLCPP_INFO(
      logger_,
      "KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str());
  } else {
    RCLCPP_WARN(
      logger_,
      "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
      mask_topic_.c_str());
    filter_mask_.reset();
  }
 
  // Store filter_mask_
  filter_mask_ = msg;
  has_updated_data_ = true;
  x_ = y_ = 0;
  width_ = msg->info.width;
  height_ = msg->info.height;
}
 
void KeepoutFilter::updateBounds(
  double robot_x, double robot_y, double robot_yaw,
  double * min_x, double * min_y, double * max_x, double * max_y)
{
  if (!enabled_) {
    return;
  }
 
  CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
 
  if(!has_updated_data_) {
    return;
  }
 
  double wx, wy;
 
  layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);
 
  layered_costmap_->getCostmap()->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;
}
 
void KeepoutFilter::process(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j,
  const geometry_msgs::msg::Pose2D & pose)
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  if (!filter_mask_) {
    // Show warning message every 2 seconds to not litter an output
    RCLCPP_WARN_THROTTLE(
      logger_, *(clock_), 2000,
      "KeepoutFilter: Filter mask was not received");
    return;
  }
 
  tf2::Transform tf2_transform;
  tf2_transform.setIdentity();  // initialize by identical transform
  int mg_min_x, mg_min_y;  // masger_grid indexes of bottom-left window corner
  int mg_max_x, mg_max_y;  // masger_grid indexes of top-right window corner
 
  const std::string mask_frame = filter_mask_->header.frame_id;
 
  if (mask_frame != global_frame_) {
    // Filter mask and current layer are in different frames:
    // prepare frame transformation if mask_frame != global_frame_
    geometry_msgs::msg::TransformStamped transform;
    try {
      transform = tf_->lookupTransform(
        mask_frame, global_frame_, tf2::TimePointZero,
        transform_tolerance_);
    } catch (tf2::TransformException & ex) {
      RCLCPP_ERROR(
        logger_,
        "KeepoutFilter: Failed to get costmap frame (%s) "
        "transformation to mask frame (%s) with error: %s",
        global_frame_.c_str(), mask_frame.c_str(), ex.what());
      return;
    }
    tf2::fromMsg(transform.transform, tf2_transform);
 
    mg_min_x = min_i;
    mg_min_y = min_j;
    mg_max_x = max_i;
    mg_max_y = max_j;
  } else {
    // Filter mask and current layer are in the same frame:
    // apply the following optimization - iterate only in overlapped
    // (min_i, min_j)..(max_i, max_j) & filter_mask_ area.
    //
    //           filter_mask_
    //       *----------------------------*
    //       |                            |
    //       |                            |
    //       |      (2)                   |
    // *-----+-------*                    |
    // |     |///////|<- overlapped area  |
    // |     |///////|   to iterate in    |
    // |     *-------+--------------------*
    // |    (1)      |
    // |             |
    // *-------------*
    //  master_grid (min_i, min_j)..(max_i, max_j) window
    //
    // ToDo: after costmap rotation will be added, this should be re-worked.
 
    double wx, wy;  // world coordinates
 
    // Calculating bounds corresponding to bottom-left overlapping (1) corner
    // filter_mask_ -> master_grid indexes conversion
    const double half_cell_size = 0.5 * filter_mask_->info.resolution;
    wx = filter_mask_->info.origin.position.x + half_cell_size;
    wy = filter_mask_->info.origin.position.y + half_cell_size;
    master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y);
    // Calculation of (1) corner bounds
    if (mg_min_x >= max_i || mg_min_y >= max_j) {
      // There is no overlapping. Do nothing.
      return;
    }
    mg_min_x = std::max(min_i, mg_min_x);
    mg_min_y = std::max(min_j, mg_min_y);
 
    // Calculating bounds corresponding to top-right window (2) corner
    // filter_mask_ -> master_grid intexes conversion
    wx = filter_mask_->info.origin.position.x +
      filter_mask_->info.width * filter_mask_->info.resolution + half_cell_size;
    wy = filter_mask_->info.origin.position.y +
      filter_mask_->info.height * filter_mask_->info.resolution + half_cell_size;
    master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y);
    // Calculation of (2) corner bounds
    if (mg_max_x <= min_i || mg_max_y <= min_j) {
      // There is no overlapping. Do nothing.
      return;
    }
    mg_max_x = std::min(max_i, mg_max_x);
    mg_max_y = std::min(max_j, mg_max_y);
  }
 
  // unsigned<-signed conversions.
  unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
  unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
  unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
  unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
 
  // Let's find the pose's cost if we are allowed to override the lethal cost
  bool is_pose_lethal = false;
  if (override_lethal_cost_) {
    geometry_msgs::msg::Pose2D mask_pose;
    if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
      unsigned int mask_robot_i, mask_robot_j;
      if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
        auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
        is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
        if (is_pose_lethal) {
          RCLCPP_WARN_THROTTLE(
            logger_, *(clock_), 2000,
            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
        }
      }
    }
 
    // If in lethal space or just exited lethal space,
    // we need to update all possible spaces touched during this state
    if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) {
      lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_);
      mg_min_x_u = lethal_state_update_min_x_;
      lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_);
      mg_min_y_u = lethal_state_update_min_y_;
      lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_);
      mg_max_x_u = lethal_state_update_max_x_;
      lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_);
      mg_max_y_u = lethal_state_update_max_y_;
    } else {
      // If out of lethal space, reset managed lethal state sizes
      lethal_state_update_min_x_ = master_grid.getSizeInCellsX();
      lethal_state_update_min_y_ = master_grid.getSizeInCellsY();
      lethal_state_update_max_x_ = 0u;
      lethal_state_update_max_y_ = 0u;
    }
  }
 
  unsigned int i, j;  // master_grid iterators
  unsigned int index;  // corresponding index of master_grid
  double gl_wx, gl_wy;  // world coordinates in a global_frame_
  double msk_wx, msk_wy;  // world coordinates in a mask_frame
  unsigned int mx, my;  // filter_mask_ coordinates
  unsigned char data, old_data;  // master_grid element data
 
  // Main master_grid updating loop
  // Iterate in costmap window by master_grid indexes
  unsigned char * master_array = master_grid.getCharMap();
  for (i = mg_min_x_u; i < mg_max_x_u; i++) {
    for (j = mg_min_y_u; j < mg_max_y_u; j++) {
      index = master_grid.getIndex(i, j);
      old_data = master_array[index];
      // Calculating corresponding to (i, j) point at filter_mask_:
      // Get world coordinates in global_frame_
      master_grid.mapToWorld(i, j, gl_wx, gl_wy);
      if (mask_frame != global_frame_) {
        // Transform (i, j) point from global_frame_ to mask_frame
        tf2::Vector3 point(gl_wx, gl_wy, 0);
        point = tf2_transform * point;
        msk_wx = point.x();
        msk_wy = point.y();
      } else {
        // In this case master_grid and filter-mask are in the same frame
        msk_wx = gl_wx;
        msk_wy = gl_wy;
      }
      // Get mask coordinates corresponding to (i, j) point at filter_mask_
      if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) {
        data = getMaskCost(filter_mask_, mx, my);
        // Update if mask_ data is valid and greater than existing master_grid's one
        if (data == NO_INFORMATION) {
          continue;
        }
 
        if (data > old_data || old_data == NO_INFORMATION) {
          if (override_lethal_cost_ && is_pose_lethal) {
            master_array[index] = lethal_override_cost_;
          } else {
            master_array[index] = data;
          }
        }
      }
    }
  }
 
  last_pose_lethal_ = is_pose_lethal;
}
 
void KeepoutFilter::resetFilter()
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  filter_info_sub_.reset();
  mask_sub_.reset();
}
 
bool KeepoutFilter::isActive()
{
  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
  if (filter_mask_) {
    return true;
  }
  return false;
}
 
}  // namespace nav2_costmap_2d
 
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer)