FIXME Add collision_detector (ros-navigation#12)

Co-authored-by: Tony Najjar <[email protected]>

Author: RBT22

nav2_collision_monitor/CMakeLists.txt

@@ -37,10 +37,12 @@ set(dependencies
   nav2_costmap_2d
 )
 
-set(executable_name collision_monitor)
-set(library_name ${executable_name}_core)
+set(monitor_executable_name collision_monitor)
+set(detector_executable_name collision_detector)
+set(monitor_library_name ${monitor_executable_name}_core)
+set(detector_library_name ${detector_executable_name}_core)
 
-add_library(${library_name} SHARED
+add_library(${monitor_library_name} SHARED
   src/collision_monitor_node.cpp
   src/polygon.cpp
   src/circle.cpp
@@ -50,34 +52,59 @@ add_library(${library_name} SHARED
   src/range.cpp
   src/kinematics.cpp
 )
+add_library(${detector_library_name} SHARED
+  src/collision_detector_node.cpp
+  src/polygon.cpp
+  src/circle.cpp
+  src/source.cpp
+  src/scan.cpp
+  src/pointcloud.cpp
+  src/range.cpp
+  src/kinematics.cpp
+)
 
-add_executable(${executable_name}
-  src/main.cpp
+add_executable(${monitor_executable_name}
+  src/collision_monitor_main.cpp
+)
+add_executable(${detector_executable_name}
+  src/collision_detector_main.cpp
 )
 
-ament_target_dependencies(${library_name}
+ament_target_dependencies(${monitor_library_name}
+  ${dependencies}
+)
+ament_target_dependencies(${detector_library_name}
   ${dependencies}
 )
 
-target_link_libraries(${executable_name}
-  ${library_name}
+target_link_libraries(${monitor_executable_name}
+  ${monitor_library_name}
+)
+target_link_libraries(${detector_executable_name}
+  ${detector_library_name}
+)
+
+ament_target_dependencies(${monitor_executable_name}
+  ${dependencies}
 )
 
-ament_target_dependencies(${executable_name}
+ament_target_dependencies(${detector_executable_name}
   ${dependencies}
 )
 
-rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
+rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor")
+
+rclcpp_components_register_nodes(${detector_library_name} "nav2_collision_monitor::CollisionDetector")
 
 ### Install ###
 
-install(TARGETS ${library_name}
+install(TARGETS ${monitor_library_name} ${detector_library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-install(TARGETS ${executable_name}
+install(TARGETS ${monitor_executable_name} ${detector_executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -98,13 +125,12 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
 endif()
 
 ### Ament stuff ###
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(${monitor_library_name} ${detector_library_name})
 ament_export_dependencies(${dependencies})
 
 ament_package()

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+// 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__COLLISION_DETECTOR_NODE_HPP_
+#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tf2/time.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/msg/collision_detector_state.hpp"
+
+#include "nav2_collision_monitor/types.hpp"
+#include "nav2_collision_monitor/polygon.hpp"
+#include "nav2_collision_monitor/circle.hpp"
+#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
+{
+
+/**
+ * @brief Collision Monitor ROS2 node
+ */
+class CollisionDetector : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief Constructor for the nav2_collision_monitor::CollisionDetector
+   * @param options Additional options to control creation of the node.
+   */
+  explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief Destructor for the nav2_collision_monitor::CollisionDetector
+   */
+  ~CollisionDetector();
+
+protected:
+  /**
+   * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers,
+   * creates polygons and data sources objects
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Resets all subscribers/publishers, polygons/data sources arrays
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called in shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+protected:
+  /**
+   * @brief Supporting routine obtaining all ROS-parameters
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool getParameters();
+  /**
+   * @brief Supporting routine creating and configuring all polygons
+   * @param base_frame_id Robot base frame ID
+   * @param transform_tolerance Transform tolerance
+   * @return True if all polygons were configured successfully or false in failure case
+   */
+  bool configurePolygons(
+    const std::string & base_frame_id,
+    const tf2::Duration & transform_tolerance);
+  /**
+   * @brief Supporting routine creating and configuring all data sources
+   * @param base_frame_id Robot base frame ID
+   * @param odom_frame_id Odometry frame ID. Used as global frame to get
+   * source->base time interpolated transform.
+   * @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
+   * @return True if all sources were configured successfully or false in failure case
+   */
+  bool configureSources(
+    const std::string & base_frame_id,
+    const std::string & odom_frame_id,
+    const tf2::Duration & transform_tolerance,
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
+
+  /**
+   * @brief Main processing routine
+   */
+  void process();
+
+  /**
+   * @brief Polygons publishing routine. Made for visualization.
+   */
+  void publishPolygons() const;
+
+  // ----- Variables -----
+
+  /// @brief TF buffer
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  /// @brief TF listener
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  /// @brief Polygons array
+  std::vector<std::shared_ptr<Polygon>> polygons_;
+  /// @brief Data sources array
+  std::vector<std::shared_ptr<Source>> sources_;
+
+  /// @brief collision monitor state publisher
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
+    state_pub_;
+  /// @brief timer that runs actions
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  /// @brief main loop frequency
+  double frequency_;
+};  // class CollisionDetector
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -47,12 +47,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode
 {
 public:
   /**
-   * @brief Constructor for the nav2_collision_safery::CollisionMonitor
+   * @brief Constructor for the nav2_collision_monitor::CollisionMonitor
    * @param options Additional options to control creation of the node.
    */
   explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   /**
-   * @brief Destructor for the nav2_collision_safery::CollisionMonitor
+   * @brief Destructor for the nav2_collision_monitor::CollisionMonitor
    */
   ~CollisionMonitor();
 
@@ -125,7 +125,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @brief Supporting routine creating and configuring all data sources
    * @param base_frame_id Robot base frame ID
    * @param odom_frame_id Odometry frame ID. Used as global frame to get
-   * source->base time inerpolated transform.
+   * source->base time interpolated transform.
    * @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,