CM/CD: Add polygon source

nav2_collision_monitor/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(${monitor_library_name} SHARED
   src/source.cpp
   src/scan.cpp
   src/pointcloud.cpp
+  src/polygon_source.cpp
   src/range.cpp
   src/kinematics.cpp
 )
@@ -63,6 +64,7 @@ add_library(${detector_library_name} SHARED
   src/source.cpp
   src/scan.cpp
   src/pointcloud.cpp
+  src/polygon_source.cpp
   src/range.cpp
   src/kinematics.cpp
 )

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -37,6 +37,7 @@
 #include "nav2_collision_monitor/scan.hpp"
 #include "nav2_collision_monitor/pointcloud.hpp"
 #include "nav2_collision_monitor/range.hpp"
+#include "nav2_collision_monitor/polygon_source.hpp"
 
 namespace nav2_collision_monitor
 {

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -37,6 +37,7 @@
 #include "nav2_collision_monitor/scan.hpp"
 #include "nav2_collision_monitor/pointcloud.hpp"
 #include "nav2_collision_monitor/range.hpp"
+#include "nav2_collision_monitor/polygon_source.hpp"
 
 namespace nav2_collision_monitor
 {

nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp

@@ -0,0 +1,127 @@
+// 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.
+
+#ifndef NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_
+#define NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_
+
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "geometry_msgs/msg/polygon_stamped.hpp"
+
+#include "nav2_collision_monitor/source.hpp"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Implementation for polygon source
+ */
+class PolygonSource : public Source
+{
+public:
+  /**
+   * @brief PolygonSource constructor
+   * @param node Collision Monitor node pointer
+   * @param source_name Name of data source
+   * @param tf_buffer Shared pointer to a TF buffer
+   * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame.
+   * @param global_frame_id Global frame ID for correct transform calculation
+   * @param transform_tolerance Transform tolerance
+   * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
+   */
+  PolygonSource(
+    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);
+  /**
+   * @brief PolygonSource destructor
+   */
+  ~PolygonSource();
+
+  /**
+   * @brief Data source configuration routine. Obtains ROS-parameters
+   * and creates subscriber.
+   */
+  void configure();
+
+  /**
+   * @brief Adds latest data from polygon source to the data array.
+   * @param curr_time Current node time for data interpolation
+   * @param data Array where the data from source to be added.
+   * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
+   */
+  bool getData(
+    const rclcpp::Time & curr_time,
+    std::vector<Point> & data) const;
+
+  /**
+   * @brief Converts a PolygonStamped to a std::vector<Point>
+   * @param polygon Input Polygon to be converted
+   * @param data Output vector of Point
+   */
+  void convertPolygonStampedToPoints(
+    const geometry_msgs::msg::PolygonStamped & polygon,
+    std::vector<Point> & data) const;
+
+protected:
+  /**
+   * @brief Getting sensor-specific ROS-parameters
+   * @param source_topic Output name of source subscription topic
+   */
+  void getParameters(std::string & source_topic);
+
+  /**
+   * @brief PolygonSource data callback
+   * @param msg Shared pointer to PolygonSource message
+   */
+  void dataCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
+
+  /**
+   * @brief Checks if two polygons are similar
+   * @param polygon1 First polygon
+   * @param polygon2 Second polygon
+   * @return True if polygons are similar, false otherwise
+   */
+  bool arePolygonsSimilar(
+    const geometry_msgs::msg::Polygon & polygon1,
+    const geometry_msgs::msg::Polygon & polygon2) const;
+
+  // ----- Variables -----
+
+  /// @brief PolygonSource data subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr data_sub_;
+
+  /// @brief Latest data obtained
+  std::vector<geometry_msgs::msg::PolygonStamped> data_;
+
+  /// @brief distance between sampled points on polygon edges
+  double sampling_distance_;
+
+  /// @brief maximum distance between points of polygons to be considered similar
+  double polygon_similarity_threshold_;
+};  // class PolygonSource
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -283,6 +283,13 @@ bool CollisionDetector::configureSources(
         r->configure();
 
         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);
+        ps->configure();
+
+        sources_.push_back(ps);
       } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),
@@ -341,6 +348,7 @@ void CollisionDetector::process()
     marker.color.r = 1.0;
     marker.color.a = 1.0;
     marker.lifetime = rclcpp::Duration(0, 0);
+    marker.frame_locked = true;
 
     for (const auto & point : collision_points) {
       geometry_msgs::msg::Point p;

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -349,6 +349,13 @@ bool CollisionMonitor::configureSources(
         r->configure();
 
         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);
+        ps->configure();
+
+        sources_.push_back(ps);
       } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),
@@ -415,6 +422,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     marker.color.r = 1.0;
     marker.color.a = 1.0;
     marker.lifetime = rclcpp::Duration(0, 0);
+    marker.frame_locked = true;
 
     for (const auto & point : collision_points) {
       geometry_msgs::msg::Point p;

nav2_collision_monitor/src/pointcloud.cpp

@@ -17,6 +17,7 @@
 #include <functional>
 
 #include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "tf2/transform_datatypes.h"
 
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -78,7 +79,7 @@ bool PointCloud::getData(
     return false;
   }
 
-  tf2::Transform tf_transform;
+  tf2::Stamped<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

nav2_collision_monitor/src/polygon.cpp

@@ -19,6 +19,7 @@
 
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/point32.hpp"
+#include "tf2/transform_datatypes.h"
 
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -203,7 +204,7 @@ void Polygon::updatePolygon()
     std::size_t new_size = polygon_.polygon.points.size();
 
     // Get the transform from PolygonStamped frame to base_frame_id_
-    tf2::Transform tf_transform;
+    tf2::Stamped<tf2::Transform> tf_transform;
     if (
       !nav2_util::getTransform(
         polygon_.header.frame_id, base_frame_id_,
@@ -476,7 +477,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m
   }
 
   // Get the transform from PolygonStamped frame to base_frame_id_
-  tf2::Transform tf_transform;
+  tf2::Stamped<tf2::Transform> tf_transform;
   if (
     !nav2_util::getTransform(
       msg->header.frame_id, base_frame_id_,