add humble-navigation2

navigation2/nav2_collision_monitor/src/circle.cpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/circle.hpp"
+
+#include <math.h>
+#include <cmath>
+#include <exception>
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_collision_monitor
+{
+
+Circle::Circle(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & polygon_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const tf2::Duration & transform_tolerance)
+: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str());
+}
+
+Circle::~Circle()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying Circle", polygon_name_.c_str());
+}
+
+void Circle::getPolygon(std::vector<Point> & poly) const
+{
+  // Number of polygon points. More edges means better approximation.
+  const double polygon_edges = 16;
+  // Increment of angle during points position calculation
+  double angle_increment = 2 * M_PI / polygon_edges;
+
+  // Clear polygon before filling
+  poly.clear();
+
+  // Making new polygon looks like a circle
+  Point p;
+  for (double angle = 0.0; angle < 2 * M_PI; angle += angle_increment) {
+    p.x = radius_ * std::cos(angle);
+    p.y = radius_ * std::sin(angle);
+    poly.push_back(p);
+  }
+}
+
+int Circle::getPointsInside(const std::vector<Point> & points) const
+{
+  int num = 0;
+  for (Point point : points) {
+    if (point.x * point.x + point.y * point.y < radius_squared_) {
+      num++;
+    }
+  }
+
+  return num;
+}
+
+bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!getCommonParameters(polygon_pub_topic)) {
+    return false;
+  }
+
+  // There is no footprint subscription for the Circle. Thus, set string as empty.
+  footprint_topic.clear();
+
+  try {
+    // Leave it not initialized: the will cause an error if it will not set
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
+    radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
+    radius_squared_ = radius_ * radius_;
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Error while getting circle parameters: %s",
+      polygon_name_.c_str(), ex.what());
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -0,0 +1,502 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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_monitor_node.hpp"
+
+#include <exception>
+#include <utility>
+#include <functional>
+
+#include "tf2_ros/create_timer_ros.h"
+
+#include "nav2_util/node_utils.hpp"
+
+#include "nav2_collision_monitor/kinematics.hpp"
+
+namespace nav2_collision_monitor
+{
+
+CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("collision_monitor", "", options),
+  process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}},
+  stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
+{
+}
+
+CollisionMonitor::~CollisionMonitor()
+{
+  polygons_.clear();
+  sources_.clear();
+}
+
+nav2_util::CallbackReturn
+CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Transform buffer and listener initialization
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+    this->get_node_base_interface(),
+    this->get_node_timers_interface());
+  tf_buffer_->setCreateTimerInterface(timer_interface);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  std::string cmd_vel_in_topic;
+  std::string cmd_vel_out_topic;
+
+  // Obtaining ROS parameters
+  if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) {
+    return nav2_util::CallbackReturn::FAILURE;
+  }
+
+  cmd_vel_in_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    cmd_vel_in_topic, 1,
+    std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
+  cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
+    cmd_vel_out_topic, 1);
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  // Activating lifecycle publisher
+  cmd_vel_out_pub_->on_activate();
+
+  // Activating polygons
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    polygon->activate();
+  }
+
+  // Since polygons are being published when cmd_vel_in appears,
+  // we need to publish polygons first time to display them at startup
+  publishPolygons();
+
+  // Activating main worker
+  process_active_ = true;
+
+  // Creating bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  // Deactivating main worker
+  process_active_ = false;
+
+  // Reset action type to default after worker deactivating
+  robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}};
+
+  // Deactivating polygons
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    polygon->deactivate();
+  }
+
+  // Deactivating lifecycle publishers
+  cmd_vel_out_pub_->on_deactivate();
+
+  // Destroying bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  cmd_vel_in_sub_.reset();
+  cmd_vel_out_pub_.reset();
+
+  polygons_.clear();
+  sources_.clear();
+
+  tf_listener_.reset();
+  tf_buffer_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
+{
+  // If message contains NaN or Inf, ignore
+  if (!nav2_util::validateTwist(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
+
+  process({msg->linear.x, msg->linear.y, msg->angular.z});
+}
+
+void CollisionMonitor::publishVelocity(const Action & robot_action)
+{
+  if (robot_action.req_vel.isZero()) {
+    if (!robot_action_prev_.req_vel.isZero()) {
+      // Robot just stopped: saving stop timestamp and continue
+      stop_stamp_ = this->now();
+    } else if (this->now() - stop_stamp_ > stop_pub_timeout_) {
+      // More than stop_pub_timeout_ passed after robot has been stopped.
+      // Cease publishing output cmd_vel.
+      return;
+    }
+  }
+
+  std::unique_ptr<geometry_msgs::msg::Twist> cmd_vel_out_msg =
+    std::make_unique<geometry_msgs::msg::Twist>();
+  cmd_vel_out_msg->linear.x = robot_action.req_vel.x;
+  cmd_vel_out_msg->linear.y = robot_action.req_vel.y;
+  cmd_vel_out_msg->angular.z = robot_action.req_vel.tw;
+  // linear.z, angular.x and angular.y will remain 0.0
+
+  cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg));
+}
+
+bool CollisionMonitor::getParameters(
+  std::string & cmd_vel_in_topic,
+  std::string & cmd_vel_out_topic)
+{
+  std::string base_frame_id, odom_frame_id;
+  tf2::Duration transform_tolerance;
+  rclcpp::Duration source_timeout(2.0, 0.0);
+
+  auto node = shared_from_this();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw"));
+  cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel"));
+  cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
+  base_frame_id = get_parameter("base_frame_id").as_string();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "odom_frame_id", rclcpp::ParameterValue("odom"));
+  odom_frame_id = get_parameter("odom_frame_id").as_string();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
+  transform_tolerance =
+    tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, "source_timeout", rclcpp::ParameterValue(2.0));
+  source_timeout =
+    rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, "base_shift_correction", rclcpp::ParameterValue(true));
+  const bool base_shift_correction =
+    get_parameter("base_shift_correction").as_bool();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, "stop_pub_timeout", rclcpp::ParameterValue(1.0));
+  stop_pub_timeout_ =
+    rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double());
+
+  if (!configurePolygons(base_frame_id, transform_tolerance)) {
+    return false;
+  }
+
+  if (
+    !configureSources(
+      base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
+  {
+    return false;
+  }
+
+  return true;
+}
+
+bool CollisionMonitor::configurePolygons(
+  const std::string & base_frame_id,
+  const tf2::Duration & transform_tolerance)
+{
+  try {
+    auto node = shared_from_this();
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "polygons", rclcpp::ParameterValue(std::vector<std::string>()));
+    std::vector<std::string> polygon_names = get_parameter("polygons").as_string_array();
+    for (std::string polygon_name : polygon_names) {
+      // Leave it not initialized: the will cause an error if it will not set
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name + ".type", rclcpp::PARAMETER_STRING);
+      const std::string polygon_type = get_parameter(polygon_name + ".type").as_string();
+
+      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 {  // 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;
+      }
+    }
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());
+    return false;
+  }
+
+  return true;
+}
+
+bool CollisionMonitor::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
+    nav2_util::declare_parameter_if_not_declared(
+      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
+    std::vector<std::string> source_names = get_parameter("observation_sources").as_string_array();
+    for (std::string source_name : source_names) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, source_name + ".type",
+        rclcpp::ParameterValue("scan"));  // Laser scanner by default
+      const std::string source_type = get_parameter(source_name + ".type").as_string();
+
+      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);
+
+        s->configure();
+
+        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);
+
+        p->configure();
+
+        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);
+
+        r->configure();
+
+        sources_.push_back(r);
+      } 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 CollisionMonitor::process(const Velocity & cmd_vel_in)
+{
+  // Current timestamp for all inner routines prolongation
+  rclcpp::Time curr_time = this->now();
+
+  // Do nothing if main worker in non-active state
+  if (!process_active_) {
+    return;
+  }
+
+  // Points array collected from different data sources in a robot base frame
+  std::vector<Point> collision_points;
+
+  // Fill collision_points array from different data sources
+  for (std::shared_ptr<Source> source : sources_) {
+    if (source->getEnabled()) {
+      source->getData(curr_time, collision_points);
+    }
+  }
+
+  // By default - there is no action
+  Action robot_action{DO_NOTHING, cmd_vel_in};
+  // Polygon causing robot action (if any)
+  std::shared_ptr<Polygon> action_polygon;
+
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (!polygon->getEnabled()) {
+      continue;
+    }
+    if (robot_action.action_type == STOP) {
+      // If robot already should stop, do nothing
+      break;
+    }
+
+    const ActionType at = polygon->getActionType();
+    if (at == STOP || at == SLOWDOWN) {
+      // Process STOP/SLOWDOWN for the selected polygon
+      if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) {
+        action_polygon = polygon;
+      }
+    } else if (at == APPROACH) {
+      // Process APPROACH for the selected polygon
+      if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) {
+        action_polygon = polygon;
+      }
+    }
+  }
+
+  if (robot_action.action_type != robot_action_prev_.action_type) {
+    // Report changed robot behavior
+    printAction(robot_action, action_polygon);
+  }
+
+  // Publish required robot velocity
+  publishVelocity(robot_action);
+
+  // Publish polygons for better visualization
+  publishPolygons();
+
+  robot_action_prev_ = robot_action;
+}
+
+bool CollisionMonitor::processStopSlowdown(
+  const std::shared_ptr<Polygon> polygon,
+  const std::vector<Point> & collision_points,
+  const Velocity & velocity,
+  Action & robot_action) const
+{
+  if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
+    if (polygon->getActionType() == STOP) {
+      // Setting up zero velocity for STOP model
+      robot_action.action_type = STOP;
+      robot_action.req_vel.x = 0.0;
+      robot_action.req_vel.y = 0.0;
+      robot_action.req_vel.tw = 0.0;
+      return true;
+    } else {  // SLOWDOWN
+      const Velocity safe_vel = velocity * polygon->getSlowdownRatio();
+      // Check that currently calculated velocity is safer than
+      // chosen for previous shapes one
+      if (safe_vel < robot_action.req_vel) {
+        robot_action.action_type = SLOWDOWN;
+        robot_action.req_vel = safe_vel;
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+bool CollisionMonitor::processApproach(
+  const std::shared_ptr<Polygon> polygon,
+  const std::vector<Point> & collision_points,
+  const Velocity & velocity,
+  Action & robot_action) const
+{
+  polygon->updatePolygon();
+
+  // Obtain time before a collision
+  const double collision_time = polygon->getCollisionTime(collision_points, velocity);
+  if (collision_time >= 0.0) {
+    // If collision will occurr, reduce robot speed
+    const double change_ratio = collision_time / polygon->getTimeBeforeCollision();
+    const Velocity safe_vel = velocity * change_ratio;
+    // Check that currently calculated velocity is safer than
+    // chosen for previous shapes one
+    if (safe_vel < robot_action.req_vel) {
+      robot_action.action_type = APPROACH;
+      robot_action.req_vel = safe_vel;
+      return true;
+    }
+  }
+
+  return false;
+}
+
+void CollisionMonitor::printAction(
+  const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
+{
+  if (robot_action.action_type == STOP) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to stop due to %s polygon",
+      action_polygon->getName().c_str());
+  } else if (robot_action.action_type == SLOWDOWN) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to slowdown for %f percents due to %s polygon",
+      action_polygon->getSlowdownRatio() * 100,
+      action_polygon->getName().c_str());
+  } else if (robot_action.action_type == APPROACH) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to approach for %f seconds away from collision",
+      action_polygon->getTimeBeforeCollision());
+  } else {  // robot_action.action_type == DO_NOTHING
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to continue normal operation");
+  }
+}
+
+void CollisionMonitor::publishPolygons() const
+{
+  for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (polygon->getEnabled()) {
+      polygon->publish();
+    }
+  }
+}
+
+}  // 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::CollisionMonitor)

navigation2/nav2_collision_monitor/src/kinematics.cpp

@@ -0,0 +1,70 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/kinematics.hpp"
+
+#include <cmath>
+
+namespace nav2_collision_monitor
+{
+
+void transformPoints(const Pose & pose, std::vector<Point> & points)
+{
+  const double cos_theta = std::cos(pose.theta);
+  const double sin_theta = std::sin(pose.theta);
+
+  for (Point & point : points) {
+    // p = R*p' + pose
+    // p' = Rt * (p - pose)
+    // where:
+    //   p - point coordinates in initial frame
+    //   p' - point coordinates in a new frame
+    //   R - rotation matrix =
+    //     [cos_theta -sin_theta]
+    //     [sin_theta  cos_theta]
+    //   Rt - transposed (inverted) rotation matrix
+    const double mul_x = point.x - pose.x;
+    const double mul_y = point.y - pose.y;
+    point.x = mul_x * cos_theta + mul_y * sin_theta;
+    point.y = -mul_x * sin_theta + mul_y * cos_theta;
+  }
+}
+
+void projectState(const double & dt, Pose & pose, Velocity & velocity)
+{
+  const double theta = velocity.tw * dt;
+  const double cos_theta = std::cos(theta);
+  const double sin_theta = std::sin(theta);
+
+  // p' = p + vel*dt
+  // where:
+  //   p - initial pose
+  //   p' - projected pose
+  pose.x = pose.x + velocity.x * dt;
+  pose.y = pose.y + velocity.y * dt;
+  // Rotate the pose on theta
+  pose.theta = pose.theta + theta;
+
+  // vel' = R*vel
+  // where:
+  //   vel - initial velocity
+  //   R - rotation matrix
+  //   vel' - rotated velocity
+  const double velocity_upd_x = velocity.x * cos_theta - velocity.y * sin_theta;
+  const double velocity_upd_y = velocity.x * sin_theta + velocity.y * cos_theta;
+  velocity.x = velocity_upd_x;
+  velocity.y = velocity_upd_y;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/main.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "nav2_collision_monitor/collision_monitor_node.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_collision_monitor::CollisionMonitor>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_collision_monitor/src/pointcloud.cpp

@@ -0,0 +1,144 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/pointcloud.hpp"
+
+#include <functional>
+
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_collision_monitor
+{
+
+PointCloud::PointCloud(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & source_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const std::string & global_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
+: Source(
+    node, source_name, tf_buffer, base_frame_id, global_frame_id,
+    transform_tolerance, source_timeout, base_shift_correction),
+  data_(nullptr)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str());
+}
+
+PointCloud::~PointCloud()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str());
+  data_sub_.reset();
+}
+
+void PointCloud::configure()
+{
+  Source::configure();
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  std::string source_topic;
+
+  getParameters(source_topic);
+
+  rclcpp::QoS pointcloud_qos = rclcpp::SensorDataQoS();  // set to default
+  data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
+    source_topic, pointcloud_qos,
+    std::bind(&PointCloud::dataCallback, this, std::placeholders::_1));
+}
+
+void PointCloud::getData(
+  const rclcpp::Time & curr_time,
+  std::vector<Point> & data) const
+{
+  // Ignore data from the source if it is not being published yet or
+  // not published for a long time
+  if (data_ == nullptr) {
+    return;
+  }
+  if (!sourceValid(data_->header.stamp, curr_time)) {
+    return;
+  }
+
+  tf2::Transform tf_transform;
+  if (base_shift_correction_) {
+    // Obtaining the transform to get data from source frame and time where it was received
+    // to the base frame and current time
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, data_->header.stamp,
+        base_frame_id_, curr_time, global_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  } else {
+    // Obtaining the transform to get data from source frame to base frame without time shift
+    // considered. Less accurate but much more faster option not dependent on state estimation
+    // frames.
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, base_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  }
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*data_, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*data_, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*data_, "z");
+
+  // Refill data array with PointCloud points in base frame
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    // Transform point coordinates from source frame -> to base frame
+    tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z);
+    tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+    // Refill data array
+    if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) {
+      data.push_back({p_v3_b.x(), p_v3_b.y()});
+    }
+  }
+}
+
+void PointCloud::getParameters(std::string & source_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  getCommonParameters(source_topic);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".min_height", rclcpp::ParameterValue(0.05));
+  min_height_ = node->get_parameter(source_name_ + ".min_height").as_double();
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5));
+  max_height_ = node->get_parameter(source_name_ + ".max_height").as_double();
+}
+
+void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+  data_ = msg;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/polygon.cpp

@@ -0,0 +1,419 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/polygon.hpp"
+
+#include <exception>
+#include <utility>
+
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/point32.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+#include "nav2_collision_monitor/kinematics.hpp"
+
+namespace nav2_collision_monitor
+{
+
+Polygon::Polygon(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & polygon_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const tf2::Duration & transform_tolerance)
+: node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
+  slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer),
+  base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
+}
+
+Polygon::~Polygon()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str());
+  poly_.clear();
+  dyn_params_handler_.reset();
+}
+
+bool Polygon::configure()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  std::string polygon_pub_topic, footprint_topic;
+
+  if (!getParameters(polygon_pub_topic, footprint_topic)) {
+    return false;
+  }
+
+  if (!footprint_topic.empty()) {
+    footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
+      node, footprint_topic, *tf_buffer_,
+      base_frame_id_, tf2::durationToSec(transform_tolerance_));
+  }
+
+  if (visualize_) {
+    // Fill polygon_ points for future usage
+    std::vector<Point> poly;
+    getPolygon(poly);
+    for (const Point & p : poly) {
+      geometry_msgs::msg::Point32 p_s;
+      p_s.x = p.x;
+      p_s.y = p.y;
+      // p_s.z will remain 0.0
+      polygon_.points.push_back(p_s);
+    }
+
+    rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
+    polygon_pub_ = node->create_publisher<geometry_msgs::msg::PolygonStamped>(
+      polygon_pub_topic, polygon_qos);
+  }
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1));
+  
+  return true;
+}
+
+void Polygon::activate()
+{
+  if (visualize_) {
+    polygon_pub_->on_activate();
+  }
+}
+
+void Polygon::deactivate()
+{
+  if (visualize_) {
+    polygon_pub_->on_deactivate();
+  }
+}
+
+std::string Polygon::getName() const
+{
+  return polygon_name_;
+}
+
+ActionType Polygon::getActionType() const
+{
+  return action_type_;
+}
+
+bool Polygon::getEnabled() const
+{
+  return enabled_;
+}
+
+int Polygon::getMaxPoints() const
+{
+  return max_points_;
+}
+
+double Polygon::getSlowdownRatio() const
+{
+  return slowdown_ratio_;
+}
+
+double Polygon::getTimeBeforeCollision() const
+{
+  return time_before_collision_;
+}
+
+void Polygon::getPolygon(std::vector<Point> & poly) const
+{
+  poly = poly_;
+}
+
+void Polygon::updatePolygon()
+{
+  if (footprint_sub_ != nullptr) {
+    // Get latest robot footprint from footprint subscriber
+    std::vector<geometry_msgs::msg::Point> footprint_vec;
+    std_msgs::msg::Header footprint_header;
+    footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header);
+
+    std::size_t new_size = footprint_vec.size();
+    poly_.resize(new_size);
+    polygon_.points.resize(new_size);
+
+    geometry_msgs::msg::Point32 p_s;
+    for (std::size_t i = 0; i < new_size; i++) {
+      poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
+      p_s.x = footprint_vec[i].x;
+      p_s.y = footprint_vec[i].y;
+      polygon_.points[i] = p_s;
+    }
+  }
+}
+
+int Polygon::getPointsInside(const std::vector<Point> & points) const
+{
+  int num = 0;
+  for (const Point & point : points) {
+    if (isPointInside(point)) {
+      num++;
+    }
+  }
+  return num;
+}
+
+double Polygon::getCollisionTime(
+  const std::vector<Point> & collision_points,
+  const Velocity & velocity) const
+{
+  // Initial robot pose is {0,0} in base_footprint coordinates
+  Pose pose = {0.0, 0.0, 0.0};
+  Velocity vel = velocity;
+
+  // Array of points transformed to the frame concerned with pose on each simulation step
+  std::vector<Point> points_transformed = collision_points;
+
+  // Check static polygon
+  if (getPointsInside(points_transformed) >= max_points_) {
+    return 0.0;
+  }
+
+  // Robot movement simulation
+  for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
+    // Shift the robot pose towards to the vel during simulation_time_step_ time interval
+    // NOTE: vel is changing during the simulation
+    projectState(simulation_time_step_, pose, vel);
+    // Transform collision_points to the frame concerned with current robot pose
+    points_transformed = collision_points;
+    transformPoints(pose, points_transformed);
+    // If the collision occurred on this stage, return the actual time before a collision
+    // as if robot was moved with given velocity
+    if (getPointsInside(points_transformed) > max_points_) {
+      return time;
+    }
+  }
+
+  // There is no collision
+  return -1.0;
+}
+
+void Polygon::publish() const
+{
+  if (!visualize_) {
+    return;
+  }
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Fill PolygonStamped struct
+  std::unique_ptr<geometry_msgs::msg::PolygonStamped> poly_s =
+    std::make_unique<geometry_msgs::msg::PolygonStamped>();
+  poly_s->header.stamp = node->now();
+  poly_s->header.frame_id = base_frame_id_;
+  poly_s->polygon = polygon_;
+
+  // Publish polygon
+  polygon_pub_->publish(std::move(poly_s));
+}
+
+bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  try {
+    // Get action type.
+    // Leave it not initialized: the will cause an error if it will not set.
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".action_type", rclcpp::PARAMETER_STRING);
+    const std::string at_str =
+      node->get_parameter(polygon_name_ + ".action_type").as_string();
+    if (at_str == "stop") {
+      action_type_ = STOP;
+    } else if (at_str == "slowdown") {
+      action_type_ = SLOWDOWN;
+    } else if (at_str == "approach") {
+      action_type_ = APPROACH;
+    } else {  // Error if something else
+      RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str());
+      return false;
+    }
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true));
+    enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool();
+    
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3));
+    max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int();
+
+    if (action_type_ == SLOWDOWN) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".slowdown_ratio", rclcpp::ParameterValue(0.5));
+      slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double();
+    }
+
+    if (action_type_ == APPROACH) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0));
+      time_before_collision_ =
+        node->get_parameter(polygon_name_ + ".time_before_collision").as_double();
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".simulation_time_step", rclcpp::ParameterValue(0.1));
+      simulation_time_step_ =
+        node->get_parameter(polygon_name_ + ".simulation_time_step").as_double();
+    }
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".visualize", rclcpp::ParameterValue(false));
+    visualize_ = node->get_parameter(polygon_name_ + ".visualize").as_bool();
+    if (visualize_) {
+      // Get polygon topic parameter in case if it is going to be published
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_));
+      polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
+    }
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Error while getting common polygon parameters: %s",
+      polygon_name_.c_str(), ex.what());
+    return false;
+  }
+
+  return true;
+}
+
+bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!getCommonParameters(polygon_pub_topic)) {
+    return false;
+  }
+
+  try {
+    if (action_type_ == APPROACH) {
+      // Obtain the footprint topic to make a footprint subscription for approach polygon
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".footprint_topic",
+        rclcpp::ParameterValue("local_costmap/published_footprint"));
+      footprint_topic =
+        node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+
+      // This is robot footprint: do not need to get polygon points from ROS parameters.
+      // It will be set dynamically later.
+      return true;
+    } else {
+      // Make it empty otherwise
+      footprint_topic.clear();
+    }
+
+    // Leave it not initialized: the will cause an error if it will not set
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);
+    std::vector<double> poly_row =
+      node->get_parameter(polygon_name_ + ".points").as_double_array();
+    // Check for points format correctness
+    if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) {
+      RCLCPP_ERROR(
+        logger_,
+        "[%s]: Polygon has incorrect points description",
+        polygon_name_.c_str());
+      return false;
+    }
+
+    // Obtain polygon vertices
+    Point point;
+    bool first = true;
+    for (double val : poly_row) {
+      if (first) {
+        point.x = val;
+      } else {
+        point.y = val;
+        poly_.push_back(point);
+      }
+      first = !first;
+    }
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Error while getting polygon parameters: %s",
+      polygon_name_.c_str(), ex.what());
+    return false;
+  }
+
+  return true;
+}
+
+rcl_interfaces::msg::SetParametersResult
+Polygon::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      if (param_name == polygon_name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+inline bool Polygon::isPointInside(const Point & point) const
+{
+  // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
+  // Communications of the ACM 5.8 (1962): 434.
+  // Implementation of ray crossings algorithm for point in polygon task solving.
+  // Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
+  // Odd number of intersections with polygon boundaries means the point is inside polygon.
+  const int poly_size = poly_.size();
+  int i, j;  // Polygon vertex iterators
+  bool res = false;  // Final result, initialized with already inverted value
+
+  // Starting from the edge where the last point of polygon is connected to the first
+  i = poly_size - 1;
+  for (j = 0; j < poly_size; j++) {
+    // Checking the edge only if given point is between edge boundaries by Y coordinates.
+    // One of the condition should contain equality in order to exclude the edges
+    // parallel to X+ ray.
+    if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) {
+      // Calculating the intersection coordinate of X+ ray
+      const double x_inter = poly_[i].x +
+        (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) /
+        (poly_[j].y - poly_[i].y);
+      // If intersection with checked edge is greater than point.x coordinate, inverting the result
+      if (x_inter > point.x) {
+        res = !res;
+      }
+    }
+    i = j;
+  }
+  return res;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/range.cpp

@@ -0,0 +1,165 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/range.hpp"
+
+#include <math.h>
+#include <cmath>
+#include <functional>
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_collision_monitor
+{
+
+Range::Range(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & source_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const std::string & global_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
+: Source(
+    node, source_name, tf_buffer, base_frame_id, global_frame_id,
+    transform_tolerance, source_timeout, base_shift_correction),
+  data_(nullptr)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str());
+}
+
+Range::~Range()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying Range", source_name_.c_str());
+  data_sub_.reset();
+}
+
+void Range::configure()
+{
+  Source::configure();
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  std::string source_topic;
+
+  getParameters(source_topic);
+
+  rclcpp::QoS range_qos = rclcpp::SensorDataQoS();  // set to default
+  data_sub_ = node->create_subscription<sensor_msgs::msg::Range>(
+    source_topic, range_qos,
+    std::bind(&Range::dataCallback, this, std::placeholders::_1));
+}
+
+void Range::getData(
+  const rclcpp::Time & curr_time,
+  std::vector<Point> & data) const
+{
+  // Ignore data from the source if it is not being published yet or
+  // not being published for a long time
+  if (data_ == nullptr) {
+    return;
+  }
+  if (!sourceValid(data_->header.stamp, curr_time)) {
+    return;
+  }
+
+  // Ignore data, if its range is out of scope of range sensor abilities
+  if (data_->range < data_->min_range || data_->range > data_->max_range) {
+    RCLCPP_DEBUG(
+      logger_,
+      "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...",
+      source_name_.c_str(), data_->range, data_->min_range, data_->max_range);
+    return;
+  }
+
+  tf2::Transform tf_transform;
+  if (base_shift_correction_) {
+    // Obtaining the transform to get data from source frame and time where it was received
+    // to the base frame and current time
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, data_->header.stamp,
+        base_frame_id_, curr_time, global_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  } else {
+    // Obtaining the transform to get data from source frame to base frame without time shift
+    // considered. Less accurate but much more faster option not dependent on state estimation
+    // frames.
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, base_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  }
+
+  // Calculate poses and refill data array
+  float angle;
+  for (
+    angle = -data_->field_of_view / 2;
+    angle < data_->field_of_view / 2;
+    angle += obstacles_angle_)
+  {
+    // Transform point coordinates from source frame -> to base frame
+    tf2::Vector3 p_v3_s(
+      data_->range * std::cos(angle),
+      data_->range * std::sin(angle),
+      0.0);
+    tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+    // Refill data array
+    data.push_back({p_v3_b.x(), p_v3_b.y()});
+  }
+
+  // Make sure that last (field_of_view / 2) point will be in the data array
+  angle = data_->field_of_view / 2;
+
+  // Transform point coordinates from source frame -> to base frame
+  tf2::Vector3 p_v3_s(
+    data_->range * std::cos(angle),
+    data_->range * std::sin(angle),
+    0.0);
+  tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+  // Refill data array
+  data.push_back({p_v3_b.x(), p_v3_b.y()});
+}
+
+void Range::getParameters(std::string & source_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  getCommonParameters(source_topic);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180));
+  obstacles_angle_ = node->get_parameter(source_name_ + ".obstacles_angle").as_double();
+}
+
+void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg)
+{
+  data_ = msg;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/scan.cpp

@@ -0,0 +1,126 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/scan.hpp"
+
+#include <cmath>
+#include <functional>
+
+namespace nav2_collision_monitor
+{
+
+Scan::Scan(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & source_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const std::string & global_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
+: Source(
+    node, source_name, tf_buffer, base_frame_id, global_frame_id,
+    transform_tolerance, source_timeout, base_shift_correction),
+  data_(nullptr)
+{
+  RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str());
+}
+
+Scan::~Scan()
+{
+  RCLCPP_INFO(logger_, "[%s]: Destroying Scan", source_name_.c_str());
+  data_sub_.reset();
+}
+
+void Scan::configure()
+{
+  Source::configure();
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  std::string source_topic;
+
+  // Laser scanner has no own parameters
+  getCommonParameters(source_topic);
+
+  rclcpp::QoS scan_qos = rclcpp::SensorDataQoS();  // set to default
+  data_sub_ = node->create_subscription<sensor_msgs::msg::LaserScan>(
+    source_topic, scan_qos,
+    std::bind(&Scan::dataCallback, this, std::placeholders::_1));
+}
+
+void Scan::getData(
+  const rclcpp::Time & curr_time,
+  std::vector<Point> & data) const
+{
+  // Ignore data from the source if it is not being published yet or
+  // not being published for a long time
+  if (data_ == nullptr) {
+    return;
+  }
+  if (!sourceValid(data_->header.stamp, curr_time)) {
+    return;
+  }
+
+  tf2::Transform tf_transform;
+  if (base_shift_correction_) {
+    // Obtaining the transform to get data from source frame and time where it was received
+    // to the base frame and current time
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, data_->header.stamp,
+        base_frame_id_, curr_time, global_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  } else {
+    // Obtaining the transform to get data from source frame to base frame without time shift
+    // considered. Less accurate but much more faster option not dependent on state estimation
+    // frames.
+    if (
+      !nav2_util::getTransform(
+        data_->header.frame_id, base_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+  }
+
+  // Calculate poses and refill data array
+  float angle = data_->angle_min;
+  for (size_t i = 0; i < data_->ranges.size(); i++) {
+    if (data_->ranges[i] >= data_->range_min && data_->ranges[i] <= data_->range_max) {
+      // Transform point coordinates from source frame -> to base frame
+      tf2::Vector3 p_v3_s(
+        data_->ranges[i] * std::cos(angle),
+        data_->ranges[i] * std::sin(angle),
+        0.0);
+      tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+      // Refill data array
+      data.push_back({p_v3_b.x(), p_v3_b.y()});
+    }
+    angle += data_->angle_increment;
+  }
+}
+
+void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
+{
+  data_ = msg;
+}
+
+}  // namespace nav2_collision_monitor

navigation2/nav2_collision_monitor/src/source.cpp

@@ -0,0 +1,121 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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/source.hpp"
+
+#include <exception>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_collision_monitor
+{
+
+Source::Source(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & source_name,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const std::string & base_frame_id,
+  const std::string & global_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const rclcpp::Duration & source_timeout,
+  const bool base_shift_correction)
+: node_(node), source_name_(source_name), tf_buffer_(tf_buffer),
+  base_frame_id_(base_frame_id), global_frame_id_(global_frame_id),
+  transform_tolerance_(transform_tolerance), source_timeout_(source_timeout),
+  base_shift_correction_(base_shift_correction)
+{
+}
+
+Source::~Source()
+{
+}
+
+bool Source::configure()
+{
+  auto node = node_.lock();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1));
+
+  return true;
+}
+
+void Source::getCommonParameters(std::string & source_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".topic",
+    rclcpp::ParameterValue("scan"));  // Set deafult topic for laser scanner
+  source_topic = node->get_parameter(source_name_ + ".topic").as_string();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".enabled", rclcpp::ParameterValue(true));
+  enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool();
+}
+
+bool Source::sourceValid(
+  const rclcpp::Time & source_time,
+  const rclcpp::Time & curr_time) const
+{
+  // Source is considered as not valid, if latest received data timestamp is earlier
+  // than current time by source_timeout_ interval
+  const rclcpp::Duration dt = curr_time - source_time;
+  if (dt > source_timeout_) {
+    RCLCPP_WARN(
+      logger_,
+      "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. "
+      "Ignoring the source.",
+      source_name_.c_str(), dt.seconds());
+    return false;
+  }
+
+  return true;
+}
+
+bool Source::getEnabled() const
+{
+  return enabled_;
+}
+
+rcl_interfaces::msg::SetParametersResult
+Source::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      if (param_name == source_name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_collision_monitor