Collision monitor: Add polygon source (ros-navigation#4275)

* Rename PushRosNamespace to PushROSNamespace

* Fix min_points checking

* Add polygon source

* Revert "Merge branch 'add-collision-points-debug' into add-polygon-source"

This reverts commit 15c261c, reversing
changes made to 5a63584.

* .

* fix

* fix

* fix lint

* PR comments fixes

* fixes

* add test

* fix space

* use standard msg Polygonstamped

* draft

* .

* fixes

* fixes

* fixes

* fix

* revert

* fixes

* add detector test

* rebasing fixes

* Adding polygon source

* adjusting tests

* linting

---------

Co-authored-by: Tony Najjar <tony.najjar@logivations.com>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

Author: 3 people

nav2_collision_monitor/CMakeLists.txt

@@ -56,6 +56,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
 )
@@ -67,6 +68,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

@@ -38,6 +38,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

@@ -41,6 +41,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/pointcloud.hpp

@@ -73,7 +73,7 @@ class PointCloud : public Source
    */
   bool getData(
     const rclcpp::Time & curr_time,
-    std::vector<Point> & data) const;
+    std::vector<Point> & data);
 
 protected:
   /**

nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp

@@ -0,0 +1,115 @@
+// 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_instance_stamped.hpp"
+#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);
+
+  /**
+   * @brief Converts a PolygonInstanceStamped 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::PolygonInstanceStamped::ConstSharedPtr msg);
+
+  // ----- Variables -----
+
+  /// @brief PolygonSource data subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PolygonInstanceStamped>::SharedPtr data_sub_;
+
+  /// @brief Latest data obtained
+  std::vector<geometry_msgs::msg::PolygonInstanceStamped> data_;
+
+  /// @brief distance between sampled points on polygon edges
+  double sampling_distance_;
+};  // class PolygonSource
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_

nav2_collision_monitor/include/nav2_collision_monitor/range.hpp

@@ -73,7 +73,7 @@ class Range : public Source
    */
   bool getData(
     const rclcpp::Time & curr_time,
-    std::vector<Point> & data) const;
+    std::vector<Point> & data);
 
 protected:
   /**

nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp

@@ -73,7 +73,7 @@ class Scan : public Source
    */
   bool getData(
     const rclcpp::Time & curr_time,
-    std::vector<Point> & data) const;
+    std::vector<Point> & data);
 
 protected:
   /**

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -73,7 +73,7 @@ class Source
    */
   virtual bool getData(
     const rclcpp::Time & curr_time,
-    std::vector<Point> & data) const = 0;
+    std::vector<Point> & data) = 0;
 
   /**
    * @brief Obtains source enabled state

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -287,6 +287,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(),
@@ -345,6 +352,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

@@ -378,6 +378,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(),
@@ -444,6 +451,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
     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;