From 354e6bd3da9d5e6de6e89077caf2df9189f59ad4 Mon Sep 17 00:00:00 2001 From: Yadu Date: Mon, 18 Mar 2024 12:13:59 +0800 Subject: [PATCH] rmf_obstacle_ros2 (#210) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Publish nav graph Signed-off-by: Yadunund * Added ObstacleManager Signed-off-by: Yadunund * Moved ObstacleManager into src Signed-off-by: Yadunund * Build as ROS 2 component. Dynamically load detector and responder plugins Signed-off-by: Yadunund * Refactor into data struct Signed-off-by: Yadunund * Uncrustify Signed-off-by: Yadunund * Serialize using octomap Signed-off-by: Yadunund * Removed ObstacleData Signed-off-by: Yadunund * Detector accepts a callback Signed-off-by: Yadunund * Export dependencies Signed-off-by: Yadunund * Export include dir Signed-off-by: Yadunund * Explicitly link pluginlib Signed-off-by: Yadunund * Keep ClassLoaders alive Signed-off-by: Yadunund * Revert to msg builder API Signed-off-by: Yadunund * Explicitly include message headers Signed-off-by: Yadunund * Add skeleton for LaneBlocker Signed-off-by: Yadunund * Use api from rmf_traffic_ros2 for graph serialization Signed-off-by: Yadunund * Fix typo Signed-off-by: Yadunund * Uncrustify Signed-off-by: Yadunund * Implement speed limit API and publish lane_states Signed-off-by: Yadunund * Added speed limit request interface to legacy full_control Signed-off-by: Yadunund * Uncrustify Signed-off-by: Yadunund * Compute transfroms to rmf frame Signed-off-by: Yadunund * Added skeletop for processing Signed-off-by: Yadunund * Warn users of unsupported speed limits Signed-off-by: Yadunund * Serialize/deserialize speed limits Signed-off-by: Yadunund * Added logic for publishing lane closures Signed-off-by: Yadunund * Add cull timer Signed-off-by: Yadunund * Finished implementing cull Signed-off-by: Yadunund * Updated intersection checker Signed-off-by: Yadunund * WIP Signed-off-by: Yadunund * Fix tf2_geometry_msgs dep for humble Signed-off-by: Yadunund * Rough collision check implementation Signed-off-by: Yadunund * Added tests for IntersectionChecker Signed-off-by: Yadunund * Fixes to SAT Signed-off-by: Yadunund * Debug rotation Signed-off-by: Yadunund * Kinda working Signed-off-by: Yadunund * Use lowest not min Signed-off-by: Yadunund * Added more failing tests Signed-off-by: Yadunund * Use dist Signed-off-by: Yadunund * Cleanup Signed-off-by: Yadunund * Many logic fixes Signed-off-by: Yadunund * Uncrustify Signed-off-by: Yadunund * Refactor Signed-off-by: Yadunund * Fix API and use bb msgs from rmf_obstacle_ros2 Signed-off-by: Yadunund * Simplify pkg Signed-off-by: Yadunund * Fix rmf_obstacle_ros2 intersection checker (#219) * fix intersection checker Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * fix uncrustify and build warning Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * use Eigen library for transformations Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * cleanup Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * Use local bb2d Signed-off-by: Yadunund * Uncrustify Signed-off-by: Yadunund * Added feeback Signed-off-by: Alejandro Hernández Cordero * Add Speed Limit Requests (#222) * add FleetUpdateHandle speed limit API to python bindings Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * add continuous checker option for dynamic obstacles Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * refactor Lane Close and Open msg building Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * add speed limits Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> * Fix style Signed-off-by: Yadunund * Fix const correctness and do not pass unordered_map by ref to publisher Signed-off-by: Yadunund Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Signed-off-by: Yadunund Co-authored-by: Yadunund Co-authored-by: Yadunund * Filled PointCloud message Signed-off-by: Alejandro Hernández Cordero * Insert lanes with their state Signed-off-by: Alejandro Hernández Cordero * Added MessageFilter to rmf_obstacles topic Signed-off-by: Alejandro Hernández Cordero * Added optional return value to deserialize_key method Signed-off-by: Alejandro Hernández Cordero * Combine close/open and limit/unlimit functions Signed-off-by: Alejandro Hernández Cordero * Use member variables instead of passing unordered_maps by reference between these functions Signed-off-by: Alejandro Hernández Cordero * Defined templated pure abstract class to perform serialization/deserialization Signed-off-by: Alejandro Hernández Cordero * Fix build Signed-off-by: Yadunund * Link against targets in test Signed-off-by: Yadunund --------- Signed-off-by: Yadunund Signed-off-by: Yadunund Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Co-authored-by: Alejandro Hernández Cordero --- rmf_obstacle_ros2/CMakeLists.txt | 145 +++ .../rmf_obstacle_ros2/StandardNames.hpp | 29 + .../rmf_obstacle_ros2/obstacles/Convert.hpp | 23 + .../obstacles/ConvertDecl.hpp | 32 + .../convert/ConvertPointCloudObstacles.hpp | 40 + rmf_obstacle_ros2/package.xml | 33 + .../src/lane_blocker/IntersectionChecker.cpp | 193 ++++ .../src/lane_blocker/IntersectionChecker.hpp | 58 ++ .../src/lane_blocker/LaneBlocker.cpp | 964 ++++++++++++++++++ .../src/lane_blocker/LaneBlocker.hpp | 228 +++++ .../src/lane_blocker/test/CMakeLists.txt | 28 + .../src/lane_blocker/test/main.cpp | 21 + .../test/test_IntersectionChecker.cpp | 377 +++++++ .../obstacles/ConvertPointCloudObstacles.cpp | 134 +++ 14 files changed, 2305 insertions(+) create mode 100644 rmf_obstacle_ros2/CMakeLists.txt create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/ConvertDecl.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/convert/ConvertPointCloudObstacles.hpp create mode 100644 rmf_obstacle_ros2/package.xml create mode 100644 rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt create mode 100644 rmf_obstacle_ros2/src/lane_blocker/test/main.cpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp create mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt new file mode 100644 index 000000000..69c6a0bdf --- /dev/null +++ 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 + $ + $ + ${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 + $ + $ + ${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 new file mode 100644 index 000000000..3e9e0d328 --- /dev/null +++ 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 + +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 new file mode 100644 index 000000000..bab6c2e97 --- /dev/null +++ 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 + +#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 new file mode 100644 index 000000000..93f39eecb --- /dev/null +++ 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 +void fill_obstacle_data(const SensorMsg& msg, Obstacle& obstacle); + +//============================================================================== +/// Convert an RMF Obstacle msg into a SensorMsg type +template +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 new file mode 100644 index 000000000..bf2b7b9c7 --- /dev/null +++ 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 +#include +#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 new file mode 100644 index 000000000..99d31e8b6 --- /dev/null +++ b/rmf_obstacle_ros2/package.xml @@ -0,0 +1,33 @@ + + + + rmf_obstacle_ros2 + 2.1.0 + A package containing utilities for managing obstacles in RMF + Yadunund + Apache License 2.0 + + ament_cmake + + 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 + + ament_cmake_catch2 + ament_cmake_uncrustify + + + ament_cmake + + diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp new file mode 100644 index 000000000..470c7fd69 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -0,0 +1,193 @@ +/* + * 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. + * +*/ + +#include "IntersectionChecker.hpp" + +#ifndef NDEBUG +#include +#endif + +//============================================================================== +namespace IntersectionChecker { + +namespace { + +struct CollisionBox +{ + Eigen::Vector2d min; // min x & y + Eigen::Vector2d max; // max x & y +}; + +CollisionBox make_reference_box(const CollisionGeometry& o) +{ + CollisionBox box; + box.min = Eigen::Vector2d{-o.size_x * 0.5, -o.size_y * 0.5}; + box.max = Eigen::Vector2d{o.size_x* 0.5, o.size_y* 0.5}; + return box; +} + +std::pair> make_transformed_box( + const CollisionGeometry& to, + const CollisionGeometry& from) +{ + // Create the 'from' geometry's vertices at the origin + std::vector vertices; + vertices.push_back({-from.size_x * 0.5, from.size_y * 0.5}); + vertices.push_back({-from.size_x * 0.5, -from.size_y * 0.5}); + vertices.push_back({from.size_x* 0.5, from.size_y* 0.5}); + vertices.push_back({from.size_x* 0.5, -from.size_y* 0.5}); + + // Rotate then translate the 'from' geometry's vertices to their actual positions + Eigen::Rotation2D rot_from(from.center.theta); + Eigen::Translation trans_from(from.center.x, from.center.y); + for (auto& v : vertices) + { + v = trans_from * rot_from * v; + } + + #ifndef NDEBUG + std::cout << "Obs rot matrix: " << std::endl; + std::cout << rot_from.matrix() << std::endl; + #endif + + #ifndef NDEBUG + std::cout << "Obs vertices before trans: "; + for (const auto& v : vertices) + std::cout << "{" << v[0] << "," << v[1] << "}" << " "; + std::cout << std::endl; + #endif + + // Translate and rotate the 'from' geometry's vertices by + // the inverse of the 'to' geometry. + // Vertex coordinates of 'from' geometry are now w.r.t. the 'to' geometry's frame, + // where the 'to' geometry's frame is at the origin without rotation. + Eigen::Rotation2D rot_to(to.center.theta); + Eigen::Translation trans_to(to.center.x, to.center.y); + for (auto& v : vertices) + { + v = rot_to.inverse() * trans_to.inverse() * v; + } + + #ifndef NDEBUG + std::cout << "Lane rot inv matrix: " << std::endl; + std::cout << rot_to.inverse().matrix() << std::endl; + #endif + + #ifndef NDEBUG + std::cout << "Obs vertices after trans: "; + for (const auto& v : vertices) + std::cout << "{" << v[0] << "," << v[1] << "}" << " "; + std::cout << std::endl; + #endif + + CollisionBox o2_box; + o2_box.min = + {std::numeric_limits::max(), std::numeric_limits::max()}; + o2_box.max = + {std::numeric_limits::lowest(), + std::numeric_limits::lowest()}; + + for (const auto& v : vertices) + { + for (std::size_t i = 0; i < 2; ++i) + { + if (v[i] < o2_box.min[i]) + o2_box.min[i] = v[i]; + if (v[i] > o2_box.max[i]) + o2_box.max[i] = v[i]; + } + } + + return {o2_box, vertices}; +} + +} // anonymous namespace + + +//============================================================================== +bool between( + const CollisionGeometry& o1, + const CollisionGeometry& o2, + double& how_much) +{ + #ifndef NDEBUG + auto print_geom = + [](const CollisionGeometry& o, const std::string& name) + { + std::cout << name << ": {" << o.center.x << "," + << o.center.y << "," << o.center.theta << "} [" << o.size_x + << "," << o.size_y << "]" << std::endl; + }; + + auto print_box = + [](const CollisionBox& box, const std::string& name) + { + std::cout << name << "_min: " << box.min[0] << "," << box.min[1] + << std::endl; + std::cout << name << "_max: " << box.max[0] << "," << box.max[1] + << std::endl; + }; + + std::cout << "================================================" << std::endl; + std::cout << "Checking collision between: " << std::endl; + print_geom(o1, "Lane"); + print_geom(o2, "obs"); + #endif + + const auto o1_box = make_reference_box(o1); + const auto result = make_transformed_box(o1, o2); + const auto& o2_box = result.first; + + #ifndef NDEBUG + print_box(o1_box, "Lane box"); + print_box(o2_box, "obs transformed box"); + #endif + + // TODO(YV): Consider moving this to a separate narrowphase implementation + // to speed up compute + how_much = std::numeric_limits::min(); + for (std::size_t i = 0; i < 2; ++i) + { + double dist = std::numeric_limits::min(); + // O2 projections are on the left of O1 extremas + if (o2_box.max[i] < o1_box.min[i]) + { + dist = std::abs(o2_box.max[i] - o1_box.min[i]); + } + // O2 projections are on the right of O1 extremas + else if (o2_box.min[i] > o1_box.max[i]) + { + dist = std::abs(o2_box.min[i] - o1_box.max[i]); + } + // They intersect + else + continue; + how_much = std::max(how_much, dist); + } + + // Simple SAT theorem application + for (std::size_t i = 0; i < 2; ++i) + { + if (o2_box.min[i] > o1_box.max[i] || o2_box.max[i] < o1_box.min[i]) + { + return false; + } + } + + return true; +} +} // namespace IntersectionChecker diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp new file mode 100644 index 000000000..9bbb698cd --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -0,0 +1,58 @@ +/* + * 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 SRC__INTERSECTIONCHECKER_HPP +#define SRC__INTERSECTIONCHECKER_HPP + +#include + +#include + +//============================================================================== +// TODO(YV): Consider making this a loadable plugin via pluginlib +namespace IntersectionChecker { + +struct CollisionGeometry +{ + geometry_msgs::msg::Pose2D center; + double size_x; + double size_y; + + CollisionGeometry() + {} + + CollisionGeometry( + geometry_msgs::msg::Pose2D center_, + double size_x_, + double size_y_) + : center(std::move(center_)), + size_x(size_x_), + size_y(size_y_) + {} +}; + +// Return true if intersect. +// If intersect, how_much represents the overlap in meters +// If not intersect, how_much represents the separating distance in meters. +bool between( + const CollisionGeometry& o1, + const CollisionGeometry& o2, + double& how_much); + +} // namespace IntersectionChecker + +#endif // SRC__INTERSECTIONCHECKER_HPP diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp new file mode 100644 index 000000000..45deb5e27 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -0,0 +1,964 @@ +/* + * 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. + * +*/ + + +#include "LaneBlocker.hpp" +#include "IntersectionChecker.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include + +#ifndef NDEBUG +#include +#endif + +//============================================================================== +namespace { + +IntersectionChecker::CollisionGeometry make_collision_geometry( + const rmf_traffic::agv::Graph& graph, + const rmf_traffic::agv::Graph::Lane& lane, + const double lane_width) +{ + IntersectionChecker::CollisionGeometry geometry; + const Eigen::Vector2d& entry_loc = + graph.get_waypoint(lane.entry().waypoint_index()).get_location(); + + const Eigen::Vector2d& exit_loc = + graph.get_waypoint(lane.exit().waypoint_index()).get_location(); + + const auto& center_loc = (exit_loc + entry_loc) * 0.5; + const auto& axis = (exit_loc - entry_loc); + double theta = std::atan2(std::abs(axis[1]), std::abs(axis[0])); + if (theta > M_PI) + theta = M_PI -theta; + if (theta < -M_PI) + theta = M_PI + theta; + const double length = axis.norm(); + + geometry.center.x = center_loc[0]; + geometry.center.y = center_loc[1]; + geometry.center.theta = theta; + geometry.size_y = lane_width; + geometry.size_x = length; + return geometry; +} + +IntersectionChecker::CollisionGeometry make_collision_geometry( + const LaneBlocker::BoundingBox& obstacle) +{ + IntersectionChecker::CollisionGeometry geometry; + + const auto& p = obstacle.center.position; + const auto& q = obstacle.center.orientation; + // Convert quaternion to yaw + const double siny_cosp = 2 * (q.w * q.z + q.x * q.y); + const double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); + geometry.center.theta = std::atan2(siny_cosp, cosy_cosp); + + geometry.center.x = p.x; + geometry.center.y = p.y; + geometry.size_x = obstacle.size.x; + geometry.size_y = obstacle.size.y; + + return geometry; +} + +} //namespace anonymous +//============================================================================== +LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) +: Node("lane_blocker_node", options) +{ + _tf2_buffer = + std::make_unique(this->get_clock()); + + if (_tf2_buffer != nullptr) + { + _transform_listener = + std::make_shared(*_tf2_buffer); + } + + _rmf_frame = this->declare_parameter("rmf_frame_id", "map"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter rmf_frame_id to %s", _rmf_frame.c_str() + ); + + _obstacle_lane_threshold = this->declare_parameter( + "obstacle_lane_threshold", 0.25); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter obstacle_lane_threshold to %f", _obstacle_lane_threshold + ); + + _lane_width = this->declare_parameter( + "lane_width", 0.5); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter lane_width to %f", _lane_width + ); + + _tf2_lookup_duration = this->declare_parameter( + "tf2_lookup_duration", 0.5); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter tf2_lookup_duration to %f", _tf2_lookup_duration + ); + + const double process_rate = this->declare_parameter("process_rate", 1.0); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter process_rate to %f hz", process_rate + ); + + const double cull_rate = this->declare_parameter("cull_rate", 0.1); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cull_rate to %f hz", cull_rate + ); + + std::size_t search_millis = + this->declare_parameter("max_search_millis", 1000); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter max_search_millis to %ld milliseconds", search_millis + ); + _max_search_duration = std::chrono::milliseconds(search_millis); + + _continuous_checker = + this->declare_parameter("continuous_checker", false); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter continuous_checker to %s", _continuous_checker ? "true" : "false" + ); + + _lane_closure_threshold = + this->declare_parameter("lane_closure_threshold", 5); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter lane_closure_threshold to %ld", _lane_closure_threshold + ); + + _speed_limit_threshold = + this->declare_parameter("speed_limit_threshold", 3); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter speed_limit_threshold to %ld", _speed_limit_threshold + ); + + _speed_limit = + this->declare_parameter("speed_limit", 0.5); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter speed_limit to %f", _speed_limit + ); + + auto process_timer_period = + std::chrono::duration_cast( + std::chrono::duration>(1.0 / process_rate)); + _process_timer = this->create_wall_timer( + std::move(process_timer_period), + [=]() + { + this->process(); + }); + + _cull_timer_period = + std::chrono::duration_cast( + std::chrono::duration>(1.0 / cull_rate)); + _cull_timer = this->create_wall_timer( + _cull_timer_period, + [=]() + { + this->cull(); + }); + + _lane_closure_pub = this->create_publisher( + rmf_fleet_adapter::LaneClosureRequestTopicName, + rclcpp::SystemDefaultsQoS()); + + _speed_limit_pub = this->create_publisher( + rmf_fleet_adapter::SpeedLimitRequestTopicName, + rclcpp::SystemDefaultsQoS()); + + std::chrono::duration buffer_timeout(1); + + _obstacle_sub = std::make_shared>( + this, "rmf_obstacles", rclcpp::QoS(10).best_effort().get_rmw_qos_profile()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + _tf2_buffer->setCreateTimerInterface(timer_interface); + + _tf2_filter_obstacles = std::make_shared>( + *_tf2_buffer, _rmf_frame, 100, this->get_node_logging_interface(), + this->get_node_clock_interface(), buffer_timeout); + _tf2_filter_obstacles->connectInput(*_obstacle_sub); + _tf2_filter_obstacles->registerCallback(&LaneBlocker::obstacle_cb, this); + + // Selectively disable intra-process comms for non-volatile subscriptions + // so that this node can be run in a container with intra-process comms. + auto transient_qos = rclcpp::QoS(10).transient_local(); + rclcpp::SubscriptionOptionsWithAllocator< + std::allocator> ipc_sub_options; + ipc_sub_options.use_intra_process_comm = + rclcpp::IntraProcessSetting::Disable; + + _graph_sub = this->create_subscription( + "nav_graphs", + transient_qos, + [=](NavGraph::ConstSharedPtr msg) + { + if (msg->name.empty()) + return; + auto traffic_graph = rmf_traffic_ros2::convert(*msg); + if (!traffic_graph.has_value()) + { + RCLCPP_WARN( + this->get_logger(), + "Unable to convert NavGraph from fleet %s into a traffic graph", + msg->name.c_str() + ); + } + _traffic_graphs[msg->name] = std::move(traffic_graph.value()); + for (std::size_t i = 0; i < _traffic_graphs[msg->name].num_lanes(); ++i) + { + const std::string lane_key = get_lane_key(msg->name, i); + if (_internal_lane_states.find(lane_key) == _internal_lane_states.end()) + { + if(!_traffic_graphs[msg->name].get_lane(i). + properties().speed_limit().has_value()) + { + _internal_lane_states.insert({lane_key, LaneState::Normal}); + } + else + { + _internal_lane_states.insert({lane_key, LaneState::SpeedLimited}); + } + } + } + }, + ipc_sub_options); + + _lane_states_sub = this->create_subscription( + "lane_states", + transient_qos, + [=](LaneStates::ConstSharedPtr msg) + { + if (msg->fleet_name.empty()) + return; + _lane_states[msg->fleet_name] = msg; + }, + ipc_sub_options); + + RCLCPP_INFO( + this->get_logger(), + "Started lane_blocker node" + ); +} + +//============================================================================== +void LaneBlocker::obstacle_cb(const message_filters::MessageEvent& evt) +{ + auto msg = evt.getMessage(); + + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + + if (msg->obstacles.empty() || _transform_listener == nullptr) + return; + + for (const auto& obstacle : msg->obstacles) + { + const auto& obstacle_frame = obstacle.header.frame_id; + const auto& transform = _tf2_buffer->lookupTransform( + _rmf_frame, + obstacle_frame, + tf2::TimePointZero + ); + + // doTransform only works on Stamped messages + const auto before_pose = geometry_msgs::build() + .header(obstacle.header) + .pose(obstacle.bbox.center); + const auto before_size = geometry_msgs::build() + .header(obstacle.header) + .vector(obstacle.bbox.size); + + PoseStamped after_pose; + Vector3Stamped after_size; + tf2::doTransform(before_pose, after_pose, transform); + tf2::doTransform(before_size, after_size, transform); + RCLCPP_DEBUG( + this->get_logger(), + "Pose of obstacle id %d in RMF %s frame is [%f, %f, %f]", + obstacle.id, _rmf_frame.c_str(), + after_pose.pose.position.x, + after_pose.pose.position.y, after_pose.pose.position.z + ); + auto new_box = + rmf_obstacle_msgs::build() + .center(std::move(after_pose.pose)) + .size(std::move(after_size.vector)); + + ObstacleData obs{ + rclcpp::Time(obstacle.header.stamp) + rclcpp::Duration(obstacle.lifetime), + obstacle.id, + obstacle.source, + std::move(new_box) + }; + + // Add to obstacle queue for processing in a separate thread/callback + const auto& obs_key = LaneBlocker::get_obstacle_key(obs); + _obstacle_buffer[obs_key] = std::move(obs); + } +} + +//============================================================================== +void LaneBlocker::process() +{ + if (_obstacle_buffer.empty()) + return; + + // Keep track of which lanes were updated to decided whether to modify lane + // state. + std::unordered_set lanes_with_changes = {}; + // Map obstacle_id with list of lanes it is no longer in the vicinity of + std::unordered_map> obstacles_with_changes = {}; + + for (const auto& [obstacle_key, obstacle] : _obstacle_buffer) + { + // If the lifetime of the obstacle has passed cull() will handle the purging + if (obstacle.expiry_time < get_clock()->now()) + { + continue; + } + + // If this obstacle was previously assigned to a lane, + // check if it is still in the vicinity of that lane + auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle_key); + if (obs_lane_it != _obstacle_to_lanes_map.end()) + { + const auto& lanes_keys = obs_lane_it->second; + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s was previously in the vicinity of %ld lanes", + obstacle_key.c_str(), lanes_keys.size()); + + // Check if obstacle is still in the vicinity of these lanes. + for (const auto& lane_key : lanes_keys) + { + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + continue; + const auto fleet_name = deserialize_key_value.value().first; + const auto lane_id = deserialize_key_value.value().second; + if (_traffic_graphs.find(fleet_name) == _traffic_graphs.end()) + { + RCLCPP_ERROR( + this->get_logger(), + "Lane %s which belongs to fleet %s does not have a traffic graph " + "This bug should be reported.", + lane_key.c_str(), fleet_name.c_str() + ); + continue; + } + double how_much; + const auto& traffic_graph = _traffic_graphs.at(fleet_name); + const auto& lane = traffic_graph.get_lane(lane_id); + const auto& o1 = make_collision_geometry( + traffic_graph, + lane, + _lane_width); + const auto& o2 = make_collision_geometry(obstacle.transformed_bbox); + auto intersect = IntersectionChecker::between( + o1, + o2, + how_much + ); + if (intersect || how_much <= _obstacle_lane_threshold) + { + // Obstacle is still in the vicinity of this lane + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s is still in the vicinity of lane %s", + obstacle_key.c_str(), lane_key.c_str() + ); + } + else + { + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s is no longer in the vicinity of lane %s. " + "Updating cache...", obstacle_key.c_str(), lane_key.c_str() + ); + // Obstacle is no longer in the vicinity and needs to be removed + // Remove from _obstacle_to_lanes_map + obstacles_with_changes[obstacle_key].insert(lane_key); + } + } + } + + if (obs_lane_it == _obstacle_to_lanes_map.end() || _continuous_checker) + { + // New obstacle or re-check current obstacle. + // It needs to be assigned a lane if within the vicinity of one. + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s was not previously in the vicinity of any lane. Checking " + "for any changes", obstacle_key.c_str() + ); + std::unordered_set vicinity_lane_keys = {}; + std::mutex mutex; + auto search_vicinity_lanes = + [&vicinity_lane_keys, &mutex]( + const std::string& fleet_name, + const TrafficGraph& graph, + const ObstacleData& obstacle, + const double threshold, + const double lane_width, + const std::chrono::nanoseconds max_duration) + { + const auto start_time = std::chrono::steady_clock::now(); + const auto max_time = start_time + max_duration; + for (std::size_t i = 0; i < graph.num_lanes(); ++i) + { + if (std::chrono::steady_clock::now() > max_time) + return; + const auto& lane = graph.get_lane(i); + double how_much; + const auto& o1 = make_collision_geometry( + graph, + lane, + lane_width); + const auto& o2 = make_collision_geometry( + obstacle.transformed_bbox); + auto intersect = IntersectionChecker::between( + o1, + o2, + how_much + ); + if (intersect || how_much < threshold) + { + std::lock_guard lock(mutex); + vicinity_lane_keys.insert( + LaneBlocker::get_lane_key(fleet_name, i)); + } + } + #ifndef NDEBUG + const auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Obstacle " << obstacle.id + << " search in graph for fleet " << fleet_name << " took " + << (finish_time - start_time).count() /1e6 + << " ms" << std::endl; + #endif + + }; + std::vector search_threads = {}; + for (const auto& [fleet_name, graph] : _traffic_graphs) + { + search_threads.push_back( + std::thread( + search_vicinity_lanes, + fleet_name, + graph, + obstacle, + _obstacle_lane_threshold, + _lane_width, + _max_search_duration) + ); + } + + for (auto& t : search_threads) + { + if (t.joinable()) + t.join(); + } + + RCLCPP_INFO( + this->get_logger(), + "Search concluded with %ld lanes in the vicinity of obstacle %s", + vicinity_lane_keys.size(), obstacle_key.c_str() + ); + + // Update caches + for (const auto& lane_key : vicinity_lane_keys) + { + // new obstacle + if (obs_lane_it == _obstacle_to_lanes_map.end()) + { + _obstacle_to_lanes_map[obstacle_key].insert(lane_key); + _lane_to_obstacles_map[lane_key].insert(obstacle_key); + lanes_with_changes.insert(lane_key); + } + // current obstacle + else + { + const auto& existing_lane_keys = obs_lane_it->second; + if (existing_lane_keys.find(lane_key) == existing_lane_keys.end()) + { + _obstacle_to_lanes_map[obstacle_key].insert(lane_key); + _lane_to_obstacles_map[lane_key].insert(obstacle_key); + lanes_with_changes.insert(lane_key); + } + } + } + } + } + + RCLCPP_INFO( + this->get_logger(), + "There are %ld lanes with changes to the number of obstacles in their " + "vicinity", lanes_with_changes.size() + ); + + // Remove obstacles from lanes + for (const auto& [obstacle_key, lane_ids] : obstacles_with_changes) + { + for (const auto& lane_id : lane_ids) + { + _lane_to_obstacles_map[lane_id].erase(obstacle_key); + _obstacle_to_lanes_map[obstacle_key].erase(lane_id); + if (_obstacle_to_lanes_map[obstacle_key].empty()) + _obstacle_to_lanes_map.erase(obstacle_key); + lanes_with_changes.insert(lane_id); + } + } + request_lane_modifications(std::move(lanes_with_changes)); +} + +//============================================================================== +void LaneBlocker::request_lane_modifications( + const std::unordered_set& changes) +{ + if (changes.empty()) + return; + + std::lock_guard lock(_mutex_msgs); + _lane_req_msgs.clear(); + _speed_limit_req_msgs.clear(); + + // For now we implement a simple heuristic to decide whether to close a lane + // or not. + for (const auto& lane_key : changes) + { + if (_lane_to_obstacles_map.find(lane_key) == _lane_to_obstacles_map.end()) + { + RCLCPP_ERROR( + this->get_logger(), + "[LaneBlocker::request_lane_modifications()]: key error. This is a " + "bug and should be reported." + ); + continue; + } + const auto& obstacles = _lane_to_obstacles_map.at(lane_key); + const auto& lane_state = _internal_lane_states.at(lane_key); + if (obstacles.size() >= _lane_closure_threshold && + lane_state == LaneState::Normal) + { + transition_lane_state(lane_state, LaneState::Closed, lane_key); + } + else if (obstacles.size() >= _speed_limit_threshold && + lane_state == LaneState::Normal) + { + transition_lane_state(lane_state, LaneState::SpeedLimited, lane_key); + } + else if (obstacles.size() >= _lane_closure_threshold && + lane_state == LaneState::SpeedLimited) + { + transition_lane_state(lane_state, LaneState::Closed, lane_key); + } + else + { + RCLCPP_INFO( + this->get_logger(), + "Lane %s has %ld obstacles in its vicinity but will not be closed or speed limited as " + "the closure threshold is %ld and the speed limit threshold is %ld", + lane_key.c_str(), + obstacles.size(), _lane_closure_threshold, _speed_limit_threshold + ); + continue; + } + } + + publish_lane_req_msgs(); + publish_speed_limit_req_msgs(); +} + +//============================================================================== +void LaneBlocker::transition_lane_state( + const LaneState& old_state, + const LaneState& new_state, + const std::string& lane_key) +{ + if (new_state == old_state) + { + return; + } + + if (old_state == LaneState::Normal && new_state == LaneState::Closed) + { + add_lane_open_close_req(lane_key, LaneState::Closed); + } + else if (old_state == LaneState::Closed && new_state == LaneState::Normal) + { + add_lane_open_close_req(lane_key, LaneState::Normal); + } + else if (old_state == LaneState::Normal && + new_state == LaneState::SpeedLimited) + { + add_speed_limit_req(lane_key, LaneState::SpeedLimited); + } + else if (old_state == LaneState::SpeedLimited && + new_state == LaneState::Normal) + { + add_speed_limit_req(lane_key, LaneState::SpeedUnlimited); + } + else if (old_state == LaneState::SpeedLimited && + new_state == LaneState::Closed) + { + add_lane_open_close_req(lane_key, new_state); + add_speed_limit_req(lane_key, LaneState::SpeedUnlimited); + } + else if (old_state == LaneState::Closed && + new_state == LaneState::SpeedLimited) + { + add_lane_open_close_req(lane_key, LaneState::Normal); + add_speed_limit_req(lane_key, LaneState::SpeedLimited); + } + + // update lane state + auto it = _internal_lane_states.find(lane_key); + if (it != _internal_lane_states.end()) + { + it->second = new_state; + } +} + +//============================================================================== +void LaneBlocker::add_lane_open_close_req( + const std::string& lane_key, + const LaneState& desired_state) +{ + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_lane_open_close_req: Failure deserializing key"); + return; + } + const auto fleet_name = deserialize_key_value.value().first; + const auto lane_id = deserialize_key_value.value().second; + // construct Lane Closure msg + auto msg_it = _lane_req_msgs.insert({fleet_name, nullptr}); + if (msg_it.second) + { + LaneRequest request; + request.fleet_name = std::move(fleet_name); + if (desired_state == LaneState::Closed) + { + request.close_lanes.push_back(std::move(lane_id)); + } + else if (desired_state == LaneState::Normal) + { + request.open_lanes.push_back(std::move(lane_id)); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_lane_open_close_req: Invalid desired state"); + return; + } + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the lane id + if (desired_state == LaneState::Closed) + { + msg_it.first->second->close_lanes.push_back(std::move(lane_id)); + } + else if (desired_state == LaneState::Normal) + { + msg_it.first->second->open_lanes.push_back(std::move(lane_id)); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_lane_open_close_req: Invalid desired state"); + } + } +} + +//============================================================================== +void LaneBlocker::add_speed_limit_req( + const std::string& lane_key, + const LaneState& desired_state) +{ + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_speed_limit_req: Failure deserializing key"); + return; + } + const auto fleet_name = deserialize_key_value.value().first; + const auto lane_id = deserialize_key_value.value().second; + // construct Speed Limit msg + auto msg_it = _speed_limit_req_msgs.insert({fleet_name, nullptr}); + + SpeedLimitedLane speed_limited_lane = + rmf_fleet_msgs::build() + .lane_index(std::move(lane_id)) + .speed_limit(_speed_limit); + + if (msg_it.second) + { + SpeedLimitRequest request; + request.fleet_name = std::move(fleet_name); + if (desired_state == LaneState::SpeedLimited) + { + request.speed_limits.push_back(std::move(speed_limited_lane)); + } + else if (desired_state == LaneState::SpeedUnlimited) + { + request.remove_limits.push_back(std::move(lane_id)); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_speed_limit_req: Invalid desired state"); + return; + } + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + if (desired_state == LaneState::SpeedLimited) + { + // Msg was created before. We simply append the new speed limited lane + msg_it.first->second->speed_limits.push_back( + std::move(speed_limited_lane)); + } + else if (desired_state == LaneState::SpeedUnlimited) + { + msg_it.first->second->remove_limits.push_back(std::move(lane_id)); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_speed_limit_req: Invalid desired state"); + return; + } + } +} + +//============================================================================== +void LaneBlocker::publish_lane_req_msgs() +{ + for (auto& [_, msg] : _lane_req_msgs) + { + if (msg->close_lanes.empty() && msg->open_lanes.empty()) + { + RCLCPP_DEBUG( + this->get_logger(), + "None of the lanes for fleet %s need to be opened or closed", + msg->fleet_name.c_str() + ); + continue; + } + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to close for fleet %s", + msg->close_lanes.size(), msg->fleet_name.c_str() + ); + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to open for fleet %s", + msg->open_lanes.size(), msg->fleet_name.c_str() + ); + _lane_closure_pub->publish(std::move(msg)); + } +} + +//============================================================================== +void LaneBlocker::publish_speed_limit_req_msgs() +{ + for (auto& [_, msg] : _speed_limit_req_msgs) + { + if (msg->speed_limits.empty() && msg->remove_limits.empty()) + { + RCLCPP_DEBUG( + this->get_logger(), + "None of the lanes for fleet %s have speed limits modified", + msg->fleet_name.c_str() + ); + continue; + } + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to adhere to speed limit %f, for fleet %s", + msg->speed_limits.size(), _speed_limit, msg->fleet_name.c_str() + ); + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to remove speed limit, for fleet %s", + msg->remove_limits.size(), msg->fleet_name.c_str() + ); + _speed_limit_pub->publish(std::move(msg)); + } +} + +//============================================================================== +std::optional> LaneBlocker::deserialize_key( + const std::string& key) const +{ + const std::string delimiter = "_"; + try + { + std::string name = key.substr(0, key.find(delimiter)); + std::string id_str = + key.substr(key.find(delimiter) + 1, key.size() - name.size()); + std::stringstream ss(id_str); + std::size_t id; ss >> id; + return std::make_pair(name, id); + } + catch (const std::exception& e) + { + RCLCPP_INFO( + this->get_logger(), + "[LaneBlocker::deserialize_key] Unable to parse key. This is a bug and " + "should be reported. Detailed error: %s", e.what()); + return std::nullopt; + } +} + +//============================================================================== +void LaneBlocker::purge_obstacles( + const std::unordered_set& obstacle_keys, + const bool erase_from_buffer) +{ + for (const auto& obs : obstacle_keys) + { + auto lanes_it = _obstacle_to_lanes_map.find(obs); + if (lanes_it != _obstacle_to_lanes_map.end()) + { + const auto& lanes = lanes_it->second; + for (const auto& lane_key : lanes) + { + auto obs_it = _lane_to_obstacles_map.find(lane_key); + if (obs_it != _lane_to_obstacles_map.end()) + { + obs_it->second.erase(obs); + } + } + } + _obstacle_to_lanes_map.erase(obs); + if (erase_from_buffer) + _obstacle_buffer.erase(obs); + } +} + +//============================================================================== +void LaneBlocker::cull() +{ + // Cull obstacles that are past their expiry times. + // Also decide whether previously closed lanes should be re-opened. + std::unordered_set obstacles_to_cull; + std::unordered_set lanes_with_changes; + + const auto now = this->get_clock()->now(); + for (const auto& [obstacle_key, lanes] : _obstacle_to_lanes_map) + { + auto it = _obstacle_buffer.find(obstacle_key); + if (it == _obstacle_buffer.end()) + { + // TODO(YV): Purge + obstacles_to_cull.insert(obstacle_key); + continue; + } + const auto& obstacle = it->second; + if (now - obstacle.expiry_time > _cull_timer_period) + { + obstacles_to_cull.insert(obstacle_key); + // Then remove this obstacles from lanes map which is used to decide + // whether to open/close + for (const auto& lane : lanes) + { + _lane_to_obstacles_map[lane].erase(obstacle_key); + lanes_with_changes.insert(lane); + } + } + } + + // Cull + purge_obstacles(obstacles_to_cull); + + std::lock_guard lock(_mutex_msgs); + _lane_req_msgs.clear(); + _speed_limit_req_msgs.clear(); + + for (const auto& [lane_key, lane_state] : _internal_lane_states) + { + if (lane_state == LaneState::Normal) + { + // Normal lane states are handled in request_lane_modifications() + continue; + } + + const auto& obstacles = _lane_to_obstacles_map.at(lane_key); + if (obstacles.size() < _speed_limit_threshold && + lane_state == LaneState::Closed) + { + transition_lane_state(lane_state, LaneState::Normal, lane_key); + } + else if (obstacles.size() < _lane_closure_threshold && + lane_state == LaneState::Closed) + { + transition_lane_state(lane_state, LaneState::SpeedLimited, lane_key); + } + else if (obstacles.size() < _speed_limit_threshold && + lane_state == LaneState::SpeedLimited) + { + transition_lane_state(lane_state, LaneState::Normal, lane_key); + } + } + + publish_lane_req_msgs(); + publish_speed_limit_req_msgs(); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(LaneBlocker) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp new file mode 100644 index 000000000..55f6e2b3c --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -0,0 +1,228 @@ +/* + * 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 SRC__LANE_BLOCKER__LANEBLOCKER_HPP +#define SRC__LANE_BLOCKER__LANEBLOCKER_HPP + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include +#include +#include + +//============================================================================== +/// Modify states of lanes for fleet adapters based on density of obstacles +class LaneBlocker : public rclcpp::Node +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + using Obstacle = rmf_obstacle_msgs::msg::Obstacle; + using NavGraph = rmf_building_map_msgs::msg::Graph; + using TrafficGraph = rmf_traffic::agv::Graph; + using LaneRequest = rmf_fleet_msgs::msg::LaneRequest; + using SpeedLimitRequest = rmf_fleet_msgs::msg::SpeedLimitRequest; + using SpeedLimitedLane = rmf_fleet_msgs::msg::SpeedLimitedLane; + using LaneStates = rmf_fleet_msgs::msg::LaneStates; + using BoundingBox = rmf_obstacle_msgs::msg::BoundingBox3D; + using Header = std_msgs::msg::Header; + + /// Constructor + LaneBlocker( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + void obstacle_cb(const message_filters::MessageEvent& evt); + void process(); + void cull(); + + // Internal data struct + struct ObstacleData + { + rclcpp::Time expiry_time; + int id; + std::string source; + BoundingBox transformed_bbox; + + ObstacleData() {} + + ObstacleData( + rclcpp::Time expiry_time_, + int id_, + const std::string& source_, + BoundingBox transformed_bbox_) + : expiry_time(expiry_time_), + id(id_), + source(std::move(source_)), + transformed_bbox(std::move(transformed_bbox_)) + {} + + // Overload == for hashing + inline bool operator==(const ObstacleData& other) + const + { + const auto lhs_key = LaneBlocker::get_obstacle_key(source, id); + const auto rhs_key = + LaneBlocker::get_obstacle_key(other.source, other.id); + return lhs_key == rhs_key; + } + }; + using ObstacleDataConstSharedPtr = std::shared_ptr; + + static inline std::string get_obstacle_key( + const std::string& source, const std::size_t id) + { + return source + "_" + std::to_string(id); + } + + static inline std::string get_obstacle_key(const ObstacleData& obstacle) + { + return LaneBlocker::get_obstacle_key( + obstacle.source, obstacle.id); + } + + static inline std::string get_lane_key( + const std::string& fleet_name, + const std::size_t lane_index) + { + return fleet_name + "_" + std::to_string(lane_index); + } + + struct ObstacleHash + { + std::size_t operator()( + const ObstacleData& obstacle) const + { + const std::string key = LaneBlocker::get_obstacle_key(obstacle); + return std::hash()(key); + } + }; + + std::optional> + deserialize_key(const std::string& key) const; + + // Modify lanes with changes in number of vicinity obstacles + void request_lane_modifications( + const std::unordered_set& changes); + + enum class LaneState : uint8_t + { + Normal = 0, + Closed, + SpeedLimited, + SpeedUnlimited + }; + + std::unordered_map< + std::string, + LaneState> _internal_lane_states = {}; + + void transition_lane_state( + const LaneState& old_state, + const LaneState& new_state, + const std::string& lane_key); + + void add_lane_open_close_req( + const std::string& lane_key, + const LaneState& desired_state); + + void add_speed_limit_req( + const std::string& lane_key, + const LaneState& desired_state); + + void publish_lane_req_msgs(); + + void publish_speed_limit_req_msgs(); + + void purge_obstacles( + const std::unordered_set& obstacle_keys, + const bool erase_from_buffer = true); + + // Store obstacle after transformation into RMF frame. + // Generate key using get_obstacle_key() + // We cache them based on source + id so that we keep only the latest + // version of that obstacle. + std::unordered_map + _obstacle_buffer = {}; + + // TODO(YV): Based on the current implementation, we should be able to + // cache obstacle_key directly + // Map an obstacle key to the lanes keys in its vicinity + std::unordered_map< + std::string, + std::unordered_set> _obstacle_to_lanes_map = {}; + + // Map lane to a set of obstacles in its vicinity. This is only used to + // check the number of obstacles in the vicinity of a lane. The obstacles + // are represented as their obstacle keys. + std::unordered_map< + std::string, + std::unordered_set> + _lane_to_obstacles_map = {}; + + rclcpp::Subscription::SharedPtr _graph_sub; + rclcpp::Subscription::SharedPtr _lane_states_sub; + rclcpp::Publisher::SharedPtr _lane_closure_pub; + rclcpp::Publisher::SharedPtr _speed_limit_pub; + double _tf2_lookup_duration; + + std::string _rmf_frame; + std::shared_ptr> _obstacle_sub; + std::shared_ptr> _tf2_filter_obstacles; + std::unique_ptr _tf2_buffer; + std::shared_ptr _transform_listener; + + std::unordered_map _traffic_graphs; + std::unordered_map _lane_states; + double _lane_width; + double _obstacle_lane_threshold; + std::chrono::nanoseconds _max_search_duration; + std::chrono::nanoseconds _cull_timer_period; + bool _continuous_checker; + std::size_t _lane_closure_threshold; + std::size_t _speed_limit_threshold; + double _speed_limit; + + rclcpp::TimerBase::SharedPtr _process_timer; + rclcpp::TimerBase::SharedPtr _cull_timer; + + std::mutex _mutex_msgs; + // A map to collate lanes per fleet that need to be opened or closed + std::unordered_map> _lane_req_msgs; + // A map to collate lanes per fleet that need to be speed limited or unlimited + std::unordered_map> _speed_limit_req_msgs; +}; + +#endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt new file mode 100644 index 000000000..2ab8deb82 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt @@ -0,0 +1,28 @@ +file(GLOB_RECURSE unit_test_srcs "*.cpp" "../IntersectionChecker.cpp") + +ament_add_catch2( + test_intersection_checker ${unit_test_srcs} + TIMEOUT 300 +) + +target_link_libraries(test_intersection_checker + PRIVATE + ${rmf_obstacle_msgs_TARGETS} + ${geometry_msgs_TARGETS} + rmf_utils::rmf_utils +) + +target_include_directories(test_intersection_checker + PRIVATE + $ + ${EIGEN3_INCLUDE_DIRS} + ${rmf_obstacle_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${rmf_utils_INCLUDE_DIRS} +) + +install( + TARGETS test_intersection_checker + EXPORT test_intersection_checker + RUNTIME DESTINATION lib/${PROJECT_NAME} +) diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/main.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/main.cpp new file mode 100644 index 000000000..ddf9152e4 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/test/main.cpp @@ -0,0 +1,21 @@ +/* + * 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. + * +*/ + +#define CATCH_CONFIG_MAIN +#include + +// This will create the main(int argc, char* argv[]) entry point for testing diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp new file mode 100644 index 000000000..dad051dff --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -0,0 +1,377 @@ +/* + * 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. + * +*/ + + +#include "../IntersectionChecker.hpp" + +#include + +#include + +#include + +using CollisionGeometry = IntersectionChecker::CollisionGeometry; + +SCENARIO("Test IntersectionChecker") +{ + auto radians = + [](double degree) -> double + { + return degree * M_PI / 180.0; + }; + + WHEN("AABB geometries are not intersecting and 1m apart") + { + double how_much; + const double expected = 1.0; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(4.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("OBB geometries are not intersecting and 1m apart") + { + double how_much; + const double expected = 0.586; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(4.0) + .y(0.0) + .theta(radians(45.0)), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("AABB geometries are overlapping along X-Axis") + { + double how_much; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(2.5) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("AABB geometries are overlapping along Y-Axis") + { + double how_much; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(1.5) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("AABB geometries are overlapping along X & Y-Axis") + { + double how_much; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(2.5) + .y(0.5) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("AABB geometries are touching") + { + double how_much; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(3.0) + .y(0.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("OBB geometries are overlapping along X & Y-Axis") + { + double how_much; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(0.0) + .y(0.0) + .theta(radians(45.0)), + 2.0, + 2.0 + ); + + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(1.414) + .y(1.0) + .theta(0.0), + 2.0, + 2.0 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("Test #1") + { + double how_much; + const double expected = 2.344; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(8.6824) + .y(-10.9616) + .theta(0.25553), + 1.43478, + 0.5); + + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(12.002) + .y(-10.1094) + .theta(0.0), + 0.6, + 0.6 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("Test #2") + { + double how_much; + const double expected = 6.593; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(11.6892) + .y(-3.52843) + .theta(0.29391), + 3.01193, + 0.5 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(12.002) + .y(-10.9738) + .theta(0.0), + 0.6, + 0.6 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("Test #3") + { + double how_much; + const double expected = 4.292; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(9.57985) + .y(-4.6367) + .theta(1.1626), + 3.36581, + 0.5 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(12.1453) + .y(-11.1404) + .theta(0.0), + 0.6, + 0.6 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("Test #4") + { + double how_much; + const double expected = 7.702; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(-3.7801) + .y(-2.48618) + .theta(-2.9587), + 4.76905, + 0.734582 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(6.81831) + .y(-1.99772) + .theta(0.99046), + 0.6, + 0.6 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } + + WHEN("Test #5") + { + double how_much; + const double expected = 17.126; + const auto ob1 = CollisionGeometry( + geometry_msgs::build() + .x(-9.12337) + .y(2.63674) + .theta(7.0577), + 4.92557, + 1.66422 + ); + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() + .x(8.87474) + .y(-5.78416) + .theta(-1.9070), + 0.7, + 1.1 + ); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + REQUIRE_FALSE(intersect); + std::cout <<"expected: " << expected << std::endl; + std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); + } +} diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp new file mode 100644 index 000000000..2a92c084d --- /dev/null +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp @@ -0,0 +1,134 @@ +/* + * 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. + * +*/ + +#include + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +template<> +void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle) +{ + const double resolution = obstacle.data_resolution > 0 ? + obstacle.data_resolution : 0.1; + octomap::OcTree tree(resolution); + + // First convert to octomap::Pointcloud + octomap::Pointcloud cloud; + cloud.reserve(msg.data.size() / msg.point_step); + + sensor_msgs::PointCloud2ConstIterator iter_x(msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(msg, "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) + { + // Check if the point is invalid + if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) + { + cloud.push_back(*iter_x, *iter_y, *iter_z); + } + } + // We assume the point cloud data has its origin at the frame id specified + const octomap::point3d sensor_origin(0.0, 0.0, 0.0); + tree.insertPointCloud(cloud, sensor_origin); + + // Write binary data to ObstacleData msg + std::stringstream datastream; + tree.writeBinaryData(datastream); + const std::string datastring = datastream.str(); + obstacle.data = std::vector(datastring.begin(), datastring.end()); +} + +//============================================================================== +template<> +PointCloud convert( + const Obstacle& msg) +{ + PointCloud cloud; + cloud.header = msg.header; + + octomap::OcTree tree(msg.data_resolution); + // Construct octree + if (msg.data.empty()) + return cloud; + std::stringstream datastream; + datastream.write((const char*) &msg.data[0], msg.data.size()); + tree.readBinaryData(datastream); + + sensor_msgs::PointCloud2Modifier pcd_modifier(cloud); + pcd_modifier.resize(tree.calcNumNodes()); + + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + int number_of_points = 0; + // Note that the non-trivial call to tree->end_leafs() should be done only once for efficiency + for (auto leaf_it = tree.begin_leafs(), + end = tree.end_leafs(); leaf_it != end; + ++leaf_it) + { + const auto& p = leaf_it.getCoordinate(); + *iter_x = p.x(); + *iter_y = p.y(); + *iter_z = p.z(); + ++iter_x; + ++iter_y; + ++iter_z; + ++number_of_points; + } + + cloud.is_bigendian = false; + // If the cloud is unordered, height is 1 and width is the length of + // the point cloud. + cloud.height = 1; + cloud.width = number_of_points; + // Length of a point in bytes, each point has 3 float coordinates + cloud.point_step = sizeof(float) * 3; + cloud.row_step = cloud.width * cloud.point_step; + + sensor_msgs::msg::PointField point_field; + point_field.name = "x"; + point_field.offset = 0; + point_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + point_field.count = 1; + + cloud.fields.push_back(point_field); + point_field.name = "y"; + point_field.offset = 4; + point_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + point_field.count = 1; + + cloud.fields.push_back(point_field); + + point_field.name = "z"; + point_field.offset = 8; + point_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + point_field.count = 1; + + cloud.fields.push_back(point_field); + + cloud.is_dense = true; + return cloud; +} + +} // namespace rmf_obstacle_ros2