Add the support of range sensors to Collision Monitor (#3099)

* Support range sensors in Collision Monitor

* Adjust README.md

* Meet review fixes

nav2_collision_monitor/CMakeLists.txt

@@ -47,6 +47,7 @@ add_library(${library_name} SHARED
   src/source.cpp
   src/scan.cpp
   src/pointcloud.cpp
+  src/range.cpp
   src/kinematics.cpp
 )
 

nav2_collision_monitor/README.md

@@ -35,7 +35,7 @@ The data may be obtained from different data sources:
 
 * Laser scanners (`sensor_msgs::msg::LaserScan` messages)
 * PointClouds (`sensor_msgs::msg::PointCloud2` messages)
-* IR/Sonars (`sensor_msgs::msg::Range` messages, not implemented yet)
+* IR/Sonars (`sensor_msgs::msg::Range` messages)
 
 ## Design
 

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.
@@ -34,6 +34,7 @@
 #include "nav2_collision_monitor/source.hpp"
 #include "nav2_collision_monitor/scan.hpp"
 #include "nav2_collision_monitor/pointcloud.hpp"
+#include "nav2_collision_monitor/range.hpp"
 
 namespace nav2_collision_monitor
 {

nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.
@@ -35,7 +35,7 @@ class PointCloud : public Source
   /**
    * @brief PointCloud constructor
    * @param node Collision Monitor node pointer
-   * @param polygon_name Name of data source
+   * @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

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/include/nav2_collision_monitor/range.hpp

@@ -0,0 +1,97 @@
+// 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.
+
+#ifndef NAV2_COLLISION_MONITOR__RANGE_HPP_
+#define NAV2_COLLISION_MONITOR__RANGE_HPP_
+
+#include "sensor_msgs/msg/range.hpp"
+
+#include "nav2_collision_monitor/source.hpp"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Implementation for IR/ultrasound range sensor source
+ */
+class Range : public Source
+{
+public:
+  /**
+   * @brief Range 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
+   */
+  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);
+  /**
+   * @brief Range destructor
+   */
+  ~Range();
+
+  /**
+   * @brief Data source configuration routine. Obtains ROS-parameters
+   * and creates range sensor subscriber.
+   */
+  void configure();
+
+  /**
+   * @brief Adds latest data from range sensor 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.
+   */
+  void getData(
+    const rclcpp::Time & curr_time,
+    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 Range sensor data callback
+   * @param msg Shared pointer to Range sensor message
+   */
+  void dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg);
+
+  // ----- Variables -----
+
+  /// @brief Range sensor data subscriber
+  rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr data_sub_;
+
+  /// @brief Angle increment (in rad) between two obstacle points at the range arc
+  double obstacles_angle_;
+
+  /// @brief Latest data obtained from range sensor
+  sensor_msgs::msg::Range::ConstSharedPtr data_;
+};  // class Range
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__RANGE_HPP_

nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.
@@ -35,7 +35,7 @@ class Scan : public Source
   /**
    * @brief Scan constructor
    * @param node Collision Monitor node pointer
-   * @param polygon_name Name of data source
+   * @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

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.
@@ -40,7 +40,7 @@ class Source
   /**
    * @brief Source constructor
    * @param node Collision Monitor node pointer
-   * @param polygon_name Name of data source
+   * @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

nav2_collision_monitor/include/nav2_collision_monitor/types.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/launch/collision_monitor_node.launch.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright (c) 2022 Samsung Research Russia
+# 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.

nav2_collision_monitor/src/circle.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.
@@ -302,6 +302,14 @@ bool CollisionMonitor::configureSources(
         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);
+
+        r->configure();
+
+        sources_.push_back(r);
       } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),

nav2_collision_monitor/src/kinematics.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/src/main.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/src/pointcloud.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.

nav2_collision_monitor/src/polygon.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Samsung Research Russia
+// 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.