Collision monitor

nav2_collision_monitor/CMakeLists.txt

@@ -0,0 +1,108 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_collision_monitor)
+
+### Dependencies ###
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_util REQUIRED)
+
+### Header ###
+
+nav2_package()
+
+### Libraries and executables ###
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  nav2_msgs
+  nav2_util
+)
+
+set(executable_name collision_monitor)
+set(library_name ${executable_name}_core)
+
+add_library(${library_name} SHARED
+  src/collision_monitor_node.cpp
+  src/polygon_base.cpp
+  src/polygon.cpp
+  src/circle.cpp
+  src/source_base.cpp
+  src/scan.cpp
+  src/pointcloud.cpp
+  src/kinematics.cpp
+)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name}
+  ${library_name}
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
+
+### Install ###
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
+
+### Testing ###
+
+# ToDo - later
+#if(BUILD_TESTING)
+#  find_package(ament_lint_auto REQUIRED)
+#  # the following line skips the linter which checks for copyrights
+#  set(ament_cmake_copyright_FOUND TRUE)
+#  ament_lint_auto_find_test_dependencies()
+#  add_subdirectory(test)
+#endif()
+
+### Ament stuff ###
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -0,0 +1,77 @@
+// Copyright (c) 2022 Samsung Research 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__CIRCLE_HPP_
+#define NAV2_COLLISION_MONITOR__CIRCLE_HPP_
+
+#include <vector>
+#include <string>
+
+#include "nav2_collision_monitor/polygon_base.hpp"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Circle shape implementaiton
+ */
+class Circle : public PolygonBase
+{
+public:
+  /**
+   * @brief Circle class constructor
+   */
+  Circle(
+    const nav2_util::LifecycleNode::WeakPtr & node,
+    const std::string & polygon_name,
+    const std::string & base_frame_id,
+    const double simulation_time_step);
+  /**
+   * @brief Circle class destructor
+   */
+  virtual ~Circle();
+
+  /**
+   * @brief Gets polygon points, approximated for circle.
+   * To be used in visualization purposes.
+   * @param poly Output polygon points (vertices)
+   */
+  virtual void getPolygon(std::vector<Point> & poly);
+
+  /**
+   * @brief Gets number of points inside circle
+   * @param points Input array of points to be checked
+   * @return Number of points inside circle. If there are no points,
+   * returns zero-value
+   */
+  virtual int getPointsInside(const std::vector<Point> & points);
+
+protected:
+  /**
+   * @brief Supporting routine obtaining all ROS-parameters.
+   * Implementation for Circle class. Calls PolygonBase::getParameters() inside.
+   * @param polygon_topic Output name of polygon publishing topic
+   * @return True if all parameters were obtained or false in failure case
+   */
+  virtual bool getParameters(std::string & polygon_topic);
+
+  /// @brief Radius of the circle
+  double radius_;
+  /// @brief (radius * radius) value. Stored for optimization.
+  double radius_squared_;
+};  // class Circle
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__CIRCLE_HPP_