rmf_obstacle_ros2 (#210)

* Publish nav graph Signed-off-by: Yadunund <[email protected]> * Added ObstacleManager Signed-off-by: Yadunund <[email protected]> * Moved ObstacleManager into src Signed-off-by: Yadunund <[email protected]> * Build as ROS 2 component. Dynamically load detector and responder plugins Signed-off-by: Yadunund <[email protected]> * Refactor into data struct Signed-off-by: Yadunund <[email protected]> * Uncrustify Signed-off-by: Yadunund <[email protected]> * Serialize using octomap Signed-off-by: Yadunund <[email protected]> * Removed ObstacleData Signed-off-by: Yadunund <[email protected]> * Detector accepts a callback Signed-off-by: Yadunund <[email protected]> * Export dependencies Signed-off-by: Yadunund <[email protected]> * Export include dir Signed-off-by: Yadunund <[email protected]> * Explicitly link pluginlib Signed-off-by: Yadunund <[email protected]> * Keep ClassLoaders alive Signed-off-by: Yadunund <[email protected]> * Revert to msg builder API Signed-off-by: Yadunund <[email protected]> * Explicitly include message headers Signed-off-by: Yadunund <[email protected]> * Add skeleton for LaneBlocker Signed-off-by: Yadunund <[email protected]> * Use api from rmf_traffic_ros2 for graph serialization Signed-off-by: Yadunund <[email protected]> * Fix typo Signed-off-by: Yadunund <[email protected]> * Uncrustify Signed-off-by: Yadunund <[email protected]> * Implement speed limit API and publish lane_states Signed-off-by: Yadunund <[email protected]> * Added speed limit request interface to legacy full_control Signed-off-by: Yadunund <[email protected]> * Uncrustify Signed-off-by: Yadunund <[email protected]> * Compute transfroms to rmf frame Signed-off-by: Yadunund <[email protected]> * Added skeletop for processing Signed-off-by: Yadunund <[email protected]> * Warn users of unsupported speed limits Signed-off-by: Yadunund <[email protected]> * Serialize/deserialize speed limits Signed-off-by: Yadunund <[email protected]> * Added logic for publishing lane closures Signed-off-by: Yadunund <[email protected]> * Add cull timer Signed-off-by: Yadunund <[email protected]> * Finished implementing cull Signed-off-by: Yadunund <[email protected]> * Updated intersection checker Signed-off-by: Yadunund <[email protected]> * WIP Signed-off-by: Yadunund <[email protected]> * Fix tf2_geometry_msgs dep for humble Signed-off-by: Yadunund <[email protected]> * Rough collision check implementation Signed-off-by: Yadunund <[email protected]> * Added tests for IntersectionChecker Signed-off-by: Yadunund <[email protected]> * Fixes to SAT Signed-off-by: Yadunund <[email protected]> * Debug rotation Signed-off-by: Yadunund <[email protected]> * Kinda working Signed-off-by: Yadunund <[email protected]> * Use lowest not min Signed-off-by: Yadunund <[email protected]> * Added more failing tests Signed-off-by: Yadunund <[email protected]> * Use dist Signed-off-by: Yadunund <[email protected]> * Cleanup Signed-off-by: Yadunund <[email protected]> * Many logic fixes Signed-off-by: Yadunund <[email protected]> * Uncrustify Signed-off-by: Yadunund <[email protected]> * Refactor Signed-off-by: Yadunund <[email protected]> * Fix API and use bb msgs from rmf_obstacle_ros2 Signed-off-by: Yadunund <[email protected]> * Simplify pkg Signed-off-by: Yadunund <[email protected]> * Fix rmf_obstacle_ros2 intersection checker (#219) * fix intersection checker Signed-off-by: Pranay Shirodkar <[email protected]> * fix uncrustify and build warning Signed-off-by: Pranay Shirodkar <[email protected]> * use Eigen library for transformations Signed-off-by: Pranay Shirodkar <[email protected]> * cleanup Signed-off-by: Pranay Shirodkar <[email protected]> * Use local bb2d Signed-off-by: Yadunund <[email protected]> * Uncrustify Signed-off-by: Yadunund <[email protected]> * Added feeback Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Add Speed Limit Requests (#222) * add FleetUpdateHandle speed limit API to python bindings Signed-off-by: Pranay Shirodkar <[email protected]> * add continuous checker option for dynamic obstacles Signed-off-by: Pranay Shirodkar <[email protected]> * refactor Lane Close and Open msg building Signed-off-by: Pranay Shirodkar <[email protected]> * add speed limits Signed-off-by: Pranay Shirodkar <[email protected]> * Fix style Signed-off-by: Yadunund <[email protected]> * Fix const correctness and do not pass unordered_map by ref to publisher Signed-off-by: Yadunund <[email protected]> Signed-off-by: Pranay Shirodkar <[email protected]> Signed-off-by: Yadunund <[email protected]> Co-authored-by: Yadunund <[email protected]> Co-authored-by: Yadunund <[email protected]> * Filled PointCloud message Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Insert lanes with their state Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Added MessageFilter to rmf_obstacles topic Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Added optional return value to deserialize_key method Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Combine close/open and limit/unlimit functions Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Use member variables instead of passing unordered_maps by reference between these functions Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Defined templated pure abstract class to perform serialization/deserialization Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Fix build Signed-off-by: Yadunund <[email protected]> * Link against targets in test Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]> Signed-off-by: Yadunund <[email protected]> Signed-off-by: Pranay Shirodkar <[email protected]> Signed-off-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Pranay Shirodkar <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
open-rmf · Mar 18, 2024 · 354e6bd · 354e6bd
1 parent 1c26611
commit 354e6bd
Show file tree

Hide file tree

Showing 14 changed files with 2,305 additions and 0 deletions.
diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt
@@ -0,0 +1,145 @@
+cmake_minimum_required(VERSION 3.8)
+project(rmf_obstacle_ros2)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include(GNUInstallDirs)
+
+find_package(ament_cmake REQUIRED)
+
+set(dep_pkgs
+  geometry_msgs
+  rclcpp
+  rclcpp_components
+  rmf_utils
+  rmf_obstacle_msgs
+  rmf_fleet_msgs
+  rmf_building_map_msgs
+  sensor_msgs
+  OCTOMAP
+  rmf_fleet_adapter
+  rmf_traffic
+  rmf_traffic_ros2
+  tf2_ros
+  tf2_geometry_msgs
+)
+foreach(pkg ${dep_pkgs})
+  find_package(${pkg} REQUIRED)
+endforeach()
+
+#===============================================================================
+file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/*.cpp")
+add_library(rmf_obstacle_ros2 SHARED ${core_lib_srcs})
+
+target_link_libraries(rmf_obstacle_ros2
+  PUBLIC
+    rmf_utils::rmf_utils
+    ${rclcpp_TARGETS}
+    ${sensor_msgs_TARGETS}
+    ${rmf_obstacle_msgs_TARGETS}
+  PRIVATE
+    ${OCTOMAP_TARGETS}
+)
+
+target_include_directories(rmf_obstacle_ros2
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+    ${rclcpp_INCLUDE_DIRS}
+    ${sensor_msgs_INCLUDE_DIRS}
+    ${rmf_obstacle_msgs_INCLUDE_DIRS}
+  PRIVATE
+    ${OCTOMAP_INCLUDE_DIRS}
+)
+
+target_compile_features(rmf_obstacle_ros2 INTERFACE cxx_std_17)
+
+ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET)
+
+ament_export_dependencies(${dep_pkgs})
+
+ament_export_include_directories(
+  include
+)
+
+#===============================================================================
+file(GLOB_RECURSE lane_blocker_srcs
+  "src/lane_blocker/*.cpp"
+)
+add_library(lane_blocker SHARED ${lane_blocker_srcs})
+
+target_link_libraries(lane_blocker
+  PRIVATE
+    rclcpp::rclcpp
+    ${rclcpp_components_TARGETS}
+    ${rmf_obstacle_msgs_TARGETS}
+    ${rmf_fleet_msgs_TARGETS}
+    ${rmf_building_map_msgs_TARGETS}
+    rmf_fleet_adapter::rmf_fleet_adapter
+    rmf_traffic::rmf_traffic
+    ${rmf_traffic_ros2_TARGETS}
+    ${tf2_ros_TARGETS}
+    ${geometry_msgs_TARGETS}
+    tf2_geometry_msgs::tf2_geometry_msgs
+)
+
+target_include_directories(lane_blocker
+  PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+    ${rclcpp_INCLUDE_DIRS}
+    ${rclcpp_components_INCLUDE_DIRS}
+    ${rmf_obstacle_msgs_INCLUDE_DIRS}
+    ${rmf_fleet_msgs_INCLUDE_DIRS}
+    ${rmf_building_map_msgs_INCLUDE_DIRS}
+    ${rmf_fleet_adapter_INCLUDE_DIRS}
+    ${rmf_traffic_INCLUDE_DIRS}
+    ${rmf_traffic_ros2_INCLUDE_DIRS}
+    ${tf2_ros_INCLUDE_DIRS}
+    ${geometry_msgs_INCLUDE_DIRS}
+    ${tf2_geometry_msgs_include_dirs}
+)
+
+target_compile_features(lane_blocker INTERFACE cxx_std_17)
+
+rclcpp_components_register_node(lane_blocker
+  PLUGIN "LaneBlocker"
+  EXECUTABLE lane_blocker_node)
+
+#===============================================================================
+if(BUILD_TESTING)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  find_package(ament_cmake_catch2 REQUIRED)
+
+  find_file(uncrustify_config_file
+    NAMES "rmf_code_style.cfg"
+    PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/")
+
+  ament_uncrustify(
+    ARGN include src
+    CONFIG_FILE ${uncrustify_config_file}
+    MAX_LINE_LENGTH 80
+  )
+
+  add_subdirectory(src/lane_blocker/test)
+endif()
+
+#===============================================================================
+install(
+  DIRECTORY include/
+  DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)
+
+install(
+  TARGETS
+    rmf_obstacle_ros2
+    lane_blocker
+  EXPORT rmf_obstacle_ros2
+  RUNTIME DESTINATION lib/rmf_obstacle_ros2
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)
+
+ament_package()
diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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 RMF_OBSTACLE_ROS2__STANDARDNAMES_HPP
+#define RMF_OBSTACLE_ROS2__STANDARDNAMES_HPP
+
+#include <string>
+
+namespace rmf_obstacle_ros2 {
+
+const std::string ObstacleTopicName = "rmf_obstacles";
+
+} // namespace rmf_obstacle_ros2
+
+#endif // RMF_OBSTACLE_ROS2__STANDARDNAMES_HPP
diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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 RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP
+#define RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP
+
+#include <rmf_obstacle_ros2/obstacles/convert/ConvertPointCloudObstacles.hpp>
+
+#endif  // RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP
diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/ConvertDecl.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/ConvertDecl.hpp
@@ -0,0 +1,32 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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 RMF_OBSTACLE_ROS2__OBSTACLES__CONVERTDECL_HPP
+#define RMF_OBSTACLE_ROS2__OBSTACLES__CONVERTDECL_HPP
+
+namespace rmf_obstacle_ros2 {
+using Obstacle = rmf_obstacle_msgs::msg::Obstacle;
+
+//==============================================================================
+/// Serialize a SensorMsg into RMF Obstacle msg
+template<typename SensorMsg>
+void fill_obstacle_data(const SensorMsg& msg, Obstacle& obstacle);
+
+//==============================================================================
+/// Convert an RMF Obstacle msg into a SensorMsg type
+template<typename SensorMsg>
+SensorMsg convert(const Obstacle& msg);
+}  // namespace ros_gz_bridge
+
+#endif // #indef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERTDECL_HPP
diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/convert/ConvertPointCloudObstacles.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/convert/ConvertPointCloudObstacles.hpp
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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 RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT__CONVERTPOINTCLOUDOBSTACLES_HPP
+#define RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT__CONVERTPOINTCLOUDOBSTACLES_HPP
+
+#include <rmf_obstacle_msgs/msg/obstacle.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include "rmf_obstacle_ros2/obstacles/ConvertDecl.hpp"
+
+namespace rmf_obstacle_ros2 {
+
+using PointCloud = sensor_msgs::msg::PointCloud2;
+
+//==============================================================================
+/// Serialize a PointCloud2 msg into RMF Obstacle msg
+template<>
+void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle);
+
+//==============================================================================
+/// Convert an RMF Obstacle msg into a PointCloud2 msg
+template<>
+PointCloud convert(const Obstacle& msg);
+} // namespace rmf_obstacle_ros2
+
+#endif // #indef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT__CONVERTPOINTCLOUDOBSTACLES_HPP
diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rmf_obstacle_ros2</name>
+  <version>2.1.0</version>
+  <description>A package containing utilities for managing obstacles in RMF</description>
+  <maintainer email="[email protected]">Yadunund</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rmf_utils</depend>
+  <depend>rmf_obstacle_msgs</depend>
+  <depend>rmf_fleet_msgs</depend>
+  <depend>rmf_building_map_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>octomap</depend>
+  <depend>rmf_fleet_adapter</depend>
+  <depend>rmf_traffic</depend>
+  <depend>rmf_traffic_ros2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <test_depend>ament_cmake_catch2</test_depend>
+  <test_depend>ament_cmake_uncrustify</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>