From 818ddd8d96901b429c359559d106b177db05afa0 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 11 May 2022 18:13:31 +0800 Subject: [PATCH 01/60] Publish nav graph Signed-off-by: Yadunund --- rmf_fleet_adapter/CMakeLists.txt | 5 ++ .../rmf_fleet_adapter/StandardNames.hpp | 1 + rmf_fleet_adapter/package.xml | 1 + .../agv/FleetUpdateHandle.cpp | 61 +++++++++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 16 +++++ 5 files changed, 84 insertions(+) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index ce99ddd1b..26d2b383f 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -38,6 +38,7 @@ set(dep_pkgs rmf_task_sequence std_msgs rmf_api_msgs + rmf_building_map_msgs websocketpp nlohmann_json nlohmann_json_schema_validator_vendor @@ -89,6 +90,7 @@ target_link_libraries(rmf_fleet_adapter ${rmf_lift_msgs_LIBRARIES} ${rmf_dispenser_msgs_LIBRARIES} ${rmf_ingestor_msgs_LIBRARIES} + ${rmf_building_map_msgs_LIBRARIES} ${websocketpp_LIBRARIES} nlohmann_json_schema_validator ) @@ -109,6 +111,7 @@ target_include_directories(rmf_fleet_adapter ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} ${rmf_api_msgs_INCLUDE_DIRS} + ${rmf_building_map_msgs_INCLUDE_DIRS} ${WEBSOCKETPP_INCLUDE_DIR} ${nlohmann_json_schema_validator_INCLUDE_DIRS} ) @@ -145,6 +148,7 @@ if (BUILD_TESTING) ${rmf_ingestor_msgs_INCLUDE_DIRS} ${rmf_api_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} + ${rmf_building_map_msgs_INCLUDE_DIRS} ${WEBSOCKETPP_INCLUDE_DIR} ${nlohmann_json_schema_validator_INCLUDE_DIRS} ) @@ -161,6 +165,7 @@ if (BUILD_TESTING) rmf_utils::rmf_utils rmf_api_msgs::rmf_api_msgs ${std_msgs_LIBRARIES} + ${rmf_building_map_msgs_LIBRARIES} ${websocketpp_LIBRARIES} nlohmann_json_schema_validator ) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 5a7df07e5..a5fd5f221 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -57,6 +57,7 @@ const std::string DispatchAckTopicName = "rmf_task/dispatch_ack"; const std::string DockSummaryTopicName = "dock_summary"; +const std::string NavGraphTopicName = "nav_graphs"; const std::string LaneClosureRequestTopicName = "lane_closure_requests"; const std::string ClosedLaneTopicName = "closed_lanes"; diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 02782d39a..217379872 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -28,6 +28,7 @@ rmf_task_sequence std_msgs rmf_api_msgs + rmf_building_map_msgs stubborn_buddies stubborn_buddies_msgs diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 61c3e0033..31db6257c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -135,6 +135,67 @@ TaskDeserialization::TaskDeserialization() } +//============================================================================== +void FleetUpdateHandle::Implementation::publish_nav_graph() const +{ + if (nav_graph_pub == nullptr) + return; + + const auto& graph = (*planner)->get_configuration().graph(); + std::unique_ptr msg = std::make_unique(); + // TODO(YV): Serialize Graph in a separate function if it needs to be reused elsewhere + const std::size_t n_waypoints = graph.num_waypoints(); + const std::size_t n_lanes = graph.num_lanes(); + // Populate vertices + for (std::size_t i = 0; i < n_waypoints; ++i) + { + const auto& wp = graph.get_waypoint(i); + const auto& loc = wp.get_location(); + GraphNodeMsg node_msg; + node_msg.x = loc[0]; + node_msg.y = loc[1]; + node_msg.name = wp.name_or_index(); + GraphParamMsg param_msg; + // Add the map name of this waypoint + param_msg.type = param_msg.TYPE_STRING; + param_msg.name = "map_name"; + param_msg.value_string = wp.get_map_name(); + node_msg.params.push_back(param_msg); + // Add other boolean params + param_msg.value_string = ""; + param_msg.type = param_msg.TYPE_BOOL; + param_msg.name = "is_holding_point"; + param_msg.value_bool = wp.is_holding_point(); + node_msg.params.push_back(param_msg); + param_msg.name = "is_passthrough_point"; + param_msg.value_bool = wp.is_passthrough_point(); + node_msg.params.push_back(param_msg); + param_msg.name = "is_parking_spot"; + param_msg.value_bool = wp.is_parking_spot(); + node_msg.params.push_back(param_msg); + param_msg.name = "is_charger"; + param_msg.value_bool = wp.is_charger(); + node_msg.params.push_back(param_msg); + msg->vertices.emplace_back(node_msg); + } + // Populate edges + for (std::size_t i = 0; i < n_lanes; ++i) + { + const auto& lane = graph.get_lane(i); + // All lanes in rmf_traffic::agv::Graph are unidirectional + GraphEdgeMsg edge_msg; + edge_msg.v1_idx = lane.entry().waypoint_index(); + edge_msg.v2_idx = lane.exit().waypoint_index(); + edge_msg.edge_type = edge_msg.EDGE_TYPE_UNIDIRECTIONAL; + msg->edges.emplace_back(edge_msg); + } + + // Populate the fleet name + msg->name = name; + + nav_graph_pub->publish(std::move(msg)); +} + //============================================================================== void FleetUpdateHandle::Implementation::dock_summary_cb( const DockSummary::SharedPtr& msg) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 54e0d9796..3f20e017c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -35,6 +35,8 @@ #include #include +#include + #include #include @@ -310,6 +312,12 @@ class FleetUpdateHandle::Implementation using DockSummarySub = rclcpp::Subscription::SharedPtr; DockSummarySub dock_summary_sub = nullptr; + using GraphMsg = rmf_building_map_msgs::msg::Graph; + using GraphNodeMsg = rmf_building_map_msgs::msg::GraphNode; + using GraphEdgeMsg = rmf_building_map_msgs::msg::GraphEdge; + using GraphParamMsg = rmf_building_map_msgs::msg::Param; + rclcpp::Publisher::SharedPtr nav_graph_pub = nullptr; + mutable rmf_task::Log::Reader log_reader = {}; template @@ -369,6 +377,12 @@ class FleetUpdateHandle::Implementation self->_pimpl->bid_notice_cb(msg, std::move(respond)); }); + // Publisher for navigation graph + handle->_pimpl->nav_graph_pub = + handle->_pimpl->node->create_publisher( + NavGraphTopicName, transient_qos); + handle->_pimpl->publish_nav_graph(); + // Subscribe DockSummary handle->_pimpl->dock_summary_sub = handle->_pimpl->node->create_subscription( @@ -492,6 +506,8 @@ class FleetUpdateHandle::Implementation return handle; } + void publish_nav_graph() const; + void dock_summary_cb(const DockSummary::SharedPtr& msg); void bid_notice_cb( From f337d67211b373bc036878180b001a9924982f84 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 12 May 2022 18:55:47 +0800 Subject: [PATCH 02/60] Added ObstacleManager Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 79 +++++++++++++ .../include/rmf_obstacle_ros2/Detector.hpp | 53 +++++++++ .../rmf_obstacle_ros2/ObstacleManager.hpp | 51 ++++++++ .../include/rmf_obstacle_ros2/Responder.hpp | 49 ++++++++ .../rmf_obstacle_ros2/StandardNames.hpp | 29 +++++ rmf_obstacle_ros2/package.xml | 23 ++++ .../src/rmf_obstacle_ros2/ObstacleManager.cpp | 111 ++++++++++++++++++ 7 files changed, 395 insertions(+) create mode 100644 rmf_obstacle_ros2/CMakeLists.txt create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp create mode 100644 rmf_obstacle_ros2/package.xml create mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt new file mode 100644 index 000000000..ce1254835 --- /dev/null +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -0,0 +1,79 @@ +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 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rmf_utils REQUIRED) +find_package(rmf_obstacle_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +#=============================================================================== +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_LIBRARIES} + ${sensor_msgs_LIBRARIES} + PRIVATE + ${rmf_obstacle_msgs_LIBRARIES} +) + +target_include_directories(rmf_obstacle_ros2 + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + PRIVATE + ${rmf_obstacle_msgs_INCLUDE_DIRS} +) + +target_compile_features(rmf_obstacle_ros2 INTERFACE cxx_std_17) + +ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + sensor_msgs +) + +#=============================================================================== +if(BUILD_TESTING) + find_package(ament_cmake_uncrustify 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 + ) + +endif() + +#=============================================================================== +install( + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install( + TARGETS + rmf_obstacle_ros2 + 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/Detector.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp new file mode 100644 index 000000000..9b634dee4 --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp @@ -0,0 +1,53 @@ +/* + * 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__DETECTOR_HPP +#define RMF_OBSTACLE_ROS2__DETECTOR_HPP + +#include + +#include + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +/// Pure abstract class for detecting and reporting obstacles +class Detector +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + + virtual void initialize(const rclcpp::Node& node) = 0; + + virtual std::string name() const = 0; + + virtual std::optional obstacles() const = 0; + + ~Detector() = default; + +}; + +using DetectorPtr = std::shared_ptr; +using ConstDetectorPtr = std::shared_ptr; + +} // namespace rmf_obstacle_ros2 + +#endif // RMF_OBSTACLE_ROS2__DETECTOR_HPP diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp new file mode 100644 index 000000000..fb2d63a01 --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp @@ -0,0 +1,51 @@ +/* + * 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__OBSTACLEMANAGER_HPP +#define RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP + +#include + +#include + +#include +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +/// Node to detect and publish obstacles to RMF +class ObstacleManager : public rclcpp::Node +{ +public: + // TODO(YV): Since detector and responder will be dynamically loaded plugins, + // move them into constructor implementation. + // The names of the plugins can be read from ROS 2 params. + ObstacleManager( + DetectorPtr detctor, + ResponderPtr responder = nullptr, + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + class Implementation; + +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_obstacle_ros2 + +#endif // RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp new file mode 100644 index 000000000..e36557c99 --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp @@ -0,0 +1,49 @@ +/* + * 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__RESPONDER_HPP +#define RMF_OBSTACLE_ROS2__RESPONDER_HPP + +#include + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +/// Pure abstract class for detecting and reporting obstacles +class Responder +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + + virtual void initialize(const rclcpp::Node& node) = 0; + + virtual void respond(const Obstacles& obstacles) const = 0; + + ~Responder() = default; + +}; + +using ResponderPtr = std::shared_ptr; +using ConstResponderPtr = std::shared_ptr; + +} // namespace rmf_obstacle_ros2 + +#endif // RMF_OBSTACLE_ROS2__RESPONDER_HPP 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..f29ef4f15 --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2019 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 ObstaclesTopicName = "rmf_obstacles"; + +} // namespace rmf_obstacle_ros2 + +#endif // RMF_OBSTACLE_ROS2__STANDARDNAMES_HPP diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml new file mode 100644 index 000000000..e3f9eb94a --- /dev/null +++ b/rmf_obstacle_ros2/package.xml @@ -0,0 +1,23 @@ + + + + rmf_obstacle_ros2 + 2.1.0 + A package contiainings utilities for managing obstacle data + Yadunund + Apache License 2.0 + + ament_cmake + + rclcpp + rclcpp_components + rmf_utils + rmf_obstacle_msgs + sensor_msgs + + ament_cmake_uncrustify + + + ament_cmake + + diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp new file mode 100644 index 000000000..cf90f0762 --- /dev/null +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp @@ -0,0 +1,111 @@ +/* + * 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 { + +//============================================================================== +class ObstacleManager::Implementation +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + Implementation( + DetectorPtr detector_, + ResponderPtr responder_, + std::weak_ptr node_) + { + detector = detector_; + responder = responder_; + node = node_; + + + auto n = node.lock(); + if (!detector) + return; + + detector->initialize(*n); + if (responder) + responder->initialize(*n); + + double rate = n->declare_parameter("rate", 1.0); + const auto timer_rate = + std::chrono::duration_cast( + std::chrono::duration(rate)); + + + detection_pub = n->create_publisher( + ObstaclesTopicName, + rclcpp::QoS(10)); + + detection_timer = n->create_wall_timer( + timer_rate, + [detector = detector, responder = responder, node = node, pub = detection_pub]() + { + const auto obstacles_opt = detector->obstacles(); + if (!obstacles_opt.has_value()) + { + if (auto n = node.lock()) + { + RCLCPP_ERROR( + n->get_logger(), + "No obstacles detected by detector %s", + detector->name().c_str()); + } + } + + const auto& obstacles = obstacles_opt.value(); + if (auto n = node.lock()) + { + RCLCPP_ERROR( + n->get_logger(), + "Detector %s detected %ld obstacles", + detector->name().c_str(), obstacles.obstacles.size()); + } + // Publish obstacles + pub->publish(obstacles); + if (responder) + responder->respond(obstacles); + }); + } + + DetectorPtr detector; + ResponderPtr responder; + std::weak_ptr node; + rclcpp::TimerBase::SharedPtr detection_timer; + rclcpp::Publisher::SharedPtr detection_pub; +}; + +//============================================================================== +ObstacleManager::ObstacleManager( + DetectorPtr detector, + ResponderPtr responder, + const rclcpp::NodeOptions& options) +: Node("obstacle_manager", options), + _pimpl(rmf_utils::make_unique_impl( + std::move(detector), + std::move(responder), + this->ObstacleManager::weak_from_this())) +{ + // Do nothing +} + +} // namespace rmf_obstacle_ros2 From 7447de952eb8272fb70624a1814735c43e19707b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 16 May 2022 10:33:16 +0800 Subject: [PATCH 03/60] Moved ObstacleManager into src Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 30 +++++ .../rmf_obstacle_ros2/ObstacleManager.hpp | 51 -------- .../src/rmf_obstacle_ros2/ObstacleManager.cpp | 111 ------------------ 3 files changed, 30 insertions(+), 162 deletions(-) delete mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp delete mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index ce1254835..eb1a43d0e 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -46,6 +46,35 @@ ament_export_dependencies( sensor_msgs ) +#=============================================================================== +add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) + +target_link_libraries(obstacle_manager + PRIVATE + rmf_utils::rmf_utils + ${rclcpp_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${rmf_obstacle_msgs_LIBRARIES} + ${rclcpp_components_LIBRARIES} + +) + +target_include_directories(obstacle_manager + PRIVATE + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${rmf_obstacle_msgs_INCLUDE_DIRS} +) + +target_compile_features(obstacle_manager INTERFACE cxx_std_17) + +rclcpp_components_register_node(obstacle_manager + PLUGIN "rmf_obstacle_ros::ObstacleManager" + EXECUTABLE obstacle_manager_node) + #=============================================================================== if(BUILD_TESTING) find_package(ament_cmake_uncrustify REQUIRED) @@ -70,6 +99,7 @@ install( install( TARGETS rmf_obstacle_ros2 + obstacle_manager EXPORT rmf_obstacle_ros2 RUNTIME DESTINATION lib/rmf_obstacle_ros2 LIBRARY DESTINATION lib diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp deleted file mode 100644 index fb2d63a01..000000000 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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__OBSTACLEMANAGER_HPP -#define RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP - -#include - -#include - -#include -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -/// Node to detect and publish obstacles to RMF -class ObstacleManager : public rclcpp::Node -{ -public: - // TODO(YV): Since detector and responder will be dynamically loaded plugins, - // move them into constructor implementation. - // The names of the plugins can be read from ROS 2 params. - ObstacleManager( - DetectorPtr detctor, - ResponderPtr responder = nullptr, - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - class Implementation; - -private: - rmf_utils::unique_impl_ptr _pimpl; -}; - -} // namespace rmf_obstacle_ros2 - -#endif // RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp deleted file mode 100644 index cf90f0762..000000000 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * 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 { - -//============================================================================== -class ObstacleManager::Implementation -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - Implementation( - DetectorPtr detector_, - ResponderPtr responder_, - std::weak_ptr node_) - { - detector = detector_; - responder = responder_; - node = node_; - - - auto n = node.lock(); - if (!detector) - return; - - detector->initialize(*n); - if (responder) - responder->initialize(*n); - - double rate = n->declare_parameter("rate", 1.0); - const auto timer_rate = - std::chrono::duration_cast( - std::chrono::duration(rate)); - - - detection_pub = n->create_publisher( - ObstaclesTopicName, - rclcpp::QoS(10)); - - detection_timer = n->create_wall_timer( - timer_rate, - [detector = detector, responder = responder, node = node, pub = detection_pub]() - { - const auto obstacles_opt = detector->obstacles(); - if (!obstacles_opt.has_value()) - { - if (auto n = node.lock()) - { - RCLCPP_ERROR( - n->get_logger(), - "No obstacles detected by detector %s", - detector->name().c_str()); - } - } - - const auto& obstacles = obstacles_opt.value(); - if (auto n = node.lock()) - { - RCLCPP_ERROR( - n->get_logger(), - "Detector %s detected %ld obstacles", - detector->name().c_str(), obstacles.obstacles.size()); - } - // Publish obstacles - pub->publish(obstacles); - if (responder) - responder->respond(obstacles); - }); - } - - DetectorPtr detector; - ResponderPtr responder; - std::weak_ptr node; - rclcpp::TimerBase::SharedPtr detection_timer; - rclcpp::Publisher::SharedPtr detection_pub; -}; - -//============================================================================== -ObstacleManager::ObstacleManager( - DetectorPtr detector, - ResponderPtr responder, - const rclcpp::NodeOptions& options) -: Node("obstacle_manager", options), - _pimpl(rmf_utils::make_unique_impl( - std::move(detector), - std::move(responder), - this->ObstacleManager::weak_from_this())) -{ - // Do nothing -} - -} // namespace rmf_obstacle_ros2 From 05aee96f00c621f493601d5a17bdc97e9e41eaba Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 16 May 2022 12:08:36 +0800 Subject: [PATCH 04/60] Build as ROS 2 component. Dynamically load detector and responder plugins Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 10 +- .../include/rmf_obstacle_ros2/Obstacle.hpp | 37 ++++ .../include/rmf_obstacle_ros2/Responder.hpp | 2 + rmf_obstacle_ros2/package.xml | 1 + .../src/obstacle_manager/ObstacleManager.cpp | 166 ++++++++++++++++++ .../src/obstacle_manager/ObstacleManager.hpp | 46 +++++ .../src/rmf_obstacle_ros2/Obstacle.cpp | 30 ++++ 7 files changed, 290 insertions(+), 2 deletions(-) create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp create mode 100644 rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp create mode 100644 rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp create mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index eb1a43d0e..3af033026 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -11,6 +11,7 @@ include(GNUInstallDirs) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(pluginlib REQUIRED) find_package(rmf_utils REQUIRED) find_package(rmf_obstacle_msgs REQUIRED) find_package(sensor_msgs REQUIRED) @@ -41,6 +42,9 @@ target_include_directories(rmf_obstacle_ros2 target_compile_features(rmf_obstacle_ros2 INTERFACE cxx_std_17) ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET) +ament_export_include_directories( + include +) ament_export_dependencies( rclcpp sensor_msgs @@ -51,12 +55,13 @@ add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) target_link_libraries(obstacle_manager PRIVATE + rmf_obstacle_ros2 rmf_utils::rmf_utils ${rclcpp_LIBRARIES} ${sensor_msgs_LIBRARIES} ${rmf_obstacle_msgs_LIBRARIES} ${rclcpp_components_LIBRARIES} - + ${pluginlib_LIBRARIES} ) target_include_directories(obstacle_manager @@ -67,12 +72,13 @@ target_include_directories(obstacle_manager ${rclcpp_components_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} ${rmf_obstacle_msgs_INCLUDE_DIRS} + ${pluginlib_INCLUDE_DIRS} ) target_compile_features(obstacle_manager INTERFACE cxx_std_17) rclcpp_components_register_node(obstacle_manager - PLUGIN "rmf_obstacle_ros::ObstacleManager" + PLUGIN "rmf_obstacle_ros2::ObstacleManager" EXECUTABLE obstacle_manager_node) #=============================================================================== diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp new file mode 100644 index 000000000..ede10a576 --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp @@ -0,0 +1,37 @@ +/* + * 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_OBSTACLE_HPP +#define RMF_OBSTACLE_ROS2_OBSTACLE_HPP + +#include + +#include + +//============================================================================== +namespace rmf_obstacle_ros2 { + +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using Obstacle = rmf_obstacle_msgs::msg::Obstacle; + +//============================================================================== +static Obstacle convert (const PointCloud2& msg); + + +} // namespace rmf_obstacle_ros2 + +#endif // #indef RMF_OBSTACLE_ROS2_OBSTACLE_HPP \ No newline at end of file diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp index e36557c99..7e85d3a29 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp @@ -35,6 +35,8 @@ class Responder virtual void initialize(const rclcpp::Node& node) = 0; + virtual std::string name() const = 0; + virtual void respond(const Obstacles& obstacles) const = 0; ~Responder() = default; diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index e3f9eb94a..7bc5830ca 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -11,6 +11,7 @@ rclcpp rclcpp_components + pluginlib rmf_utils rmf_obstacle_msgs sensor_msgs diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp new file mode 100644 index 000000000..0673b3564 --- /dev/null +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -0,0 +1,166 @@ +/* + * 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 "ObstacleManager.hpp" +#include +#include +#include + +#include + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +class ObstacleManager::Implementation +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + Implementation( + std::shared_ptr node_) + { + responder = nullptr; + + RCLCPP_INFO( + node_->get_logger(), + "Setting up ObstacleManager..."); + // First argument is the package name of the tempalted base class. + // Second argument is the fully qualified base class type + pluginlib::ClassLoader detector_loader( + "rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"); + pluginlib::ClassLoader responder_loader( + "rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder"); + + // Parameters to receive the fully qualified plugin strings + const std::string detector_plugin = node_->declare_parameter( + "detector_plugin", ""); + + const std::string responder_plugin = node_->declare_parameter( + "responder_plugin", ""); + + // Initialize the detector + try + { + detector = detector_loader.createSharedInstance(detector_plugin); + } + catch(pluginlib::PluginlibException& e) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to load detector plugin provided via the detector_plugin ROS 2 " + "parameter. Please ensure the fully qualified name of the plugin is " + "provided. Detailed error: %s", + e.what()); + return; + } + detector->initialize(*node_); + + // Initialize the detector + try + { + responder = responder_loader.createSharedInstance(responder_plugin); + } + catch(pluginlib::PluginlibException& e) + { + RCLCPP_WARN( + node_->get_logger(), + "Failed to load responder plugin provided via the responder_plugin " + "ROS 2 parameter. Please ensure the fully qualified name of the " + "plugin is provided. The ObstacleManager will not respond to any " + "obstacles detected. Detailed error: %s", + e.what()); + } + if (responder) + responder->initialize(*node_); + + double rate = node_->declare_parameter("rate", 1.0); + const auto timer_rate = + std::chrono::duration_cast( + std::chrono::duration(rate)); + + detection_pub = node_->create_publisher( + ObstaclesTopicName, + rclcpp::QoS(10)); + + // TODO(YV): Bundle capture args into a data struct shared_ptr + detection_timer = node_->create_wall_timer( + timer_rate, + [detector = detector, responder = responder, node = node, pub = detection_pub]() + { + const auto obstacles_opt = detector->obstacles(); + if (!obstacles_opt.has_value()) + { + if (auto n = node.lock()) + { + RCLCPP_INFO( + n->get_logger(), + "No obstacles detected by detector %s", + detector->name().c_str()); + } + } + + const auto& obstacles = obstacles_opt.value(); + if (auto n = node.lock()) + { + RCLCPP_INFO( + n->get_logger(), + "Detector %s detected %ld obstacles", + detector->name().c_str(), obstacles.obstacles.size()); + } + // Publish obstacles + pub->publish(obstacles); + if (responder) + { + responder->respond(obstacles); + if (auto n = node.lock()) + { + RCLCPP_INFO( + n->get_logger(), + "Responder %s has responded to obstacles", + responder->name().c_str()); + } + } + + }); + + node = node_; + + } + + DetectorPtr detector; + ResponderPtr responder; + std::weak_ptr node; + rclcpp::TimerBase::SharedPtr detection_timer; + rclcpp::Publisher::SharedPtr detection_pub; +}; + +//============================================================================== +ObstacleManager::ObstacleManager( + const rclcpp::NodeOptions& options) +: Node("obstacle_manager", options) +{ + _pimpl = rmf_utils::make_unique_impl( + this->ObstacleManager::shared_from_this()); +} + +} // namespace rmf_obstacle_ros2 + +RCLCPP_COMPONENTS_REGISTER_NODE(rmf_obstacle_ros2::ObstacleManager) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp new file mode 100644 index 000000000..e7859434d --- /dev/null +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp @@ -0,0 +1,46 @@ +/* + * 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_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP +#define SRC_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +/// Node to detect and publish obstacles to RMF +class ObstacleManager : public rclcpp::Node +{ +public: + /// The implementation expects users to pass the fully qualified plugin paths + /// for the detector and responder via ROS 2 parameters. The parameter names + /// are detector_plugin and responder_plugin respectively. + ObstacleManager( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + class Implementation; + +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_obstacle_ros2 + +#endif // SRC_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp new file mode 100644 index 000000000..e06d917c4 --- /dev/null +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp @@ -0,0 +1,30 @@ +/* + * 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 + +//============================================================================== +namespace rmf_obstacle_ros2 +{ + +Obstacle convert(const PointCloud2& msg) +{ + Obstacle obstacle_msg; + return obstacle_msg; +} + +} // namespace rmf_obstacle_ros2 From ced9304463e54fa0738067f3bc4901870825f82f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 16 May 2022 12:21:34 +0800 Subject: [PATCH 05/60] Refactor into data struct Signed-off-by: Yadunund --- .../src/obstacle_manager/ObstacleManager.cpp | 234 +++++++++--------- 1 file changed, 121 insertions(+), 113 deletions(-) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index 0673b3564..d2955b9eb 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -34,131 +34,139 @@ class ObstacleManager::Implementation { public: using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + + struct Data + { + DetectorPtr detector; + ResponderPtr responder; + std::weak_ptr node; + rclcpp::TimerBase::SharedPtr detection_timer; + rclcpp::Publisher::SharedPtr detection_pub; + }; + Implementation( - std::shared_ptr node_) + std::shared_ptr data_) { - responder = nullptr; - - RCLCPP_INFO( - node_->get_logger(), - "Setting up ObstacleManager..."); - // First argument is the package name of the tempalted base class. - // Second argument is the fully qualified base class type - pluginlib::ClassLoader detector_loader( - "rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"); - pluginlib::ClassLoader responder_loader( - "rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder"); - - // Parameters to receive the fully qualified plugin strings - const std::string detector_plugin = node_->declare_parameter( - "detector_plugin", ""); - - const std::string responder_plugin = node_->declare_parameter( - "responder_plugin", ""); - - // Initialize the detector - try - { - detector = detector_loader.createSharedInstance(detector_plugin); - } - catch(pluginlib::PluginlibException& e) - { - RCLCPP_ERROR( - node_->get_logger(), - "Failed to load detector plugin provided via the detector_plugin ROS 2 " - "parameter. Please ensure the fully qualified name of the plugin is " - "provided. Detailed error: %s", - e.what()); - return; - } - detector->initialize(*node_); - - // Initialize the detector - try - { - responder = responder_loader.createSharedInstance(responder_plugin); - } - catch(pluginlib::PluginlibException& e) + data = std::move(data_); + } + + std::shared_ptr data; +}; + +//============================================================================== +ObstacleManager::ObstacleManager( + const rclcpp::NodeOptions& options) +: Node("obstacle_manager", options) +{ + + auto data = std::make_shared(); + data->responder = nullptr; + + RCLCPP_INFO( + this->get_logger(), + "Setting up ObstacleManager..."); + + // First argument is the package name of the tempalted base class. + // Second argument is the fully qualified base class type + pluginlib::ClassLoader detector_loader( + "rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"); + pluginlib::ClassLoader responder_loader( + "rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder"); + + // Parameters to receive the fully qualified plugin strings + const std::string detector_plugin = this->declare_parameter( + "detector_plugin", ""); + + const std::string responder_plugin = this->declare_parameter( + "responder_plugin", ""); + + // Initialize the detector + try + { + data->detector = detector_loader.createSharedInstance(detector_plugin); + } + catch(pluginlib::PluginlibException& e) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to load detector plugin provided via the detector_plugin ROS 2 " + "parameter. Please ensure the fully qualified name of the plugin is " + "provided. Detailed error: %s", + e.what()); + return; + } + data->detector->initialize(*this); + + // Initialize the detector + try + { + data->responder = responder_loader.createSharedInstance(responder_plugin); + } + catch(pluginlib::PluginlibException& e) + { + RCLCPP_WARN( + this->get_logger(), + "Failed to load responder plugin provided via the responder_plugin " + "ROS 2 parameter. Please ensure the fully qualified name of the " + "plugin is provided. The ObstacleManager will not respond to any " + "obstacles detected. Detailed error: %s", + e.what()); + } + if (data->responder) + data->responder->initialize(*this); + + double rate = this->declare_parameter("rate", 1.0); + const auto timer_rate = + std::chrono::duration_cast( + std::chrono::duration(rate)); + + data->detection_pub = this->create_publisher( + ObstaclesTopicName, + rclcpp::QoS(10)); + + data->detection_timer = this->create_wall_timer( + timer_rate, + [data = data]() { - RCLCPP_WARN( - node_->get_logger(), - "Failed to load responder plugin provided via the responder_plugin " - "ROS 2 parameter. Please ensure the fully qualified name of the " - "plugin is provided. The ObstacleManager will not respond to any " - "obstacles detected. Detailed error: %s", - e.what()); - } - if (responder) - responder->initialize(*node_); - - double rate = node_->declare_parameter("rate", 1.0); - const auto timer_rate = - std::chrono::duration_cast( - std::chrono::duration(rate)); - - detection_pub = node_->create_publisher( - ObstaclesTopicName, - rclcpp::QoS(10)); - - // TODO(YV): Bundle capture args into a data struct shared_ptr - detection_timer = node_->create_wall_timer( - timer_rate, - [detector = detector, responder = responder, node = node, pub = detection_pub]() + const auto obstacles_opt = data->detector->obstacles(); + if (!obstacles_opt.has_value()) { - const auto obstacles_opt = detector->obstacles(); - if (!obstacles_opt.has_value()) + if (auto n = data->node.lock()) { - if (auto n = node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "No obstacles detected by detector %s", - detector->name().c_str()); - } + RCLCPP_INFO( + n->get_logger(), + "No obstacles detected by detector %s", + data->detector->name().c_str()); } + } - const auto& obstacles = obstacles_opt.value(); - if (auto n = node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "Detector %s detected %ld obstacles", - detector->name().c_str(), obstacles.obstacles.size()); - } - // Publish obstacles - pub->publish(obstacles); - if (responder) + const auto& obstacles = obstacles_opt.value(); + if (auto n = data->node.lock()) + { + RCLCPP_INFO( + n->get_logger(), + "Detector %s detected %ld obstacles", + data->detector->name().c_str(), obstacles.obstacles.size()); + } + // Publish obstacles + data->detection_pub->publish(obstacles); + if (data->responder) + { + data->responder->respond(obstacles); + if (auto n = data->node.lock()) { - responder->respond(obstacles); - if (auto n = node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "Responder %s has responded to obstacles", - responder->name().c_str()); - } + RCLCPP_INFO( + n->get_logger(), + "Responder %s has responded to obstacles", + data->responder->name().c_str()); } + } - }); - - node = node_; + }); - } - - DetectorPtr detector; - ResponderPtr responder; - std::weak_ptr node; - rclcpp::TimerBase::SharedPtr detection_timer; - rclcpp::Publisher::SharedPtr detection_pub; -}; + data->node = this->ObstacleManager::shared_from_this(); -//============================================================================== -ObstacleManager::ObstacleManager( - const rclcpp::NodeOptions& options) -: Node("obstacle_manager", options) -{ - _pimpl = rmf_utils::make_unique_impl( - this->ObstacleManager::shared_from_this()); + _pimpl = rmf_utils::make_unique_impl(data); } } // namespace rmf_obstacle_ros2 From 8ec7d88868d2640ae5a37b64a7ed2c048184154b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 16 May 2022 12:49:16 +0800 Subject: [PATCH 06/60] Uncrustify Signed-off-by: Yadunund --- .../include/rmf_obstacle_ros2/Obstacle.hpp | 2 +- .../src/obstacle_manager/ObstacleManager.cpp | 14 +++++++------- .../src/rmf_obstacle_ros2/Obstacle.cpp | 3 +-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp index ede10a576..bc39c0ee1 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp @@ -29,7 +29,7 @@ using PointCloud2 = sensor_msgs::msg::PointCloud2; using Obstacle = rmf_obstacle_msgs::msg::Obstacle; //============================================================================== -static Obstacle convert (const PointCloud2& msg); +static Obstacle convert(const PointCloud2& msg); } // namespace rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index d2955b9eb..ebad710d1 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -85,7 +85,7 @@ ObstacleManager::ObstacleManager( { data->detector = detector_loader.createSharedInstance(detector_plugin); } - catch(pluginlib::PluginlibException& e) + catch (pluginlib::PluginlibException& e) { RCLCPP_ERROR( this->get_logger(), @@ -102,7 +102,7 @@ ObstacleManager::ObstacleManager( { data->responder = responder_loader.createSharedInstance(responder_plugin); } - catch(pluginlib::PluginlibException& e) + catch (pluginlib::PluginlibException& e) { RCLCPP_WARN( this->get_logger(), @@ -118,7 +118,7 @@ ObstacleManager::ObstacleManager( double rate = this->declare_parameter("rate", 1.0); const auto timer_rate = std::chrono::duration_cast( - std::chrono::duration(rate)); + std::chrono::duration(rate)); data->detection_pub = this->create_publisher( ObstaclesTopicName, @@ -143,10 +143,10 @@ ObstacleManager::ObstacleManager( const auto& obstacles = obstacles_opt.value(); if (auto n = data->node.lock()) { - RCLCPP_INFO( - n->get_logger(), - "Detector %s detected %ld obstacles", - data->detector->name().c_str(), obstacles.obstacles.size()); + RCLCPP_INFO( + n->get_logger(), + "Detector %s detected %ld obstacles", + data->detector->name().c_str(), obstacles.obstacles.size()); } // Publish obstacles data->detection_pub->publish(obstacles); diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp index e06d917c4..dad427c90 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp @@ -18,8 +18,7 @@ #include //============================================================================== -namespace rmf_obstacle_ros2 -{ +namespace rmf_obstacle_ros2 { Obstacle convert(const PointCloud2& msg) { From 89a2088ea7d4ac69ee644c8be73f9b2858b5902d Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 19 May 2022 23:18:12 +0800 Subject: [PATCH 07/60] Serialize using octomap Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 10 ++- .../include/rmf_obstacle_ros2/Obstacle.hpp | 26 ++++-- rmf_obstacle_ros2/package.xml | 1 + .../src/rmf_obstacle_ros2/Obstacle.cpp | 79 ++++++++++++++++++- 4 files changed, 102 insertions(+), 14 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 3af033026..1b768f867 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -15,9 +15,10 @@ find_package(pluginlib REQUIRED) find_package(rmf_utils REQUIRED) find_package(rmf_obstacle_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(octomap REQUIRED) #=============================================================================== -file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/*.cpp") +file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/Obstacle.cpp") add_library(rmf_obstacle_ros2 SHARED ${core_lib_srcs}) target_link_libraries(rmf_obstacle_ros2 @@ -27,6 +28,7 @@ target_link_libraries(rmf_obstacle_ros2 ${sensor_msgs_LIBRARIES} PRIVATE ${rmf_obstacle_msgs_LIBRARIES} + ${OCTOMAP_LIBRARIES} ) target_include_directories(rmf_obstacle_ros2 @@ -37,17 +39,17 @@ target_include_directories(rmf_obstacle_ros2 ${sensor_msgs_INCLUDE_DIRS} PRIVATE ${rmf_obstacle_msgs_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} ) target_compile_features(rmf_obstacle_ros2 INTERFACE cxx_std_17) ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET) -ament_export_include_directories( - include -) + ament_export_dependencies( rclcpp sensor_msgs + rmf_obstacle_msgs ) #=============================================================================== diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp index bc39c0ee1..79c111e77 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp @@ -18,20 +18,34 @@ #ifndef RMF_OBSTACLE_ROS2_OBSTACLE_HPP #define RMF_OBSTACLE_ROS2_OBSTACLE_HPP -#include +#include +#include #include -//============================================================================== +#include + namespace rmf_obstacle_ros2 { -using PointCloud2 = sensor_msgs::msg::PointCloud2; -using Obstacle = rmf_obstacle_msgs::msg::Obstacle; +using Header = std_msgs::msg::Header; +using PointCloud = sensor_msgs::msg::PointCloud2; +using ObstacleData = rmf_obstacle_msgs::msg::ObstacleData; + +// TODO(YV): Consider defining a pure abstract class to perform +// serialization/deserialization. The abstract class could also have a function +// to generate MarkerArrays for rviz visualization. +// Provide a default implementation. +//============================================================================== +/// Serialize a PointCloud2 msg into RMF obstacle octree +static ObstacleData convert(const PointCloud& msg); //============================================================================== -static Obstacle convert(const PointCloud2& msg); +/// Deserialize an RMF obstacle octree into a PointCloud2 msg +static PointCloud convert( + const Header& header, + const ObstacleData& msg); } // namespace rmf_obstacle_ros2 -#endif // #indef RMF_OBSTACLE_ROS2_OBSTACLE_HPP \ No newline at end of file +#endif // #indef RMF_OBSTACLE_ROS2_OBSTACLE_HPP diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 7bc5830ca..963332661 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -15,6 +15,7 @@ rmf_utils rmf_obstacle_msgs sensor_msgs + octomap ament_cmake_uncrustify diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp index dad427c90..f579cfb55 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp @@ -17,13 +17,84 @@ #include -//============================================================================== +#include + +#include + namespace rmf_obstacle_ros2 { -Obstacle convert(const PointCloud2& msg) +//============================================================================== +ObstacleData convert(const PointCloud& msg) +{ + ObstacleData data; + octomap::OcTree tree(data.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 is in global RMF coordinates. + 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(); + data.data = std::vector(datastring.begin(), datastring.end()); + + return data; +} + +//============================================================================== +PointCloud convert( + const Header& header, + const ObstacleData& msg) { - Obstacle obstacle_msg; - return obstacle_msg; + PointCloud cloud; + cloud.header = header; + + octomap::OcTree tree(msg.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"); + + // 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; + } + + // TODO(YV): Fill other fields + return cloud; } } // namespace rmf_obstacle_ros2 From 7f02d1856bde9702239539da7e3a1b03407b360c Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 23 May 2022 23:33:27 +0800 Subject: [PATCH 08/60] Removed ObstacleData Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 10 ++++---- .../include/rmf_obstacle_ros2/Obstacle.hpp | 12 +++------ .../src/rmf_obstacle_ros2/Obstacle.cpp | 25 ++++++++++--------- 3 files changed, 22 insertions(+), 25 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 1b768f867..dd21a6e20 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -15,10 +15,10 @@ find_package(pluginlib REQUIRED) find_package(rmf_utils REQUIRED) find_package(rmf_obstacle_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(octomap REQUIRED) +find_package(OCTOMAP REQUIRED) #=============================================================================== -file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/Obstacle.cpp") +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 @@ -26,8 +26,8 @@ target_link_libraries(rmf_obstacle_ros2 rmf_utils::rmf_utils ${rclcpp_LIBRARIES} ${sensor_msgs_LIBRARIES} - PRIVATE ${rmf_obstacle_msgs_LIBRARIES} + PRIVATE ${OCTOMAP_LIBRARIES} ) @@ -37,8 +37,8 @@ target_include_directories(rmf_obstacle_ros2 $ ${rclcpp_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} - PRIVATE ${rmf_obstacle_msgs_INCLUDE_DIRS} + PRIVATE ${OCTOMAP_INCLUDE_DIRS} ) @@ -100,7 +100,7 @@ endif() #=============================================================================== install( - DIRECTORY include/ + DIRECTORY include/rmf_obstacle_ros2 DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp index 79c111e77..784a23964 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp @@ -20,16 +20,13 @@ #include -#include +#include #include -#include - namespace rmf_obstacle_ros2 { -using Header = std_msgs::msg::Header; using PointCloud = sensor_msgs::msg::PointCloud2; -using ObstacleData = rmf_obstacle_msgs::msg::ObstacleData; +using Obstacle = rmf_obstacle_msgs::msg::Obstacle; // TODO(YV): Consider defining a pure abstract class to perform // serialization/deserialization. The abstract class could also have a function @@ -37,13 +34,12 @@ using ObstacleData = rmf_obstacle_msgs::msg::ObstacleData; // Provide a default implementation. //============================================================================== /// Serialize a PointCloud2 msg into RMF obstacle octree -static ObstacleData convert(const PointCloud& msg); +static void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle); //============================================================================== /// Deserialize an RMF obstacle octree into a PointCloud2 msg static PointCloud convert( - const Header& header, - const ObstacleData& msg); + const Obstacle& msg); } // namespace rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp index f579cfb55..c52e0001b 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp @@ -17,17 +17,18 @@ #include -#include +#include #include namespace rmf_obstacle_ros2 { //============================================================================== -ObstacleData convert(const PointCloud& msg) +void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle) { - ObstacleData data; - octomap::OcTree tree(data.RESOLUTION); + const double resolution = obstacle.data_resolution > 0 ? + obstacle.data_resolution : 0.1; + octomap::OcTree tree(resolution); // First convert to octomap::Pointcloud octomap::Pointcloud cloud; @@ -45,7 +46,7 @@ ObstacleData convert(const PointCloud& msg) cloud.push_back(*iter_x, *iter_y, *iter_z); } } - // We assume the point cloud data is in global RMF coordinates. + // 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); @@ -53,20 +54,17 @@ ObstacleData convert(const PointCloud& msg) std::stringstream datastream; tree.writeBinaryData(datastream); const std::string datastring = datastream.str(); - data.data = std::vector(datastring.begin(), datastring.end()); - - return data; + obstacle.data = std::vector(datastring.begin(), datastring.end()); } //============================================================================== PointCloud convert( - const Header& header, - const ObstacleData& msg) + const Obstacle& msg) { PointCloud cloud; - cloud.header = header; + cloud.header = msg.header; - octomap::OcTree tree(msg.RESOLUTION); + octomap::OcTree tree(msg.data_resolution); // Construct octree if (msg.data.empty()) return cloud; @@ -94,6 +92,9 @@ PointCloud convert( } // TODO(YV): Fill other fields + cloud.is_bigendian = false; + // cloud.point_step = 6; + // cloud.row_step = 6; return cloud; } From 00dc6a1c8ec63a0c815c8c4e87982c89b68a1d43 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 2 Jun 2022 14:12:18 +0800 Subject: [PATCH 09/60] Detector accepts a callback Signed-off-by: Yadunund --- .../include/rmf_obstacle_ros2/Detector.hpp | 18 +++-- .../include/rmf_obstacle_ros2/Responder.hpp | 5 +- .../src/obstacle_manager/ObstacleManager.cpp | 71 ++++++++----------- 3 files changed, 45 insertions(+), 49 deletions(-) diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp index 9b634dee4..d5526824f 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp @@ -30,18 +30,28 @@ namespace rmf_obstacle_ros2 { //============================================================================== /// Pure abstract class for detecting and reporting obstacles +/// This class should be implemented as a plugin using pluginlib class Detector { public: using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + using DetectorCallback = std::function; - virtual void initialize(const rclcpp::Node& node) = 0; - virtual std::string name() const = 0; + /// param[in] node + /// A reference to rclcpp::Node + /// + /// param[in] cb + /// The callback that should be triggered when this detector detects any + /// obstacles. If nullptr, the detector will not do anything. + virtual void initialize( + const rclcpp::Node& node, + DetectorCallback cb) = 0; - virtual std::optional obstacles() const = 0; + /// Get the name of this detector + virtual std::string name() const = 0; - ~Detector() = default; + virtual ~Detector() = default; }; diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp index 7e85d3a29..e5b2b20c4 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp @@ -27,7 +27,8 @@ namespace rmf_obstacle_ros2 { //============================================================================== -/// Pure abstract class for detecting and reporting obstacles +/// Pure abstract class that reacts to obstacles detected. This should be +/// implemented as a plugin using pluginlib. class Responder { public: @@ -39,7 +40,7 @@ class Responder virtual void respond(const Obstacles& obstacles) const = 0; - ~Responder() = default; + virtual ~Responder() = default; }; diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index ebad710d1..765048d25 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -80,24 +80,14 @@ ObstacleManager::ObstacleManager( const std::string responder_plugin = this->declare_parameter( "responder_plugin", ""); - // Initialize the detector - try - { - data->detector = detector_loader.createSharedInstance(detector_plugin); - } - catch (pluginlib::PluginlibException& e) - { - RCLCPP_ERROR( - this->get_logger(), - "Failed to load detector plugin provided via the detector_plugin ROS 2 " - "parameter. Please ensure the fully qualified name of the plugin is " - "provided. Detailed error: %s", - e.what()); - return; - } - data->detector->initialize(*this); - // Initialize the detector + data->node = this->ObstacleManager::shared_from_this(); + + data->detection_pub = this->create_publisher( + ObstaclesTopicName, + rclcpp::QoS(10)); + + // Initialize the responder try { data->responder = responder_loader.createSharedInstance(responder_plugin); @@ -115,32 +105,25 @@ ObstacleManager::ObstacleManager( if (data->responder) data->responder->initialize(*this); - double rate = this->declare_parameter("rate", 1.0); - const auto timer_rate = - std::chrono::duration_cast( - std::chrono::duration(rate)); - - data->detection_pub = this->create_publisher( - ObstaclesTopicName, - rclcpp::QoS(10)); + // Initialize the detector + try + { + data->detector = detector_loader.createSharedInstance(detector_plugin); + } + catch (pluginlib::PluginlibException& e) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to load detector plugin provided via the detector_plugin ROS 2 " + "parameter. Please ensure the fully qualified name of the plugin is " + "provided. Detailed error: %s", + e.what()); + return; + } - data->detection_timer = this->create_wall_timer( - timer_rate, - [data = data]() + auto detection_cb = + [data = data](const Responder::Obstacles& obstacles) { - const auto obstacles_opt = data->detector->obstacles(); - if (!obstacles_opt.has_value()) - { - if (auto n = data->node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "No obstacles detected by detector %s", - data->detector->name().c_str()); - } - } - - const auto& obstacles = obstacles_opt.value(); if (auto n = data->node.lock()) { RCLCPP_INFO( @@ -148,6 +131,7 @@ ObstacleManager::ObstacleManager( "Detector %s detected %ld obstacles", data->detector->name().c_str(), obstacles.obstacles.size()); } + // Publish obstacles data->detection_pub->publish(obstacles); if (data->responder) @@ -161,10 +145,11 @@ ObstacleManager::ObstacleManager( data->responder->name().c_str()); } } + }; - }); - data->node = this->ObstacleManager::shared_from_this(); + data->detector->initialize(*this, detection_cb); + _pimpl = rmf_utils::make_unique_impl(data); } From dd94cde1e871d8d87842ced6b484369cf19c5138 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 2 Jun 2022 17:08:22 +0800 Subject: [PATCH 10/60] Export dependencies Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 35 ++++++++----------- .../src/obstacle_manager/ObstacleManager.cpp | 4 +-- .../src/obstacle_manager/ObstacleManager.hpp | 6 ++-- 3 files changed, 20 insertions(+), 25 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index dd21a6e20..750f6af2a 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -7,15 +7,20 @@ endif() include(GNUInstallDirs) -# find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rmf_utils REQUIRED) -find_package(rmf_obstacle_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(OCTOMAP REQUIRED) + +set(dep_pkgs + rclcpp + rclcpp_components + rmf_utils + pluginlib + rmf_obstacle_msgs + sensor_msgs + OCTOMAP +) +foreach(pkg ${dep_pkgs}) + find_package(${pkg} REQUIRED) +endforeach() #=============================================================================== file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/*.cpp") @@ -46,11 +51,7 @@ target_compile_features(rmf_obstacle_ros2 INTERFACE cxx_std_17) ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET) -ament_export_dependencies( - rclcpp - sensor_msgs - rmf_obstacle_msgs -) +ament_export_dependencies(${dep_pkgs}) #=============================================================================== add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) @@ -58,12 +59,6 @@ add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) target_link_libraries(obstacle_manager PRIVATE rmf_obstacle_ros2 - rmf_utils::rmf_utils - ${rclcpp_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${rmf_obstacle_msgs_LIBRARIES} - ${rclcpp_components_LIBRARIES} - ${pluginlib_LIBRARIES} ) target_include_directories(obstacle_manager @@ -100,7 +95,7 @@ endif() #=============================================================================== install( - DIRECTORY include/rmf_obstacle_ros2 + DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index 765048d25..c0c71008f 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -75,10 +75,10 @@ ObstacleManager::ObstacleManager( // Parameters to receive the fully qualified plugin strings const std::string detector_plugin = this->declare_parameter( - "detector_plugin", ""); + "detector_plugin", "dummy_detector"); const std::string responder_plugin = this->declare_parameter( - "responder_plugin", ""); + "responder_plugin", "dummy_responder"); data->node = this->ObstacleManager::shared_from_this(); diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp index e7859434d..2b77979a3 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp @@ -15,8 +15,8 @@ * */ -#ifndef SRC_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP -#define SRC_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP +#ifndef SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP +#define SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP #include @@ -43,4 +43,4 @@ class ObstacleManager : public rclcpp::Node } // namespace rmf_obstacle_ros2 -#endif // SRC_RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP +#endif // SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP From 6425b22519b97cce03a8ba84ca4353c0536ee6fa Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 2 Jun 2022 20:09:13 +0800 Subject: [PATCH 11/60] Export include dir Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 4 ++++ rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp | 4 +--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 750f6af2a..b8f50500e 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -53,6 +53,10 @@ ament_export_targets(rmf_obstacle_ros2 HAS_LIBRARY_TARGET) ament_export_dependencies(${dep_pkgs}) +ament_export_include_directories( + include +) + #=============================================================================== add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index c0c71008f..965c6f5ec 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -80,9 +80,6 @@ ObstacleManager::ObstacleManager( const std::string responder_plugin = this->declare_parameter( "responder_plugin", "dummy_responder"); - - data->node = this->ObstacleManager::shared_from_this(); - data->detection_pub = this->create_publisher( ObstaclesTopicName, rclcpp::QoS(10)); @@ -150,6 +147,7 @@ ObstacleManager::ObstacleManager( data->detector->initialize(*this, detection_cb); + data->node = this->ObstacleManager::shared_from_this(); _pimpl = rmf_utils::make_unique_impl(data); } From 914cffc90ac97ba0c7e00db44ad5c359ff45eb86 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 2 Jun 2022 21:11:40 +0800 Subject: [PATCH 12/60] Explicitly link pluginlib Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index b8f50500e..908264019 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -63,6 +63,8 @@ add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) target_link_libraries(obstacle_manager PRIVATE rmf_obstacle_ros2 + pluginlib::pluginlib + rclcpp::rclcpp ) target_include_directories(obstacle_manager From 9cb6556787bcda2cf0bc388f371a63849c99547b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 2 Jun 2022 22:24:30 +0800 Subject: [PATCH 13/60] Keep ClassLoaders alive Signed-off-by: Yadunund --- .../src/obstacle_manager/ObstacleManager.cpp | 96 +++++-------------- .../src/obstacle_manager/ObstacleManager.hpp | 18 +++- 2 files changed, 38 insertions(+), 76 deletions(-) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index 965c6f5ec..a7a59d4e7 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -18,76 +18,43 @@ #include "ObstacleManager.hpp" #include -#include -#include #include #include -#include namespace rmf_obstacle_ros2 { -//============================================================================== -class ObstacleManager::Implementation -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - - struct Data - { - DetectorPtr detector; - ResponderPtr responder; - std::weak_ptr node; - rclcpp::TimerBase::SharedPtr detection_timer; - rclcpp::Publisher::SharedPtr detection_pub; - }; - - Implementation( - std::shared_ptr data_) - { - data = std::move(data_); - } - - std::shared_ptr data; -}; - //============================================================================== ObstacleManager::ObstacleManager( const rclcpp::NodeOptions& options) -: Node("obstacle_manager", options) +: Node("obstacle_manager", options), + _detector_loader("rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"), + _responder_loader("rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder") { - auto data = std::make_shared(); - data->responder = nullptr; + _responder = nullptr; RCLCPP_INFO( this->get_logger(), "Setting up ObstacleManager..."); - // First argument is the package name of the tempalted base class. - // Second argument is the fully qualified base class type - pluginlib::ClassLoader detector_loader( - "rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"); - pluginlib::ClassLoader responder_loader( - "rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder"); - // Parameters to receive the fully qualified plugin strings const std::string detector_plugin = this->declare_parameter( - "detector_plugin", "dummy_detector"); + "detector_plugin", ""); const std::string responder_plugin = this->declare_parameter( - "responder_plugin", "dummy_responder"); + "responder_plugin", ""); - data->detection_pub = this->create_publisher( + _detection_pub = this->create_publisher( ObstaclesTopicName, rclcpp::QoS(10)); // Initialize the responder try { - data->responder = responder_loader.createSharedInstance(responder_plugin); + _responder = _responder_loader.createSharedInstance(responder_plugin); } catch (pluginlib::PluginlibException& e) { @@ -99,13 +66,13 @@ ObstacleManager::ObstacleManager( "obstacles detected. Detailed error: %s", e.what()); } - if (data->responder) - data->responder->initialize(*this); + if (_responder != nullptr) + _responder->initialize(*this); // Initialize the detector try { - data->detector = detector_loader.createSharedInstance(detector_plugin); + _detector = _detector_loader.createSharedInstance(detector_plugin); } catch (pluginlib::PluginlibException& e) { @@ -118,38 +85,23 @@ ObstacleManager::ObstacleManager( return; } - auto detection_cb = - [data = data](const Responder::Obstacles& obstacles) - { - if (auto n = data->node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "Detector %s detected %ld obstacles", - data->detector->name().c_str(), obstacles.obstacles.size()); - } + RCLCPP_INFO( + this->get_logger(), + "Successfully loaded detector_plugin: %s", + detector_plugin.c_str()); + - // Publish obstacles - data->detection_pub->publish(obstacles); - if (data->responder) + _detector->initialize(*this, + [pub=_detection_pub, responder = _responder]( + const Responder::Obstacles& obstacles) + { + pub->publish(obstacles); + if (responder) { - data->responder->respond(obstacles); - if (auto n = data->node.lock()) - { - RCLCPP_INFO( - n->get_logger(), - "Responder %s has responded to obstacles", - data->responder->name().c_str()); - } + responder->respond(obstacles); } - }; - - - data->detector->initialize(*this, detection_cb); - - data->node = this->ObstacleManager::shared_from_this(); + }); - _pimpl = rmf_utils::make_unique_impl(data); } } // namespace rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp index 2b77979a3..b521531d7 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp @@ -18,10 +18,15 @@ #ifndef SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP #define SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP -#include - #include +#include +#include + +#include + +#include + namespace rmf_obstacle_ros2 { //============================================================================== @@ -29,16 +34,21 @@ namespace rmf_obstacle_ros2 { class ObstacleManager : public rclcpp::Node { public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; /// The implementation expects users to pass the fully qualified plugin paths /// for the detector and responder via ROS 2 parameters. The parameter names - /// are detector_plugin and responder_plugin respectively. + /// are "detector_plugin" and "responder_plugin" respectively. ObstacleManager( const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); class Implementation; private: - rmf_utils::unique_impl_ptr _pimpl; + pluginlib::ClassLoader _detector_loader; + pluginlib::ClassLoader _responder_loader; + DetectorPtr _detector; + ResponderPtr _responder; + rclcpp::Publisher::SharedPtr _detection_pub; }; } // namespace rmf_obstacle_ros2 From ed1fd4fc79a8a041be56ef1c1b6e5cc5bf1fab1b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 6 Jun 2022 21:20:56 +0800 Subject: [PATCH 14/60] Revert to msg builder API Signed-off-by: Yadunund --- .../agv/FleetUpdateHandle.cpp | 92 ++++++++++++------- 1 file changed, 57 insertions(+), 35 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 2f6c7278e..f284a67c1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -142,56 +142,78 @@ void FleetUpdateHandle::Implementation::publish_nav_graph() const return; const auto& graph = (*planner)->get_configuration().graph(); - std::unique_ptr msg = std::make_unique(); - // TODO(YV): Serialize Graph in a separate function if it needs to be reused elsewhere const std::size_t n_waypoints = graph.num_waypoints(); const std::size_t n_lanes = graph.num_lanes(); + std::vector vertices; + std::vector edges; // Populate vertices for (std::size_t i = 0; i < n_waypoints; ++i) { const auto& wp = graph.get_waypoint(i); const auto& loc = wp.get_location(); - GraphNodeMsg node_msg; - node_msg.x = loc[0]; - node_msg.y = loc[1]; - node_msg.name = wp.name_or_index(); - GraphParamMsg param_msg; - // Add the map name of this waypoint - param_msg.type = param_msg.TYPE_STRING; - param_msg.name = "map_name"; - param_msg.value_string = wp.get_map_name(); - node_msg.params.push_back(param_msg); - // Add other boolean params - param_msg.value_string = ""; - param_msg.type = param_msg.TYPE_BOOL; - param_msg.name = "is_holding_point"; - param_msg.value_bool = wp.is_holding_point(); - node_msg.params.push_back(param_msg); - param_msg.name = "is_passthrough_point"; - param_msg.value_bool = wp.is_passthrough_point(); - node_msg.params.push_back(param_msg); - param_msg.name = "is_parking_spot"; - param_msg.value_bool = wp.is_parking_spot(); - node_msg.params.push_back(param_msg); - param_msg.name = "is_charger"; - param_msg.value_bool = wp.is_charger(); - node_msg.params.push_back(param_msg); - msg->vertices.emplace_back(node_msg); + std::vector params; + params.push_back(rmf_building_map_msgs::build() + .name("map_name") + .type(GraphParamMsg::TYPE_STRING) + .value_int(0) + .value_float(0.0) + .value_string(wp.get_map_name()) + .value_bool(false)); + params.push_back(rmf_building_map_msgs::build() + .name("is_holding_point") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_holding_point())); + params.push_back(rmf_building_map_msgs::build() + .name("is_passthrough_point") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_passthrough_point())); + params.push_back(rmf_building_map_msgs::build() + .name("is_parking_spot") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_parking_spot())); + params.push_back(rmf_building_map_msgs::build() + .name("is_charger") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_charger())); + vertices.push_back(rmf_building_map_msgs::build() + .x(loc[0]) + .y(loc[1]) + .name(wp.name_or_index()) + .params(std::move(params))); } // Populate edges for (std::size_t i = 0; i < n_lanes; ++i) { const auto& lane = graph.get_lane(i); // All lanes in rmf_traffic::agv::Graph are unidirectional - GraphEdgeMsg edge_msg; - edge_msg.v1_idx = lane.entry().waypoint_index(); - edge_msg.v2_idx = lane.exit().waypoint_index(); - edge_msg.edge_type = edge_msg.EDGE_TYPE_UNIDIRECTIONAL; - msg->edges.emplace_back(edge_msg); + edges.push_back( + rmf_building_map_msgs::build() + .v1_idx(lane.entry().waypoint_index()) + .v2_idx(lane.exit().waypoint_index()) + .params({}) + .edge_type(GraphEdgeMsg::EDGE_TYPE_UNIDIRECTIONAL) + ); } - // Populate the fleet name - msg->name = name; + std::unique_ptr msg = std::make_unique( + rmf_building_map_msgs::build() + .name(name) + .vertices(std::move(vertices)) + .edges(std::move(edges)) + .params({}) + ); nav_graph_pub->publish(std::move(msg)); } From ad9ee118581363bb8b2f2fe2181ab2bb33c5c3f8 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 6 Jun 2022 21:37:52 +0800 Subject: [PATCH 15/60] Explicitly include message headers Signed-off-by: Yadunund --- .../rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 14 +++++++------- .../agv/internal_FleetUpdateHandle.hpp | 3 +++ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index f284a67c1..5e441b88e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -152,42 +152,42 @@ void FleetUpdateHandle::Implementation::publish_nav_graph() const const auto& wp = graph.get_waypoint(i); const auto& loc = wp.get_location(); std::vector params; - params.push_back(rmf_building_map_msgs::build() + params.emplace_back(rmf_building_map_msgs::build() .name("map_name") .type(GraphParamMsg::TYPE_STRING) .value_int(0) .value_float(0.0) .value_string(wp.get_map_name()) .value_bool(false)); - params.push_back(rmf_building_map_msgs::build() + params.emplace_back(rmf_building_map_msgs::build() .name("is_holding_point") .type(GraphParamMsg::TYPE_BOOL) .value_int(0) .value_float(0.0) .value_string("") .value_bool(wp.is_holding_point())); - params.push_back(rmf_building_map_msgs::build() + params.emplace_back(rmf_building_map_msgs::build() .name("is_passthrough_point") .type(GraphParamMsg::TYPE_BOOL) .value_int(0) .value_float(0.0) .value_string("") .value_bool(wp.is_passthrough_point())); - params.push_back(rmf_building_map_msgs::build() + params.emplace_back(rmf_building_map_msgs::build() .name("is_parking_spot") .type(GraphParamMsg::TYPE_BOOL) .value_int(0) .value_float(0.0) .value_string("") .value_bool(wp.is_parking_spot())); - params.push_back(rmf_building_map_msgs::build() + params.emplace_back(rmf_building_map_msgs::build() .name("is_charger") .type(GraphParamMsg::TYPE_BOOL) .value_int(0) .value_float(0.0) .value_string("") .value_bool(wp.is_charger())); - vertices.push_back(rmf_building_map_msgs::build() + vertices.emplace_back(rmf_building_map_msgs::build() .x(loc[0]) .y(loc[1]) .name(wp.name_or_index()) @@ -198,7 +198,7 @@ void FleetUpdateHandle::Implementation::publish_nav_graph() const { const auto& lane = graph.get_lane(i); // All lanes in rmf_traffic::agv::Graph are unidirectional - edges.push_back( + edges.emplace_back( rmf_building_map_msgs::build() .v1_idx(lane.entry().waypoint_index()) .v2_idx(lane.exit().waypoint_index()) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 5bde3619e..11a22e6bd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -36,6 +36,9 @@ #include #include +#include +#include +#include #include From 9de2e00ad7ab47e69b7a4b03f1a92bba98a2adce Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 9 Jun 2022 15:51:53 +0800 Subject: [PATCH 16/60] Add skeleton for LaneBlocker Signed-off-by: Yadunund --- .../rmf_fleet_adapter/StandardNames.hpp | 1 + rmf_obstacle_ros2/CMakeLists.txt | 35 ++++++++ .../rmf_obstacle_ros2/StandardNames.hpp | 2 +- rmf_obstacle_ros2/package.xml | 3 + .../src/lane_blocker/LaneBlocker.cpp | 81 +++++++++++++++++++ .../src/lane_blocker/LaneBlocker.hpp | 60 ++++++++++++++ .../src/obstacle_manager/ObstacleManager.cpp | 2 +- 7 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 5a7df07e5..6e7bbd372 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -59,6 +59,7 @@ const std::string DockSummaryTopicName = "dock_summary"; const std::string LaneClosureRequestTopicName = "lane_closure_requests"; const std::string ClosedLaneTopicName = "closed_lanes"; +const std::string LaneStateTopicName = "lane_states"; const std::string InterruptRequestTopicName = "robot_interrupt_request"; diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 908264019..0373a1bdb 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -15,8 +15,11 @@ set(dep_pkgs rmf_utils pluginlib rmf_obstacle_msgs + rmf_fleet_msgs + rmf_building_map_msgs sensor_msgs OCTOMAP + rmf_fleet_adapter ) foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) @@ -84,6 +87,37 @@ rclcpp_components_register_node(obstacle_manager PLUGIN "rmf_obstacle_ros2::ObstacleManager" EXECUTABLE obstacle_manager_node) +#=============================================================================== +add_library(lane_blocker SHARED src/lane_blocker/LaneBlocker.cpp) + +target_link_libraries(lane_blocker + PRIVATE + rclcpp::rclcpp + ${rclcpp_components_LIBRARIES} + ${rmf_obstacle_msgs_LIBRARIES} + ${rmf_fleet_msgs_LIBRARIES} + ${rmf_building_map_msgs_LIBRARIES} + rmf_fleet_adapter::rmf_fleet_adapter +) + +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} +) + +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) @@ -109,6 +143,7 @@ install( TARGETS rmf_obstacle_ros2 obstacle_manager + lane_blocker EXPORT rmf_obstacle_ros2 RUNTIME DESTINATION lib/rmf_obstacle_ros2 LIBRARY DESTINATION lib diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp index f29ef4f15..3219c0aa2 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp @@ -22,7 +22,7 @@ namespace rmf_obstacle_ros2 { -const std::string ObstaclesTopicName = "rmf_obstacles"; +const std::string ObstacleTopicName = "rmf_obstacles"; } // namespace rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 963332661..5c95ee33f 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -14,8 +14,11 @@ pluginlib rmf_utils rmf_obstacle_msgs + rmf_fleet_msgs + rmf_building_map_msgs sensor_msgs octomap + rmf_fleet_adapter ament_cmake_uncrustify 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..18728d09e --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -0,0 +1,81 @@ +/* + * 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 + +#include + +//============================================================================== +LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) +: Node("lane_blocker_node", options) +{ + _data = std::make_shared(); + + _data->lane_closure_pub = this->create_publisher( + rmf_fleet_adapter::LaneClosureRequestTopicName, + rclcpp::SystemDefaultsQoS()); + + _data->obstacle_sub = this->create_subscription( + "rmf_obstacles", + rclcpp::SystemDefaultsQoS(), + [data = _data](const Obstacles& msg) + { + data->obstacle_cb(msg); + }); + + auto transient_qos = rclcpp::SystemDefaultsQoS().reliable().transient_local(); + + _data->graph_sub = this->create_subscription( + "nav_graphs", + transient_qos, + [data = _data](std::shared_ptr msg) + { + data->graph_cb(std::move(msg)); + }); + + _data->lane_states_sub = this->create_subscription( + "lane_states", + rclcpp::SystemDefaultsQoS(), + [data = _data](std::shared_ptr msg) + { + data->lane_states_cb(std::move(msg)); + }); + +} + +//============================================================================== +void LaneBlocker::Data::obstacle_cb(const Obstacles& msg) +{ + +} + +//============================================================================== +void LaneBlocker::Data::graph_cb(std::shared_ptr msg) +{ + +} + +//============================================================================== +void LaneBlocker::Data::lane_states_cb(std::shared_ptr msg) +{ + +} + +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..a2d3cd298 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -0,0 +1,60 @@ +/* + * 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 + +//============================================================================== +/// 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 NavGraph = rmf_building_map_msgs::msg::Graph; + using LaneRequest = rmf_fleet_msgs::msg::LaneRequest; + using LaneStates = rmf_fleet_msgs::msg::LaneStates; + + /// Constructor + LaneBlocker( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + struct Data + { + void obstacle_cb(const Obstacles& msg); + void graph_cb(std::shared_ptr msg); + void lane_states_cb(std::shared_ptr msg); + + rclcpp::Subscription::SharedPtr obstacle_sub; + rclcpp::Subscription::SharedPtr graph_sub; + rclcpp::Subscription::SharedPtr lane_states_sub; + rclcpp::Publisher::SharedPtr lane_closure_pub; + }; + + std::shared_ptr _data; + +}; + + +#endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP \ No newline at end of file diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp index a7a59d4e7..4c9c23b6d 100644 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp @@ -48,7 +48,7 @@ ObstacleManager::ObstacleManager( "responder_plugin", ""); _detection_pub = this->create_publisher( - ObstaclesTopicName, + ObstacleTopicName, rclcpp::QoS(10)); // Initialize the responder From 81dffba67b4cf5f8854139d5a2d98dc4600aed22 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 22 Jun 2022 18:18:55 +0800 Subject: [PATCH 17/60] Use api from rmf_traffic_ros2 for graph serialization Signed-off-by: Yadunund --- .../agv/FleetUpdateHandle.cpp | 80 +-------- .../agv/internal_FleetUpdateHandle.hpp | 6 - rmf_traffic_ros2/CMakeLists.txt | 3 + .../include/rmf_traffic_ros2/agv/Graph.hpp | 24 +++ rmf_traffic_ros2/package.xml | 1 + .../src/rmf_traffic_ros2/convert_Graph.cpp | 169 ++++++++++++++++++ 6 files changed, 203 insertions(+), 80 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 5e441b88e..a48710aac 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -20,6 +20,7 @@ #include #include +#include #include "internal_FleetUpdateHandle.hpp" #include "internal_RobotUpdateHandle.hpp" @@ -141,81 +142,12 @@ void FleetUpdateHandle::Implementation::publish_nav_graph() const if (nav_graph_pub == nullptr) return; - const auto& graph = (*planner)->get_configuration().graph(); - const std::size_t n_waypoints = graph.num_waypoints(); - const std::size_t n_lanes = graph.num_lanes(); - std::vector vertices; - std::vector edges; - // Populate vertices - for (std::size_t i = 0; i < n_waypoints; ++i) - { - const auto& wp = graph.get_waypoint(i); - const auto& loc = wp.get_location(); - std::vector params; - params.emplace_back(rmf_building_map_msgs::build() - .name("map_name") - .type(GraphParamMsg::TYPE_STRING) - .value_int(0) - .value_float(0.0) - .value_string(wp.get_map_name()) - .value_bool(false)); - params.emplace_back(rmf_building_map_msgs::build() - .name("is_holding_point") - .type(GraphParamMsg::TYPE_BOOL) - .value_int(0) - .value_float(0.0) - .value_string("") - .value_bool(wp.is_holding_point())); - params.emplace_back(rmf_building_map_msgs::build() - .name("is_passthrough_point") - .type(GraphParamMsg::TYPE_BOOL) - .value_int(0) - .value_float(0.0) - .value_string("") - .value_bool(wp.is_passthrough_point())); - params.emplace_back(rmf_building_map_msgs::build() - .name("is_parking_spot") - .type(GraphParamMsg::TYPE_BOOL) - .value_int(0) - .value_float(0.0) - .value_string("") - .value_bool(wp.is_parking_spot())); - params.emplace_back(rmf_building_map_msgs::build() - .name("is_charger") - .type(GraphParamMsg::TYPE_BOOL) - .value_int(0) - .value_float(0.0) - .value_string("") - .value_bool(wp.is_charger())); - vertices.emplace_back(rmf_building_map_msgs::build() - .x(loc[0]) - .y(loc[1]) - .name(wp.name_or_index()) - .params(std::move(params))); - } - // Populate edges - for (std::size_t i = 0; i < n_lanes; ++i) - { - const auto& lane = graph.get_lane(i); - // All lanes in rmf_traffic::agv::Graph are unidirectional - edges.emplace_back( - rmf_building_map_msgs::build() - .v1_idx(lane.entry().waypoint_index()) - .v2_idx(lane.exit().waypoint_index()) - .params({}) - .edge_type(GraphEdgeMsg::EDGE_TYPE_UNIDIRECTIONAL) - ); - } - - std::unique_ptr msg = std::make_unique( - rmf_building_map_msgs::build() - .name(name) - .vertices(std::move(vertices)) - .edges(std::move(edges)) - .params({}) - ); + auto msg = rmf_traffic_ros2::convert( + (*planner)->get_configuration().graph(), + name); - nav_graph_pub->publish(std::move(msg)); + if (msg != nullptr) + nav_graph_pub->publish(std::move(msg)); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 11a22e6bd..1ff87a52f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -36,9 +36,6 @@ #include #include -#include -#include -#include #include @@ -316,9 +313,6 @@ class FleetUpdateHandle::Implementation DockSummarySub dock_summary_sub = nullptr; using GraphMsg = rmf_building_map_msgs::msg::Graph; - using GraphNodeMsg = rmf_building_map_msgs::msg::GraphNode; - using GraphEdgeMsg = rmf_building_map_msgs::msg::GraphEdge; - using GraphParamMsg = rmf_building_map_msgs::msg::Param; rclcpp::Publisher::SharedPtr nav_graph_pub = nullptr; mutable rmf_task::Log::Reader log_reader = {}; diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 88342f7c9..cf3707538 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(ament_cmake REQUIRED) find_package(rmf_traffic 2 REQUIRED) find_package(rmf_traffic_msgs REQUIRED) find_package(rmf_site_map_msgs REQUIRED) +find_package(rmf_building_map_msgs REQUIRED) find_package(rmf_fleet_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) @@ -170,6 +171,7 @@ target_link_libraries(rmf_traffic_ros2 nlohmann_json::nlohmann_json ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} + ${rmf_building_map_msgs_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ZLIB::ZLIB @@ -182,6 +184,7 @@ target_include_directories(rmf_traffic_ros2 $ ${rmf_traffic_msgs_INCLUDE_DIRS} ${rmf_site_map_msgs_INCLUDE_DIRS} + ${rmf_building_map_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index 580acafa8..66888353a 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -19,10 +19,34 @@ #include +#include + +#include + +#include +#include + namespace rmf_traffic_ros2 { //============================================================================== rmf_traffic::agv::Graph convert(const rmf_site_map_msgs::msg::SiteMap& from, int graph_idx = 0, double wp_tolerance = 1e-3); +//============================================================================== +/// Convert a valid rmf_building_map_msgs::msg::Graph message to an +/// rmf_traffic::agv::Graph object. +/// Returns nullopt if required fields are missing. +std::optional convert( + const rmf_building_map_msgs::msg::Graph& from); + +//============================================================================== +/// Convert a valid rmf_traffic::agv::Graph object to an +/// rmf_building_map_msgs::msg::Graph message. +/// Returns nullptr if required fields are missing or fleet_name is empty. +/// \note The returned graph may not be suitable for traffic planning & control +/// as the lane events, properties and constraints will not be defined. +std::unique_ptr convert( + const rmf_traffic::agv::Graph& from, const::std::string& fleet_name); + + } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 97d573b2c..33f9ad7ab 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -15,6 +15,7 @@ rmf_traffic_msgs rmf_fleet_msgs rmf_site_map_msgs + rmf_building_map_msgs rclcpp yaml-cpp nlohmann-json-dev diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index c984824cc..9ee9efcbe 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -25,6 +25,12 @@ #include +#include +#include +#include + +#include + namespace rmf_traffic_ros2 { // Usage map[level_idx][truncated_x][truncated_y] = id; @@ -338,4 +344,167 @@ rmf_traffic::agv::Graph json_to_graph( return graph; } +//============================================================================== +std::optional convert( + const rmf_building_map_msgs::msg::Graph& navgraph) +{ + rmf_traffic::agv::Graph graph; + std::unordered_set added_waypoints = {}; + + for (const auto& v : navgraph.vertices) + { + const std::string wp_name = v.name; + std::string map_name = ""; + bool is_holding_point = false; + bool is_passthrough_point = false; + bool is_parking_spot = false; + bool is_charger = false; + Eigen::Vector2d loc = {v.x, v.y}; + for (const auto& p : v.params) + { + if (p.name == "is_holding_point") + { + is_holding_point = p.value_bool; + } + else if (p.name == "is_passthrough_point") + { + is_passthrough_point = p.value_bool; + } + else if (p.name == "is_parking_spot") + { + is_parking_spot = p.value_bool; + } + else if (p.name == "is_charger") + { + is_charger = p.value_bool; + } + else if (p.name == "map_name") + { + map_name = p.value_string; + } + } + // The map_name field is essential for a Graph::Waypoint + if (map_name.empty()) + return std::nullopt; + graph.add_waypoint(map_name, loc) + .set_holding_point(is_holding_point) + .set_passthrough_point(is_passthrough_point) + .set_parking_spot(is_parking_spot) + .set_charger(is_charger); + const auto wp_index = graph.num_waypoints() - 1; + if (!graph.set_key(wp_name, wp_index)) + return std::nullopt; // Duplicate name + added_waypoints.insert(wp_index); + } + for (const auto& e : navgraph.edges) + { + if (added_waypoints.find(e.v1_idx) == added_waypoints.end() || + added_waypoints.find(e.v2_idx) == added_waypoints.end()) + { + return std::nullopt; + } + // TODO(YV): Deserialize events and properties when available + graph.add_lane( + {e.v1_idx}, + {e.v2_idx}); + } + + return graph; +} + +//============================================================================== +std::unique_ptr convert( + const rmf_traffic::agv::Graph& graph, const::std::string& fleet_name) +{ + using GraphMsg = rmf_building_map_msgs::msg::Graph; + using GraphNodeMsg = rmf_building_map_msgs::msg::GraphNode; + using GraphEdgeMsg = rmf_building_map_msgs::msg::GraphEdge; + using GraphParamMsg = rmf_building_map_msgs::msg::Param; + + if (fleet_name.empty()) + return nullptr; + + const std::size_t n_waypoints = graph.num_waypoints(); + const std::size_t n_lanes = graph.num_lanes(); + + if (n_waypoints == 0 || n_lanes == 0) + return nullptr; + + std::vector vertices; + std::vector edges; + // Populate vertices + for (std::size_t i = 0; i < n_waypoints; ++i) + { + const auto& wp = graph.get_waypoint(i); + const auto& loc = wp.get_location(); + std::vector params; + params.emplace_back(rmf_building_map_msgs::build() + .name("map_name") + .type(GraphParamMsg::TYPE_STRING) + .value_int(0) + .value_float(0.0) + .value_string(wp.get_map_name()) + .value_bool(false)); + params.emplace_back(rmf_building_map_msgs::build() + .name("is_holding_point") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_holding_point())); + params.emplace_back(rmf_building_map_msgs::build() + .name("is_passthrough_point") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_passthrough_point())); + params.emplace_back(rmf_building_map_msgs::build() + .name("is_parking_spot") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_parking_spot())); + params.emplace_back(rmf_building_map_msgs::build() + .name("is_charger") + .type(GraphParamMsg::TYPE_BOOL) + .value_int(0) + .value_float(0.0) + .value_string("") + .value_bool(wp.is_charger())); + const std::string wp_name = wp.name() ? *wp.name() : ""; + vertices.emplace_back(rmf_building_map_msgs::build() + .x(loc[0]) + .y(loc[1]) + .name(wp_name) + .params(std::move(params))); + } + // Populate edges + // TODO(YV): Serialize entry and exit events along with properties + // into params + for (std::size_t i = 0; i < n_lanes; ++i) + { + const auto& lane = graph.get_lane(i); + // All lanes in rmf_traffic::agv::Graph are unidirectional + edges.emplace_back( + rmf_building_map_msgs::build() + .v1_idx(lane.entry().waypoint_index()) + .v2_idx(lane.exit().waypoint_index()) + .params({}) + .edge_type(GraphEdgeMsg::EDGE_TYPE_UNIDIRECTIONAL) + ); + } + + std::unique_ptr msg = std::make_unique( + rmf_building_map_msgs::build() + .name(fleet_name) + .vertices(std::move(vertices)) + .edges(std::move(edges)) + .params({}) + ); + + return msg; +} + } // namespace rmf_traffic_ros2 From e88eb73161bdd027fae6faf3d1e73fb37879259d Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 22 Jun 2022 18:22:23 +0800 Subject: [PATCH 18/60] Fix typo Signed-off-by: Yadunund --- rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp | 2 +- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp index 66888353a..9cd039d31 100644 --- a/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp +++ b/rmf_traffic_ros2/include/rmf_traffic_ros2/agv/Graph.hpp @@ -46,7 +46,7 @@ std::optional convert( /// \note The returned graph may not be suitable for traffic planning & control /// as the lane events, properties and constraints will not be defined. std::unique_ptr convert( - const rmf_traffic::agv::Graph& from, const::std::string& fleet_name); + const rmf_traffic::agv::Graph& from, const std::string& fleet_name); } // namespace rmf_traffic_ros2 diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 9ee9efcbe..b4a62f542 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -414,7 +414,7 @@ std::optional convert( //============================================================================== std::unique_ptr convert( - const rmf_traffic::agv::Graph& graph, const::std::string& fleet_name) + const rmf_traffic::agv::Graph& graph, const std::string& fleet_name) { using GraphMsg = rmf_building_map_msgs::msg::Graph; using GraphNodeMsg = rmf_building_map_msgs::msg::GraphNode; From eaa61dfca91822f3eaaf4dbfa191004e6ebb2305 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 23 Jun 2022 10:12:28 +0800 Subject: [PATCH 19/60] Uncrustify Signed-off-by: Yadunund --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index b4a62f542..f74b00a37 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -393,7 +393,7 @@ std::optional convert( .set_charger(is_charger); const auto wp_index = graph.num_waypoints() - 1; if (!graph.set_key(wp_name, wp_index)) - return std::nullopt; // Duplicate name + return std::nullopt; added_waypoints.insert(wp_index); } for (const auto& e : navgraph.edges) From 4e9bee77e4343163dc927624c79ff56cc50b511b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 28 Jun 2022 19:38:58 +0800 Subject: [PATCH 20/60] Implement speed limit API and publish lane_states Signed-off-by: Yadunund --- .../rmf_fleet_adapter/StandardNames.hpp | 1 + .../agv/FleetUpdateHandle.hpp | 33 +++++ .../agv/FleetUpdateHandle.cpp | 136 ++++++++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 15 ++ 4 files changed, 185 insertions(+) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 5a7df07e5..8c7e2188d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -59,6 +59,7 @@ const std::string DockSummaryTopicName = "dock_summary"; const std::string LaneClosureRequestTopicName = "lane_closure_requests"; const std::string ClosedLaneTopicName = "closed_lanes"; +const std::string LaneStatesTopicName = "lane_states"; const std::string InterruptRequestTopicName = "robot_interrupt_request"; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 609ef829a..2863e34d0 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -206,6 +206,39 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// Specify a set of lanes that should be open. void open_lanes(std::vector lane_indices); + /// A class used to describe speed limit imposed on lanes. + class SpeedLimitRequest + { + public: + /// Constructor + /// + /// \param[in] lane_index + /// The index of the lane to impose a speed limit upon. + /// + /// \param[in] speed_limit + /// The speed limit to be imposed for this lane. + SpeedLimitRequest( + std::size_t lane_index, + double speed_limit); + + /// Get the lane_index + std::size_t lane_index() const; + + /// Get the speed_limit + double speed_limit() const; + + class Implementation; + + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Impose speed limits on specified lanes. + void limit_lane_speeds(std::vector requests); + + /// Remove speed limits from specified lanes. + void remove_speed_limits(std::vector requests); + /// Set the parameters required for task planning. Without calling this /// function, this fleet will not bid for and accept tasks. /// diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 58132d401..6ba9c46d9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -1510,13 +1511,18 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); for (const auto& lane : lane_indices) + { new_lane_closures.close(lane); + // Bookkeeping + self->_pimpl->closed_lanes.insert(lane); + } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); self->_pimpl->task_parameters->planner(*self->_pimpl->planner); + self->_pimpl->publish_lane_states(); }); } @@ -1552,16 +1558,146 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); for (const auto& lane : lane_indices) + { new_lane_closures.open(lane); + // Bookkeeping + self->_pimpl->closed_lanes.erase(lane); + } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); self->_pimpl->task_parameters->planner(*self->_pimpl->planner); + self->_pimpl->publish_lane_states(); }); } +//============================================================================== +class FleetUpdateHandle::SpeedLimitRequest::Implementation +{ +public: + std::size_t lane_index; + double speed_limit; +}; + +//============================================================================== +FleetUpdateHandle::SpeedLimitRequest::SpeedLimitRequest( + std::size_t lane_index, + double speed_limit) +: _pimpl(rmf_utils::make_impl(Implementation{ + std::move(lane_index), + std::move(speed_limit) + })) +{ + // Do nothing +} + +//============================================================================== +std::size_t FleetUpdateHandle::SpeedLimitRequest::lane_index() const +{ + return _pimpl->lane_index; +} + +//============================================================================== +double FleetUpdateHandle::SpeedLimitRequest::speed_limit() const +{ + return _pimpl->speed_limit; +} + +//============================================================================== +auto FleetUpdateHandle::limit_lane_speeds( + std::vector requests) -> void +{ + _pimpl->worker.schedule( + [w = weak_from_this(), requests = std::move(requests)](const auto&) + { + if (requests.empty()) + return; + const auto self = w.lock(); + if (!self) + return; + + auto new_config = (*self->_pimpl->planner)->get_configuration(); + auto& new_graph = new_config.graph(); + for (const auto& request : requests) + { + // TODO: Check if planner supports negative speed limits. + if (request.lane_index() >= new_graph.num_lanes() || + request.speed_limit() < 0.0) + continue; + auto& properties = + new_graph.get_lane(request.lane_index()).properties(); + properties.speed_limit(request.speed_limit()); + // Bookkeeping + self->_pimpl->speed_limited_lanes[request.lane_index()] = + request.speed_limit(); + } + + *self->_pimpl->planner = + std::make_shared( + new_config, rmf_traffic::agv::Planner::Options(nullptr)); + + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); + self->_pimpl->publish_lane_states(); + }); +} + +//============================================================================== +void FleetUpdateHandle::remove_speed_limits(std::vector requests) +{ + _pimpl->worker.schedule( + [w = weak_from_this(), requests = std::move(requests)](const auto&) + { + if (requests.empty()) + return; + const auto self = w.lock(); + if (!self) + return; + + auto new_config = (*self->_pimpl->planner)->get_configuration(); + auto& new_graph = new_config.graph(); + for (const auto& request : requests) + { + + if (auto it = self->_pimpl->speed_limited_lanes.find(request) == + self->_pimpl->speed_limited_lanes.end()) + continue; + auto& properties = + new_graph.get_lane(request).properties(); + properties.speed_limit(std::nullopt); + // Bookkeeping + self->_pimpl->speed_limited_lanes.erase(request); + } + + *self->_pimpl->planner = + std::make_shared( + new_config, rmf_traffic::agv::Planner::Options(nullptr)); + + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); + self->_pimpl->publish_lane_states(); + }); +} + +//============================================================================== +void FleetUpdateHandle::Implementation::publish_lane_states() const +{ + if (lane_states_pub == nullptr) + return; + auto msg = std::make_unique(); + msg->fleet_name = name; + for (const auto& index : closed_lanes) + msg->closed_lanes.push_back(index); + for (const auto& [index, limit] : speed_limited_lanes) + { + // TODO(YV): Use type_adapter in Humble to avoid this copy + msg->speed_limits.push_back( + rmf_fleet_msgs::build() + .lane_index(index) + .speed_limit(limit)); + } + lane_states_pub->publish(std::move(msg)); +} //============================================================================== FleetUpdateHandle& FleetUpdateHandle::accept_task_requests( AcceptTaskRequest check) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index acb7d3d0e..0f4ac9fcb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -312,6 +313,11 @@ class FleetUpdateHandle::Implementation mutable rmf_task::Log::Reader log_reader = {}; + using LaneStates = rmf_fleet_msgs::msg::LaneStates; + rclcpp::Publisher::SharedPtr lane_states_pub = nullptr; + std::unordered_map speed_limited_lanes = {}; + std::unordered_set closed_lanes = {}; + template static std::shared_ptr make(Args&& ... args) { @@ -380,6 +386,13 @@ class FleetUpdateHandle::Implementation self->_pimpl->dock_summary_cb(msg); }); + // Publish LaneStates + handle->_pimpl->lane_states_pub = + handle->_pimpl->node->create_publisher( + LaneStatesTopicName, + transient_qos); + handle->_pimpl->publish_lane_states(); + // Populate charging waypoints const auto& graph = (*handle->_pimpl->planner)->get_configuration().graph(); for (std::size_t i = 0; i < graph.num_waypoints(); ++i) @@ -535,6 +548,8 @@ class FleetUpdateHandle::Implementation void publish_fleet_state_topic() const; + void publish_lane_states() const; + void update_fleet() const; void update_fleet_state() const; From 2e14cb4485e8c3348a1039ad4e25246311db8c04 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 28 Jun 2022 20:05:55 +0800 Subject: [PATCH 21/60] Added speed limit request interface to legacy full_control Signed-off-by: Yadunund --- .../rmf_fleet_adapter/StandardNames.hpp | 1 + rmf_fleet_adapter/src/full_control/main.cpp | 34 +++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 8c7e2188d..4fdf1d610 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -59,6 +59,7 @@ const std::string DockSummaryTopicName = "dock_summary"; const std::string LaneClosureRequestTopicName = "lane_closure_requests"; const std::string ClosedLaneTopicName = "closed_lanes"; +const std::string SpeedLimitRequestTopicName = "speed_limit_requests"; const std::string LaneStatesTopicName = "lane_states"; const std::string InterruptRequestTopicName = "robot_interrupt_request"; diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index daad0532c..3de3d32ef 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include // RMF Task messages @@ -760,6 +761,10 @@ struct Connections : public std::enable_shared_from_this rclcpp::Publisher::SharedPtr closed_lanes_pub; + /// The topic subscription for listening for speed limit requests + rclcpp::Subscription::SharedPtr + speed_limit_request_sub; + /// Container for remembering which lanes are currently closed std::unordered_set closed_lanes; @@ -1018,6 +1023,35 @@ std::shared_ptr make_fleet( connections->closed_lanes_pub->publish(state_msg); }); + connections->speed_limit_request_sub = + adapter->node()->create_subscription< + rmf_fleet_msgs::msg::SpeedLimitRequest>( + rmf_fleet_adapter::SpeedLimitRequestTopicName, + rclcpp::SystemDefaultsQoS(), + [w = connections->weak_from_this(), fleet_name]( + rmf_fleet_msgs::msg::SpeedLimitRequest::ConstSharedPtr request_msg) + { + const auto connections = w.lock(); + if (!connections) + return; + + if (request_msg->fleet_name != fleet_name && + !request_msg->fleet_name.empty()) + return; + + std::vector + requests; + for (const auto& limit : request_msg->speed_limits) + { + auto request = + rmf_fleet_adapter::agv::FleetUpdateHandle::SpeedLimitRequest( + limit.lane_index, limit.speed_limit); + requests.push_back(std::move(request)); + } + connections->fleet->limit_lane_speeds(requests); + connections->fleet->remove_speed_limits(request_msg->remove_limits); + }); + connections->interrupt_request_sub = adapter->node()->create_subscription( rmf_fleet_adapter::InterruptRequestTopicName, From a43540f3584b3c0786edf70b4dbf765d94e897d7 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 28 Jun 2022 20:11:20 +0800 Subject: [PATCH 22/60] Uncrustify Signed-off-by: Yadunund --- .../agv/FleetUpdateHandle.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 6ba9c46d9..29c2ac2eb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1586,9 +1586,9 @@ FleetUpdateHandle::SpeedLimitRequest::SpeedLimitRequest( std::size_t lane_index, double speed_limit) : _pimpl(rmf_utils::make_impl(Implementation{ - std::move(lane_index), - std::move(speed_limit) - })) + std::move(lane_index), + std::move(speed_limit) + })) { // Do nothing } @@ -1624,14 +1624,14 @@ auto FleetUpdateHandle::limit_lane_speeds( { // TODO: Check if planner supports negative speed limits. if (request.lane_index() >= new_graph.num_lanes() || - request.speed_limit() < 0.0) + request.speed_limit() < 0.0) continue; - auto& properties = - new_graph.get_lane(request.lane_index()).properties(); + auto& properties = new_graph.get_lane( + request.lane_index()).properties(); properties.speed_limit(request.speed_limit()); // Bookkeeping self->_pimpl->speed_limited_lanes[request.lane_index()] = - request.speed_limit(); + request.speed_limit(); } *self->_pimpl->planner = @@ -1661,10 +1661,10 @@ void FleetUpdateHandle::remove_speed_limits(std::vector requests) { if (auto it = self->_pimpl->speed_limited_lanes.find(request) == - self->_pimpl->speed_limited_lanes.end()) + self->_pimpl->speed_limited_lanes.end()) continue; - auto& properties = - new_graph.get_lane(request).properties(); + auto& properties = new_graph.get_lane( + request).properties(); properties.speed_limit(std::nullopt); // Bookkeeping self->_pimpl->speed_limited_lanes.erase(request); From 53117169f3f6872638d44d8386caa2e0315abf02 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 29 Jun 2022 20:16:19 +0800 Subject: [PATCH 23/60] Compute transfroms to rmf frame Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 9 + rmf_obstacle_ros2/package.xml | 3 + .../src/lane_blocker/LaneBlocker.cpp | 164 ++++++++++++++---- .../src/lane_blocker/LaneBlocker.hpp | 34 ++-- 4 files changed, 168 insertions(+), 42 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 0373a1bdb..5330b8222 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -20,6 +20,9 @@ set(dep_pkgs sensor_msgs OCTOMAP rmf_fleet_adapter + tf2_ros + geometry_msgs + tf2_geometry_msgs ) foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) @@ -98,6 +101,9 @@ target_link_libraries(lane_blocker ${rmf_fleet_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} rmf_fleet_adapter::rmf_fleet_adapter + ${tf2_ros_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${tf2_geometry_msgs_LIBRARIES} ) target_include_directories(lane_blocker @@ -110,6 +116,9 @@ target_include_directories(lane_blocker ${rmf_fleet_msgs_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} ${rmf_fleet_adapter_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} ) target_compile_features(lane_blocker INTERFACE cxx_std_17) diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 5c95ee33f..88f4cfe02 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -19,6 +19,9 @@ sensor_msgs octomap rmf_fleet_adapter + tf2_ros + tf2_geometry_msgs + geometry_msgs ament_cmake_uncrustify diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 18728d09e..2a2457e78 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -18,64 +18,166 @@ #include "LaneBlocker.hpp" +#include #include +#include +#include +#include + +#include + #include //============================================================================== LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) : Node("lane_blocker_node", options) { - _data = std::make_shared(); - _data->lane_closure_pub = this->create_publisher( + _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.5); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter obstacle_lane_threshold to %f", _obstacle_lane_threshold + ); + + _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 + ); + + _lane_closure_pub = this->create_publisher( rmf_fleet_adapter::LaneClosureRequestTopicName, rclcpp::SystemDefaultsQoS()); - _data->obstacle_sub = this->create_subscription( + _speed_limit_pub = this->create_publisher( + rmf_fleet_adapter::SpeedLimitRequestTopicName, + rclcpp::SystemDefaultsQoS()); + + _obstacle_sub = this->create_subscription( "rmf_obstacles", - rclcpp::SystemDefaultsQoS(), - [data = _data](const Obstacles& msg) + rclcpp::QoS(10).best_effort(), + [=](Obstacles::ConstSharedPtr msg) { - data->obstacle_cb(msg); + obstacle_cb(*msg); }); - auto transient_qos = rclcpp::SystemDefaultsQoS().reliable().transient_local(); + // 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; - _data->graph_sub = this->create_subscription( + _graph_sub = this->create_subscription( "nav_graphs", transient_qos, - [data = _data](std::shared_ptr msg) + [=](NavGraph::ConstSharedPtr msg) { - data->graph_cb(std::move(msg)); - }); - - _data->lane_states_sub = this->create_subscription( + 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()); + }, + ipc_sub_options); + + _lane_states_sub = this->create_subscription( "lane_states", - rclcpp::SystemDefaultsQoS(), - [data = _data](std::shared_ptr msg) + transient_qos, + [=](LaneStates::ConstSharedPtr msg) { - data->lane_states_cb(std::move(msg)); - }); - -} - -//============================================================================== -void LaneBlocker::Data::obstacle_cb(const Obstacles& msg) -{ - -} - -//============================================================================== -void LaneBlocker::Data::graph_cb(std::shared_ptr msg) -{ + if (msg->fleet_name.empty()) + return; + _lane_states[msg->fleet_name] = msg; + }, + ipc_sub_options); } //============================================================================== -void LaneBlocker::Data::lane_states_cb(std::shared_ptr msg) +void LaneBlocker::obstacle_cb(const Obstacles& msg) { - + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + + if (msg.obstacles.empty() || _transform_listener == nullptr) + return; + + // TODO(YV): Consider using tf2_ros::MessageFilter instead of this callback + for (const auto& obstacle : msg.obstacles) + { + const auto& obstacle_frame = obstacle.header.frame_id; + std::string tf2_error; + const bool can_transform = _tf2_buffer->canTransform( + _rmf_frame, + obstacle_frame, + tf2::TimePointZero, + tf2::durationFromSec(_tf2_lookup_duration), + &tf2_error); + + // TODO(YV): Cache these transforms since most of them would be static + if (!can_transform) + { + RCLCPP_WARN( + this->get_logger(), + "Unable to lookup transform between between obstacle frame %s and RMF " + "frame %s.", obstacle_frame.c_str(), _rmf_frame.c_str() + ); + continue; + } + + 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_INFO( + 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 + ); + } } 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 index a2d3cd298..38c969364 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -20,11 +20,19 @@ #include +#include + #include #include #include +#include #include +#include +#include + +#include + //============================================================================== /// Modify states of lanes for fleet adapters based on density of obstacles class LaneBlocker : public rclcpp::Node @@ -32,7 +40,9 @@ class LaneBlocker : public rclcpp::Node public: using Obstacles = rmf_obstacle_msgs::msg::Obstacles; 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 LaneStates = rmf_fleet_msgs::msg::LaneStates; /// Constructor @@ -40,21 +50,23 @@ class LaneBlocker : public rclcpp::Node const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - struct Data - { void obstacle_cb(const Obstacles& msg); - void graph_cb(std::shared_ptr msg); - void lane_states_cb(std::shared_ptr msg); - rclcpp::Subscription::SharedPtr obstacle_sub; - rclcpp::Subscription::SharedPtr graph_sub; - rclcpp::Subscription::SharedPtr lane_states_sub; - rclcpp::Publisher::SharedPtr lane_closure_pub; - }; + rclcpp::Subscription::SharedPtr _obstacle_sub; + 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::shared_ptr _data; + std::string _rmf_frame; + std::unique_ptr _tf2_buffer; + std::shared_ptr _transform_listener; + std::unordered_map _traffic_graphs; + std::unordered_map _lane_states; + double _obstacle_lane_threshold; }; -#endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP \ No newline at end of file +#endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP From 4681128e7547abe995a7e97fd436b9d8a2d7c19f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 30 Jun 2022 20:35:49 +0800 Subject: [PATCH 24/60] Added skeletop for processing Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 14 ++- rmf_obstacle_ros2/package.xml | 3 + .../src/lane_blocker/IntersectionChecker.cpp | 31 ++++++ .../src/lane_blocker/IntersectionChecker.hpp | 44 +++++++++ .../src/lane_blocker/LaneBlocker.cpp | 96 ++++++++++++++++++- .../src/lane_blocker/LaneBlocker.hpp | 90 +++++++++++++++++ 6 files changed, 276 insertions(+), 2 deletions(-) create mode 100644 rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp create mode 100644 rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 5330b8222..32ebead35 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -15,6 +15,7 @@ set(dep_pkgs rmf_utils pluginlib rmf_obstacle_msgs + vision_msgs rmf_fleet_msgs rmf_building_map_msgs sensor_msgs @@ -23,6 +24,8 @@ set(dep_pkgs tf2_ros geometry_msgs tf2_geometry_msgs + rmf_traffic + rmf_traffic_ros2 ) foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) @@ -91,16 +94,22 @@ rclcpp_components_register_node(obstacle_manager EXECUTABLE obstacle_manager_node) #=============================================================================== -add_library(lane_blocker SHARED src/lane_blocker/LaneBlocker.cpp) +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_LIBRARIES} ${rmf_obstacle_msgs_LIBRARIES} + ${vision_msgs_LIBRARIES} ${rmf_fleet_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} rmf_fleet_adapter::rmf_fleet_adapter + rmf_traffic::rmf_traffic + ${rmf_traffic_ros2_LIBRARIES} ${tf2_ros_LIBRARIES} ${geometry_msgs_LIBRARIES} ${tf2_geometry_msgs_LIBRARIES} @@ -113,9 +122,12 @@ target_include_directories(lane_blocker ${rclcpp_INCLUDE_DIRS} ${rclcpp_components_INCLUDE_DIRS} ${rmf_obstacle_msgs_INCLUDE_DIRS} + ${vision_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} diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 88f4cfe02..1eb5464aa 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -14,10 +14,13 @@ pluginlib rmf_utils rmf_obstacle_msgs + vision_msgs rmf_fleet_msgs rmf_building_map_msgs sensor_msgs octomap + rmf_traffic + rmf_traffic_ros2 rmf_fleet_adapter tf2_ros tf2_geometry_msgs 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..e10286b35 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -0,0 +1,31 @@ +/* + * 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" + + +bool IntersectionChecker::between( + const Lane& lane, + const double lane_width, + const BoundingBox& obstacle, + double& how_much) +{ + + return false; + +} + 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..415fc9961 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -0,0 +1,44 @@ +/* + * 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 + +#include + +// TODO(YV): Consider making this a loadable plugin via pluginlib +namespace IntersectionChecker { + +using Lane = rmf_traffic::agv::Graph::Lane; +using BoundingBox = vision_msgs::msg::BoundingBox3D; + +// Return true if intersect. +// If intersect, how_much represents the overlap in meters +// If not intersect, how_much represents the shortest distance. +bool between( + const Lane& lane, + const double lane_width, + const BoundingBox& obstacle, + 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 index 2a2457e78..ec2205390 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -17,6 +17,7 @@ #include "LaneBlocker.hpp" +#include "IntersectionChecker.hpp" #include #include @@ -33,7 +34,6 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) : Node("lane_blocker_node", options) { - _tf2_buffer = std::make_unique(this->get_clock()); @@ -56,6 +56,13 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "Setting parameter obstacle_lane_threshold to %f", _obstacle_lane_threshold ); + _lane_width = this->declare_parameter( + "lane_width", 1.0); + 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( @@ -63,6 +70,22 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "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 + ); + + auto timer_period = + std::chrono::duration_cast( + std::chrono::duration>(1.0 / process_rate)); + _process_timer = this->create_wall_timer( + std::move(timer_period), + [=]() + { + this->process(); + }); + _lane_closure_pub = this->create_publisher( rmf_fleet_adapter::LaneClosureRequestTopicName, rclcpp::SystemDefaultsQoS()); @@ -118,6 +141,10 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) }, ipc_sub_options); + RCLCPP_INFO( + this->get_logger(), + "Started lane_blocker node" + ); } //============================================================================== @@ -177,7 +204,74 @@ void LaneBlocker::obstacle_cb(const Obstacles& msg) after_pose.pose.position.x, after_pose.pose.position.y, after_pose.pose.position.z ); + auto new_box = + vision_msgs::build() + .center(std::move(after_pose.pose)) + .size(std::move(after_size.vector)); + + auto obs = std::make_shared( + 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 + _obstacle_buffer[LaneBlocker::get_obstacle_key(*obs)] = + std::move(obs); + } +} + +//============================================================================== +void LaneBlocker::process() +{ + if (_obstacle_buffer.empty()) + return; + + for (const auto& [key, obstacle] : _obstacle_buffer) + { + // If the lifetime of the obstacle has passed, we skip it. + if (obstacle->expiry_time < get_clock()->now()) + { + continue; + } + + // Then check if this obstacle was previously assigned to a lane. If so, + // check if it is still in the vicinity of that lane + auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle); + if (obs_lane_it != _obstacle_to_lanes_map.end()) + { + + auto& lanes = obs_lane_it->second; + + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s was previously in the vicinity of %d lanes", + key.c_str(), lanes.size()); + + // Check if obstacle is still in the vicinity of these lanes. + } + else + { + // New 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", key.c_str() + ); + } } + + // Reinitialize the buffer + _obstacle_buffer = {}; +} + +//============================================================================== +void LaneBlocker::cull( + const std::string& obstacle_key, const std::string& lane_key) +{ + } 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 index 38c969364..c8ab75d32 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -28,10 +28,14 @@ #include #include +#include + #include #include #include +#include +#include //============================================================================== /// Modify states of lanes for fleet adapters based on density of obstacles @@ -39,11 +43,14 @@ 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 LaneStates = rmf_fleet_msgs::msg::LaneStates; + using BoundingBox = vision_msgs::msg::BoundingBox3D; + using Header = std_msgs::msg::Header; /// Constructor LaneBlocker( @@ -51,6 +58,9 @@ class LaneBlocker : public rclcpp::Node private: void obstacle_cb(const Obstacles& msg); + void process(); + void cull( + const std::string& obstacle_key, const std::string& lane_key); rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; @@ -65,7 +75,87 @@ class LaneBlocker : public rclcpp::Node std::unordered_map _traffic_graphs; std::unordered_map _lane_states; + double _lane_width; double _obstacle_lane_threshold; + + + struct ObstacleData + { + rclcpp::Time expiry_time; + std::size_t id; + std::string source; + BoundingBox transformed_bbox; + + ObstacleData( + rclcpp::Time expiry_time_, + std::size_t 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 ObstacleDataConstSharedPtr& obstacle) const + { + const std::string key = LaneBlocker::get_obstacle_key(*obstacle); + return std::hash()(key); + } + }; + + // Store obstacle after transformation into RMF frame. + // Generate key using get_obstacle_key() + std::unordered_map + _obstacle_buffer = {}; + + // Map an obstacle to the lanes in its vicinity + std::unordered_map< + ObstacleDataConstSharedPtr, + std::unordered_set, + ObstacleHash> _obstacle_to_lanes_map = {}; + + // Map lane to a set of obstacles in its vicinity + std::unordered_map< + std::string, + std::unordered_set> + _lane_to_obstacles_map = {}; + + rclcpp::TimerBase::SharedPtr _process_timer; + rclcpp::TimerBase::SharedPtr _cull_obstacles_timer; }; From 06366c5b06930845e5e5c63c68f2ec2a7bd4c16a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 1 Jul 2022 11:20:29 +0800 Subject: [PATCH 25/60] Warn users of unsupported speed limits Signed-off-by: Yadunund --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 29c2ac2eb..7dcbf3dfe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1624,8 +1624,19 @@ auto FleetUpdateHandle::limit_lane_speeds( { // TODO: Check if planner supports negative speed limits. if (request.lane_index() >= new_graph.num_lanes() || - request.speed_limit() < 0.0) + request.speed_limit() <= 0.0) + { + RCLCPP_WARN( + self->_pimpl->node->get_logger(), + "Ignoring speed limit request %f for lane %d in fleet %s as it is " + "not greater than zero. If you would like to close the lane, use " + "the FleetUpdateHandle::close_lanes(~) API instead.", + request.speed_limit(), + request.lane_index(), + self->_pimpl->name.c_str() + ); continue; + } auto& properties = new_graph.get_lane( request.lane_index()).properties(); properties.speed_limit(request.speed_limit()); From 247e019d8a9f2e77589386ef3f9ae07a006634ac Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 1 Jul 2022 11:45:28 +0800 Subject: [PATCH 26/60] Serialize/deserialize speed limits Signed-off-by: Yadunund --- .../src/rmf_traffic_ros2/convert_Graph.cpp | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index f74b00a37..c25e1ebc5 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -403,10 +403,21 @@ std::optional convert( { return std::nullopt; } - // TODO(YV): Deserialize events and properties when available + auto lane_properties = rmf_traffic::agv::Graph::Lane::Properties(); + for (const auto& param : e.params) + { + if (param.name == "speed_limit" && + param.type == param.TYPE_DOUBLE && + param.value_float > 0.0) + { + lane_properties.speed_limit(param.value_float); + } + // TODO(YV): Deserialize other events and properties when available + } graph.add_lane( {e.v1_idx}, - {e.v2_idx}); + {e.v2_idx}, + std::move(lane_properties)); } return graph; @@ -487,11 +498,21 @@ std::unique_ptr convert( { const auto& lane = graph.get_lane(i); // All lanes in rmf_traffic::agv::Graph are unidirectional + std::vector params; + const auto& properties = graph.get_lane(i).properties(); + if (properties.speed_limit().has_value()) + params.emplace_back(rmf_building_map_msgs::build() + .name("speed_limit") + .type(GraphParamMsg::TYPE_DOUBLE) + .value_int(0) + .value_float(properties.speed_limit().value()) + .value_string("") + .value_bool(false)); edges.emplace_back( rmf_building_map_msgs::build() .v1_idx(lane.entry().waypoint_index()) .v2_idx(lane.exit().waypoint_index()) - .params({}) + .params(std::move(params)) .edge_type(GraphEdgeMsg::EDGE_TYPE_UNIDIRECTIONAL) ); } From 19ba11cc22447d09bd88a53344a9d2358bde41a5 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 1 Jul 2022 16:40:01 +0800 Subject: [PATCH 27/60] Added logic for publishing lane closures Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 3 +- .../src/lane_blocker/LaneBlocker.cpp | 278 +++++++++++++++++- .../src/lane_blocker/LaneBlocker.hpp | 48 +-- 3 files changed, 303 insertions(+), 26 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index e10286b35..dd719b08a 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -24,8 +24,7 @@ bool IntersectionChecker::between( const BoundingBox& obstacle, double& how_much) { - + how_much = std::numeric_limits::max(); return false; - } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index ec2205390..d4215be4e 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -30,6 +30,9 @@ #include +#include +#include + //============================================================================== LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) : Node("lane_blocker_node", options) @@ -76,6 +79,21 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "Setting parameter process_rate to %f hz", process_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); + + _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 + ); + auto timer_period = std::chrono::duration_cast( std::chrono::duration>(1.0 / process_rate)); @@ -197,7 +215,7 @@ void LaneBlocker::obstacle_cb(const Obstacles& msg) Vector3Stamped after_size; tf2::doTransform(before_pose, after_pose, transform); tf2::doTransform(before_size, after_size, transform); - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Pose of obstacle id %d in RMF %s frame is [%f, %f, %f]", obstacle.id, _rmf_frame.c_str(), @@ -228,6 +246,10 @@ 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; + for (const auto& [key, obstacle] : _obstacle_buffer) { // If the lifetime of the obstacle has passed, we skip it. @@ -241,15 +263,63 @@ void LaneBlocker::process() auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle); if (obs_lane_it != _obstacle_to_lanes_map.end()) { - - auto& lanes = obs_lane_it->second; - + auto& lanes_keys = obs_lane_it->second; RCLCPP_INFO( this->get_logger(), - "Obstacle %s was previously in the vicinity of %d lanes", - key.c_str(), lanes.size()); + "Obstacle %s was previously in the vicinity of %ld lanes", + key.c_str(), lanes_keys.size()); // Check if obstacle is still in the vicinity of these lanes. + for (const auto& lane_key : lanes_keys) + { + const auto& lane = this->lane_from_key(lane_key); + double how_much; + auto intersect = IntersectionChecker::between( + lane, + _lane_width, + obstacle->transformed_bbox, + 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", + key.c_str(), lane_key.c_str() + ); + // TODO(YV): Is there any value in updating the actual ObstaclePtr + // in the other caches? + continue; + } + else + { + try + { + RCLCPP_INFO( + this->get_logger(), + "Obstacle %s is no longer in the vicinity of lane %s. " + "Updating cache...", 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 + _obstacle_to_lanes_map[obstacle].erase(lane_key); + //Remove from _lane_to_obstacles_map + _lane_to_obstacles_map[lane_key].erase(obstacle); + lanes_with_changes.insert(lane_key); + } + catch(const std::exception& e) + { + RCLCPP_ERROR( + this->get_logger(), + "[LaneBlocker::process()]: Unable to update obstacle caches." + "This is a bug and should be reported. Detailed error: %s", + e.what() + ); + continue; + } + } + } } else { @@ -260,13 +330,209 @@ void LaneBlocker::process() "Obstacle %s was not previously in the vicinity of any lane. Checking " "for any changes", 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; + auto intersect = IntersectionChecker::between( + lane, + lane_width, + obstacle.transformed_bbox, + how_much + ); + if (intersect || how_much < threshold) + { + std::lock_guardlock(mutex); + vicinity_lane_keys.insert( + LaneBlocker::get_lane_key(fleet_name, i)); + } + } + 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; + + }; + 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(), key.c_str() + ); + + // Update caches + for (const auto& lane_key : vicinity_lane_keys) + { + _obstacle_to_lanes_map[obstacle].insert(lane_key); + _lane_to_obstacles_map[lane_key].insert(obstacle); + 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() + ); + request_lane_modifications(std::move(lanes_with_changes)); // Reinitialize the buffer _obstacle_buffer = {}; } +//============================================================================== +void LaneBlocker::request_lane_modifications( + const std::unordered_set& changes) +{ + if (changes.empty()) + return; + + // A map to collate lanes per fleet that need to be closed + std::unordered_map> closure_msgs; + // 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; + } + auto [fleet_name, lane_id] = deserialize_key(lane_key); + const auto& obstacles = _lane_to_obstacles_map.at(lane_key); + if (obstacles.size() >= _lane_closure_threshold) + { + auto msg_it = closure_msgs.insert({fleet_name, nullptr}); + if (msg_it.second) + { + LaneRequest request; + request.fleet_name = std::move(fleet_name); + request.close_lanes.push_back(std::move(lane_id)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the new lane id + msg_it.first->second->close_lanes.push_back(std::move(lane_id)); + } + } + else + { + RCLCPP_INFO( + this->get_logger(), + "Lane %s has %ld obstacles in its vicinity but will not be closed as " + "the threshold is %ld", + lane_key.c_str(), obstacles.size(), _lane_closure_threshold + ); + continue; + } + } + + // Publish lane closures + for (auto& [_, msg] : closure_msgs) + { + if (msg->close_lanes.empty()) + { + RCLCPP_DEBUG( + this->get_logger(), + "None of the lanes for fleet %s need to be closed", + msg->fleet_name.c_str() + ); + continue; + } + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lane closures for fleet %s", + msg->close_lanes.size(), msg->fleet_name.c_str() + ); + _lane_closure_pub->publish(std::move(msg)); + } +} + +//============================================================================== +auto LaneBlocker::deserialize_key( + const std::string& key) const-> std::pair +{ + const std::string delimiter = "_"; + // This should not throw any errors if keys are constructed using get_key() + // TODO(YV): Consider returning an optional instead + try + { + std::string name = key.substr(0, key.find(delimiter)); + std::string id_str = + key.substr(key.find(delimiter) + 1, key.size() - name.size()); + RCLCPP_INFO( + this->get_logger(), + "Parsed key %s into [%s, %s]", + key.c_str(), + name.c_str(), + id_str.c_str() + ); + std::stringstream ss(id_str); + std::size_t id; ss >> id; + return std::make_pair(std::move(name), std::move(id)); + } + catch(const std::exception& e) + { + // *INDENT-OFF* + throw std::runtime_error( + "[LaneBlocker::lane_from_key] Unable to parse key. This is a bug and " + "should be reported. Detailed error: " + std::string(e.what())); + // *INDENT-ON* + } +} +//============================================================================== +auto LaneBlocker::lane_from_key( + const std::string& key) const-> const TrafficGraph::Lane& +{ + const auto [fleet_name, id] = deserialize_key(key); + return _traffic_graphs.at(fleet_name).get_lane(id); + +} + + //============================================================================== void LaneBlocker::cull( const std::string& obstacle_key, const std::string& lane_key) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index c8ab75d32..eaab62099 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -36,6 +36,7 @@ #include #include #include +#include //============================================================================== /// Modify states of lanes for fleet adapters based on density of obstacles @@ -62,23 +63,6 @@ class LaneBlocker : public rclcpp::Node void cull( const std::string& obstacle_key, const std::string& lane_key); - rclcpp::Subscription::SharedPtr _obstacle_sub; - 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::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; - - struct ObstacleData { rclcpp::Time expiry_time; @@ -137,8 +121,19 @@ class LaneBlocker : public rclcpp::Node } }; + std::pair + deserialize_key(const std::string& key) const; + + const TrafficGraph::Lane& lane_from_key(const std::string& key) const; + + // Modify lanes with changes in number of vicinity obstacles + void request_lane_modifications( + const std::unordered_set& changes); + // 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 = {}; @@ -154,9 +149,26 @@ class LaneBlocker : public rclcpp::Node std::unordered_set> _lane_to_obstacles_map = {}; + rclcpp::Subscription::SharedPtr _obstacle_sub; + 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::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::size_t _lane_closure_threshold; + rclcpp::TimerBase::SharedPtr _process_timer; rclcpp::TimerBase::SharedPtr _cull_obstacles_timer; }; - #endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP From 8d9101d37eb9fe68e1ebed58a5d8261c56632d13 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 1 Jul 2022 17:09:04 +0800 Subject: [PATCH 28/60] Add cull timer Signed-off-by: Yadunund --- .../src/lane_blocker/LaneBlocker.cpp | 28 +++++++++++++++---- .../src/lane_blocker/LaneBlocker.hpp | 10 ++++--- 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index d4215be4e..b31e654c3 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -31,6 +31,7 @@ #include #include +#include #include //============================================================================== @@ -79,6 +80,12 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "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( @@ -94,16 +101,26 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "Setting parameter lane_closure_threshold to %ld", _lane_closure_threshold ); - auto timer_period = + auto process_timer_period = std::chrono::duration_cast( std::chrono::duration>(1.0 / process_rate)); _process_timer = this->create_wall_timer( - std::move(timer_period), + std::move(process_timer_period), [=]() { this->process(); }); + auto cull_timer_period = + std::chrono::duration_cast( + std::chrono::duration>(1.0 / cull_rate)); + _cull_timer = this->create_wall_timer( + std::move(cull_timer_period), + [=]() + { + this->cull(); + }); + _lane_closure_pub = this->create_publisher( rmf_fleet_adapter::LaneClosureRequestTopicName, rclcpp::SystemDefaultsQoS()); @@ -457,6 +474,7 @@ void LaneBlocker::request_lane_modifications( // Msg was created before. We simply append the new lane id msg_it.first->second->close_lanes.push_back(std::move(lane_id)); } + _currently_closed_lanes.insert(lane_key); } else { @@ -534,10 +552,10 @@ auto LaneBlocker::lane_from_key( //============================================================================== -void LaneBlocker::cull( - const std::string& obstacle_key, const std::string& lane_key) +void LaneBlocker::cull() { - + // Cull obstacles that are past their expiry times. + // Also decide whether previously closed lanes should be re-opened. } 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 index eaab62099..51cd8f233 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -36,7 +36,6 @@ #include #include #include -#include //============================================================================== /// Modify states of lanes for fleet adapters based on density of obstacles @@ -60,8 +59,7 @@ class LaneBlocker : public rclcpp::Node private: void obstacle_cb(const Obstacles& msg); void process(); - void cull( - const std::string& obstacle_key, const std::string& lane_key); + void cull(); struct ObstacleData { @@ -137,6 +135,8 @@ class LaneBlocker : public rclcpp::Node std::unordered_map _obstacle_buffer = {}; + // TODO(YV): Based on the current implementation, we should be able to + // cache obstacle_key directly // Map an obstacle to the lanes in its vicinity std::unordered_map< ObstacleDataConstSharedPtr, @@ -149,6 +149,8 @@ class LaneBlocker : public rclcpp::Node std::unordered_set> _lane_to_obstacles_map = {}; + std::unordered_set _currently_closed_lanes; + rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; rclcpp::Subscription::SharedPtr _lane_states_sub; @@ -168,7 +170,7 @@ class LaneBlocker : public rclcpp::Node std::size_t _lane_closure_threshold; rclcpp::TimerBase::SharedPtr _process_timer; - rclcpp::TimerBase::SharedPtr _cull_obstacles_timer; + rclcpp::TimerBase::SharedPtr _cull_timer; }; #endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP From 560abe568c7979c2ef39a8da3f0738af8e54a135 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 1 Jul 2022 18:19:50 +0800 Subject: [PATCH 29/60] Finished implementing cull Signed-off-by: Yadunund --- .../src/lane_blocker/LaneBlocker.cpp | 103 +++++++++++++++++- .../src/lane_blocker/LaneBlocker.hpp | 8 +- 2 files changed, 103 insertions(+), 8 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index b31e654c3..3cda2336d 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -270,12 +270,28 @@ void LaneBlocker::process() for (const auto& [key, obstacle] : _obstacle_buffer) { // If the lifetime of the obstacle has passed, we skip it. - if (obstacle->expiry_time < get_clock()->now()) + if (obstacle->expiry_time < get_clock()->now() || obstacle == nullptr) { continue; } - // Then check if this obstacle was previously assigned to a lane. If so, + const std::string& obstacle_key = LaneBlocker::get_obstacle_key( + *obstacle); + + // The keys in _obstacle_to_lanes_map are hashed only based on source + // and id. We should check if this obstacle has expired and if, create + // a new entry. This is helpful for culling. + if (auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle) != + _obstacle_to_lanes_map.end()) + { + // We update the obstacle key with latest expiry + // extract is a C++17 feature + auto handler = _obstacle_to_lanes_map.extract(obstacle); + handler.key() = obstacle; + _obstacle_to_lanes_map.insert(std::move(handler)); + } + + // 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); if (obs_lane_it != _obstacle_to_lanes_map.end()) @@ -322,7 +338,7 @@ void LaneBlocker::process() // Remove from _obstacle_to_lanes_map _obstacle_to_lanes_map[obstacle].erase(lane_key); //Remove from _lane_to_obstacles_map - _lane_to_obstacles_map[lane_key].erase(obstacle); + _lane_to_obstacles_map[lane_key].erase(obstacle_key); lanes_with_changes.insert(lane_key); } catch(const std::exception& e) @@ -417,7 +433,7 @@ void LaneBlocker::process() for (const auto& lane_key : vicinity_lane_keys) { _obstacle_to_lanes_map[obstacle].insert(lane_key); - _lane_to_obstacles_map[lane_key].insert(obstacle); + _lane_to_obstacles_map[lane_key].insert(obstacle_key); lanes_with_changes.insert(lane_key); } } @@ -502,7 +518,7 @@ void LaneBlocker::request_lane_modifications( } RCLCPP_INFO( this->get_logger(), - "Requested %ld lane closures for fleet %s", + "Requested %ld lanes to close for fleet %s", msg->close_lanes.size(), msg->fleet_name.c_str() ); _lane_closure_pub->publish(std::move(msg)); @@ -556,6 +572,83 @@ void LaneBlocker::cull() { // Cull obstacles that are past their expiry times. // Also decide whether previously closed lanes should be re-opened. + std::unordered_set to_cull; + std::unordered_set lanes_with_changes; + + const auto now = this->get_clock()->now(); + for (const auto& [obstacle, lanes] : _obstacle_to_lanes_map) + { + if (obstacle->expiry_time > now) + { + to_cull.insert(obstacle); + // Then remove this obstacles from lanes map which is used to decide + // whether to open/close + for (const auto& lane : lanes) + { + const std::string obstacle_key = + LaneBlocker::get_obstacle_key(*obstacle); + _lane_to_obstacles_map[lane].erase(obstacle_key); + lanes_with_changes.insert(lane); + } + } + } + // Cull + for (const auto& obs : to_cull) + { + _obstacle_to_lanes_map.erase(obs); + } + + // Open lanes if needed + // A map to collate lanes per fleet that need to be closed + std::unordered_map> open_msgs; + for (const auto& lane : lanes_with_changes) + { + if (_currently_closed_lanes.find(lane) == _currently_closed_lanes.end()) + continue; + // The lane has changes and is currently closed. We check if the obstacle + // count is below the threshold and if so open. + if (_lane_to_obstacles_map.at(lane).size() < _lane_closure_threshold) + { + // The lane can be opened + auto [fleet_name, lane_id] = deserialize_key(lane); + auto msg_it = open_msgs.insert({fleet_name, nullptr}); + if (msg_it.second) + { + LaneRequest request; + request.fleet_name = std::move(fleet_name); + request.open_lanes.push_back(std::move(lane_id)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the new lane id + msg_it.first->second->open_lanes.push_back(std::move(lane_id)); + } + } + } + + // Publish lane closures + for (auto& [_, msg] : open_msgs) + { + if (msg->open_lanes.empty()) + { + RCLCPP_DEBUG( + this->get_logger(), + "None of the lanes for fleet %s need to be opened", + msg->fleet_name.c_str() + ); + continue; + } + 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)); + } + } 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 index 51cd8f233..61c0f7d2b 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -141,12 +141,14 @@ class LaneBlocker : public rclcpp::Node std::unordered_map< ObstacleDataConstSharedPtr, std::unordered_set, - ObstacleHash> _obstacle_to_lanes_map = {}; + ObstacleHash> _obstacle_to_lanes_map = {}; - // Map lane to a set of obstacles in its vicinity + // 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> + std::unordered_set> _lane_to_obstacles_map = {}; std::unordered_set _currently_closed_lanes; From 64a27fd506c1c00d7b6397879be5d5f1671c4613 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 4 Jul 2022 18:19:58 +0800 Subject: [PATCH 30/60] Updated intersection checker Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 43 +++++++++-- .../src/lane_blocker/IntersectionChecker.hpp | 20 +++--- .../src/lane_blocker/LaneBlocker.cpp | 72 +++++++++++++++++-- 3 files changed, 114 insertions(+), 21 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index dd719b08a..3cb5a2c79 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -17,14 +17,45 @@ #include "IntersectionChecker.hpp" +//============================================================================== +namespace IntersectionChecker { -bool IntersectionChecker::between( - const Lane& lane, - const double lane_width, - const BoundingBox& obstacle, - double& how_much) +namespace { + + + +// CollisionGeometry make_collision_geometry( +// Eigen::Vector2d origin, +// Eigen::Vector2d p_x, +// Eigen::Vector2d p_y) +// { +// CollisionGeometry geometry; + +// return geometry; +// } + + + + + + + +} // anonymous namespace + + +//============================================================================== +bool broadpahse( + const CollisionGeometry& o1, + const CollisionGeometry& o2) { - how_much = std::numeric_limits::max(); return false; } +bool narrowphase( + const CollisionGeometry& o1, + const CollisionGeometry& o2, + double& how_much) +{ + return false; +} +} // namespace IntersectionChecker diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp index 415fc9961..63c222077 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -20,23 +20,25 @@ #include -#include - -#include +#include +//============================================================================== // TODO(YV): Consider making this a loadable plugin via pluginlib namespace IntersectionChecker { -using Lane = rmf_traffic::agv::Graph::Lane; -using BoundingBox = vision_msgs::msg::BoundingBox3D; +using CollisionGeometry = vision_msgs::msg::BoundingBox2D; + +// Return true if intersect. +bool broadpahse( + const CollisionGeometry& o1, + const CollisionGeometry& o2); // Return true if intersect. // If intersect, how_much represents the overlap in meters // If not intersect, how_much represents the shortest distance. -bool between( - const Lane& lane, - const double lane_width, - const BoundingBox& obstacle, +bool narrowphase( + const CollisionGeometry& o1, + const CollisionGeometry& o2, double& how_much); } // namespace IntersectionChecker diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 3cda2336d..7836c7806 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -34,6 +34,56 @@ #include #include + +//============================================================================== +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.entry().waypoint_index()).get_location(); + + const auto& center_loc = (exit_loc + entry_loc) * 0.5; + const auto& axis = (exit_loc - entry_loc); + const double theta = std::atan2(axis[1], axis[2]); + const double length = axis.norm(); + + geometry.center.x = center_loc[0]; + geometry.center.y = center_loc[1]; + geometry.center.theta = theta; + geometry.size_x = lane_width; + geometry.size_y = 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) @@ -306,11 +356,16 @@ void LaneBlocker::process() for (const auto& lane_key : lanes_keys) { const auto& lane = this->lane_from_key(lane_key); + const auto [fleet_name, id] = deserialize_key(key); double how_much; - auto intersect = IntersectionChecker::between( + const auto& o1 = make_collision_geometry( + _traffic_graphs.at(fleet_name), lane, - _lane_width, - obstacle->transformed_bbox, + _lane_width); + const auto& o2 = make_collision_geometry(obstacle->transformed_bbox); + auto intersect = IntersectionChecker::narrowphase( + o1, + o2, how_much ); if (intersect || how_much <= _obstacle_lane_threshold) @@ -382,10 +437,15 @@ void LaneBlocker::process() return; const auto& lane = graph.get_lane(i); double how_much; - auto intersect = IntersectionChecker::between( + const auto& o1 = make_collision_geometry( + graph, lane, - lane_width, - obstacle.transformed_bbox, + lane_width); + const auto& o2 = make_collision_geometry( + obstacle.transformed_bbox); + auto intersect = IntersectionChecker::narrowphase( + o1, + o2, how_much ); if (intersect || how_much < threshold) From 8f43431dff76ef24f4d2f8ac27d478afc615edca Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Jul 2022 17:34:04 +0800 Subject: [PATCH 31/60] WIP Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 56 ++++++++++++++----- 1 file changed, 41 insertions(+), 15 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 3cb5a2c79..5ffc5b9f0 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -22,23 +22,23 @@ namespace IntersectionChecker { namespace { +Eigen::Matrix3d get_transform(const CollisionGeometry& to, + const CollisionGeometry& from) +{ + Eigen::Transform t; + Eigen::Vector2d p1 = {to.center.x, to.center.y}; + Eigen::Vector2d p2 = {from.center.x, from.center.y}; + t = Eigen::Translation(p2 - p1); + t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); + return t.matrix(); +} -// CollisionGeometry make_collision_geometry( -// Eigen::Vector2d origin, -// Eigen::Vector2d p_x, -// Eigen::Vector2d p_y) -// { -// CollisionGeometry geometry; - -// return geometry; -// } - - - - - - +struct CollisionBox +{ + Eigen::Vector2d min; // min x & y + Eigen::Vector2d max; // max x & y +}; } // anonymous namespace @@ -56,6 +56,32 @@ bool narrowphase( const CollisionGeometry& o2, double& how_much) { + + auto make_collision_box = + [](const CollisionGeometry& o) -> CollisionBox + { + CollisionBox box; + box.min = {o.center.x - o.size_x * 0.5, o.center.y - o.size_y * 0.5}; + box.max = {o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; + return box; + }; + + const auto& o1_box = make_collision_box(o1); + + + // std::vector vertices; + // const auto o2_box = make_collision_box(o2); + // for (std::size_t i = 0; i < 2; ++i) + // { + // for (std::size_t j = 1; j >= 0; --j) + // { + // vertices.push_back({o2_box.min[0], o2_box.min[0], o2_box.min[0]}); + // } + // } + + // Use SAT + + return false; } } // namespace IntersectionChecker From 32cc4618ebc4a37ed48029324d84097a0532471f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Jul 2022 17:47:32 +0800 Subject: [PATCH 32/60] Fix tf2_geometry_msgs dep for humble Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 5330b8222..ca692b562 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -28,6 +28,14 @@ foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) endforeach() +if ($ENV{ROS_DISTRO} MATCHES humble) + set(tf2_geometry_msgs_include_dirs "") + set(tf2_geometry_msgs_link_libraries tf2_geometry_msgs::tf2_geometry_msgs) +else() + set(tf2_geometry_msgs_include_dir ${tf2_geometry_msgs_INCLUDE_DIRS}) + set(tf2_geometry_msgs_link_libraries ${tf2_geometry_msgs_LIBRARIES}) +endif() + #=============================================================================== file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/*.cpp") add_library(rmf_obstacle_ros2 SHARED ${core_lib_srcs}) @@ -103,7 +111,7 @@ target_link_libraries(lane_blocker rmf_fleet_adapter::rmf_fleet_adapter ${tf2_ros_LIBRARIES} ${geometry_msgs_LIBRARIES} - ${tf2_geometry_msgs_LIBRARIES} + ${tf2_geometry_msgs_link_libraries} ) target_include_directories(lane_blocker @@ -118,7 +126,7 @@ target_include_directories(lane_blocker ${rmf_fleet_adapter_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} - ${tf2_geometry_msgs_INCLUDE_DIRS} + ${tf2_geometry_msgs_include_dirs} ) target_compile_features(lane_blocker INTERFACE cxx_std_17) From 08975489d69770de221e2eb5e5c33857a5ae15ec Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Jul 2022 18:34:58 +0800 Subject: [PATCH 33/60] Rough collision check implementation Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 68 ++++++++++++++++--- 1 file changed, 57 insertions(+), 11 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 5ffc5b9f0..ddb311a57 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -69,17 +69,63 @@ bool narrowphase( const auto& o1_box = make_collision_box(o1); - // std::vector vertices; - // const auto o2_box = make_collision_box(o2); - // for (std::size_t i = 0; i < 2; ++i) - // { - // for (std::size_t j = 1; j >= 0; --j) - // { - // vertices.push_back({o2_box.min[0], o2_box.min[0], o2_box.min[0]}); - // } - // } - - // Use SAT + std::vector vertices; + const auto o2_box = make_collision_box(o2); + vertices.push_back({o2_box.min[0], o2_box.min[1]}); + vertices.push_back({o2_box.min[0], o2_box.max[1]}); + vertices.push_back({o2_box.max[0], o2_box.min[1]}); + vertices.push_back({o2_box.max[0], o2_box.max[1]}); + // Transform o2 vertices into o1 coordinates + const auto& transform = get_transform(o1, o2); + for (auto& v : vertices) + { + v = (transform * Eigen::Vector3d{v[0], v[1], 1.0}).block<2,1>(0, 0); + } + + // Use SAT. Project o2 vertices onto o1 X & Y axes + // Get the unit vector from o1 center along X Axis + // auto x_axis = Eigen::Vector2d{o1_box.max[0] - o1.center.x, 0.0}; + // x_axis /= x_axis.norm(); + // auto y_axis = Eigen::Vector2d{0.0, o1_box.max[1] - o1.center.y}; + // y_axis /= y_axis.norm(); + + // Project all transformed o2 points onto each axis and compare bounds with o1_box + + // X-Axis + double min_value_x = std::numeric_limits::max(); + double max_value_x = std::numeric_limits::min(); + for (const auto& v : vertices) + { + if (v[0] < min_value_x) + min_value_x = v[0]; + } + for (const auto& v : vertices) + { + if (v[0] > max_value_x) + max_value_x = v[0]; + } + + // X-Axis + double min_value_y = std::numeric_limits::max(); + double max_value_y = std::numeric_limits::min(); + for (const auto& v : vertices) + { + if (v[1] < min_value_y) + min_value_y = v[1]; + } + for (const auto& v : vertices) + { + if (v[1] > max_value_y) + max_value_y = v[1]; + } + + if ((min_value_x >= o1_box.min[0] && min_value_x <= o1_box.max[0]) || + (max_value_x >= o1_box.min[0] && min_value_x <= o1_box.max[0]) || + (min_value_y >= o1_box.min[1] && min_value_y <= o1_box.max[1]) || + (max_value_y >= o1_box.min[1] && min_value_y <= o1_box.max[1])) + { + return true; + } return false; From d60ca5f2c579d6e80f9d226a4c1b97181317b5ad Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 6 Jul 2022 22:58:27 +0800 Subject: [PATCH 34/60] Added tests for IntersectionChecker Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 4 + rmf_obstacle_ros2/package.xml | 1 + .../src/lane_blocker/IntersectionChecker.cpp | 100 +++------ .../src/lane_blocker/IntersectionChecker.hpp | 9 +- .../src/lane_blocker/LaneBlocker.cpp | 6 +- .../src/lane_blocker/test/CMakeLists.txt | 33 +++ .../src/lane_blocker/test/main.cpp | 21 ++ .../test/test_IntersectionChecker.cpp | 206 ++++++++++++++++++ 8 files changed, 305 insertions(+), 75 deletions(-) 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 diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 7cfb7a16c..3a5490ef0 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -150,6 +150,8 @@ rclcpp_components_register_node(lane_blocker #=============================================================================== 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/") @@ -160,6 +162,8 @@ if(BUILD_TESTING) MAX_LINE_LENGTH 80 ) + add_subdirectory(src/lane_blocker/test) + endif() #=============================================================================== diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 1eb5464aa..5ea056be7 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -26,6 +26,7 @@ tf2_geometry_msgs geometry_msgs + ament_cmake_catch2 ament_cmake_uncrustify diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index ddb311a57..bc983124d 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -29,9 +29,10 @@ Eigen::Matrix3d get_transform(const CollisionGeometry& to, Eigen::Vector2d p1 = {to.center.x, to.center.y}; Eigen::Vector2d p2 = {from.center.x, from.center.y}; - t = Eigen::Translation(p2 - p1); + t = Eigen::Translation(-p1); t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); - return t.matrix(); + const auto& matrix = t.matrix(); + return matrix; } struct CollisionBox @@ -44,90 +45,59 @@ struct CollisionBox //============================================================================== -bool broadpahse( - const CollisionGeometry& o1, - const CollisionGeometry& o2) -{ - return false; -} - -bool narrowphase( +bool between( const CollisionGeometry& o1, const CollisionGeometry& o2, double& how_much) { - auto make_collision_box = - [](const CollisionGeometry& o) -> CollisionBox + [](const CollisionGeometry& o, const bool origin) -> CollisionBox { CollisionBox box; - box.min = {o.center.x - o.size_x * 0.5, o.center.y - o.size_y * 0.5}; - box.max = {o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; + box.min = origin ? Eigen::Vector2d{-o.size_x * 0.5, -o.size_y * 0.5} : + Eigen::Vector2d{o.center.x -o.size_x * 0.5, o.center.y -o.size_y * 0.5}; + box.max = origin ? Eigen::Vector2d{o.size_x * 0.5, o.size_y * 0.5} : + Eigen::Vector2d{o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; return box; }; - const auto& o1_box = make_collision_box(o1); - - + const auto& o1_box = make_collision_box(o1, true); std::vector vertices; - const auto o2_box = make_collision_box(o2); + const auto o2_box = make_collision_box(o2, false); vertices.push_back({o2_box.min[0], o2_box.min[1]}); - vertices.push_back({o2_box.min[0], o2_box.max[1]}); - vertices.push_back({o2_box.max[0], o2_box.min[1]}); vertices.push_back({o2_box.max[0], o2_box.max[1]}); // Transform o2 vertices into o1 coordinates const auto& transform = get_transform(o1, o2); for (auto& v : vertices) - { v = (transform * Eigen::Vector3d{v[0], v[1], 1.0}).block<2,1>(0, 0); - } // Use SAT. Project o2 vertices onto o1 X & Y axes - // Get the unit vector from o1 center along X Axis - // auto x_axis = Eigen::Vector2d{o1_box.max[0] - o1.center.x, 0.0}; - // x_axis /= x_axis.norm(); - // auto y_axis = Eigen::Vector2d{0.0, o1_box.max[1] - o1.center.y}; - // y_axis /= y_axis.norm(); - - // Project all transformed o2 points onto each axis and compare bounds with o1_box - - // X-Axis - double min_value_x = std::numeric_limits::max(); - double max_value_x = std::numeric_limits::min(); - for (const auto& v : vertices) + CollisionBox o2t_box; + o2t_box.min = vertices[0]; + o2t_box.max = vertices[1]; + + how_much = + [&]() -> double + { + double dist = std::numeric_limits::max(); + for (std::size_t i = 0; i < 2; ++i) + { + dist = std::min( + dist, + std::min( + std::abs(o2t_box.min[i] - o1_box.max[i]), + std::abs(o1_box.min[i] - o2t_box.max[i])) + ); + } + return dist; + }(); + + for (std::size_t i = 0; i < 2; ++i) { - if (v[0] < min_value_x) - min_value_x = v[0]; + if (o2t_box.min[i] > o1_box.max[i] || o2t_box.max[i] < o1_box.min[i]) + return false; } - for (const auto& v : vertices) - { - if (v[0] > max_value_x) - max_value_x = v[0]; - } - - // X-Axis - double min_value_y = std::numeric_limits::max(); - double max_value_y = std::numeric_limits::min(); - for (const auto& v : vertices) - { - if (v[1] < min_value_y) - min_value_y = v[1]; - } - for (const auto& v : vertices) - { - if (v[1] > max_value_y) - max_value_y = v[1]; - } - - if ((min_value_x >= o1_box.min[0] && min_value_x <= o1_box.max[0]) || - (max_value_x >= o1_box.min[0] && min_value_x <= o1_box.max[0]) || - (min_value_y >= o1_box.min[1] && min_value_y <= o1_box.max[1]) || - (max_value_y >= o1_box.min[1] && min_value_y <= o1_box.max[1])) - { - return true; - } - - 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 index 63c222077..b6f258a39 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -28,15 +28,10 @@ namespace IntersectionChecker { using CollisionGeometry = vision_msgs::msg::BoundingBox2D; -// Return true if intersect. -bool broadpahse( - const CollisionGeometry& o1, - const CollisionGeometry& o2); - // Return true if intersect. // If intersect, how_much represents the overlap in meters -// If not intersect, how_much represents the shortest distance. -bool narrowphase( +// If not intersect, how_much represents the separating distance in meters. +bool between( const CollisionGeometry& o1, const CollisionGeometry& o2, double& how_much); diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 7836c7806..71005f82f 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -104,7 +104,7 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) ); _obstacle_lane_threshold = this->declare_parameter( - "obstacle_lane_threshold", 0.5); + "obstacle_lane_threshold", 1.0); RCLCPP_INFO( this->get_logger(), "Setting parameter obstacle_lane_threshold to %f", _obstacle_lane_threshold @@ -363,7 +363,7 @@ void LaneBlocker::process() lane, _lane_width); const auto& o2 = make_collision_geometry(obstacle->transformed_bbox); - auto intersect = IntersectionChecker::narrowphase( + auto intersect = IntersectionChecker::between( o1, o2, how_much @@ -443,7 +443,7 @@ void LaneBlocker::process() lane_width); const auto& o2 = make_collision_geometry( obstacle.transformed_bbox); - auto intersect = IntersectionChecker::narrowphase( + auto intersect = IntersectionChecker::between( o1, o2, how_much 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..a789f2e8e --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt @@ -0,0 +1,33 @@ + +file(GLOB_RECURSE unit_test_srcs "*.cpp" "../IntersectionChecker.cpp") +# add_executable(test_intersection_checker ${unit_test_srcs}) + +ament_add_catch2( + test_intersection_checker ${unit_test_srcs} + TIMEOUT 300) +# target_link_libraries(test_intersection_checker +# rmf_traffic +# ${FCL_LIBRARIES} +# Threads::Threads + +target_link_libraries(test_intersection_checker + PRIVATE + ${vision_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} + rmf_utils::rmf_utils +) + +target_include_directories(test_intersection_checker + PRIVATE + $ + ${EIGEN3_INCLUDE_DIRS} + ${vision_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..86f83b891 --- /dev/null +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -0,0 +1,206 @@ +/* + * 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 auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(4.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK_FALSE(intersect); + CHECK(how_much - 1.0 == Approx(0.0).margin(1e-3)); + } + + WHEN("OBB geometries are not intersecting and 1m apart") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(4.0) + .y(0.0) + .theta(radians(45.0))) + .size_x(2.0) + .size_y(2.0); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK_FALSE(intersect); + CHECK((how_much - 0.414) == Approx(0.0).margin(1e-3)); + } + + WHEN("AABB geometries are overlapping along X-Axis") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(2.5) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(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 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(1.5) + .theta(0.0)) + .size_x(2.0) + .size_y(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 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(2.5) + .y(0.5) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } + + WHEN("AABB geometries are touching") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(3.0) + .y(0.0) + .theta(0.0)) + .size_x(2.0) + .size_y(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 = vision_msgs::build() + .center(geometry_msgs::build() + .x(0.0) + .y(0.0) + .theta(radians(45.0))) + .size_x(2.0) + .size_y(2.0); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(1.414) + .y(1.0) + .theta(0.0)) + .size_x(2.0) + .size_y(2.0); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK(intersect); + } +} \ No newline at end of file From 8738be30d11536bd962ee4f6160eefa8e4bac85c Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Jul 2022 01:54:01 +0800 Subject: [PATCH 35/60] Fixes to SAT Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 82 +++++++++++++++---- .../src/lane_blocker/LaneBlocker.cpp | 58 +++++++------ .../src/lane_blocker/LaneBlocker.hpp | 14 ++-- .../test/test_IntersectionChecker.cpp | 30 +++++++ 4 files changed, 136 insertions(+), 48 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index bc983124d..1f7c510e7 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -17,6 +17,8 @@ #include "IntersectionChecker.hpp" +#include + //============================================================================== namespace IntersectionChecker { @@ -30,9 +32,18 @@ Eigen::Matrix3d get_transform(const CollisionGeometry& to, Eigen::Vector2d p2 = {from.center.x, from.center.y}; t = Eigen::Translation(-p1); - t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); - const auto& matrix = t.matrix(); - return matrix; + // We need to rotate the point about p1. To do this we translate p2 to p1, + // apply rotation and translate back. + auto r = + t.inverse() * Eigen::Rotation2D(to.center.theta - from.center.theta) * t; + // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); + std::cout << "Transformation matrix: " << std::endl; + auto final_t = (t * r).matrix(); + std::cout << final_t << std::endl; + return final_t; + // const auto& matrix = t.matrix(); + // std::cout << matrix << std::endl; + // return matrix; } struct CollisionBox @@ -50,9 +61,30 @@ bool between( const CollisionGeometry& o2, double& how_much) { + 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"); + auto make_collision_box = [](const CollisionGeometry& o, const bool origin) -> CollisionBox { + // TODO(YV): Account for rotation CollisionBox box; box.min = origin ? Eigen::Vector2d{-o.size_x * 0.5, -o.size_y * 0.5} : Eigen::Vector2d{o.center.x -o.size_x * 0.5, o.center.y -o.size_y * 0.5}; @@ -62,8 +94,10 @@ bool between( }; const auto& o1_box = make_collision_box(o1, true); + print_box(o1_box, "Lane box"); std::vector vertices; const auto o2_box = make_collision_box(o2, false); + print_box(o2_box, "obs box"); vertices.push_back({o2_box.min[0], o2_box.min[1]}); vertices.push_back({o2_box.max[0], o2_box.max[1]}); // Transform o2 vertices into o1 coordinates @@ -75,27 +109,39 @@ bool between( CollisionBox o2t_box; o2t_box.min = vertices[0]; o2t_box.max = vertices[1]; + print_box(o2t_box, "obs transformed box"); - how_much = - [&]() -> double - { - double dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < 2; ++i) - { - dist = std::min( - dist, - std::min( - std::abs(o2t_box.min[i] - o1_box.max[i]), - std::abs(o1_box.min[i] - o2t_box.max[i])) - ); - } - return dist; - }(); + // how_much = + // [&]() -> double + // { + // double dist = std::numeric_limits::max(); + // for (std::size_t i = 0; i < 2; ++i) + // { + // std::cout << "dist: " << dist << std::endl; + // dist = std::min( + // dist, + // std::min( + // std::abs(o2t_box.min[i] - o1_box.max[i]), + // std::abs(o1_box.min[i] - o2t_box.max[i])) + // ); + // } + // std::cout << "dist: " << dist << std::endl; + + // return dist; + // }(); + + how_much = 0.0; for (std::size_t i = 0; i < 2; ++i) { if (o2t_box.min[i] > o1_box.max[i] || o2t_box.max[i] < o1_box.min[i]) + { + // Does not overlap along this axis hence the two obstacles do not overlap + how_much = std::min( + std::abs(o2t_box.min[i] - o1_box.max[i]), + std::abs(o2t_box.max[i] - o1_box.min[i])); return false; + } } return true; diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 71005f82f..8b3c1b449 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -48,18 +48,22 @@ IntersectionChecker::CollisionGeometry make_collision_geometry( graph.get_waypoint(lane.entry().waypoint_index()).get_location(); const Eigen::Vector2d& exit_loc = - graph.get_waypoint(lane.entry().waypoint_index()).get_location(); + 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); - const double theta = std::atan2(axis[1], axis[2]); + 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_x = lane_width; - geometry.size_y = length; + geometry.size_y = lane_width; + geometry.size_x = length; return geometry; } @@ -111,7 +115,7 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) ); _lane_width = this->declare_parameter( - "lane_width", 1.0); + "lane_width", 0.5); RCLCPP_INFO( this->get_logger(), "Setting parameter lane_width to %f", _lane_width @@ -294,15 +298,15 @@ void LaneBlocker::obstacle_cb(const Obstacles& msg) .center(std::move(after_pose.pose)) .size(std::move(after_size.vector)); - auto obs = std::make_shared( + 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 - _obstacle_buffer[LaneBlocker::get_obstacle_key(*obs)] = + _obstacle_buffer[LaneBlocker::get_obstacle_key(obs)] = std::move(obs); } } @@ -320,16 +324,15 @@ void LaneBlocker::process() for (const auto& [key, obstacle] : _obstacle_buffer) { // If the lifetime of the obstacle has passed, we skip it. - if (obstacle->expiry_time < get_clock()->now() || obstacle == nullptr) + if (obstacle.expiry_time < get_clock()->now()) { continue; } - const std::string& obstacle_key = LaneBlocker::get_obstacle_key( - *obstacle); + const std::string& obstacle_key = LaneBlocker::get_obstacle_key(obstacle); // The keys in _obstacle_to_lanes_map are hashed only based on source - // and id. We should check if this obstacle has expired and if, create + // and id. We should check if this obstacle has expired and if so, create // a new entry. This is helpful for culling. if (auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle) != _obstacle_to_lanes_map.end()) @@ -358,11 +361,13 @@ void LaneBlocker::process() const auto& lane = this->lane_from_key(lane_key); const auto [fleet_name, id] = deserialize_key(key); double how_much; + if (_traffic_graphs.find(fleet_name) == _traffic_graphs.end()) + continue; const auto& o1 = make_collision_geometry( _traffic_graphs.at(fleet_name), lane, _lane_width); - const auto& o2 = make_collision_geometry(obstacle->transformed_bbox); + const auto& o2 = make_collision_geometry(obstacle.transformed_bbox); auto intersect = IntersectionChecker::between( o1, o2, @@ -450,6 +455,14 @@ void LaneBlocker::process() ); if (intersect || how_much < threshold) { + if (intersect) + { + std::cout << "INTERSECTION!!" << std::endl; + } + else + { + std::cout << "HOW_MUCH: " << how_much << std::endl; + } std::lock_guardlock(mutex); vicinity_lane_keys.insert( LaneBlocker::get_lane_key(fleet_name, i)); @@ -470,7 +483,7 @@ void LaneBlocker::process() search_vicinity_lanes, fleet_name, graph, - *obstacle, + obstacle, _obstacle_lane_threshold, _lane_width, _max_search_duration) @@ -597,13 +610,6 @@ auto LaneBlocker::deserialize_key( std::string name = key.substr(0, key.find(delimiter)); std::string id_str = key.substr(key.find(delimiter) + 1, key.size() - name.size()); - RCLCPP_INFO( - this->get_logger(), - "Parsed key %s into [%s, %s]", - key.c_str(), - name.c_str(), - id_str.c_str() - ); std::stringstream ss(id_str); std::size_t id; ss >> id; return std::make_pair(std::move(name), std::move(id)); @@ -632,13 +638,14 @@ void LaneBlocker::cull() { // Cull obstacles that are past their expiry times. // Also decide whether previously closed lanes should be re-opened. - std::unordered_set to_cull; + std::unordered_set to_cull; std::unordered_set lanes_with_changes; const auto now = this->get_clock()->now(); for (const auto& [obstacle, lanes] : _obstacle_to_lanes_map) { - if (obstacle->expiry_time > now) + + if (obstacle.expiry_time > now) { to_cull.insert(obstacle); // Then remove this obstacles from lanes map which is used to decide @@ -646,7 +653,7 @@ void LaneBlocker::cull() for (const auto& lane : lanes) { const std::string obstacle_key = - LaneBlocker::get_obstacle_key(*obstacle); + LaneBlocker::get_obstacle_key(obstacle); _lane_to_obstacles_map[lane].erase(obstacle_key); lanes_with_changes.insert(lane); } @@ -658,6 +665,9 @@ void LaneBlocker::cull() _obstacle_to_lanes_map.erase(obs); } + // TODO(YV): Open lanes that no longer have obstacles + + // Open lanes if needed // A map to collate lanes per fleet that need to be closed std::unordered_map> open_msgs; diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 61c0f7d2b..72e2a72b8 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -64,13 +64,15 @@ class LaneBlocker : public rclcpp::Node struct ObstacleData { rclcpp::Time expiry_time; - std::size_t id; + int id; std::string source; BoundingBox transformed_bbox; + ObstacleData() {} + ObstacleData( rclcpp::Time expiry_time_, - std::size_t id_, + int id_, const std::string& source_, BoundingBox transformed_bbox_) : expiry_time(expiry_time_), @@ -112,9 +114,9 @@ class LaneBlocker : public rclcpp::Node struct ObstacleHash { std::size_t operator()( - const ObstacleDataConstSharedPtr& obstacle) const + const ObstacleData& obstacle) const { - const std::string key = LaneBlocker::get_obstacle_key(*obstacle); + const std::string key = LaneBlocker::get_obstacle_key(obstacle); return std::hash()(key); } }; @@ -132,14 +134,14 @@ class LaneBlocker : public rclcpp::Node // 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 + std::unordered_map _obstacle_buffer = {}; // TODO(YV): Based on the current implementation, we should be able to // cache obstacle_key directly // Map an obstacle to the lanes in its vicinity std::unordered_map< - ObstacleDataConstSharedPtr, + ObstacleData, std::unordered_set, ObstacleHash> _obstacle_to_lanes_map = {}; diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 86f83b891..76aa0a127 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -23,6 +23,8 @@ #include #include +#include + using CollisionGeometry = IntersectionChecker::CollisionGeometry; SCENARIO("Test IntersectionChecker") @@ -82,6 +84,7 @@ SCENARIO("Test IntersectionChecker") ob1, ob2, how_much); CHECK_FALSE(intersect); CHECK((how_much - 0.414) == Approx(0.0).margin(1e-3)); + std::cout <<"how_much: " << how_much << std::endl; } WHEN("AABB geometries are overlapping along X-Axis") @@ -203,4 +206,31 @@ SCENARIO("Test IntersectionChecker") ob1, ob2, how_much); CHECK(intersect); } + + WHEN("Test #1") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(8.6824) + .y(-10.9616) + .theta(0.255513)) + .size_x(1.43478) + .size_y(0.5); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(12.002) + .y(-10.1094) + .theta(0.0)) + .size_x(0.6) + .size_y(0.6); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK_FALSE(intersect); + CHECK((how_much - 2.336) == Approx(0.0).margin(1e-1)); + std::cout <<"how_much: " << how_much << std::endl; + } + } \ No newline at end of file From a4315a47996188747b3d9e647499f8d7dd391054 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Jul 2022 10:44:23 +0800 Subject: [PATCH 36/60] Debug rotation Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 40 +++++++++++++++++-- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 1f7c510e7..8989b2e12 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -86,10 +86,42 @@ bool between( { // TODO(YV): Account for rotation CollisionBox box; - box.min = origin ? Eigen::Vector2d{-o.size_x * 0.5, -o.size_y * 0.5} : - Eigen::Vector2d{o.center.x -o.size_x * 0.5, o.center.y -o.size_y * 0.5}; - box.max = origin ? Eigen::Vector2d{o.size_x * 0.5, o.size_y * 0.5} : - Eigen::Vector2d{o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; + if (origin) + { + 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}; + } + else + { + 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}; + const auto& th = o.center.theta; + Eigen::Matrix mat; + + mat(0,0) = std::cos(th); mat(0,1) = std::sin(-1.0 *th); mat(0,2) = o.center.x; + mat(1,0) = std::sin(th); mat(1,1) = std::cos(th); mat(1,2) = o.center.y; + mat(2,0) = 0; mat(2,1) = 0; mat(2,2) = 1.0; + // Eigen::Transform t; + // t = Eigen::Translation(Eigen::Vector2d{o.center.x, o.center.y}); + // // We need to rotate the point about p1. To do this we translate p2 to p1, + // // apply rotation and translate back. + // t.rotate(Eigen::Rotation2D(o.center.theta)); + // // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); + std::cout << "make collision Transformation matrix: " << std::endl; + std::cout << mat << std::endl; + // auto final_t = t.matrix(); + box.min = (mat * Eigen::Vector3d{box.min[0], box.min[1], 1.0}).block<2,1>(0, 0); + box.max = (mat * Eigen::Vector3d{box.max[0], box.max[1], 1.0}).block<2,1>(0, 0); + + // box.min = mat * box.min; + // box.max = mat * box.max; + + // box.min = + // Eigen::Vector2d{o.center.x -o.size_x * 0.5, o.center.y -o.size_y * 0.5}; + // box.max = + // Eigen::Vector2d{o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; + } + return box; }; From 14ec46bdae282e025decada878268da66880f376 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 00:39:36 +0800 Subject: [PATCH 37/60] Kinda working Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 41 +++++++++++-------- .../test/test_IntersectionChecker.cpp | 28 ++++++++++++- 2 files changed, 51 insertions(+), 18 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 8989b2e12..394a8f968 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -38,7 +38,7 @@ Eigen::Matrix3d get_transform(const CollisionGeometry& to, t.inverse() * Eigen::Rotation2D(to.center.theta - from.center.theta) * t; // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); std::cout << "Transformation matrix: " << std::endl; - auto final_t = (t * r).matrix(); + auto final_t = t.matrix(); std::cout << final_t << std::endl; return final_t; // const auto& matrix = t.matrix(); @@ -84,7 +84,6 @@ bool between( auto make_collision_box = [](const CollisionGeometry& o, const bool origin) -> CollisionBox { - // TODO(YV): Account for rotation CollisionBox box; if (origin) { @@ -93,11 +92,8 @@ bool between( } else { - 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}; - const auto& th = o.center.theta; Eigen::Matrix mat; - + const double th = o.center.theta; mat(0,0) = std::cos(th); mat(0,1) = std::sin(-1.0 *th); mat(0,2) = o.center.x; mat(1,0) = std::sin(th); mat(1,1) = std::cos(th); mat(1,2) = o.center.y; mat(2,0) = 0; mat(2,1) = 0; mat(2,2) = 1.0; @@ -109,17 +105,28 @@ bool between( // // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); std::cout << "make collision Transformation matrix: " << std::endl; std::cout << mat << std::endl; - // auto final_t = t.matrix(); - box.min = (mat * Eigen::Vector3d{box.min[0], box.min[1], 1.0}).block<2,1>(0, 0); - box.max = (mat * Eigen::Vector3d{box.max[0], box.max[1], 1.0}).block<2,1>(0, 0); - - // box.min = mat * box.min; - // box.max = mat * box.max; - - // box.min = - // Eigen::Vector2d{o.center.x -o.size_x * 0.5, o.center.y -o.size_y * 0.5}; - // box.max = - // Eigen::Vector2d{o.center.x + o.size_x * 0.5, o.center.y + o.size_y * 0.5}; + std::vector vertices; + vertices.push_back({-o.size_x * 0.5, o.size_y * 0.5, 1.0}); + vertices.push_back({-o.size_x * 0.5, -o.size_y * 0.5, 1.0}); + vertices.push_back({o.size_x * 0.5, o.size_y * 0.5, 1.0}); + vertices.push_back({o.size_x * 0.5, -o.size_y * 0.5, 1.0}); + + for (auto& v : vertices) + v = mat * v; + + box.min = {std::numeric_limits::max(), std::numeric_limits::max()}; + box.max = {std::numeric_limits::min(), std::numeric_limits::min()}; + + for (const auto& v : vertices) + { + for (std::size_t i = 0; i < 2; ++i) + { + if (v[i] < box.min[i]) + box.min[i] = v[i]; + if (v[i] > box.max[i]) + box.max[i] = v[i]; + } + } } return box; diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 76aa0a127..98153ecff 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -83,8 +83,8 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); CHECK_FALSE(intersect); - CHECK((how_much - 0.414) == Approx(0.0).margin(1e-3)); std::cout <<"how_much: " << how_much << std::endl; + CHECK((how_much - 0.586) == Approx(0.0).margin(1e-3)); } WHEN("AABB geometries are overlapping along X-Axis") @@ -233,4 +233,30 @@ SCENARIO("Test IntersectionChecker") std::cout <<"how_much: " << how_much << std::endl; } + WHEN("Test #2") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(8.6824) + .y(-10.9616) + .theta(0.255513)) + .size_x(1.43478) + .size_y(0.5); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(12.002) + .y(-10.1094) + .theta(M_PI_4)) + .size_x(0.6) + .size_y(0.6); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK_FALSE(intersect); + CHECK((how_much - 2.299) == Approx(0.0).margin(1e-1)); + std::cout <<"how_much: " << how_much << std::endl; + } + } \ No newline at end of file From a87b5b81395edf40ccd229052126ae3e3bf3c48e Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 01:10:33 +0800 Subject: [PATCH 38/60] Use lowest not min Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 5 ++++- .../lane_blocker/test/test_IntersectionChecker.cpp | 14 +++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 394a8f968..666e63a03 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -112,10 +112,13 @@ bool between( vertices.push_back({o.size_x * 0.5, -o.size_y * 0.5, 1.0}); for (auto& v : vertices) + { v = mat * v; + std::cout << v << std::endl; + } box.min = {std::numeric_limits::max(), std::numeric_limits::max()}; - box.max = {std::numeric_limits::min(), std::numeric_limits::min()}; + box.max = {std::numeric_limits::lowest(), std::numeric_limits::lowest()}; for (const auto& v : vertices) { diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 98153ecff..c160f35a1 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -238,24 +238,24 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = vision_msgs::build() .center(geometry_msgs::build() - .x(8.6824) - .y(-10.9616) - .theta(0.255513)) - .size_x(1.43478) + .x(11.6892) + .y(-3.52843) + .theta(0.293981)) + .size_x(3.01193) .size_y(0.5); const auto ob2 = vision_msgs::build() .center(geometry_msgs::build() .x(12.002) - .y(-10.1094) - .theta(M_PI_4)) + .y(-10.9738) + .theta(0.0)) .size_x(0.6) .size_y(0.6); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); CHECK_FALSE(intersect); - CHECK((how_much - 2.299) == Approx(0.0).margin(1e-1)); + CHECK((how_much - 6.773) == Approx(0.0).margin(1e-1)); std::cout <<"how_much: " << how_much << std::endl; } From 41573424a982b1cf255c9242e4b5f570d8b86ba3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 01:29:34 +0800 Subject: [PATCH 39/60] Added more failing tests Signed-off-by: Yadunund --- .../test/test_IntersectionChecker.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index c160f35a1..a405b5999 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -259,4 +259,29 @@ SCENARIO("Test IntersectionChecker") std::cout <<"how_much: " << how_much << std::endl; } + WHEN("Test #3") + { + double how_much; + const auto ob1 = vision_msgs::build() + .center(geometry_msgs::build() + .x(9.57985) + .y(-4.6367) + .theta(1.16262)) + .size_x(3.36581) + .size_y(0.5); + + const auto ob2 = vision_msgs::build() + .center(geometry_msgs::build() + .x(12.1453) + .y(-11.1404) + .theta(0.0)) + .size_x(0.6) + .size_y(0.6); + + const bool intersect = IntersectionChecker::between( + ob1, ob2, how_much); + CHECK_FALSE(intersect); + CHECK((how_much - 4.314) == Approx(0.0).margin(1e-1)); + std::cout <<"how_much: " << how_much << std::endl; + } } \ No newline at end of file From f1e4d94ab327583841468dded4e90f8c4046de6b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 03:32:30 +0800 Subject: [PATCH 40/60] Use dist Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 170 ++++++++++-------- .../test/test_IntersectionChecker.cpp | 11 +- 2 files changed, 103 insertions(+), 78 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 666e63a03..d0c9502e0 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -24,28 +24,44 @@ namespace IntersectionChecker { namespace { -Eigen::Matrix3d get_transform(const CollisionGeometry& to, - const CollisionGeometry& from) +void transform_vertices(const CollisionGeometry& to, + const CollisionGeometry& from, std::vector& vertices) { - Eigen::Transform t; - Eigen::Vector2d p1 = {to.center.x, to.center.y}; - Eigen::Vector2d p2 = {from.center.x, from.center.y}; - - t = Eigen::Translation(-p1); - // We need to rotate the point about p1. To do this we translate p2 to p1, - // apply rotation and translate back. - auto r = - t.inverse() * Eigen::Rotation2D(to.center.theta - from.center.theta) * t; - // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); - std::cout << "Transformation matrix: " << std::endl; - auto final_t = t.matrix(); - std::cout << final_t << std::endl; - return final_t; - // const auto& matrix = t.matrix(); - // std::cout << matrix << std::endl; - // return matrix; + // Eigen::Transform t; + // Eigen::Vector2d p1 = {to.center.x, to.center.y}; + // Eigen::Vector2d p2 = {from.center.x, from.center.y}; + + // t = Eigen::Translation(-p1); + // // We need to rotate the point about p1. To do this we translate p2 to p1, + // // apply rotation and translate back. + // auto r = + // t.inverse() * Eigen::Rotation2D(to.center.theta - from.center.theta) * t; + // // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); + // std::cout << "Transformation matrix: " << std::endl; + // auto final_t = t.matrix(); + // std::cout << final_t << std::endl; + // return final_t; + + Eigen::Matrix trans_mat; + trans_mat(0,0) = 1.0; trans_mat(0,1) = 0.0; trans_mat(0,2) = -to.center.x; + trans_mat(1,0) = 0.0; trans_mat(1,1) = 1.0; trans_mat(1,2) = -to.center.y; + trans_mat(2,0) = 0; trans_mat(2,1) = 0; trans_mat(2,2) = 1.0; + + // First translate to p1 + Eigen::Matrix rot_mat; + const double th = to.center.theta - from.center.theta; + rot_mat(0,0) = std::cos(th); rot_mat(0,1) = -1.0 * std::sin(th); rot_mat(0,2) = -to.center.x; + rot_mat(1,0) = std::sin(th); rot_mat(1,1) = std::cos(th); rot_mat(1,2) = -to.center.y; + rot_mat(2,0) = 0; rot_mat(2,1) = 0; rot_mat(2,2) = 1.0; + + for (auto& v : vertices) + { + // v = trans_mat * v; + v = rot_mat * v; + } } + struct CollisionBox { Eigen::Vector2d min; // min x & y @@ -82,28 +98,25 @@ bool between( print_geom(o2, "obs"); auto make_collision_box = - [](const CollisionGeometry& o, const bool origin) -> CollisionBox - { - CollisionBox box; - if (origin) + [](const CollisionGeometry& o) -> CollisionBox { + 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}; - } - else + return box; + }; + + auto get_vertices = + [](const CollisionGeometry& o) -> std::vector { + Eigen::Matrix mat; const double th = o.center.theta; mat(0,0) = std::cos(th); mat(0,1) = std::sin(-1.0 *th); mat(0,2) = o.center.x; mat(1,0) = std::sin(th); mat(1,1) = std::cos(th); mat(1,2) = o.center.y; mat(2,0) = 0; mat(2,1) = 0; mat(2,2) = 1.0; - // Eigen::Transform t; - // t = Eigen::Translation(Eigen::Vector2d{o.center.x, o.center.y}); - // // We need to rotate the point about p1. To do this we translate p2 to p1, - // // apply rotation and translate back. - // t.rotate(Eigen::Rotation2D(o.center.theta)); - // // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); - std::cout << "make collision Transformation matrix: " << std::endl; + + std::cout << "get_vertices matrix: " << std::endl; std::cout << mat << std::endl; std::vector vertices; vertices.push_back({-o.size_x * 0.5, o.size_y * 0.5, 1.0}); @@ -112,46 +125,49 @@ bool between( vertices.push_back({o.size_x * 0.5, -o.size_y * 0.5, 1.0}); for (auto& v : vertices) - { v = mat * v; - std::cout << v << std::endl; - } - - box.min = {std::numeric_limits::max(), std::numeric_limits::max()}; - 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] < box.min[i]) - box.min[i] = v[i]; - if (v[i] > box.max[i]) - box.max[i] = v[i]; - } - } - } - return box; - }; + return vertices; + }; - const auto& o1_box = make_collision_box(o1, true); + + const auto& o1_box = make_collision_box(o1); print_box(o1_box, "Lane box"); - std::vector vertices; - const auto o2_box = make_collision_box(o2, false); - print_box(o2_box, "obs box"); - vertices.push_back({o2_box.min[0], o2_box.min[1]}); - vertices.push_back({o2_box.max[0], o2_box.max[1]}); + auto vertices = get_vertices(o2); + + std::cout << "Obs vertices before trans: "; + for (const auto& v : vertices) + std::cout << "{" << v[0] << "," << v[1] << "}" << " "; + std::cout << std::endl; + // Transform o2 vertices into o1 coordinates - const auto& transform = get_transform(o1, o2); - for (auto& v : vertices) - v = (transform * Eigen::Vector3d{v[0], v[1], 1.0}).block<2,1>(0, 0); + transform_vertices(o1, o2, vertices); + + std::cout << "Obs vertices after trans: "; + for (const auto& v : vertices) + std::cout << "{" << v[0] << "," << v[1] << "}" << " "; + std::cout << std::endl; + + // for (auto& v : vertices) + // v = transform * v; // Use SAT. Project o2 vertices onto o1 X & Y axes - CollisionBox o2t_box; - o2t_box.min = vertices[0]; - o2t_box.max = vertices[1]; - print_box(o2t_box, "obs transformed box"); + 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]; + } + } + + print_box(o2_box, "obs transformed box"); // how_much = @@ -164,8 +180,8 @@ bool between( // dist = std::min( // dist, // std::min( - // std::abs(o2t_box.min[i] - o1_box.max[i]), - // std::abs(o1_box.min[i] - o2t_box.max[i])) + // std::abs(o2_box.min[i] - o1_box.max[i]), + // std::abs(o1_box.min[i] - o2_box.max[i])) // ); // } // std::cout << "dist: " << dist << std::endl; @@ -173,15 +189,23 @@ bool between( // return dist; // }(); - how_much = 0.0; + auto dist = + [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) -> double + { + return (a-b).norm(); + }; + + how_much = std::numeric_limits::max(); + how_much = std::min(how_much, dist(o1_box.min, o2_box.min)); + how_much = std::min(how_much, dist(o1_box.min, o2_box.max)); + how_much = std::min(how_much, dist(o1_box.max, o2_box.min)); + how_much = std::min(how_much, dist(o1_box.max, o2_box.max)); + + for (std::size_t i = 0; i < 2; ++i) { - if (o2t_box.min[i] > o1_box.max[i] || o2t_box.max[i] < o1_box.min[i]) + if (o2_box.min[i] > o1_box.max[i] || o2_box.max[i] < o1_box.min[i]) { - // Does not overlap along this axis hence the two obstacles do not overlap - how_much = std::min( - std::abs(o2t_box.min[i] - o1_box.max[i]), - std::abs(o2t_box.max[i] - o1_box.min[i])); return false; } } diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index a405b5999..08c6c6b3f 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -57,7 +57,8 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); - CHECK_FALSE(intersect); + REQUIRE_FALSE(intersect); + std::cout <<"how_much: " << how_much << std::endl; CHECK(how_much - 1.0 == Approx(0.0).margin(1e-3)); } @@ -82,7 +83,7 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); - CHECK_FALSE(intersect); + REQUIRE_FALSE(intersect); std::cout <<"how_much: " << how_much << std::endl; CHECK((how_much - 0.586) == Approx(0.0).margin(1e-3)); } @@ -228,7 +229,7 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); - CHECK_FALSE(intersect); + REQUIRE_FALSE(intersect); CHECK((how_much - 2.336) == Approx(0.0).margin(1e-1)); std::cout <<"how_much: " << how_much << std::endl; } @@ -254,7 +255,7 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); - CHECK_FALSE(intersect); + REQUIRE_FALSE(intersect); CHECK((how_much - 6.773) == Approx(0.0).margin(1e-1)); std::cout <<"how_much: " << how_much << std::endl; } @@ -280,7 +281,7 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); - CHECK_FALSE(intersect); + REQUIRE_FALSE(intersect); CHECK((how_much - 4.314) == Approx(0.0).margin(1e-1)); std::cout <<"how_much: " << how_much << std::endl; } From a87b5b1940c60966b01154f44fd1b62a70cf15b2 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 17:16:40 +0800 Subject: [PATCH 41/60] Cleanup Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 240 ++++++++---------- .../src/lane_blocker/LaneBlocker.cpp | 16 +- 2 files changed, 111 insertions(+), 145 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index d0c9502e0..90ec1ec1d 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -17,56 +17,95 @@ #include "IntersectionChecker.hpp" +#ifndef NDEBUG #include +#endif //============================================================================== namespace IntersectionChecker { namespace { -void transform_vertices(const CollisionGeometry& to, - const CollisionGeometry& from, std::vector& vertices) +struct CollisionBox +{ + Eigen::Vector2d min; // min x & y + Eigen::Vector2d max; // max x & y +}; + +CollisionBox make_reference_box(const CollisionGeometry& o) { - // Eigen::Transform t; - // Eigen::Vector2d p1 = {to.center.x, to.center.y}; - // Eigen::Vector2d p2 = {from.center.x, from.center.y}; - - // t = Eigen::Translation(-p1); - // // We need to rotate the point about p1. To do this we translate p2 to p1, - // // apply rotation and translate back. - // auto r = - // t.inverse() * Eigen::Rotation2D(to.center.theta - from.center.theta) * t; - // // t.rotate(Eigen::Rotation2D(from.center.theta - to.center.theta)); - // std::cout << "Transformation matrix: " << std::endl; - // auto final_t = t.matrix(); - // std::cout << final_t << std::endl; - // return final_t; - - Eigen::Matrix trans_mat; - trans_mat(0,0) = 1.0; trans_mat(0,1) = 0.0; trans_mat(0,2) = -to.center.x; - trans_mat(1,0) = 0.0; trans_mat(1,1) = 1.0; trans_mat(1,2) = -to.center.y; - trans_mat(2,0) = 0; trans_mat(2,1) = 0; trans_mat(2,2) = 1.0; - - // First translate to p1 - Eigen::Matrix rot_mat; + 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) +{ + // We will rotate second geometry about the origin by the difference in + // thetas and then translate to the first geometry's frame + Eigen::Matrix mat; const double th = to.center.theta - from.center.theta; - rot_mat(0,0) = std::cos(th); rot_mat(0,1) = -1.0 * std::sin(th); rot_mat(0,2) = -to.center.x; - rot_mat(1,0) = std::sin(th); rot_mat(1,1) = std::cos(th); rot_mat(1,2) = -to.center.y; - rot_mat(2,0) = 0; rot_mat(2,1) = 0; rot_mat(2,2) = 1.0; + mat(0,0) = std::cos(th); + mat(0,1) = std::sin(-1.0 *th); + mat(0,2) = (from.center.x - to.center.x); + mat(1,0) = std::sin(th); + mat(1,1) = std::cos(th); + mat(1,2) = (from.center.y - to.center.y); + mat(2,0) = 0; + mat(2,1) = 0; + mat(2,2) = 1.0; + + #ifndef NDEBUG + std::cout << "get_vertices matrix: " << std::endl; + std::cout << mat << std::endl; + #endif + + std::vector vertices; + vertices.push_back({-from.size_x * 0.5, from.size_y * 0.5, 1.0}); + vertices.push_back({-from.size_x * 0.5, -from.size_y * 0.5, 1.0}); + vertices.push_back({from.size_x * 0.5, from.size_y * 0.5, 1.0}); + vertices.push_back({from.size_x * 0.5, -from.size_y * 0.5, 1.0}); + + #ifndef NDEBUG + std::cout << "Obs vertices before trans: "; + for (const auto& v : vertices) + std::cout << "{" << v[0] << "," << v[1] << "}" << " "; + std::cout << std::endl; + #endif for (auto& v : vertices) + v = mat * v; + + #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) { - // v = trans_mat * v; - v = rot_mat * v; + 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]; + } } -} - -struct CollisionBox -{ - Eigen::Vector2d min; // min x & y - Eigen::Vector2d max; // max x & y -}; + return {o2_box, vertices}; +} } // anonymous namespace @@ -77,6 +116,7 @@ bool between( const CollisionGeometry& o2, double& how_much) { + #ifndef NDEBUG auto print_geom = [](const CollisionGeometry& o, const std::string& name) { @@ -88,120 +128,50 @@ bool between( 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 << 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 - auto make_collision_box = - [](const CollisionGeometry& o) -> CollisionBox - { - 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; - }; + const auto o1_box = make_reference_box(o1); + const auto result = make_transformed_box(o1, o2); + const auto& o2_box = result.first; - auto get_vertices = - [](const CollisionGeometry& o) -> std::vector - { - - Eigen::Matrix mat; - const double th = o.center.theta; - mat(0,0) = std::cos(th); mat(0,1) = std::sin(-1.0 *th); mat(0,2) = o.center.x; - mat(1,0) = std::sin(th); mat(1,1) = std::cos(th); mat(1,2) = o.center.y; - mat(2,0) = 0; mat(2,1) = 0; mat(2,2) = 1.0; - - std::cout << "get_vertices matrix: " << std::endl; - std::cout << mat << std::endl; - std::vector vertices; - vertices.push_back({-o.size_x * 0.5, o.size_y * 0.5, 1.0}); - vertices.push_back({-o.size_x * 0.5, -o.size_y * 0.5, 1.0}); - vertices.push_back({o.size_x * 0.5, o.size_y * 0.5, 1.0}); - vertices.push_back({o.size_x * 0.5, -o.size_y * 0.5, 1.0}); - - for (auto& v : vertices) - v = mat * v; - - return vertices; - }; - - - const auto& o1_box = make_collision_box(o1); + #ifndef NDEBUG print_box(o1_box, "Lane box"); - auto vertices = get_vertices(o2); - - std::cout << "Obs vertices before trans: "; - for (const auto& v : vertices) - std::cout << "{" << v[0] << "," << v[1] << "}" << " "; - std::cout << std::endl; - - // Transform o2 vertices into o1 coordinates - transform_vertices(o1, o2, vertices); - - std::cout << "Obs vertices after trans: "; - for (const auto& v : vertices) - std::cout << "{" << v[0] << "," << v[1] << "}" << " "; - std::cout << std::endl; - - // for (auto& v : vertices) - // v = transform * v; - - // Use SAT. Project o2 vertices onto o1 X & Y axes - 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()}; + print_box(o2_box, "obs transformed box"); + #endif - for (const auto& v : vertices) + // 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) { - for (std::size_t i = 0; i < 2; ++i) + double dist; + // O2 projections are on the left of O1 extremas + if (o2_box.max[i] < o1_box.min[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]; + dist = std::abs(o2_box.max[i] - o1_box.min[i]); } - } - - print_box(o2_box, "obs transformed box"); - - - // how_much = - // [&]() -> double - // { - // double dist = std::numeric_limits::max(); - // for (std::size_t i = 0; i < 2; ++i) - // { - // std::cout << "dist: " << dist << std::endl; - // dist = std::min( - // dist, - // std::min( - // std::abs(o2_box.min[i] - o1_box.max[i]), - // std::abs(o1_box.min[i] - o2_box.max[i])) - // ); - // } - // std::cout << "dist: " << dist << std::endl; - - // return dist; - // }(); - - auto dist = - [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) -> double + // O2 projections are on the right of O1 extremas + else if (o2_box.min[i] > o1_box.max[i]) { - return (a-b).norm(); - }; - - how_much = std::numeric_limits::max(); - how_much = std::min(how_much, dist(o1_box.min, o2_box.min)); - how_much = std::min(how_much, dist(o1_box.min, o2_box.max)); - how_much = std::min(how_much, dist(o1_box.max, o2_box.min)); - how_much = std::min(how_much, dist(o1_box.max, o2_box.max)); - + 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]) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 8b3c1b449..d83cac92f 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -32,8 +32,10 @@ #include #include -#include +#ifndef NDEBUG +#include +#endif //============================================================================== namespace { @@ -108,7 +110,7 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) ); _obstacle_lane_threshold = this->declare_parameter( - "obstacle_lane_threshold", 1.0); + "obstacle_lane_threshold", 0.25); RCLCPP_INFO( this->get_logger(), "Setting parameter obstacle_lane_threshold to %f", _obstacle_lane_threshold @@ -455,24 +457,18 @@ void LaneBlocker::process() ); if (intersect || how_much < threshold) { - if (intersect) - { - std::cout << "INTERSECTION!!" << std::endl; - } - else - { - std::cout << "HOW_MUCH: " << how_much << std::endl; - } std::lock_guardlock(mutex); vicinity_lane_keys.insert( LaneBlocker::get_lane_key(fleet_name, i)); } } const auto finish_time = std::chrono::steady_clock::now(); + #ifndef NDEBUG 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 = {}; From 4280b8941e4c30639fb4398b2c96f0470c15e514 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 19:57:09 +0800 Subject: [PATCH 42/60] Many logic fixes Signed-off-by: Yadunund --- .../src/lane_blocker/LaneBlocker.cpp | 187 +++++++++--------- .../src/lane_blocker/LaneBlocker.hpp | 15 +- 2 files changed, 107 insertions(+), 95 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index d83cac92f..0723ec62c 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -167,11 +167,11 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) this->process(); }); - auto cull_timer_period = + _cull_timer_period = std::chrono::duration_cast( std::chrono::duration>(1.0 / cull_rate)); _cull_timer = this->create_wall_timer( - std::move(cull_timer_period), + _cull_timer_period, [=]() { this->cull(); @@ -308,8 +308,8 @@ void LaneBlocker::obstacle_cb(const Obstacles& msg) }; // Add to obstacle queue for processing in a separate thread/callback - _obstacle_buffer[LaneBlocker::get_obstacle_key(obs)] = - std::move(obs); + const auto& obs_key = LaneBlocker::get_obstacle_key(obs); + _obstacle_buffer[obs_key] = std::move(obs); } } @@ -321,52 +321,48 @@ void LaneBlocker::process() // Keep track of which lanes were updated to decided whether to modify lane // state. - std::unordered_set lanes_with_changes; + 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& [key, obstacle] : _obstacle_buffer) + for (const auto& [obstacle_key, obstacle] : _obstacle_buffer) { - // If the lifetime of the obstacle has passed, we skip it. + // If the lifetime of the obstacle has passed cull() will handle the purging if (obstacle.expiry_time < get_clock()->now()) { continue; } - const std::string& obstacle_key = LaneBlocker::get_obstacle_key(obstacle); - - // The keys in _obstacle_to_lanes_map are hashed only based on source - // and id. We should check if this obstacle has expired and if so, create - // a new entry. This is helpful for culling. - if (auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle) != - _obstacle_to_lanes_map.end()) - { - // We update the obstacle key with latest expiry - // extract is a C++17 feature - auto handler = _obstacle_to_lanes_map.extract(obstacle); - handler.key() = obstacle; - _obstacle_to_lanes_map.insert(std::move(handler)); - } - // 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); + auto obs_lane_it = _obstacle_to_lanes_map.find(obstacle_key); if (obs_lane_it != _obstacle_to_lanes_map.end()) { - auto& lanes_keys = obs_lane_it->second; + const auto& lanes_keys = obs_lane_it->second; RCLCPP_INFO( this->get_logger(), "Obstacle %s was previously in the vicinity of %ld lanes", - key.c_str(), lanes_keys.size()); + 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) { - const auto& lane = this->lane_from_key(lane_key); - const auto [fleet_name, id] = deserialize_key(key); - double how_much; + const auto [fleet_name, lane_id] = deserialize_key(lane_key); 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_graphs.at(fleet_name), + traffic_graph, lane, _lane_width); const auto& o2 = make_collision_geometry(obstacle.transformed_bbox); @@ -381,38 +377,19 @@ void LaneBlocker::process() RCLCPP_INFO( this->get_logger(), "Obstacle %s is still in the vicinity of lane %s", - key.c_str(), lane_key.c_str() + obstacle_key.c_str(), lane_key.c_str() ); - // TODO(YV): Is there any value in updating the actual ObstaclePtr - // in the other caches? - continue; } else { - try - { - RCLCPP_INFO( - this->get_logger(), - "Obstacle %s is no longer in the vicinity of lane %s. " - "Updating cache...", 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 - _obstacle_to_lanes_map[obstacle].erase(lane_key); - //Remove from _lane_to_obstacles_map - _lane_to_obstacles_map[lane_key].erase(obstacle_key); - lanes_with_changes.insert(lane_key); - } - catch(const std::exception& e) - { - RCLCPP_ERROR( - this->get_logger(), - "[LaneBlocker::process()]: Unable to update obstacle caches." - "This is a bug and should be reported. Detailed error: %s", - e.what() - ); - continue; - } + 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); } } } @@ -423,7 +400,7 @@ void LaneBlocker::process() RCLCPP_INFO( this->get_logger(), "Obstacle %s was not previously in the vicinity of any lane. Checking " - "for any changes", key.c_str() + "for any changes", obstacle_key.c_str() ); std::unordered_set vicinity_lane_keys = {}; std::mutex mutex; @@ -462,8 +439,8 @@ void LaneBlocker::process() LaneBlocker::get_lane_key(fleet_name, i)); } } - const auto finish_time = std::chrono::steady_clock::now(); #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 @@ -495,13 +472,13 @@ void LaneBlocker::process() RCLCPP_INFO( this->get_logger(), "Search concluded with %ld lanes in the vicinity of obstacle %s", - vicinity_lane_keys.size(), key.c_str() + vicinity_lane_keys.size(), obstacle_key.c_str() ); // Update caches for (const auto& lane_key : vicinity_lane_keys) { - _obstacle_to_lanes_map[obstacle].insert(lane_key); + _obstacle_to_lanes_map[obstacle_key].insert(lane_key); _lane_to_obstacles_map[lane_key].insert(obstacle_key); lanes_with_changes.insert(lane_key); } @@ -513,9 +490,22 @@ void LaneBlocker::process() "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)); - // Reinitialize the buffer - _obstacle_buffer = {}; } //============================================================================== @@ -608,24 +598,42 @@ auto LaneBlocker::deserialize_key( 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(std::move(name), std::move(id)); + return std::make_pair(name, id); } catch(const std::exception& e) { // *INDENT-OFF* throw std::runtime_error( - "[LaneBlocker::lane_from_key] Unable to parse key. This is a bug and " + "[LaneBlocker::deserialize_key] Unable to parse key. This is a bug and " "should be reported. Detailed error: " + std::string(e.what())); // *INDENT-ON* } } + //============================================================================== -auto LaneBlocker::lane_from_key( - const std::string& key) const-> const TrafficGraph::Lane& +void LaneBlocker::purge_obstacles( + const std::unordered_set& obstacle_keys, + const bool erase_from_buffer) { - const auto [fleet_name, id] = deserialize_key(key); - return _traffic_graphs.at(fleet_name).get_lane(id); - + 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); + } } @@ -634,45 +642,42 @@ void LaneBlocker::cull() { // Cull obstacles that are past their expiry times. // Also decide whether previously closed lanes should be re-opened. - std::unordered_set to_cull; + std::unordered_set obstacles_to_cull; std::unordered_set lanes_with_changes; const auto now = this->get_clock()->now(); - for (const auto& [obstacle, lanes] : _obstacle_to_lanes_map) + for (const auto& [obstacle_key, lanes] : _obstacle_to_lanes_map) { - - if (obstacle.expiry_time > now) + auto it = _obstacle_buffer.find(obstacle_key); + if (it == _obstacle_buffer.end()) { - to_cull.insert(obstacle); + // 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) { - const std::string obstacle_key = - LaneBlocker::get_obstacle_key(obstacle); _lane_to_obstacles_map[lane].erase(obstacle_key); lanes_with_changes.insert(lane); } } } - // Cull - for (const auto& obs : to_cull) - { - _obstacle_to_lanes_map.erase(obs); - } - - // TODO(YV): Open lanes that no longer have obstacles + // Cull + purge_obstacles(obstacles_to_cull); // Open lanes if needed // A map to collate lanes per fleet that need to be closed + std::unordered_set opened_lanes = {}; std::unordered_map> open_msgs; - for (const auto& lane : lanes_with_changes) + for (const auto& lane : _currently_closed_lanes) { - if (_currently_closed_lanes.find(lane) == _currently_closed_lanes.end()) - continue; - // The lane has changes and is currently closed. We check if the obstacle - // count is below the threshold and if so open. if (_lane_to_obstacles_map.at(lane).size() < _lane_closure_threshold) { // The lane can be opened @@ -692,6 +697,7 @@ void LaneBlocker::cull() // Msg was created before. We simply append the new lane id msg_it.first->second->open_lanes.push_back(std::move(lane_id)); } + opened_lanes.insert(lane); } } @@ -715,6 +721,9 @@ void LaneBlocker::cull() _lane_closure_pub->publish(std::move(msg)); } + for (const auto& lane : opened_lanes) + _currently_closed_lanes.erase(lane); + } 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 index 72e2a72b8..dd3c85f71 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -61,6 +61,7 @@ class LaneBlocker : public rclcpp::Node void process(); void cull(); + // Internal data struct struct ObstacleData { rclcpp::Time expiry_time; @@ -124,12 +125,14 @@ class LaneBlocker : public rclcpp::Node std::pair deserialize_key(const std::string& key) const; - const TrafficGraph::Lane& lane_from_key(const std::string& key) const; - // Modify lanes with changes in number of vicinity obstacles void request_lane_modifications( const std::unordered_set& changes); + 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 @@ -139,11 +142,10 @@ class LaneBlocker : public rclcpp::Node // TODO(YV): Based on the current implementation, we should be able to // cache obstacle_key directly - // Map an obstacle to the lanes in its vicinity + // Map an obstacle key to the lanes keys in its vicinity std::unordered_map< - ObstacleData, - std::unordered_set, - ObstacleHash> _obstacle_to_lanes_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 @@ -171,6 +173,7 @@ class LaneBlocker : public rclcpp::Node double _lane_width; double _obstacle_lane_threshold; std::chrono::nanoseconds _max_search_duration; + std::chrono::nanoseconds _cull_timer_period; std::size_t _lane_closure_threshold; rclcpp::TimerBase::SharedPtr _process_timer; From 22ee0e7406e33c0e362416c1c5040546363d2eda Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 14 Jul 2022 19:58:16 +0800 Subject: [PATCH 43/60] Uncrustify Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.cpp | 44 ++-- .../src/lane_blocker/LaneBlocker.cpp | 37 +-- .../src/lane_blocker/LaneBlocker.hpp | 237 +++++++++--------- .../test/test_IntersectionChecker.cpp | 8 +- 4 files changed, 164 insertions(+), 162 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index 90ec1ec1d..cbc1d93c6 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -36,7 +36,7 @@ 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}; + box.max = Eigen::Vector2d{o.size_x* 0.5, o.size_y* 0.5}; return box; } @@ -48,15 +48,15 @@ std::pair> make_transformed_box( // thetas and then translate to the first geometry's frame Eigen::Matrix mat; const double th = to.center.theta - from.center.theta; - mat(0,0) = std::cos(th); - mat(0,1) = std::sin(-1.0 *th); - mat(0,2) = (from.center.x - to.center.x); - mat(1,0) = std::sin(th); - mat(1,1) = std::cos(th); - mat(1,2) = (from.center.y - to.center.y); - mat(2,0) = 0; - mat(2,1) = 0; - mat(2,2) = 1.0; + mat(0, 0) = std::cos(th); + mat(0, 1) = std::sin(-1.0 *th); + mat(0, 2) = (from.center.x - to.center.x); + mat(1, 0) = std::sin(th); + mat(1, 1) = std::cos(th); + mat(1, 2) = (from.center.y - to.center.y); + mat(2, 0) = 0; + mat(2, 1) = 0; + mat(2, 2) = 1.0; #ifndef NDEBUG std::cout << "get_vertices matrix: " << std::endl; @@ -66,8 +66,8 @@ std::pair> make_transformed_box( std::vector vertices; vertices.push_back({-from.size_x * 0.5, from.size_y * 0.5, 1.0}); vertices.push_back({-from.size_x * 0.5, -from.size_y * 0.5, 1.0}); - vertices.push_back({from.size_x * 0.5, from.size_y * 0.5, 1.0}); - vertices.push_back({from.size_x * 0.5, -from.size_y * 0.5, 1.0}); + vertices.push_back({from.size_x* 0.5, from.size_y* 0.5, 1.0}); + vertices.push_back({from.size_x* 0.5, -from.size_y* 0.5, 1.0}); #ifndef NDEBUG std::cout << "Obs vertices before trans: "; @@ -88,9 +88,9 @@ std::pair> make_transformed_box( CollisionBox o2_box; o2_box.min = - {std::numeric_limits::max(), std::numeric_limits::max()}; + {std::numeric_limits::max(), std::numeric_limits::max()}; o2_box.max = - {std::numeric_limits::lowest(), + {std::numeric_limits::lowest(), std::numeric_limits::lowest()}; for (const auto& v : vertices) @@ -118,12 +118,12 @@ bool between( { #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; - }; + [](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) @@ -135,7 +135,7 @@ bool between( }; std::cout << "================================================" << std::endl; - std::cout << "Checking collision between: " << std:: endl; + std::cout << "Checking collision between: " << std::endl; print_geom(o1, "Lane"); print_geom(o2, "obs"); #endif @@ -158,7 +158,7 @@ bool between( // 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]); + 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]) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 0723ec62c..e3b759a4f 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -323,7 +323,8 @@ void LaneBlocker::process() // 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 = {}; + std::unordered_map> obstacles_with_changes = {}; for (const auto& [obstacle_key, obstacle] : _obstacle_buffer) { @@ -406,12 +407,12 @@ void LaneBlocker::process() 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 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; @@ -434,7 +435,7 @@ void LaneBlocker::process() ); if (intersect || how_much < threshold) { - std::lock_guardlock(mutex); + std::lock_guard lock(mutex); vicinity_lane_keys.insert( LaneBlocker::get_lane_key(fleet_name, i)); } @@ -453,13 +454,13 @@ void LaneBlocker::process() { search_threads.push_back( std::thread( - search_vicinity_lanes, - fleet_name, - graph, - obstacle, - _obstacle_lane_threshold, - _lane_width, - _max_search_duration) + search_vicinity_lanes, + fleet_name, + graph, + obstacle, + _obstacle_lane_threshold, + _lane_width, + _max_search_duration) ); } @@ -578,7 +579,7 @@ void LaneBlocker::request_lane_modifications( RCLCPP_INFO( this->get_logger(), "Requested %ld lanes to close for fleet %s", - msg->close_lanes.size(), msg->fleet_name.c_str() + msg->close_lanes.size(), msg->fleet_name.c_str() ); _lane_closure_pub->publish(std::move(msg)); } @@ -600,7 +601,7 @@ auto LaneBlocker::deserialize_key( std::size_t id; ss >> id; return std::make_pair(name, id); } - catch(const std::exception& e) + catch (const std::exception& e) { // *INDENT-OFF* throw std::runtime_error( @@ -716,7 +717,7 @@ void LaneBlocker::cull() RCLCPP_INFO( this->get_logger(), "Requested %ld lanes to open for fleet %s", - msg->open_lanes.size(), msg->fleet_name.c_str() + msg->open_lanes.size(), msg->fleet_name.c_str() ); _lane_closure_pub->publish(std::move(msg)); } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index dd3c85f71..6b8f3a489 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -54,130 +54,131 @@ class LaneBlocker : public rclcpp::Node /// Constructor LaneBlocker( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - void obstacle_cb(const Obstacles& msg); - 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) + void obstacle_cb(const Obstacles& msg); + 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 { - return LaneBlocker::get_obstacle_key( - obstacle.source, obstacle.id); + 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; } - - static inline std::string get_lane_key( - const std::string& fleet_name, - const std::size_t lane_index) + }; + 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 { - return fleet_name + "_" + std::to_string(lane_index); + const std::string key = LaneBlocker::get_obstacle_key(obstacle); + return std::hash()(key); } - - struct ObstacleHash - { - std::size_t operator()( - const ObstacleData& obstacle) const - { - const std::string key = LaneBlocker::get_obstacle_key(obstacle); - return std::hash()(key); - } - }; - - std::pair - 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); - - 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 = {}; - - std::unordered_set _currently_closed_lanes; - - rclcpp::Subscription::SharedPtr _obstacle_sub; - 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::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; - std::size_t _lane_closure_threshold; - - rclcpp::TimerBase::SharedPtr _process_timer; - rclcpp::TimerBase::SharedPtr _cull_timer; + }; + + std::pair + 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); + + 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 = {}; + + std::unordered_set _currently_closed_lanes; + + rclcpp::Subscription::SharedPtr _obstacle_sub; + 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::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; + std::size_t _lane_closure_threshold; + + rclcpp::TimerBase::SharedPtr _process_timer; + rclcpp::TimerBase::SharedPtr _cull_timer; }; #endif // SRC__LANE_BLOCKER__LANEBLOCKER_HPP diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 08c6c6b3f..7ff3c5a9b 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -31,10 +31,10 @@ SCENARIO("Test IntersectionChecker") { auto radians = - [](double degree) -> double - { - return degree * M_PI / 180.0; - }; + [](double degree) -> double + { + return degree * M_PI / 180.0; + }; WHEN("AABB geometries are not intersecting and 1m apart") { From 992af122750f77b0c1dd3a282968166a4d653f1f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 15 Jul 2022 17:42:39 +0800 Subject: [PATCH 44/60] Refactor Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 28 ---- .../include/rmf_obstacle_ros2/Detector.hpp | 63 --------- .../rmf_obstacle_ros2/ObstacleManager.hpp | 76 +++++++++++ .../include/rmf_obstacle_ros2/Responder.hpp | 10 +- .../{Obstacle.hpp => obstacles/Convert.hpp} | 23 ++-- rmf_obstacle_ros2/package.xml | 4 +- .../src/obstacle_manager/ObstacleManager.cpp | 109 ---------------- .../src/obstacle_manager/ObstacleManager.hpp | 56 -------- .../src/rmf_obstacle_ros2/ObstacleManager.cpp | 123 ++++++++++++++++++ .../{Obstacle.cpp => obstacles/Convert.cpp} | 2 +- 10 files changed, 217 insertions(+), 277 deletions(-) delete mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp create mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp rename rmf_obstacle_ros2/include/rmf_obstacle_ros2/{Obstacle.hpp => obstacles/Convert.hpp} (67%) delete mode 100644 rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp delete mode 100644 rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp create mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp rename rmf_obstacle_ros2/src/rmf_obstacle_ros2/{Obstacle.cpp => obstacles/Convert.cpp} (98%) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 3a5490ef0..bffb1df82 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -74,33 +74,6 @@ ament_export_include_directories( include ) -#=============================================================================== -add_library(obstacle_manager SHARED src/obstacle_manager/ObstacleManager.cpp) - -target_link_libraries(obstacle_manager - PRIVATE - rmf_obstacle_ros2 - pluginlib::pluginlib - rclcpp::rclcpp -) - -target_include_directories(obstacle_manager - PRIVATE - $ - $ - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_components_INCLUDE_DIRS} - ${sensor_msgs_INCLUDE_DIRS} - ${rmf_obstacle_msgs_INCLUDE_DIRS} - ${pluginlib_INCLUDE_DIRS} -) - -target_compile_features(obstacle_manager INTERFACE cxx_std_17) - -rclcpp_components_register_node(obstacle_manager - PLUGIN "rmf_obstacle_ros2::ObstacleManager" - EXECUTABLE obstacle_manager_node) - #=============================================================================== file(GLOB_RECURSE lane_blocker_srcs "src/lane_blocker/*.cpp" @@ -175,7 +148,6 @@ install( install( TARGETS rmf_obstacle_ros2 - obstacle_manager lane_blocker EXPORT rmf_obstacle_ros2 RUNTIME DESTINATION lib/rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp deleted file mode 100644 index d5526824f..000000000 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Detector.hpp +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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__DETECTOR_HPP -#define RMF_OBSTACLE_ROS2__DETECTOR_HPP - -#include - -#include - -#include - -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -/// Pure abstract class for detecting and reporting obstacles -/// This class should be implemented as a plugin using pluginlib -class Detector -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - using DetectorCallback = std::function; - - - /// param[in] node - /// A reference to rclcpp::Node - /// - /// param[in] cb - /// The callback that should be triggered when this detector detects any - /// obstacles. If nullptr, the detector will not do anything. - virtual void initialize( - const rclcpp::Node& node, - DetectorCallback cb) = 0; - - /// Get the name of this detector - virtual std::string name() const = 0; - - virtual ~Detector() = default; - -}; - -using DetectorPtr = std::shared_ptr; -using ConstDetectorPtr = std::shared_ptr; - -} // namespace rmf_obstacle_ros2 - -#endif // RMF_OBSTACLE_ROS2__DETECTOR_HPP diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp new file mode 100644 index 000000000..a150ef90b --- /dev/null +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp @@ -0,0 +1,76 @@ +/* + * 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__OBSTACLEMANAGER_HPP +#define RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP + +#include + +#include + +#include + +#include + +namespace rmf_obstacle_ros2 { + +//============================================================================== +class ObstacleManager +{ +public: + using Obstacles = rmf_obstacle_msgs::msg::Obstacles; + + /// Make an ObstacleManager instance. + /// + /// \note You must initialize rclcpp before calling this, either by using + /// rclcpp::init(~) or rclcpp::Context::init(~). + /// + /// \param[in] node_name + /// The name of the rclcpp::Node for this ObstacleManager. + /// + /// \param[in] responder + /// An optional responder implementation to directly respond to detections + /// + /// \param[in] node_options + /// The options that the rclcpp::Node will be constructed with. + std::shared_ptr make( + const std::string& name, + ConstResponderPtr responder = nullptr, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); + + /// Call this function when you have some detections. + /// This function will publish the detection and optionally respond to them. + void process(const Obstacles& detections); + + /// Get the rclcpp::Node that this adapter will be using for communication. + std::shared_ptr node(); + + /// const-qualified node() + std::shared_ptr node() const; + + ~ObstacleManager(); + + class Implementation; + +private: + ObstacleManager(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_obstacle_ros2 + +#endif // RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp index e5b2b20c4..07c453bc5 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp @@ -18,8 +18,6 @@ #ifndef RMF_OBSTACLE_ROS2__RESPONDER_HPP #define RMF_OBSTACLE_ROS2__RESPONDER_HPP -#include - #include #include @@ -27,17 +25,19 @@ namespace rmf_obstacle_ros2 { //============================================================================== -/// Pure abstract class that reacts to obstacles detected. This should be -/// implemented as a plugin using pluginlib. +/// Pure abstract class that reacts to obstacles detected. class Responder { public: using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - virtual void initialize(const rclcpp::Node& node) = 0; + /// Initialize the responder + virtual void initialize(std::shared_ptr node) = 0; + /// Get the name of the responder virtual std::string name() const = 0; + /// Respond to detections virtual void respond(const Obstacles& obstacles) const = 0; virtual ~Responder() = default; diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp similarity index 67% rename from rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp rename to rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp index 784a23964..9d84de27f 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Obstacle.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp @@ -15,10 +15,8 @@ * */ -#ifndef RMF_OBSTACLE_ROS2_OBSTACLE_HPP -#define RMF_OBSTACLE_ROS2_OBSTACLE_HPP - -#include +#ifndef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP +#define RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP #include #include @@ -29,19 +27,20 @@ using PointCloud = sensor_msgs::msg::PointCloud2; using Obstacle = rmf_obstacle_msgs::msg::Obstacle; // TODO(YV): Consider defining a pure abstract class to perform -// serialization/deserialization. The abstract class could also have a function -// to generate MarkerArrays for rviz visualization. -// Provide a default implementation. +// serialization/deserialization which these functions can accept +// Perhaps another abstract class to generate Markers for RViZ visualization. +// Also consider templating the sensor msg + //============================================================================== -/// Serialize a PointCloud2 msg into RMF obstacle octree -static void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle); +/// Serialize a PointCloud2 msg into RMF Obstacle msg +void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle); //============================================================================== -/// Deserialize an RMF obstacle octree into a PointCloud2 msg -static PointCloud convert( +/// Convert an RMF Obstacle msg into a PointCloud2 msg +PointCloud convert( const Obstacle& msg); } // namespace rmf_obstacle_ros2 -#endif // #indef RMF_OBSTACLE_ROS2_OBSTACLE_HPP +#endif // #indef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 5ea056be7..966d55a5e 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -3,15 +3,13 @@ rmf_obstacle_ros2 2.1.0 - A package contiainings utilities for managing obstacle data + A package contiainings utilities for managing obstacles in RMF Yadunund Apache License 2.0 ament_cmake rclcpp - rclcpp_components - pluginlib rmf_utils rmf_obstacle_msgs vision_msgs diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp deleted file mode 100644 index 4c9c23b6d..000000000 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/* - * 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 "ObstacleManager.hpp" -#include - -#include - -#include - - -namespace rmf_obstacle_ros2 { - -//============================================================================== -ObstacleManager::ObstacleManager( - const rclcpp::NodeOptions& options) -: Node("obstacle_manager", options), - _detector_loader("rmf_obstacle_ros2", "rmf_obstacle_ros2::Detector"), - _responder_loader("rmf_obstacle_ros2", "rmf_obstacle_ros2::Responder") -{ - - _responder = nullptr; - - RCLCPP_INFO( - this->get_logger(), - "Setting up ObstacleManager..."); - - // Parameters to receive the fully qualified plugin strings - const std::string detector_plugin = this->declare_parameter( - "detector_plugin", ""); - - const std::string responder_plugin = this->declare_parameter( - "responder_plugin", ""); - - _detection_pub = this->create_publisher( - ObstacleTopicName, - rclcpp::QoS(10)); - - // Initialize the responder - try - { - _responder = _responder_loader.createSharedInstance(responder_plugin); - } - catch (pluginlib::PluginlibException& e) - { - RCLCPP_WARN( - this->get_logger(), - "Failed to load responder plugin provided via the responder_plugin " - "ROS 2 parameter. Please ensure the fully qualified name of the " - "plugin is provided. The ObstacleManager will not respond to any " - "obstacles detected. Detailed error: %s", - e.what()); - } - if (_responder != nullptr) - _responder->initialize(*this); - - // Initialize the detector - try - { - _detector = _detector_loader.createSharedInstance(detector_plugin); - } - catch (pluginlib::PluginlibException& e) - { - RCLCPP_ERROR( - this->get_logger(), - "Failed to load detector plugin provided via the detector_plugin ROS 2 " - "parameter. Please ensure the fully qualified name of the plugin is " - "provided. Detailed error: %s", - e.what()); - return; - } - - RCLCPP_INFO( - this->get_logger(), - "Successfully loaded detector_plugin: %s", - detector_plugin.c_str()); - - - _detector->initialize(*this, - [pub=_detection_pub, responder = _responder]( - const Responder::Obstacles& obstacles) - { - pub->publish(obstacles); - if (responder) - { - responder->respond(obstacles); - } - }); - -} - -} // namespace rmf_obstacle_ros2 - -RCLCPP_COMPONENTS_REGISTER_NODE(rmf_obstacle_ros2::ObstacleManager) diff --git a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp b/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp deleted file mode 100644 index b521531d7..000000000 --- a/rmf_obstacle_ros2/src/obstacle_manager/ObstacleManager.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * 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__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP -#define SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP - -#include - -#include -#include - -#include - -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -/// Node to detect and publish obstacles to RMF -class ObstacleManager : public rclcpp::Node -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - /// The implementation expects users to pass the fully qualified plugin paths - /// for the detector and responder via ROS 2 parameters. The parameter names - /// are "detector_plugin" and "responder_plugin" respectively. - ObstacleManager( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - class Implementation; - -private: - pluginlib::ClassLoader _detector_loader; - pluginlib::ClassLoader _responder_loader; - DetectorPtr _detector; - ResponderPtr _responder; - rclcpp::Publisher::SharedPtr _detection_pub; -}; - -} // namespace rmf_obstacle_ros2 - -#endif // SRC__RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp new file mode 100644 index 000000000..b2fdb15d5 --- /dev/null +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp @@ -0,0 +1,123 @@ +/* + * 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 +#include +namespace rmf_obstacle_ros2 { + +//============================================================================== +class ObstacleManager::Implementation +{ +public: + std::shared_ptr node; + rclcpp::Publisher::SharedPtr obs_pub; + ConstResponderPtr responder; + std::thread spin_thread; +}; + +//============================================================================== +auto ObstacleManager::make( + const std::string& name, + ConstResponderPtr responder, + const rclcpp::NodeOptions& node_options) -> std::shared_ptr +{ + if (!rclcpp::ok(node_options.context())) + { + std::cout << "rclcpp must be initialized before creating an " + << "ObstacleManager! Use rclcpp::init(int argc, char* argv[]) " + << "or rclcpp::Context::init(int argc, char* argv[]) before " + << "calling rmf_obstacle_ros2::ObstacleManager::make(~)" + << std::endl; + return nullptr; + } + + auto manager = std::shared_ptr(new ObstacleManager); + manager->_pimpl = rmf_utils::make_unique_impl(); + + manager->_pimpl->node = std::make_shared(name, node_options); + manager->_pimpl->obs_pub = + manager->_pimpl->node->create_publisher( + ObstacleTopicName, + rclcpp::QoS(10).reliable() + ); + manager->_pimpl->responder = responder; + manager->_pimpl->spin_thread = std::thread( + [n = manager->_pimpl->node]() + { + while (rclcpp::ok()) + rclcpp::spin_some(n); + }); + + return manager; + +} + +//============================================================================== +ObstacleManager::ObstacleManager() +{ + // Do nothing +} + +//============================================================================== +ObstacleManager::~ObstacleManager() +{ + if (_pimpl->spin_thread.joinable()) + _pimpl->spin_thread.join(); +} + +//============================================================================== +std::shared_ptr ObstacleManager::node() +{ + return _pimpl->node; +} + +//============================================================================== +std::shared_ptr ObstacleManager::node() const +{ + return _pimpl->node; +} + +//============================================================================== +void ObstacleManager::process(const Obstacles& detections) +{ + // Simple implementation for now + auto msg = std::make_unique(detections); + _pimpl->obs_pub->publish(std::move(msg)); + RCLCPP_DEBUG( + _pimpl->node->get_logger(), + "Published detection results with %ld obstacles.", + detections.obstacles.size() + ); + if (_pimpl->responder != nullptr) + { + _pimpl->responder->respond(detections); + RCLCPP_DEBUG( + _pimpl->node->get_logger(), + "Passed detection results to responder %s", + _pimpl->responder->name().c_str() + ); + } + + +} + +} // namespace rmf_obstacle_ros2 diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp similarity index 98% rename from rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp rename to rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp index c52e0001b..897ea2168 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/Obstacle.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp @@ -15,7 +15,7 @@ * */ -#include +#include #include From 7b39014b40445f92ec4385023306dc2fd3fdecbd Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 21 Jul 2022 19:26:26 +0800 Subject: [PATCH 45/60] Fix API and use bb msgs from rmf_obstacle_ros2 Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 3 -- .../rmf_obstacle_ros2/ObstacleManager.hpp | 9 +++- rmf_obstacle_ros2/package.xml | 1 - .../src/lane_blocker/IntersectionChecker.hpp | 4 +- .../src/lane_blocker/LaneBlocker.cpp | 2 +- .../src/lane_blocker/LaneBlocker.hpp | 4 +- .../src/lane_blocker/test/CMakeLists.txt | 4 +- .../test/test_IntersectionChecker.cpp | 42 +++++++++---------- .../src/rmf_obstacle_ros2/ObstacleManager.cpp | 37 ++++++++++++---- 9 files changed, 64 insertions(+), 42 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index bffb1df82..3f041c633 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -15,7 +15,6 @@ set(dep_pkgs rmf_utils pluginlib rmf_obstacle_msgs - vision_msgs rmf_fleet_msgs rmf_building_map_msgs sensor_msgs @@ -85,7 +84,6 @@ target_link_libraries(lane_blocker rclcpp::rclcpp ${rclcpp_components_LIBRARIES} ${rmf_obstacle_msgs_LIBRARIES} - ${vision_msgs_LIBRARIES} ${rmf_fleet_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} rmf_fleet_adapter::rmf_fleet_adapter @@ -103,7 +101,6 @@ target_include_directories(lane_blocker ${rclcpp_INCLUDE_DIRS} ${rclcpp_components_INCLUDE_DIRS} ${rmf_obstacle_msgs_INCLUDE_DIRS} - ${vision_msgs_INCLUDE_DIRS} ${rmf_fleet_msgs_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} ${rmf_fleet_adapter_INCLUDE_DIRS} diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp index a150ef90b..fba651eaf 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp @@ -47,7 +47,7 @@ class ObstacleManager /// /// \param[in] node_options /// The options that the rclcpp::Node will be constructed with. - std::shared_ptr make( + static std::shared_ptr make( const std::string& name, ConstResponderPtr responder = nullptr, const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); @@ -62,6 +62,13 @@ class ObstacleManager /// const-qualified node() std::shared_ptr node() const; + /// Begin running the event loop for this manager. The event loop will run + /// in another thread, so this function is non-blocking. + ObstacleManager& start(); + + /// Wait until the adapter is done spinning. + ObstacleManager& wait(); + ~ObstacleManager(); class Implementation; diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 966d55a5e..38f7ecb9b 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -12,7 +12,6 @@ rclcpp rmf_utils rmf_obstacle_msgs - vision_msgs rmf_fleet_msgs rmf_building_map_msgs sensor_msgs diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp index b6f258a39..e331a6c3b 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -20,13 +20,13 @@ #include -#include +#include //============================================================================== // TODO(YV): Consider making this a loadable plugin via pluginlib namespace IntersectionChecker { -using CollisionGeometry = vision_msgs::msg::BoundingBox2D; +using CollisionGeometry = rmf_obstacle_msgs::msg::BoundingBox2D; // Return true if intersect. // If intersect, how_much represents the overlap in meters diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index e3b759a4f..e44bef5f7 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -296,7 +296,7 @@ void LaneBlocker::obstacle_cb(const Obstacles& msg) after_pose.pose.position.y, after_pose.pose.position.z ); auto new_box = - vision_msgs::build() + rmf_obstacle_msgs::build() .center(std::move(after_pose.pose)) .size(std::move(after_size.vector)); diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 6b8f3a489..2d522df77 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,7 @@ class LaneBlocker : public rclcpp::Node using LaneRequest = rmf_fleet_msgs::msg::LaneRequest; using SpeedLimitRequest = rmf_fleet_msgs::msg::SpeedLimitRequest; using LaneStates = rmf_fleet_msgs::msg::LaneStates; - using BoundingBox = vision_msgs::msg::BoundingBox3D; + using BoundingBox = rmf_obstacle_msgs::msg::BoundingBox3D; using Header = std_msgs::msg::Header; /// Constructor diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt index a789f2e8e..f011f3094 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt +++ b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt @@ -12,7 +12,7 @@ ament_add_catch2( target_link_libraries(test_intersection_checker PRIVATE - ${vision_msgs_LIBRARIES} + ${rmf_obstacle_msgs_LIBRARIES} ${geometry_msgs_LIBRARIES} rmf_utils::rmf_utils ) @@ -21,7 +21,7 @@ target_include_directories(test_intersection_checker PRIVATE $ ${EIGEN3_INCLUDE_DIRS} - ${vision_msgs_INCLUDE_DIRS} + ${rmf_obstacle_msgs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${rmf_utils_INCLUDE_DIRS} ) diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 7ff3c5a9b..6fd760808 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -39,7 +39,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are not intersecting and 1m apart") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -47,7 +47,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(4.0) .y(0.0) @@ -65,7 +65,7 @@ SCENARIO("Test IntersectionChecker") WHEN("OBB geometries are not intersecting and 1m apart") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -73,7 +73,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(4.0) .y(0.0) @@ -91,7 +91,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along X-Axis") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -99,7 +99,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(2.5) .y(0.0) @@ -115,7 +115,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along Y-Axis") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -123,7 +123,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(1.5) @@ -139,7 +139,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along X & Y-Axis") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -147,7 +147,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(2.5) .y(0.5) @@ -163,7 +163,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are touching") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) .y(0.0) @@ -171,7 +171,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(3.0) .y(0.0) @@ -187,7 +187,7 @@ SCENARIO("Test IntersectionChecker") WHEN("OBB geometries are overlapping along X & Y-Axis") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(0.0) .y(0.0) @@ -195,7 +195,7 @@ SCENARIO("Test IntersectionChecker") .size_x(2.0) .size_y(2.0); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.414) .y(1.0) @@ -211,7 +211,7 @@ SCENARIO("Test IntersectionChecker") WHEN("Test #1") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(8.6824) .y(-10.9616) @@ -219,7 +219,7 @@ SCENARIO("Test IntersectionChecker") .size_x(1.43478) .size_y(0.5); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(12.002) .y(-10.1094) @@ -237,7 +237,7 @@ SCENARIO("Test IntersectionChecker") WHEN("Test #2") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(11.6892) .y(-3.52843) @@ -245,7 +245,7 @@ SCENARIO("Test IntersectionChecker") .size_x(3.01193) .size_y(0.5); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(12.002) .y(-10.9738) @@ -263,7 +263,7 @@ SCENARIO("Test IntersectionChecker") WHEN("Test #3") { double how_much; - const auto ob1 = vision_msgs::build() + const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(9.57985) .y(-4.6367) @@ -271,7 +271,7 @@ SCENARIO("Test IntersectionChecker") .size_x(3.36581) .size_y(0.5); - const auto ob2 = vision_msgs::build() + const auto ob2 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(12.1453) .y(-11.1404) diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp index b2fdb15d5..0b56813c8 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp @@ -22,6 +22,9 @@ #include #include +#include +#include + namespace rmf_obstacle_ros2 { //============================================================================== @@ -60,13 +63,6 @@ auto ObstacleManager::make( rclcpp::QoS(10).reliable() ); manager->_pimpl->responder = responder; - manager->_pimpl->spin_thread = std::thread( - [n = manager->_pimpl->node]() - { - while (rclcpp::ok()) - rclcpp::spin_some(n); - }); - return manager; } @@ -96,6 +92,31 @@ std::shared_ptr ObstacleManager::node() const return _pimpl->node; } +//============================================================================== +ObstacleManager& ObstacleManager::start() +{ + _pimpl->spin_thread = std::thread( + [n = _pimpl->node]() + { + while (rclcpp::ok()) + rclcpp::spin_some(n); + }); + + return *this; +} + +//============================================================================== +ObstacleManager& ObstacleManager::wait() +{ + std::mutex temp; + std::condition_variable cv; + std::unique_lock lock(temp); + cv.wait( + lock, [&]() { return !rclcpp::ok(); }); + + return *this; +} + //============================================================================== void ObstacleManager::process(const Obstacles& detections) { @@ -116,8 +137,6 @@ void ObstacleManager::process(const Obstacles& detections) _pimpl->responder->name().c_str() ); } - - } } // namespace rmf_obstacle_ros2 From 324d4b44b4f56ccf200e0fd15b1a57b078f12506 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 29 Jul 2022 14:43:07 +0800 Subject: [PATCH 46/60] Simplify pkg Signed-off-by: Yadunund --- .../rmf_obstacle_ros2/ObstacleManager.hpp | 83 ---------- .../include/rmf_obstacle_ros2/Responder.hpp | 52 ------- .../src/rmf_obstacle_ros2/ObstacleManager.cpp | 142 ------------------ 3 files changed, 277 deletions(-) delete mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp delete mode 100644 rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp delete mode 100644 rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp deleted file mode 100644 index fba651eaf..000000000 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/ObstacleManager.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * 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__OBSTACLEMANAGER_HPP -#define RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP - -#include - -#include - -#include - -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -class ObstacleManager -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - - /// Make an ObstacleManager instance. - /// - /// \note You must initialize rclcpp before calling this, either by using - /// rclcpp::init(~) or rclcpp::Context::init(~). - /// - /// \param[in] node_name - /// The name of the rclcpp::Node for this ObstacleManager. - /// - /// \param[in] responder - /// An optional responder implementation to directly respond to detections - /// - /// \param[in] node_options - /// The options that the rclcpp::Node will be constructed with. - static std::shared_ptr make( - const std::string& name, - ConstResponderPtr responder = nullptr, - const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); - - /// Call this function when you have some detections. - /// This function will publish the detection and optionally respond to them. - void process(const Obstacles& detections); - - /// Get the rclcpp::Node that this adapter will be using for communication. - std::shared_ptr node(); - - /// const-qualified node() - std::shared_ptr node() const; - - /// Begin running the event loop for this manager. The event loop will run - /// in another thread, so this function is non-blocking. - ObstacleManager& start(); - - /// Wait until the adapter is done spinning. - ObstacleManager& wait(); - - ~ObstacleManager(); - - class Implementation; - -private: - ObstacleManager(); - rmf_utils::unique_impl_ptr _pimpl; -}; - -} // namespace rmf_obstacle_ros2 - -#endif // RMF_OBSTACLE_ROS2__OBSTACLEMANAGER_HPP diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp deleted file mode 100644 index 07c453bc5..000000000 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/Responder.hpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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__RESPONDER_HPP -#define RMF_OBSTACLE_ROS2__RESPONDER_HPP - -#include - -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -/// Pure abstract class that reacts to obstacles detected. -class Responder -{ -public: - using Obstacles = rmf_obstacle_msgs::msg::Obstacles; - - /// Initialize the responder - virtual void initialize(std::shared_ptr node) = 0; - - /// Get the name of the responder - virtual std::string name() const = 0; - - /// Respond to detections - virtual void respond(const Obstacles& obstacles) const = 0; - - virtual ~Responder() = default; - -}; - -using ResponderPtr = std::shared_ptr; -using ConstResponderPtr = std::shared_ptr; - -} // namespace rmf_obstacle_ros2 - -#endif // RMF_OBSTACLE_ROS2__RESPONDER_HPP diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp deleted file mode 100644 index 0b56813c8..000000000 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/ObstacleManager.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - * 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 -#include -#include -#include - -namespace rmf_obstacle_ros2 { - -//============================================================================== -class ObstacleManager::Implementation -{ -public: - std::shared_ptr node; - rclcpp::Publisher::SharedPtr obs_pub; - ConstResponderPtr responder; - std::thread spin_thread; -}; - -//============================================================================== -auto ObstacleManager::make( - const std::string& name, - ConstResponderPtr responder, - const rclcpp::NodeOptions& node_options) -> std::shared_ptr -{ - if (!rclcpp::ok(node_options.context())) - { - std::cout << "rclcpp must be initialized before creating an " - << "ObstacleManager! Use rclcpp::init(int argc, char* argv[]) " - << "or rclcpp::Context::init(int argc, char* argv[]) before " - << "calling rmf_obstacle_ros2::ObstacleManager::make(~)" - << std::endl; - return nullptr; - } - - auto manager = std::shared_ptr(new ObstacleManager); - manager->_pimpl = rmf_utils::make_unique_impl(); - - manager->_pimpl->node = std::make_shared(name, node_options); - manager->_pimpl->obs_pub = - manager->_pimpl->node->create_publisher( - ObstacleTopicName, - rclcpp::QoS(10).reliable() - ); - manager->_pimpl->responder = responder; - return manager; - -} - -//============================================================================== -ObstacleManager::ObstacleManager() -{ - // Do nothing -} - -//============================================================================== -ObstacleManager::~ObstacleManager() -{ - if (_pimpl->spin_thread.joinable()) - _pimpl->spin_thread.join(); -} - -//============================================================================== -std::shared_ptr ObstacleManager::node() -{ - return _pimpl->node; -} - -//============================================================================== -std::shared_ptr ObstacleManager::node() const -{ - return _pimpl->node; -} - -//============================================================================== -ObstacleManager& ObstacleManager::start() -{ - _pimpl->spin_thread = std::thread( - [n = _pimpl->node]() - { - while (rclcpp::ok()) - rclcpp::spin_some(n); - }); - - return *this; -} - -//============================================================================== -ObstacleManager& ObstacleManager::wait() -{ - std::mutex temp; - std::condition_variable cv; - std::unique_lock lock(temp); - cv.wait( - lock, [&]() { return !rclcpp::ok(); }); - - return *this; -} - -//============================================================================== -void ObstacleManager::process(const Obstacles& detections) -{ - // Simple implementation for now - auto msg = std::make_unique(detections); - _pimpl->obs_pub->publish(std::move(msg)); - RCLCPP_DEBUG( - _pimpl->node->get_logger(), - "Published detection results with %ld obstacles.", - detections.obstacles.size() - ); - if (_pimpl->responder != nullptr) - { - _pimpl->responder->respond(detections); - RCLCPP_DEBUG( - _pimpl->node->get_logger(), - "Passed detection results to responder %s", - _pimpl->responder->name().c_str() - ); - } -} - -} // namespace rmf_obstacle_ros2 From 34c9c9ed89f0fc685923e8ebf91f203fef2b1735 Mon Sep 17 00:00:00 2001 From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Date: Wed, 3 Aug 2022 04:36:39 -0700 Subject: [PATCH 47/60] 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> --- .../src/lane_blocker/IntersectionChecker.cpp | 56 ++++++++------ .../test/test_IntersectionChecker.cpp | 76 +++++++++++++++++-- .../rmf_obstacle_ros2/obstacles/Convert.cpp | 4 +- 3 files changed, 106 insertions(+), 30 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp index cbc1d93c6..470c7fd69 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.cpp @@ -40,35 +40,30 @@ CollisionBox make_reference_box(const CollisionGeometry& o) return box; } -std::pair> make_transformed_box( +std::pair> make_transformed_box( const CollisionGeometry& to, const CollisionGeometry& from) { - // We will rotate second geometry about the origin by the difference in - // thetas and then translate to the first geometry's frame - Eigen::Matrix mat; - const double th = to.center.theta - from.center.theta; - mat(0, 0) = std::cos(th); - mat(0, 1) = std::sin(-1.0 *th); - mat(0, 2) = (from.center.x - to.center.x); - mat(1, 0) = std::sin(th); - mat(1, 1) = std::cos(th); - mat(1, 2) = (from.center.y - to.center.y); - mat(2, 0) = 0; - mat(2, 1) = 0; - mat(2, 2) = 1.0; + // 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 << "get_vertices matrix: " << std::endl; - std::cout << mat << std::endl; + std::cout << "Obs rot matrix: " << std::endl; + std::cout << rot_from.matrix() << std::endl; #endif - std::vector vertices; - vertices.push_back({-from.size_x * 0.5, from.size_y * 0.5, 1.0}); - vertices.push_back({-from.size_x * 0.5, -from.size_y * 0.5, 1.0}); - vertices.push_back({from.size_x* 0.5, from.size_y* 0.5, 1.0}); - vertices.push_back({from.size_x* 0.5, -from.size_y* 0.5, 1.0}); - #ifndef NDEBUG std::cout << "Obs vertices before trans: "; for (const auto& v : vertices) @@ -76,8 +71,21 @@ std::pair> make_transformed_box( 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 = mat * v; + { + 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: "; @@ -154,7 +162,7 @@ bool between( how_much = std::numeric_limits::min(); for (std::size_t i = 0; i < 2; ++i) { - double dist; + double dist = std::numeric_limits::min(); // O2 projections are on the left of O1 extremas if (o2_box.max[i] < o1_box.min[i]) { diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 6fd760808..94cc37cd2 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -39,6 +39,7 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are not intersecting and 1m apart") { double how_much; + const double expected = 1.0; const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) @@ -58,13 +59,15 @@ SCENARIO("Test IntersectionChecker") 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 - 1.0 == Approx(0.0).margin(1e-3)); + 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 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(1.0) @@ -84,8 +87,9 @@ SCENARIO("Test IntersectionChecker") 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 - 0.586) == Approx(0.0).margin(1e-3)); + CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); } WHEN("AABB geometries are overlapping along X-Axis") @@ -211,6 +215,7 @@ SCENARIO("Test IntersectionChecker") WHEN("Test #1") { double how_much; + const double expected = 2.344; const auto ob1 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(8.6824) @@ -230,13 +235,15 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); REQUIRE_FALSE(intersect); - CHECK((how_much - 2.336) == Approx(0.0).margin(1e-1)); + 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 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(11.6892) @@ -256,13 +263,15 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); REQUIRE_FALSE(intersect); - CHECK((how_much - 6.773) == Approx(0.0).margin(1e-1)); + 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 = rmf_obstacle_msgs::build() .center(geometry_msgs::build() .x(9.57985) @@ -282,7 +291,64 @@ SCENARIO("Test IntersectionChecker") const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); REQUIRE_FALSE(intersect); - CHECK((how_much - 4.314) == Approx(0.0).margin(1e-1)); + 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 = rmf_obstacle_msgs::build() + .center(geometry_msgs::build() + .x(-3.7801) + .y(-2.48618) + .theta(-2.95867)) + .size_x(4.76905) + .size_y(0.734582); + + const auto ob2 = rmf_obstacle_msgs::build() + .center(geometry_msgs::build() + .x(6.81831) + .y(-1.99772) + .theta(0.990476)) + .size_x(0.6) + .size_y(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 = rmf_obstacle_msgs::build() + .center(geometry_msgs::build() + .x(-9.12337) + .y(2.63674) + .theta(7.05773)) + .size_x(4.92557) + .size_y(1.66422); + + const auto ob2 = rmf_obstacle_msgs::build() + .center(geometry_msgs::build() + .x(8.87474) + .y(-5.78416) + .theta(-1.90750)) + .size_x(0.7) + .size_y(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)); } } \ No newline at end of file diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp index 897ea2168..13f80c1d9 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp @@ -80,7 +80,9 @@ PointCloud convert( sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); // 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) + 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(); From 4212cfcab99653f0d6ee35e814cf2ccc3fcade0b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 26 Aug 2022 09:30:01 +0530 Subject: [PATCH 48/60] Use local bb2d Signed-off-by: Yadunund --- .../src/lane_blocker/IntersectionChecker.hpp | 21 +- .../test/test_IntersectionChecker.cpp | 266 ++++++++++-------- 2 files changed, 164 insertions(+), 123 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp index e331a6c3b..9bbb698cd 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/IntersectionChecker.hpp @@ -20,13 +20,30 @@ #include -#include +#include //============================================================================== // TODO(YV): Consider making this a loadable plugin via pluginlib namespace IntersectionChecker { -using CollisionGeometry = rmf_obstacle_msgs::msg::BoundingBox2D; +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 diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index 94cc37cd2..b9085bb7c 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -20,7 +20,6 @@ #include -#include #include #include @@ -40,21 +39,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 1.0; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(4.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -68,21 +69,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 0.586; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(4.0) .y(0.0) - .theta(radians(45.0))) - .size_x(2.0) - .size_y(2.0); + .theta(radians(45.0)), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -95,21 +98,23 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along X-Axis") { double how_much; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(2.5) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -119,21 +124,23 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along Y-Axis") { double how_much; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(1.5) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -143,21 +150,23 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are overlapping along X & Y-Axis") { double how_much; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(2.5) .y(0.5) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -167,21 +176,23 @@ SCENARIO("Test IntersectionChecker") WHEN("AABB geometries are touching") { double how_much; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(1.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(3.0) .y(0.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -191,21 +202,24 @@ SCENARIO("Test IntersectionChecker") WHEN("OBB geometries are overlapping along X & Y-Axis") { double how_much; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(0.0) .y(0.0) - .theta(radians(45.0))) - .size_x(2.0) - .size_y(2.0); + .theta(radians(45.0)), + 2.0, + 2.0 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(1.414) .y(1.0) - .theta(0.0)) - .size_x(2.0) - .size_y(2.0); + .theta(0.0), + 2.0, + 2.0 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -216,21 +230,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 2.344; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(8.6824) .y(-10.9616) - .theta(0.255513)) - .size_x(1.43478) - .size_y(0.5); + .theta(0.25553), + 1.43478, + 0.5); + - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(12.002) .y(-10.1094) - .theta(0.0)) - .size_x(0.6) - .size_y(0.6); + .theta(0.0), + 0.6, + 0.6 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -244,21 +260,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 6.593; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(11.6892) .y(-3.52843) - .theta(0.293981)) - .size_x(3.01193) - .size_y(0.5); + .theta(0.29391), + 3.01193, + 0.5 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(12.002) .y(-10.9738) - .theta(0.0)) - .size_x(0.6) - .size_y(0.6); + .theta(0.0), + 0.6, + 0.6 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -272,21 +290,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 4.292; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(9.57985) .y(-4.6367) - .theta(1.16262)) - .size_x(3.36581) - .size_y(0.5); + .theta(1.1626), + 3.36581, + 0.5 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(12.1453) .y(-11.1404) - .theta(0.0)) - .size_x(0.6) - .size_y(0.6); + .theta(0.0), + 0.6, + 0.6 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -300,21 +320,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 7.702; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(-3.7801) .y(-2.48618) - .theta(-2.95867)) - .size_x(4.76905) - .size_y(0.734582); + .theta(-2.9587), + 4.76905, + 0.734582 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(6.81831) .y(-1.99772) - .theta(0.990476)) - .size_x(0.6) - .size_y(0.6); + .theta(0.99046), + 0.6, + 0.6 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); @@ -328,21 +350,23 @@ SCENARIO("Test IntersectionChecker") { double how_much; const double expected = 17.126; - const auto ob1 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob1 = CollisionGeometry( + geometry_msgs::build() .x(-9.12337) .y(2.63674) - .theta(7.05773)) - .size_x(4.92557) - .size_y(1.66422); + .theta(7.0577), + 4.92557, + 1.66422 + ); - const auto ob2 = rmf_obstacle_msgs::build() - .center(geometry_msgs::build() + const auto ob2 = CollisionGeometry( + geometry_msgs::build() .x(8.87474) .y(-5.78416) - .theta(-1.90750)) - .size_x(0.7) - .size_y(1.1); + .theta(-1.9070), + 0.7, + 1.1 + ); const bool intersect = IntersectionChecker::between( ob1, ob2, how_much); From 58857157e52360ea6f0a3ef193b53b7ebe1bbb09 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 10 Nov 2022 23:15:43 +0800 Subject: [PATCH 49/60] Uncrustify Signed-off-by: Yadunund --- .../test/test_IntersectionChecker.cpp | 144 +++++++++--------- 1 file changed, 72 insertions(+), 72 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index b9085bb7c..e21a22df4 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -41,18 +41,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 1.0; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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), + .x(4.0) + .y(0.0) + .theta(0.0), 2.0, 2.0 ); @@ -71,18 +71,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 0.586; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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)), + .x(4.0) + .y(0.0) + .theta(radians(45.0)), 2.0, 2.0 ); @@ -100,18 +100,18 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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), + .x(2.5) + .y(0.0) + .theta(0.0), 2.0, 2.0 ); @@ -126,18 +126,18 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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), + .x(1.0) + .y(1.5) + .theta(0.0), 2.0, 2.0 ); @@ -152,18 +152,18 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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), + .x(2.5) + .y(0.5) + .theta(0.0), 2.0, 2.0 ); @@ -178,18 +178,18 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(1.0) - .y(0.0) - .theta(0.0), + .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), + .x(3.0) + .y(0.0) + .theta(0.0), 2.0, 2.0 ); @@ -204,9 +204,9 @@ SCENARIO("Test IntersectionChecker") double how_much; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(0.0) - .y(0.0) - .theta(radians(45.0)), + .x(0.0) + .y(0.0) + .theta(radians(45.0)), 2.0, 2.0 ); @@ -214,9 +214,9 @@ SCENARIO("Test IntersectionChecker") const auto ob2 = CollisionGeometry( geometry_msgs::build() - .x(1.414) - .y(1.0) - .theta(0.0), + .x(1.414) + .y(1.0) + .theta(0.0), 2.0, 2.0 ); @@ -232,18 +232,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 2.344; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(8.6824) - .y(-10.9616) - .theta(0.25553), + .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), + .x(12.002) + .y(-10.1094) + .theta(0.0), 0.6, 0.6 ); @@ -262,18 +262,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 6.593; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(11.6892) - .y(-3.52843) - .theta(0.29391), + .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), + .x(12.002) + .y(-10.9738) + .theta(0.0), 0.6, 0.6 ); @@ -292,18 +292,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 4.292; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(9.57985) - .y(-4.6367) - .theta(1.1626), + .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), + .x(12.1453) + .y(-11.1404) + .theta(0.0), 0.6, 0.6 ); @@ -322,18 +322,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 7.702; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(-3.7801) - .y(-2.48618) - .theta(-2.9587), + .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), + .x(6.81831) + .y(-1.99772) + .theta(0.99046), 0.6, 0.6 ); @@ -352,18 +352,18 @@ SCENARIO("Test IntersectionChecker") const double expected = 17.126; const auto ob1 = CollisionGeometry( geometry_msgs::build() - .x(-9.12337) - .y(2.63674) - .theta(7.0577), + .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), + .x(8.87474) + .y(-5.78416) + .theta(-1.9070), 0.7, 1.1 ); From ddf188e02461214d52c501959c4ef54fa81716d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 18 Nov 2022 13:10:09 +0100 Subject: [PATCH 50/60] Added feeback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rmf_obstacle_ros2/CMakeLists.txt | 8 +++----- .../include/rmf_obstacle_ros2/StandardNames.hpp | 2 +- .../include/rmf_obstacle_ros2/obstacles/Convert.hpp | 2 -- rmf_obstacle_ros2/package.xml | 7 ++++--- rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp | 2 -- rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt | 9 ++------- .../src/lane_blocker/test/test_IntersectionChecker.cpp | 3 +-- 7 files changed, 11 insertions(+), 22 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 3f041c633..72e00a1ee 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -10,21 +10,20 @@ include(GNUInstallDirs) find_package(ament_cmake REQUIRED) set(dep_pkgs + geometry_msgs rclcpp rclcpp_components rmf_utils - pluginlib rmf_obstacle_msgs rmf_fleet_msgs rmf_building_map_msgs sensor_msgs OCTOMAP rmf_fleet_adapter - tf2_ros - geometry_msgs - tf2_geometry_msgs rmf_traffic rmf_traffic_ros2 + tf2_ros + tf2_geometry_msgs ) foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) @@ -133,7 +132,6 @@ if(BUILD_TESTING) ) add_subdirectory(src/lane_blocker/test) - endif() #=============================================================================== diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp index 3219c0aa2..3e9e0d328 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/StandardNames.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp index 9d84de27f..9ea038aa9 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp @@ -39,8 +39,6 @@ void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle); /// Convert an RMF Obstacle msg into a PointCloud2 msg PointCloud convert( const Obstacle& msg); - - } // namespace rmf_obstacle_ros2 #endif // #indef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP diff --git a/rmf_obstacle_ros2/package.xml b/rmf_obstacle_ros2/package.xml index 38f7ecb9b..99d31e8b6 100644 --- a/rmf_obstacle_ros2/package.xml +++ b/rmf_obstacle_ros2/package.xml @@ -3,25 +3,26 @@ rmf_obstacle_ros2 2.1.0 - A package contiainings utilities for managing obstacles in RMF + 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 - rmf_fleet_adapter tf2_ros tf2_geometry_msgs - geometry_msgs ament_cmake_catch2 ament_cmake_uncrustify diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index e44bef5f7..4a6ee41dc 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -504,8 +504,6 @@ void LaneBlocker::process() lanes_with_changes.insert(lane_id); } } - - request_lane_modifications(std::move(lanes_with_changes)); } diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt index f011f3094..7d902c009 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt +++ b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt @@ -1,14 +1,9 @@ - file(GLOB_RECURSE unit_test_srcs "*.cpp" "../IntersectionChecker.cpp") -# add_executable(test_intersection_checker ${unit_test_srcs}) ament_add_catch2( test_intersection_checker ${unit_test_srcs} - TIMEOUT 300) -# target_link_libraries(test_intersection_checker -# rmf_traffic -# ${FCL_LIBRARIES} -# Threads::Threads + TIMEOUT 300 +) target_link_libraries(test_intersection_checker PRIVATE diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp index e21a22df4..dad051dff 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/test/test_IntersectionChecker.cpp @@ -28,7 +28,6 @@ using CollisionGeometry = IntersectionChecker::CollisionGeometry; SCENARIO("Test IntersectionChecker") { - auto radians = [](double degree) -> double { @@ -375,4 +374,4 @@ SCENARIO("Test IntersectionChecker") std::cout <<"how_much: " << how_much << std::endl; CHECK((how_much - expected) == Approx(0.0).margin(1e-3)); } -} \ No newline at end of file +} From 49b5e6eb50ddf92d182673f57fb122c83ad9f398 Mon Sep 17 00:00:00 2001 From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Date: Tue, 22 Nov 2022 02:33:07 -0800 Subject: [PATCH 51/60] 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 --- rmf_fleet_adapter_python/src/adapter.cpp | 15 + .../src/lane_blocker/LaneBlocker.cpp | 394 ++++++++++++++---- .../src/lane_blocker/LaneBlocker.hpp | 59 ++- 3 files changed, 388 insertions(+), 80 deletions(-) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 80968a7cc..dbb584b8f 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -8,6 +8,7 @@ #include "rmf_traffic_ros2/Time.hpp" #include "rmf_fleet_adapter/agv/Adapter.hpp" +#include "rmf_fleet_adapter/agv/FleetUpdateHandle.hpp" #include "rmf_fleet_adapter/agv/test/MockAdapter.hpp" #include "rmf_fleet_adapter_python/PyRobotCommandHandle.hpp" #include @@ -322,6 +323,12 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("open_lanes", &agv::FleetUpdateHandle::open_lanes, py::arg("lane_indices")) + .def("limit_lane_speeds", + &agv::FleetUpdateHandle::limit_lane_speeds, + py::arg("requests")) + .def("remove_speed_limits", + &agv::FleetUpdateHandle::remove_speed_limits, + py::arg("requests")) .def("set_task_planner_params", [&](agv::FleetUpdateHandle& self, battery::BatterySystem& b_sys, @@ -504,6 +511,14 @@ PYBIND11_MODULE(rmf_adapter, m) { return self.errors(); }); + // SPEED LIMIT REQUEST =============================================== + py::class_( + m_fleet_update_handle, "SpeedLimitRequest") + .def(py::init(), + py::arg("lane_index"), + py::arg("speed_limit")); + // EASY TRAFFIC LIGHT HANDLE =============================================== py::class_(m, "Waypoint") .def(py::initdeclare_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( @@ -157,6 +164,20 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) "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)); @@ -218,6 +239,19 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) ); } _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); + // TODO(YV): This initializes all lane states to Normal which may not + // be always the case. Instead of always adding a Normal state, we + // should check the lane is speed limited or closed and then set the + // state accordingly. Eg to check if the lane is speed limited, + // check graph.get_lane(i).speed_limit().has_value(). + if (_internal_lane_states.find(lane_key) == _internal_lane_states.end()) + { + _internal_lane_states.insert({lane_key, LaneState::Normal}); + } + } }, ipc_sub_options); @@ -394,10 +428,11 @@ void LaneBlocker::process() } } } - else + + if (obs_lane_it == _obstacle_to_lanes_map.end() || _continuous_checker) { - // New obstacle. It needs to be assigned a lane if within the vicinity of - // one + // 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 " @@ -479,9 +514,24 @@ void LaneBlocker::process() // Update caches for (const auto& lane_key : vicinity_lane_keys) { - _obstacle_to_lanes_map[obstacle_key].insert(lane_key); - _lane_to_obstacles_map[lane_key].insert(obstacle_key); - lanes_with_changes.insert(lane_key); + // 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); + } + } } } } @@ -514,8 +564,11 @@ void LaneBlocker::request_lane_modifications( if (changes.empty()) return; - // A map to collate lanes per fleet that need to be closed - std::unordered_map> closure_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; // For now we implement a simple heuristic to decide whether to close a lane // or not. for (const auto& lane_key : changes) @@ -529,47 +582,213 @@ void LaneBlocker::request_lane_modifications( ); continue; } - auto [fleet_name, lane_id] = deserialize_key(lane_key); const auto& obstacles = _lane_to_obstacles_map.at(lane_key); - if (obstacles.size() >= _lane_closure_threshold) + const auto& lane_state = _internal_lane_states.at(lane_key); + if (obstacles.size() >= _lane_closure_threshold && + lane_state == LaneState::Normal) { - auto msg_it = closure_msgs.insert({fleet_name, nullptr}); - if (msg_it.second) - { - LaneRequest request; - request.fleet_name = std::move(fleet_name); - request.close_lanes.push_back(std::move(lane_id)); - msg_it.first->second = std::make_unique( - std::move(request) - ); - } - else - { - // Msg was created before. We simply append the new lane id - msg_it.first->second->close_lanes.push_back(std::move(lane_id)); - } - _currently_closed_lanes.insert(lane_key); + transition_lane_state(lane_state, LaneState::Closed, lane_key, + lane_req_msgs, speed_limit_req_msgs); + } + else if (obstacles.size() >= _speed_limit_threshold && + lane_state == LaneState::Normal) + { + transition_lane_state(lane_state, LaneState::SpeedLimited, lane_key, + lane_req_msgs, speed_limit_req_msgs); + } + else if (obstacles.size() >= _lane_closure_threshold && + lane_state == LaneState::SpeedLimited) + { + transition_lane_state(lane_state, LaneState::Closed, lane_key, + lane_req_msgs, speed_limit_req_msgs); } else { RCLCPP_INFO( this->get_logger(), - "Lane %s has %ld obstacles in its vicinity but will not be closed as " - "the threshold is %ld", - lane_key.c_str(), obstacles.size(), _lane_closure_threshold + "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 closures - for (auto& [_, msg] : closure_msgs) + publish_lane_req_msgs(std::move(lane_req_msgs)); + publish_speed_limit_req_msgs(std::move(speed_limit_req_msgs)); +} + +//============================================================================== +void LaneBlocker::transition_lane_state( + const LaneState& old_state, + const LaneState& new_state, + const std::string& lane_key, + std::unordered_map>& lane_req_msgs, + std::unordered_map>& speed_limit_req_msgs) +{ + if (new_state == old_state) + { + return; + } + + if (old_state == LaneState::Normal && new_state == LaneState::Closed) + { + add_lane_close_req(lane_key, lane_req_msgs); + } + else if (old_state == LaneState::Closed && new_state == LaneState::Normal) + { + add_lane_open_req(lane_key, lane_req_msgs); + } + else if (old_state == LaneState::Normal && + new_state == LaneState::SpeedLimited) + { + add_speed_limit_req(lane_key, speed_limit_req_msgs); + } + else if (old_state == LaneState::SpeedLimited && + new_state == LaneState::Normal) + { + add_speed_unlimit_req(lane_key, speed_limit_req_msgs); + } + else if (old_state == LaneState::SpeedLimited && + new_state == LaneState::Closed) + { + add_lane_close_req(lane_key, lane_req_msgs); + add_speed_unlimit_req(lane_key, speed_limit_req_msgs); + } + else if (old_state == LaneState::Closed && + new_state == LaneState::SpeedLimited) + { + add_lane_open_req(lane_key, lane_req_msgs); + add_speed_limit_req(lane_key, speed_limit_req_msgs); + } + + // 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_close_req( + const std::string& lane_key, + std::unordered_map>& lane_req_msgs) +{ + auto [fleet_name, lane_id] = deserialize_key(lane_key); + // 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); + request.close_lanes.push_back(std::move(lane_id)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the lane id + msg_it.first->second->close_lanes.push_back(std::move(lane_id)); + } +} + +//============================================================================== +void LaneBlocker::add_lane_open_req( + const std::string& lane_key, + std::unordered_map>& lane_req_msgs) +{ + auto [fleet_name, lane_id] = deserialize_key(lane_key); + // construct Lane Open msg + auto msg_it = lane_req_msgs.insert({fleet_name, nullptr}); + if (msg_it.second) + { + LaneRequest request; + request.fleet_name = std::move(fleet_name); + request.open_lanes.push_back(std::move(lane_id)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the lane id + msg_it.first->second->open_lanes.push_back(std::move(lane_id)); + } +} + +//============================================================================== +void LaneBlocker::add_speed_limit_req( + const std::string& lane_key, + std::unordered_map>& speed_limit_req_msgs) +{ + auto [fleet_name, lane_id] = deserialize_key(lane_key); + // 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); + request.speed_limits.push_back(std::move(speed_limited_lane)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // 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)); + } +} + +//============================================================================== +void LaneBlocker::add_speed_unlimit_req( + const std::string& lane_key, + std::unordered_map>& speed_limit_req_msgs) +{ + auto [fleet_name, lane_id] = deserialize_key(lane_key); + // construct Speed Unlimit msg + auto msg_it = speed_limit_req_msgs.insert({fleet_name, nullptr}); + if (msg_it.second) + { + SpeedLimitRequest request; + request.fleet_name = std::move(fleet_name); + request.remove_limits.push_back(std::move(lane_id)); + msg_it.first->second = std::make_unique( + std::move(request) + ); + } + else + { + // Msg was created before. We simply append the lane id + msg_it.first->second->remove_limits.push_back(std::move(lane_id)); + } +} + +//============================================================================== +void LaneBlocker::publish_lane_req_msgs( + std::unordered_map> lane_req_msgs) +{ + for (auto& [_, msg] : lane_req_msgs) { - if (msg->close_lanes.empty()) + if (msg->close_lanes.empty() && msg->open_lanes.empty()) { RCLCPP_DEBUG( this->get_logger(), - "None of the lanes for fleet %s need to be closed", + "None of the lanes for fleet %s need to be opened or closed", msg->fleet_name.c_str() ); continue; @@ -579,10 +798,45 @@ void LaneBlocker::request_lane_modifications( "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( + std::unordered_map> 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)); + } +} + //============================================================================== auto LaneBlocker::deserialize_key( const std::string& key) const-> std::pair @@ -635,7 +889,6 @@ void LaneBlocker::purge_obstacles( } } - //============================================================================== void LaneBlocker::cull() { @@ -671,58 +924,43 @@ void LaneBlocker::cull() // Cull purge_obstacles(obstacles_to_cull); - // Open lanes if needed - // A map to collate lanes per fleet that need to be closed - std::unordered_set opened_lanes = {}; - std::unordered_map> open_msgs; - for (const auto& lane : _currently_closed_lanes) + // 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; + + for (const auto& [lane_key, lane_state] : _internal_lane_states) { - if (_lane_to_obstacles_map.at(lane).size() < _lane_closure_threshold) + if (lane_state == LaneState::Normal) { - // The lane can be opened - auto [fleet_name, lane_id] = deserialize_key(lane); - auto msg_it = open_msgs.insert({fleet_name, nullptr}); - if (msg_it.second) - { - LaneRequest request; - request.fleet_name = std::move(fleet_name); - request.open_lanes.push_back(std::move(lane_id)); - msg_it.first->second = std::make_unique( - std::move(request) - ); - } - else - { - // Msg was created before. We simply append the new lane id - msg_it.first->second->open_lanes.push_back(std::move(lane_id)); - } - opened_lanes.insert(lane); + // Normal lane states are handled in request_lane_modifications() + continue; } - } - // Publish lane closures - for (auto& [_, msg] : open_msgs) - { - if (msg->open_lanes.empty()) + const auto& obstacles = _lane_to_obstacles_map.at(lane_key); + if (obstacles.size() < _speed_limit_threshold && + lane_state == LaneState::Closed) { - RCLCPP_DEBUG( - this->get_logger(), - "None of the lanes for fleet %s need to be opened", - msg->fleet_name.c_str() - ); - continue; + transition_lane_state(lane_state, LaneState::Normal, lane_key, + lane_req_msgs, speed_limit_req_msgs); + } + else if (obstacles.size() < _lane_closure_threshold && + lane_state == LaneState::Closed) + { + transition_lane_state(lane_state, LaneState::SpeedLimited, lane_key, + lane_req_msgs, speed_limit_req_msgs); + } + else if (obstacles.size() < _speed_limit_threshold && + lane_state == LaneState::SpeedLimited) + { + transition_lane_state(lane_state, LaneState::Normal, lane_key, + lane_req_msgs, speed_limit_req_msgs); } - 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)); } - for (const auto& lane : opened_lanes) - _currently_closed_lanes.erase(lane); - + publish_lane_req_msgs(std::move(lane_req_msgs)); + publish_speed_limit_req_msgs(std::move(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 index 2d522df77..190a8d969 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,7 @@ class LaneBlocker : public rclcpp::Node 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; @@ -130,6 +132,58 @@ class LaneBlocker : public rclcpp::Node void request_lane_modifications( const std::unordered_set& changes); + enum class LaneState : uint8_t + { + Normal = 0, + Closed, + SpeedLimited + }; + + std::unordered_map< + std::string, + LaneState> _internal_lane_states = {}; + + // TODO(YV): Use member variables instead of passing unordered_maps by + // reference between these functions. Especially to the publsiher functions. + void transition_lane_state( + const LaneState& old_state, + const LaneState& new_state, + const std::string& lane_key, + std::unordered_map>& lane_req_msgs, + std::unordered_map>& speed_limit_req_msgs); + + // TODO(YV): Combine close/open and limit/unlimit functions into two functions + // each by passing in a target LaneState. + void add_lane_close_req( + const std::string& lane_key, + std::unordered_map>& lane_req_msgs); + + void add_lane_open_req( + const std::string& lane_key, + std::unordered_map>& lane_req_msgs); + + void add_speed_limit_req( + const std::string& lane_key, + std::unordered_map>& speed_limit_req_msgs); + + void add_speed_unlimit_req( + const std::string& lane_key, + std::unordered_map>& speed_limit_req_msgs); + + void publish_lane_req_msgs( + std::unordered_map> lane_req_msgs); + + void publish_speed_limit_req_msgs( + std::unordered_map> speed_limit_req_msgs); + void purge_obstacles( const std::unordered_set& obstacle_keys, const bool erase_from_buffer = true); @@ -156,8 +210,6 @@ class LaneBlocker : public rclcpp::Node std::unordered_set> _lane_to_obstacles_map = {}; - std::unordered_set _currently_closed_lanes; - rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; rclcpp::Subscription::SharedPtr _lane_states_sub; @@ -175,7 +227,10 @@ class LaneBlocker : public rclcpp::Node 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; From 5f1305aa8cec5964c3d77a48fd400a611e36aaa4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 24 Nov 2022 14:58:00 +0100 Subject: [PATCH 52/60] Filled PointCloud message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../rmf_obstacle_ros2/obstacles/Convert.cpp | 35 +++++++++++++++++-- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp index 13f80c1d9..549bc2e90 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp @@ -79,6 +79,7 @@ PointCloud convert( 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; @@ -91,12 +92,40 @@ PointCloud convert( ++iter_x; ++iter_y; ++iter_z; + ++number_of_points; } - // TODO(YV): Fill other fields cloud.is_bigendian = false; - // cloud.point_step = 6; - // cloud.row_step = 6; + // 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; } From 9bc977062fe6f42627217ea370ed2555dbecc430 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 24 Nov 2022 15:25:48 +0100 Subject: [PATCH 53/60] Insert lanes with their state MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/lane_blocker/LaneBlocker.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index dcfb23abd..a82ae36b0 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include @@ -242,14 +242,16 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) 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); - // TODO(YV): This initializes all lane states to Normal which may not - // be always the case. Instead of always adding a Normal state, we - // should check the lane is speed limited or closed and then set the - // state accordingly. Eg to check if the lane is speed limited, - // check graph.get_lane(i).speed_limit().has_value(). if (_internal_lane_states.find(lane_key) == _internal_lane_states.end()) { - _internal_lane_states.insert({lane_key, LaneState::Normal}); + if(!traffic_graph->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}); + } } } }, From 06f3da17b212528b1282cde8159da6924deb3f51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 25 Nov 2022 01:24:03 +0100 Subject: [PATCH 54/60] Added MessageFilter to rmf_obstacles topic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/lane_blocker/LaneBlocker.cpp | 55 ++++++++----------- .../src/lane_blocker/LaneBlocker.hpp | 9 ++- 2 files changed, 31 insertions(+), 33 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index a82ae36b0..7bbb8aa80 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -28,6 +28,9 @@ #include +#include +#include + #include #include @@ -206,13 +209,20 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) rmf_fleet_adapter::SpeedLimitRequestTopicName, rclcpp::SystemDefaultsQoS()); - _obstacle_sub = this->create_subscription( - "rmf_obstacles", - rclcpp::QoS(10).best_effort(), - [=](Obstacles::ConstSharedPtr msg) - { - obstacle_cb(*msg); - }); + 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. @@ -244,7 +254,8 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) const std::string lane_key = get_lane_key(msg->name, i); if (_internal_lane_states.find(lane_key) == _internal_lane_states.end()) { - if(!traffic_graph->get_lane(i).properties().speed_limit().has_value()) + if(!_traffic_graphs[msg->name].get_lane(i). + properties().speed_limit().has_value()) { _internal_lane_states.insert({lane_key, LaneState::Normal}); } @@ -275,37 +286,19 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) } //============================================================================== -void LaneBlocker::obstacle_cb(const Obstacles& msg) +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) + if (msg->obstacles.empty() || _transform_listener == nullptr) return; - // TODO(YV): Consider using tf2_ros::MessageFilter instead of this callback - for (const auto& obstacle : msg.obstacles) + for (const auto& obstacle : msg->obstacles) { const auto& obstacle_frame = obstacle.header.frame_id; - std::string tf2_error; - const bool can_transform = _tf2_buffer->canTransform( - _rmf_frame, - obstacle_frame, - tf2::TimePointZero, - tf2::durationFromSec(_tf2_lookup_duration), - &tf2_error); - - // TODO(YV): Cache these transforms since most of them would be static - if (!can_transform) - { - RCLCPP_WARN( - this->get_logger(), - "Unable to lookup transform between between obstacle frame %s and RMF " - "frame %s.", obstacle_frame.c_str(), _rmf_frame.c_str() - ); - continue; - } - const auto& transform = _tf2_buffer->lookupTransform( _rmf_frame, obstacle_frame, diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 190a8d969..3ad2c0908 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -32,7 +32,11 @@ #include #include +#include #include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" #include #include @@ -59,7 +63,7 @@ class LaneBlocker : public rclcpp::Node const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: - void obstacle_cb(const Obstacles& msg); + void obstacle_cb(const message_filters::MessageEvent& evt); void process(); void cull(); @@ -210,7 +214,6 @@ class LaneBlocker : public rclcpp::Node std::unordered_set> _lane_to_obstacles_map = {}; - rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; rclcpp::Subscription::SharedPtr _lane_states_sub; rclcpp::Publisher::SharedPtr _lane_closure_pub; @@ -218,6 +221,8 @@ class LaneBlocker : public rclcpp::Node 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; From eaf094bacffa71b8f65a1424c698fe23f2edae5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 25 Nov 2022 10:10:06 +0100 Subject: [PATCH 55/60] Added optional return value to deserialize_key method MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/lane_blocker/LaneBlocker.cpp | 60 +++++++++++++++---- .../src/lane_blocker/LaneBlocker.hpp | 2 +- 2 files changed, 48 insertions(+), 14 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 7bbb8aa80..a9ca1f708 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -377,7 +377,11 @@ void LaneBlocker::process() // Check if obstacle is still in the vicinity of these lanes. for (const auto& lane_key : lanes_keys) { - const auto [fleet_name, lane_id] = deserialize_key(lane_key); + 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( @@ -673,7 +677,15 @@ void LaneBlocker::add_lane_close_req( std::unordered_map>& lane_req_msgs) { - auto [fleet_name, lane_id] = deserialize_key(lane_key); + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_lane_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) @@ -698,7 +710,15 @@ void LaneBlocker::add_lane_open_req( std::unordered_map>& lane_req_msgs) { - auto [fleet_name, lane_id] = deserialize_key(lane_key); + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_lane_open_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 Open msg auto msg_it = lane_req_msgs.insert({fleet_name, nullptr}); if (msg_it.second) @@ -723,7 +743,15 @@ void LaneBlocker::add_speed_limit_req( std::unordered_map>& speed_limit_req_msgs) { - auto [fleet_name, lane_id] = deserialize_key(lane_key); + 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}); @@ -754,7 +782,15 @@ void LaneBlocker::add_speed_unlimit_req( std::unordered_map>& speed_limit_req_msgs) { - auto [fleet_name, lane_id] = deserialize_key(lane_key); + auto deserialize_key_value = deserialize_key(lane_key); + if (!deserialize_key_value.has_value()) + { + RCLCPP_ERROR(this->get_logger(), + "[LaneBlocker::add_speed_unlimit_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 Unlimit msg auto msg_it = speed_limit_req_msgs.insert({fleet_name, nullptr}); if (msg_it.second) @@ -833,12 +869,10 @@ void LaneBlocker::publish_speed_limit_req_msgs( } //============================================================================== -auto LaneBlocker::deserialize_key( - const std::string& key) const-> std::pair +std::optional> LaneBlocker::deserialize_key( + const std::string& key) const { const std::string delimiter = "_"; - // This should not throw any errors if keys are constructed using get_key() - // TODO(YV): Consider returning an optional instead try { std::string name = key.substr(0, key.find(delimiter)); @@ -850,11 +884,11 @@ auto LaneBlocker::deserialize_key( } catch (const std::exception& e) { - // *INDENT-OFF* - throw std::runtime_error( + RCLCPP_INFO( + this->get_logger(), "[LaneBlocker::deserialize_key] Unable to parse key. This is a bug and " - "should be reported. Detailed error: " + std::string(e.what())); - // *INDENT-ON* + "should be reported. Detailed error: %s", e.what()); + return std::nullopt; } } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 3ad2c0908..ef14502f2 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -129,7 +129,7 @@ class LaneBlocker : public rclcpp::Node } }; - std::pair + std::optional> deserialize_key(const std::string& key) const; // Modify lanes with changes in number of vicinity obstacles From 02d4b0c042052b0973416d4e882597d7236e3161 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 25 Nov 2022 13:43:46 +0100 Subject: [PATCH 56/60] Combine close/open and limit/unlimit functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/lane_blocker/LaneBlocker.cpp | 158 +++++++++--------- .../src/lane_blocker/LaneBlocker.hpp | 23 +-- 2 files changed, 82 insertions(+), 99 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index a9ca1f708..f7955319e 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -634,33 +634,37 @@ void LaneBlocker::transition_lane_state( if (old_state == LaneState::Normal && new_state == LaneState::Closed) { - add_lane_close_req(lane_key, lane_req_msgs); + add_lane_open_close_req(lane_key, lane_req_msgs, LaneState::Closed); } else if (old_state == LaneState::Closed && new_state == LaneState::Normal) { - add_lane_open_req(lane_key, lane_req_msgs); + add_lane_open_close_req(lane_key, lane_req_msgs, LaneState::Normal); } else if (old_state == LaneState::Normal && new_state == LaneState::SpeedLimited) { - add_speed_limit_req(lane_key, speed_limit_req_msgs); + add_speed_limit_req( + lane_key, speed_limit_req_msgs, LaneState::SpeedLimited); } else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Normal) { - add_speed_unlimit_req(lane_key, speed_limit_req_msgs); + add_speed_limit_req( + lane_key, speed_limit_req_msgs, LaneState::SpeedUnlimited); } else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Closed) { - add_lane_close_req(lane_key, lane_req_msgs); - add_speed_unlimit_req(lane_key, speed_limit_req_msgs); + add_lane_open_close_req(lane_key, lane_req_msgs, new_state); + add_speed_limit_req( + lane_key, speed_limit_req_msgs, LaneState::SpeedUnlimited); } else if (old_state == LaneState::Closed && new_state == LaneState::SpeedLimited) { - add_lane_open_req(lane_key, lane_req_msgs); - add_speed_limit_req(lane_key, speed_limit_req_msgs); + add_lane_open_close_req(lane_key, lane_req_msgs, LaneState::Normal); + add_speed_limit_req( + lane_key, speed_limit_req_msgs, LaneState::SpeedLimited); } // update lane state @@ -672,16 +676,17 @@ void LaneBlocker::transition_lane_state( } //============================================================================== -void LaneBlocker::add_lane_close_req( +void LaneBlocker::add_lane_open_close_req( const std::string& lane_key, std::unordered_map>& lane_req_msgs) + std::unique_ptr>& lane_req_msgs, + 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_close_req: Failure deserializing key"); + "[LaneBlocker::add_lane_open_close_req: Failure deserializing key"); return; } const auto fleet_name = deserialize_key_value.value().first; @@ -692,40 +697,20 @@ void LaneBlocker::add_lane_close_req( { LaneRequest request; request.fleet_name = std::move(fleet_name); - request.close_lanes.push_back(std::move(lane_id)); - msg_it.first->second = std::make_unique( - std::move(request) - ); - } - else - { - // Msg was created before. We simply append the lane id - msg_it.first->second->close_lanes.push_back(std::move(lane_id)); - } -} - -//============================================================================== -void LaneBlocker::add_lane_open_req( - const std::string& lane_key, - std::unordered_map>& lane_req_msgs) -{ - auto deserialize_key_value = deserialize_key(lane_key); - if (!deserialize_key_value.has_value()) - { - RCLCPP_ERROR(this->get_logger(), - "[LaneBlocker::add_lane_open_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 Open msg - auto msg_it = lane_req_msgs.insert({fleet_name, nullptr}); - if (msg_it.second) - { - LaneRequest request; - request.fleet_name = std::move(fleet_name); - request.open_lanes.push_back(std::move(lane_id)); + 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) ); @@ -733,7 +718,19 @@ void LaneBlocker::add_lane_open_req( else { // Msg was created before. We simply append the lane id - msg_it.first->second->open_lanes.push_back(std::move(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"); + } } } @@ -741,7 +738,8 @@ void LaneBlocker::add_lane_open_req( void LaneBlocker::add_speed_limit_req( const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs) + std::unique_ptr>& speed_limit_req_msgs, + const LaneState& desired_state) { auto deserialize_key_value = deserialize_key(lane_key); if (!deserialize_key_value.has_value()) @@ -764,48 +762,42 @@ void LaneBlocker::add_speed_limit_req( { SpeedLimitRequest request; request.fleet_name = std::move(fleet_name); - request.speed_limits.push_back(std::move(speed_limited_lane)); - msg_it.first->second = std::make_unique( - std::move(request) - ); - } - else - { - // 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)); - } -} - -//============================================================================== -void LaneBlocker::add_speed_unlimit_req( - const std::string& lane_key, - std::unordered_map>& speed_limit_req_msgs) -{ - auto deserialize_key_value = deserialize_key(lane_key); - if (!deserialize_key_value.has_value()) - { - RCLCPP_ERROR(this->get_logger(), - "[LaneBlocker::add_speed_unlimit_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 Unlimit msg - auto msg_it = speed_limit_req_msgs.insert({fleet_name, nullptr}); - if (msg_it.second) - { - SpeedLimitRequest request; - request.fleet_name = std::move(fleet_name); - request.remove_limits.push_back(std::move(lane_id)); + 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 { - // Msg was created before. We simply append the lane id - msg_it.first->second->remove_limits.push_back(std::move(lane_id)); + 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; + } } } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index ef14502f2..b836887db 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -140,7 +140,8 @@ class LaneBlocker : public rclcpp::Node { Normal = 0, Closed, - SpeedLimited + SpeedLimited, + SpeedUnlimited }; std::unordered_map< @@ -158,27 +159,17 @@ class LaneBlocker : public rclcpp::Node std::unordered_map>& speed_limit_req_msgs); - // TODO(YV): Combine close/open and limit/unlimit functions into two functions - // each by passing in a target LaneState. - void add_lane_close_req( + void add_lane_open_close_req( const std::string& lane_key, std::unordered_map>& lane_req_msgs); - - void add_lane_open_req( - const std::string& lane_key, - std::unordered_map>& lane_req_msgs); + std::unique_ptr>& lane_req_msgs, + const LaneState& desired_state); void add_speed_limit_req( const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs); - - void add_speed_unlimit_req( - const std::string& lane_key, - std::unordered_map>& speed_limit_req_msgs); + std::unique_ptr>& speed_limit_req_msgs, + const LaneState& desired_state); void publish_lane_req_msgs( std::unordered_map Date: Fri, 25 Nov 2022 14:12:05 +0100 Subject: [PATCH 57/60] Use member variables instead of passing unordered_maps by reference between these functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/lane_blocker/LaneBlocker.cpp | 87 +++++++------------ .../src/lane_blocker/LaneBlocker.hpp | 27 +++--- 2 files changed, 42 insertions(+), 72 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index f7955319e..45deb5e27 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -563,11 +563,10 @@ void LaneBlocker::request_lane_modifications( if (changes.empty()) return; - // 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; + 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) @@ -586,20 +585,17 @@ void LaneBlocker::request_lane_modifications( if (obstacles.size() >= _lane_closure_threshold && lane_state == LaneState::Normal) { - transition_lane_state(lane_state, LaneState::Closed, lane_key, - lane_req_msgs, speed_limit_req_msgs); + 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, - lane_req_msgs, speed_limit_req_msgs); + 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, - lane_req_msgs, speed_limit_req_msgs); + transition_lane_state(lane_state, LaneState::Closed, lane_key); } else { @@ -614,18 +610,15 @@ void LaneBlocker::request_lane_modifications( } } - publish_lane_req_msgs(std::move(lane_req_msgs)); - publish_speed_limit_req_msgs(std::move(speed_limit_req_msgs)); + 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, - std::unordered_map>& lane_req_msgs, - std::unordered_map>& speed_limit_req_msgs) + const std::string& lane_key) { if (new_state == old_state) { @@ -634,37 +627,33 @@ void LaneBlocker::transition_lane_state( if (old_state == LaneState::Normal && new_state == LaneState::Closed) { - add_lane_open_close_req(lane_key, lane_req_msgs, 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, lane_req_msgs, 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, speed_limit_req_msgs, 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, speed_limit_req_msgs, LaneState::SpeedUnlimited); + 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, lane_req_msgs, new_state); - add_speed_limit_req( - lane_key, speed_limit_req_msgs, LaneState::SpeedUnlimited); + 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, lane_req_msgs, LaneState::Normal); - add_speed_limit_req( - lane_key, speed_limit_req_msgs, LaneState::SpeedLimited); + add_lane_open_close_req(lane_key, LaneState::Normal); + add_speed_limit_req(lane_key, LaneState::SpeedLimited); } // update lane state @@ -678,8 +667,6 @@ void LaneBlocker::transition_lane_state( //============================================================================== void LaneBlocker::add_lane_open_close_req( const std::string& lane_key, - std::unordered_map>& lane_req_msgs, const LaneState& desired_state) { auto deserialize_key_value = deserialize_key(lane_key); @@ -692,7 +679,7 @@ void LaneBlocker::add_lane_open_close_req( 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}); + auto msg_it = _lane_req_msgs.insert({fleet_name, nullptr}); if (msg_it.second) { LaneRequest request; @@ -737,8 +724,6 @@ void LaneBlocker::add_lane_open_close_req( //============================================================================== void LaneBlocker::add_speed_limit_req( const std::string& lane_key, - std::unordered_map>& speed_limit_req_msgs, const LaneState& desired_state) { auto deserialize_key_value = deserialize_key(lane_key); @@ -751,7 +736,7 @@ void LaneBlocker::add_speed_limit_req( 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}); + auto msg_it = _speed_limit_req_msgs.insert({fleet_name, nullptr}); SpeedLimitedLane speed_limited_lane = rmf_fleet_msgs::build() @@ -802,10 +787,9 @@ void LaneBlocker::add_speed_limit_req( } //============================================================================== -void LaneBlocker::publish_lane_req_msgs( - std::unordered_map> lane_req_msgs) +void LaneBlocker::publish_lane_req_msgs() { - for (auto& [_, msg] : lane_req_msgs) + for (auto& [_, msg] : _lane_req_msgs) { if (msg->close_lanes.empty() && msg->open_lanes.empty()) { @@ -831,11 +815,9 @@ void LaneBlocker::publish_lane_req_msgs( } //============================================================================== -void LaneBlocker::publish_speed_limit_req_msgs( - std::unordered_map> speed_limit_req_msgs) +void LaneBlocker::publish_speed_limit_req_msgs() { - for (auto& [_, msg] : speed_limit_req_msgs) + for (auto& [_, msg] : _speed_limit_req_msgs) { if (msg->speed_limits.empty() && msg->remove_limits.empty()) { @@ -945,11 +927,9 @@ void LaneBlocker::cull() // Cull purge_obstacles(obstacles_to_cull); - // 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; + 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) { @@ -963,25 +943,22 @@ void LaneBlocker::cull() if (obstacles.size() < _speed_limit_threshold && lane_state == LaneState::Closed) { - transition_lane_state(lane_state, LaneState::Normal, lane_key, - lane_req_msgs, speed_limit_req_msgs); + 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, - lane_req_msgs, speed_limit_req_msgs); + 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, - lane_req_msgs, speed_limit_req_msgs); + transition_lane_state(lane_state, LaneState::Normal, lane_key); } } - publish_lane_req_msgs(std::move(lane_req_msgs)); - publish_speed_limit_req_msgs(std::move(speed_limit_req_msgs)); + 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 index b836887db..55f6e2b3c 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -148,36 +148,22 @@ class LaneBlocker : public rclcpp::Node std::string, LaneState> _internal_lane_states = {}; - // TODO(YV): Use member variables instead of passing unordered_maps by - // reference between these functions. Especially to the publsiher functions. void transition_lane_state( const LaneState& old_state, const LaneState& new_state, - const std::string& lane_key, - std::unordered_map>& lane_req_msgs, - std::unordered_map>& speed_limit_req_msgs); + const std::string& lane_key); void add_lane_open_close_req( const std::string& lane_key, - std::unordered_map>& lane_req_msgs, const LaneState& desired_state); void add_speed_limit_req( const std::string& lane_key, - std::unordered_map>& speed_limit_req_msgs, const LaneState& desired_state); - void publish_lane_req_msgs( - std::unordered_map> lane_req_msgs); + void publish_lane_req_msgs(); - void publish_speed_limit_req_msgs( - std::unordered_map> speed_limit_req_msgs); + void publish_speed_limit_req_msgs(); void purge_obstacles( const std::unordered_set& obstacle_keys, @@ -230,6 +216,13 @@ class LaneBlocker : public rclcpp::Node 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 From 382b3cc1a81d8fb4bce0231cd575db381e0fe368 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 25 Nov 2022 16:27:14 +0100 Subject: [PATCH 58/60] Defined templated pure abstract class to perform serialization/deserialization MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../rmf_obstacle_ros2/obstacles/Convert.hpp | 25 +----------- .../obstacles/ConvertDecl.hpp | 32 +++++++++++++++ .../convert/ConvertPointCloudObstacles.hpp | 40 +++++++++++++++++++ ...ert.cpp => ConvertPointCloudObstacles.cpp} | 2 + 4 files changed, 76 insertions(+), 23 deletions(-) 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 rename rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/{Convert.cpp => ConvertPointCloudObstacles.cpp} (99%) diff --git a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp index 9ea038aa9..bab6c2e97 100644 --- a/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp +++ b/rmf_obstacle_ros2/include/rmf_obstacle_ros2/obstacles/Convert.hpp @@ -18,27 +18,6 @@ #ifndef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP #define RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_HPP -#include -#include +#include -namespace rmf_obstacle_ros2 { - -using PointCloud = sensor_msgs::msg::PointCloud2; -using Obstacle = rmf_obstacle_msgs::msg::Obstacle; - -// TODO(YV): Consider defining a pure abstract class to perform -// serialization/deserialization which these functions can accept -// Perhaps another abstract class to generate Markers for RViZ visualization. -// Also consider templating the sensor msg - -//============================================================================== -/// Serialize a PointCloud2 msg into RMF Obstacle msg -void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle); - -//============================================================================== -/// Convert an RMF Obstacle msg into a PointCloud2 msg -PointCloud convert( - const Obstacle& msg); -} // namespace rmf_obstacle_ros2 - -#endif // #indef RMF_OBSTACLE_ROS2__OBSTACLES__CONVERT_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 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/src/rmf_obstacle_ros2/obstacles/Convert.cpp b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp similarity index 99% rename from rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp rename to rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp index 549bc2e90..2a92c084d 100644 --- a/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/Convert.cpp +++ b/rmf_obstacle_ros2/src/rmf_obstacle_ros2/obstacles/ConvertPointCloudObstacles.cpp @@ -24,6 +24,7 @@ namespace rmf_obstacle_ros2 { //============================================================================== +template<> void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle) { const double resolution = obstacle.data_resolution > 0 ? @@ -58,6 +59,7 @@ void fill_obstacle_data(const PointCloud& msg, Obstacle& obstacle) } //============================================================================== +template<> PointCloud convert( const Obstacle& msg) { From 44801157a8701c1873e1c366ce41f46046af1a57 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 16 Mar 2024 23:42:30 +0800 Subject: [PATCH 59/60] Fix build Signed-off-by: Yadunund --- rmf_obstacle_ros2/CMakeLists.txt | 32 ++++++++++++-------------------- 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/rmf_obstacle_ros2/CMakeLists.txt b/rmf_obstacle_ros2/CMakeLists.txt index 72e00a1ee..69c6a0bdf 100644 --- a/rmf_obstacle_ros2/CMakeLists.txt +++ b/rmf_obstacle_ros2/CMakeLists.txt @@ -29,14 +29,6 @@ foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) endforeach() -if ($ENV{ROS_DISTRO} MATCHES humble) - set(tf2_geometry_msgs_include_dirs "") - set(tf2_geometry_msgs_link_libraries tf2_geometry_msgs::tf2_geometry_msgs) -else() - set(tf2_geometry_msgs_include_dir ${tf2_geometry_msgs_INCLUDE_DIRS}) - set(tf2_geometry_msgs_link_libraries ${tf2_geometry_msgs_LIBRARIES}) -endif() - #=============================================================================== file(GLOB_RECURSE core_lib_srcs "src/rmf_obstacle_ros2/*.cpp") add_library(rmf_obstacle_ros2 SHARED ${core_lib_srcs}) @@ -44,11 +36,11 @@ add_library(rmf_obstacle_ros2 SHARED ${core_lib_srcs}) target_link_libraries(rmf_obstacle_ros2 PUBLIC rmf_utils::rmf_utils - ${rclcpp_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${rmf_obstacle_msgs_LIBRARIES} + ${rclcpp_TARGETS} + ${sensor_msgs_TARGETS} + ${rmf_obstacle_msgs_TARGETS} PRIVATE - ${OCTOMAP_LIBRARIES} + ${OCTOMAP_TARGETS} ) target_include_directories(rmf_obstacle_ros2 @@ -81,16 +73,16 @@ add_library(lane_blocker SHARED ${lane_blocker_srcs}) target_link_libraries(lane_blocker PRIVATE rclcpp::rclcpp - ${rclcpp_components_LIBRARIES} - ${rmf_obstacle_msgs_LIBRARIES} - ${rmf_fleet_msgs_LIBRARIES} - ${rmf_building_map_msgs_LIBRARIES} + ${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_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${geometry_msgs_LIBRARIES} - ${tf2_geometry_msgs_link_libraries} + ${rmf_traffic_ros2_TARGETS} + ${tf2_ros_TARGETS} + ${geometry_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs ) target_include_directories(lane_blocker From fc0d9e324c8d0ce3ae51427a5ab3a221c83cc7f3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 18 Mar 2024 10:34:18 +0800 Subject: [PATCH 60/60] Link against targets in test Signed-off-by: Yadunund --- rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt index 7d902c009..2ab8deb82 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt +++ b/rmf_obstacle_ros2/src/lane_blocker/test/CMakeLists.txt @@ -7,8 +7,8 @@ ament_add_catch2( target_link_libraries(test_intersection_checker PRIVATE - ${rmf_obstacle_msgs_LIBRARIES} - ${geometry_msgs_LIBRARIES} + ${rmf_obstacle_msgs_TARGETS} + ${geometry_msgs_TARGETS} rmf_utils::rmf_utils )