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

* Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes
ros-navigation · Aug 26, 2022 · b3f60f5 · b3f60f5
1 parent a7d9c9b
commit b3f60f5
Show file tree

Hide file tree

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

diff --git a/nav2_collision_monitor/README.md b/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
 

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

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
@@ -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
 {

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

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

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

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

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

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

diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/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.

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

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

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

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

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

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