add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -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