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 <[email protected]> Co-authored-by: Tony Najjar <[email protected]>
aosmw · Apr 29, 2024 · c93080c · c93080c
1 parent 9f6b0dd
commit c93080c
Show file tree

Hide file tree

Showing 20 changed files with 670 additions and 11 deletions.
diff --git a/nav2_collision_monitor/CMakeLists.txt b/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
 )

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/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
 {

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/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
 {

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/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:
   /**

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp b/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_
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/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:
   /**

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/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:
   /**

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/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

diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/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;

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/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;

diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/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"
@@ -67,7 +68,7 @@ void PointCloud::configure()
 
 bool PointCloud::getData(
   const rclcpp::Time & curr_time,
-  std::vector<Point> & data) const
+  std::vector<Point> & data)
 {
   // Ignore data from the source if it is not being published yet or
   // not published for a long time

diff --git a/nav2_collision_monitor/src/polygon.cpp b/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"
@@ -195,7 +196,7 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/)
     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_,
@@ -478,7 +479,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_,