collision_detector_node.cpp

// Copyright (c) 2022 Samsung R&D Institute Russia
// Copyright (c) 2023 Pixel Robotics GmbH
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include "nav2_collision_monitor/collision_detector_node.hpp"
 
#include <exception>
#include <utility>
#include <functional>
 
#include "nav2_ros_common/node_utils.hpp"
 
using namespace std::chrono_literals;
 
namespace nav2_collision_monitor
{
 
CollisionDetector::CollisionDetector(const rclcpp::NodeOptions & options)
: nav2::LifecycleNode("collision_detector", options)
{
}
 
CollisionDetector::~CollisionDetector()
{
  polygons_.clear();
  sources_.clear();
}
 
nav2::CallbackReturn
CollisionDetector::on_configure(const rclcpp_lifecycle::State & state)
{
  RCLCPP_INFO(get_logger(), "Configuring");
 
  // Transform buffer and listener initialization
  tf_buffer_ = nav2::create_transform_buffer(this);
  tf_listener_ = nav2::create_transform_listener(*tf_buffer_, this, true);
 
  state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionDetectorState>(
    "collision_detector_state");
 
  collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
    "~/collision_points_marker");
 
  triggering_points_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
    "~/triggering_points");
 
  // Obtaining ROS parameters
  if (!getParameters()) {
    on_cleanup(state);
    return nav2::CallbackReturn::FAILURE;
  }
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Activating");
 
  // Activating lifecycle publisher
  state_pub_->on_activate();
  collision_points_marker_pub_->on_activate();
  triggering_points_pub_->on_activate();
 
  // Activating polygons
  for (std::shared_ptr<Polygon> polygon : polygons_) {
    polygon->activate();
  }
 
  // Activating sources
  for (std::shared_ptr<Source> source : sources_) {
    source->activate();
  }
 
  // Creating timer
  timer_ = this->create_timer(
    std::chrono::duration<double>{1.0 / frequency_},
    std::bind(&CollisionDetector::process, this));
 
  // Creating bond connection
  createBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Deactivating");
 
  // Resetting timer
  timer_.reset();
 
  // Deactivating lifecycle publishers
  state_pub_->on_deactivate();
  collision_points_marker_pub_->on_deactivate();
  triggering_points_pub_->on_deactivate();
 
  // Deactivating polygons
  for (std::shared_ptr<Polygon> polygon : polygons_) {
    polygon->deactivate();
  }
 
  // Deactivating sources
  for (std::shared_ptr<Source> source : sources_) {
    source->deactivate();
  }
 
  // Destroying bond connection
  destroyBond();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Cleaning up");
 
  state_pub_.reset();
  collision_points_marker_pub_.reset();
  triggering_points_pub_.reset();
 
  polygons_.clear();
  sources_.clear();
 
  tf_listener_.reset();
  tf_buffer_.reset();
 
  return nav2::CallbackReturn::SUCCESS;
}
 
nav2::CallbackReturn
CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Shutting down");
  return nav2::CallbackReturn::SUCCESS;
}
 
bool CollisionDetector::getParameters()
{
  std::string odom_frame_id;
  tf2::Duration transform_tolerance;
  rclcpp::Duration source_timeout(2.0, 0.0);
 
  auto node = shared_from_this();
 
  frequency_ = node->declare_or_get_parameter("frequency", 10.0);
  base_frame_id_ = node->declare_or_get_parameter("base_frame_id", std::string("base_footprint"));
  odom_frame_id = node->declare_or_get_parameter("odom_frame_id", std::string("odom"));
  transform_tolerance = tf2::durationFromSec(
    node->declare_or_get_parameter("transform_tolerance", 0.1));
  source_timeout = rclcpp::Duration::from_seconds(
    node->declare_or_get_parameter("source_timeout", 2.0));
  const bool base_shift_correction = node->declare_or_get_parameter("base_shift_correction", true);
  collision_points_marker_3d_ = node->declare_or_get_parameter("collision_points_marker_3d", false);
 
  if (!configureSources(
      base_frame_id_, odom_frame_id, transform_tolerance, source_timeout,
      base_shift_correction))
  {
    return false;
  }
 
  if (!configurePolygons(base_frame_id_, transform_tolerance)) {
    return false;
  }
 
  return true;
}
 
bool CollisionDetector::configurePolygons(
  const std::string & base_frame_id,
  const tf2::Duration & transform_tolerance)
{
  try {
    auto node = shared_from_this();
 
    // Leave it to be not initialized: to intentionally cause an error if it will not set
    std::vector<std::string> polygon_names =
      node->declare_or_get_parameter<std::vector<std::string>>("polygons");
    for (std::string polygon_name : polygon_names) {
      // Leave it not initialized: the will cause an error if it will not set
      const std::string polygon_type =
        node->declare_or_get_parameter<std::string>(polygon_name + ".type");
 
      if (polygon_type == "polygon") {
        polygons_.push_back(
          std::make_shared<Polygon>(
            node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
      } else if (polygon_type == "circle") {
        polygons_.push_back(
          std::make_shared<Circle>(
            node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
      } else if (polygon_type == "velocity_polygon") {
        polygons_.push_back(
          std::make_shared<VelocityPolygon>(
            node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
      } else {  // Error if something else
        RCLCPP_ERROR(
          get_logger(),
          "[%s]: Unknown polygon type: %s",
          polygon_name.c_str(), polygon_type.c_str());
        return false;
      }
 
      // Configure last added polygon
      if (!polygons_.back()->configure()) {
        return false;
      }
 
      // warn if the added polygon's action_type_ is not different than "none"
      auto action_type = polygons_.back()->getActionType();
      if (action_type != DO_NOTHING) {
        RCLCPP_ERROR(
          get_logger(),
          "[%s]: The action_type of the polygon is different than \"none\" which is "
          "not supported in the collision detector.",
          polygon_name.c_str());
        return false;
      }
    }
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());
    return false;
  }
 
  return true;
}
 
bool CollisionDetector::configureSources(
  const std::string & base_frame_id,
  const std::string & odom_frame_id,
  const tf2::Duration & transform_tolerance,
  const rclcpp::Duration & source_timeout,
  const bool base_shift_correction)
{
  try {
    auto node = shared_from_this();
 
    // Leave it to be not initialized to intentionally cause an error if it will not set
    std::vector<std::string> source_names =
      node->declare_or_get_parameter<std::vector<std::string>>("observation_sources");
    for (std::string source_name : source_names) {
      const std::string source_type = node->declare_or_get_parameter(
        source_name + ".type", std::string("scan"));  // Laser scanner by default
 
      if (source_type == "scan") {
        std::shared_ptr<Scan> s = std::make_shared<Scan>(
          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
          transform_tolerance, source_timeout, base_shift_correction);
 
        if (!s->configure()) {
          return false;
        }
 
        sources_.push_back(s);
      } else if (source_type == "pointcloud") {
        std::shared_ptr<PointCloud> p = std::make_shared<PointCloud>(
          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
          transform_tolerance, source_timeout, base_shift_correction);
 
        if (!p->configure()) {
          return false;
        }
 
        sources_.push_back(p);
      } else if (source_type == "range") {
        std::shared_ptr<Range> r = std::make_shared<Range>(
          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
          transform_tolerance, source_timeout, base_shift_correction);
 
        if (!r->configure()) {
          return false;
        }
 
        sources_.push_back(r);
      } else if (source_type == "polygon") {
        std::shared_ptr<PolygonSource> ps = std::make_shared<PolygonSource>(
          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
          transform_tolerance, source_timeout, base_shift_correction);
        if (!ps->configure()) {
          return false;
        }
 
        sources_.push_back(ps);
      } else if (source_type == "costmap") {
        auto src = std::make_shared<CostmapSource>(
          node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
          transform_tolerance, source_timeout, base_shift_correction);
 
        if (!src->configure()) {
          return false;
        }
 
        sources_.push_back(src);
      } else {  // Error if something else
        RCLCPP_ERROR(
          get_logger(),
          "[%s]: Unknown source type: %s",
          source_name.c_str(), source_type.c_str());
        return false;
      }
    }
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());
    return false;
  }
 
  return true;
}
 
void CollisionDetector::process()
{
  // Current timestamp for all inner routines prolongation
  rclcpp::Time curr_time = this->now();
 
  // Points array collected from different data sources in a robot base frame
  std::unordered_map<std::string, std::vector<Point>> sources_collision_points_map;
 
  std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
    std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
 
  // Fill collision_points map from different data sources
  for (std::shared_ptr<Source> source : sources_) {
    auto iter = sources_collision_points_map.insert(
      {source->getSourceName(), std::vector<Point>()});
 
    if (source->getEnabled()) {
      if (!source->getData(curr_time, iter.first->second) &&
        source->getSourceTimeout().seconds() != 0.0)
      {
        RCLCPP_WARN(
          get_logger(),
          "Invalid source %s detected."
          " Either due to data not published yet, or to lack of new data received within the"
          " sensor timeout, or if impossible to transform data to base frame",
          source->getSourceName().c_str());
      }
    }
  }
 
  if (collision_points_marker_pub_->get_subscription_count() > 0) {
    // visualize collision points with markers
    auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = base_frame_id_;
    marker.header.stamp = rclcpp::Time(0, 0);
    marker.ns = "collision_points";
    marker.id = 0;
    marker.type = visualization_msgs::msg::Marker::POINTS;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.scale.x = 0.02;
    marker.scale.y = 0.02;
    marker.color.r = 1.0;
    marker.color.a = 1.0;
    marker.lifetime = rclcpp::Duration(0, 0);
    marker.frame_locked = true;
 
    for (const auto & [_, points] : sources_collision_points_map) {
      for (const auto & point : points) {
        geometry_msgs::msg::Point p;
        p.x = point.x;
        p.y = point.y;
        p.z = collision_points_marker_3d_ ? point.z : 0.0;
        marker.points.push_back(p);
      }
    }
    marker_array->markers.push_back(marker);
    collision_points_marker_pub_->publish(std::move(marker_array));
  }
 
  // Per-polygon triggering points; populated only for polygons that detect.
  std::unordered_map<std::string, std::vector<Point>> all_triggering_points;
 
  for (std::shared_ptr<Polygon> polygon : polygons_) {
    if (!polygon->getEnabled()) {
      continue;
    }
    std::vector<Point> triggering_points;
    const bool detected = polygon->isTriggered(sources_collision_points_map, triggering_points);
    state_msg->polygons.push_back(polygon->getName());
    state_msg->detections.push_back(detected);
    if (detected) {
      all_triggering_points[polygon->getName()] = std::move(triggering_points);
    }
  }
 
  state_pub_->publish(std::move(state_msg));
 
  if (triggering_points_pub_->get_subscription_count() > 0) {
    publishTriggeringPoints(all_triggering_points);
  }
 
  // Publish polygons and exclusion zones for better visualization
  publishVisualizations();
}
 
void CollisionDetector::publishTriggeringPoints(
  const std::unordered_map<std::string, std::vector<Point>> & all_triggering_points)
{
  auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
 
  // Clear markers from previous cycle.
  visualization_msgs::msg::Marker clear;
  clear.action = visualization_msgs::msg::Marker::DELETEALL;
  marker_array->markers.push_back(clear);
 
  std::unordered_map<std::string, size_t> marker_index;
  for (const auto & [polygon_name, points] : all_triggering_points) {
    for (const auto & p : points) {
      const std::string key = polygon_name + "/" + p.source;
      auto [it, new_source] = marker_index.try_emplace(key, marker_array->markers.size());
 
      if (new_source) {
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = base_frame_id_;
        marker.header.stamp = rclcpp::Time(0, 0);
        marker.ns = key;
        marker.id = 0;
        marker.type = visualization_msgs::msg::Marker::POINTS;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.scale.x = 0.05;
        marker.scale.y = 0.05;
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;
        marker.color.a = 1.0f;
        marker.lifetime = rclcpp::Duration(0, 0);
        marker.frame_locked = true;
        marker_array->markers.push_back(std::move(marker));
      }
 
      geometry_msgs::msg::Point gp;
      gp.x = p.x;
      gp.y = p.y;
      gp.z = p.z;
      marker_array->markers[it->second].points.push_back(gp);
    }
  }
 
  triggering_points_pub_->publish(std::move(marker_array));
}
 
void CollisionDetector::publishVisualizations() const
{
  for (std::shared_ptr<Polygon> polygon : polygons_) {
    if (polygon->getEnabled()) {
      polygon->publish();
    }
  }
 
  for (std::shared_ptr<Source> source : sources_) {
    source->publishExclusionZones();
  }
}
 
}  // namespace nav2_collision_monitor
 
#include "rclcpp_components/register_node_macro.hpp"
 
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionDetector)