From 949e509584d85f0299894e542c23ecd4fc3db5dc Mon Sep 17 00:00:00 2001 From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Date: Thu, 11 Aug 2022 13:56:41 +0800 Subject: [PATCH 1/6] add FleetUpdateHandle speed limit API to python bindings Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> --- rmf_fleet_adapter_python/src/adapter.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 2a06ec3cb..32e7b6413 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 @@ -285,6 +286,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, @@ -467,6 +474,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::init Date: Tue, 16 Aug 2022 12:54:25 +0800 Subject: [PATCH 2/6] add continuous checker option for dynamic obstacles Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> --- .../src/lane_blocker/LaneBlocker.cpp | 35 +++++++++++++++---- .../src/lane_blocker/LaneBlocker.hpp | 1 + 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index e44bef5f7..f476192b8 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -150,6 +150,13 @@ LaneBlocker::LaneBlocker(const rclcpp::NodeOptions& options) ); _max_search_duration = std::chrono::milliseconds(search_millis); + _continuous_checker = + this->declare_parameter("continuous_checker", false); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter continuous_checker to %s", _continuous_checker ? "true" : "false" + ); + _lane_closure_threshold = this->declare_parameter("lane_closure_threshold", 5); RCLCPP_INFO( @@ -394,10 +401,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 +487,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); + } + } } } } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 2d522df77..2628ae978 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -175,6 +175,7 @@ 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; rclcpp::TimerBase::SharedPtr _process_timer; From 3113da1004dadccb3351f1897473edfe0a57e9ed Mon Sep 17 00:00:00 2001 From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Date: Tue, 16 Aug 2022 18:48:21 +0800 Subject: [PATCH 3/6] refactor Lane Close and Open msg building Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> --- .../src/lane_blocker/LaneBlocker.cpp | 176 +++++++++++++----- .../src/lane_blocker/LaneBlocker.hpp | 18 ++ 2 files changed, 144 insertions(+), 50 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index f476192b8..f69402ddd 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -164,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)); @@ -225,6 +239,14 @@ 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) + { + std::string lane_key = get_lane_key(msg->name, i); + if (_internal_lane_states.find(lane_key) == _internal_lane_states.end()) + { + _internal_lane_states.insert({lane_key, LaneState::Normal}); + } + } }, ipc_sub_options); @@ -540,7 +562,9 @@ void LaneBlocker::request_lane_modifications( return; // A map to collate lanes per fleet that need to be closed - std::unordered_map> closure_msgs; + std::unordered_map> lane_req_msgs; + // A map to collate lanes per fleet that need speed limits + 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) @@ -554,41 +578,26 @@ 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 { 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) + for (auto& [_, msg] : lane_req_msgs) { if (msg->close_lanes.empty()) { @@ -608,6 +617,84 @@ void LaneBlocker::request_lane_modifications( } } +//============================================================================== +void LaneBlocker::transition_lane_state( + LaneState old_state, + LaneState new_state, + std::string lane_key, + std::unordered_map> &lane_req_msgs, + std::unordered_map> &speed_limit_req_msgs) +{ + if (new_state == old_state) + { + return; + } + + auto [fleet_name, lane_id] = deserialize_key(lane_key); + + if (old_state == LaneState::Normal && new_state == LaneState::Closed) + { + // 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 new lane id + msg_it.first->second->close_lanes.push_back(std::move(lane_id)); + } + } + else if (old_state == LaneState::Closed && new_state == LaneState::Normal) + { + // 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 new lane id + msg_it.first->second->open_lanes.push_back(std::move(lane_id)); + } + } + else if (old_state == LaneState::Normal && new_state == LaneState::SpeedLimited) + { + // construct Speed Limit msg + } + else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Normal) + { + // construct Speed Limit msg + } + else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Closed) + { + // construct Speed Limit msg + } + else if (old_state == LaneState::Closed && new_state == LaneState::SpeedLimited) + { + // construct Speed Limit msg + } + + // update lane state + auto it = _internal_lane_states.find(lane_key); + if (it != _internal_lane_states.end()) + { + it->second = new_state; + } +} + //============================================================================== auto LaneBlocker::deserialize_key( const std::string& key) const-> std::pair @@ -697,36 +784,27 @@ void LaneBlocker::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 + std::unordered_map> lane_req_msgs; + 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; + } + + const auto& obstacles = _lane_to_obstacles_map.at(lane_key); + if (obstacles.size() < _lane_closure_threshold && lane_state == LaneState::Closed) + { + transition_lane_state(lane_state, LaneState::Normal, lane_key, lane_req_msgs, speed_limit_req_msgs); } } - // Publish lane closures - for (auto& [_, msg] : open_msgs) + // Publish lane opening + for (auto& [_, msg] : lane_req_msgs) { if (msg->open_lanes.empty()) { @@ -745,8 +823,6 @@ void LaneBlocker::cull() _lane_closure_pub->publish(std::move(msg)); } - for (const auto& lane : opened_lanes) - _currently_closed_lanes.erase(lane); } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 2628ae978..f2df159e8 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,19 @@ class LaneBlocker : public rclcpp::Node void request_lane_modifications( const std::unordered_set& changes); + enum LaneState { Normal, Closed, SpeedLimited }; + + std::unordered_map< + std::string, + LaneState> _internal_lane_states = {}; + + void transition_lane_state( + LaneState old_state, + LaneState new_state, + std::string lane_key, + std::unordered_map> &lane_req_msgs, + std::unordered_map> &speed_limit_req_msgs); + void purge_obstacles( const std::unordered_set& obstacle_keys, const bool erase_from_buffer = true); @@ -157,6 +172,7 @@ class LaneBlocker : public rclcpp::Node _lane_to_obstacles_map = {}; std::unordered_set _currently_closed_lanes; + std::unordered_set _currently_speed_limited_lanes; rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; @@ -177,6 +193,8 @@ class LaneBlocker : public rclcpp::Node 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 e9b8ccb41f5bf2bffeef2560f2a5e5eff3aaaf10 Mon Sep 17 00:00:00 2001 From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Date: Thu, 18 Aug 2022 18:02:04 +0800 Subject: [PATCH 4/6] add speed limits Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> --- .../src/lane_blocker/LaneBlocker.cpp | 268 ++++++++++++------ .../src/lane_blocker/LaneBlocker.hpp | 25 +- 2 files changed, 208 insertions(+), 85 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index f69402ddd..ce53193b3 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -561,9 +561,9 @@ void LaneBlocker::request_lane_modifications( if (changes.empty()) return; - // A map to collate lanes per fleet that need to be closed + // 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 speed limits + // 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. @@ -584,6 +584,14 @@ void LaneBlocker::request_lane_modifications( { 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( @@ -596,25 +604,8 @@ void LaneBlocker::request_lane_modifications( } } - // Publish lane closures - for (auto& [_, msg] : lane_req_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 lanes to close for fleet %s", - msg->close_lanes.size(), msg->fleet_name.c_str() - ); - _lane_closure_pub->publish(std::move(msg)); - } + publish_lane_req_msgs(lane_req_msgs); + publish_speed_limit_req_msgs(speed_limit_req_msgs); } //============================================================================== @@ -630,61 +621,31 @@ void LaneBlocker::transition_lane_state( return; } - auto [fleet_name, lane_id] = deserialize_key(lane_key); - if (old_state == LaneState::Normal && new_state == LaneState::Closed) { - // 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 new lane id - msg_it.first->second->close_lanes.push_back(std::move(lane_id)); - } + add_lane_close_req(lane_key, lane_req_msgs); } else if (old_state == LaneState::Closed && new_state == LaneState::Normal) { - // 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 new lane id - msg_it.first->second->open_lanes.push_back(std::move(lane_id)); - } + add_lane_open_req(lane_key, lane_req_msgs); } else if (old_state == LaneState::Normal && new_state == LaneState::SpeedLimited) { - // construct Speed Limit msg + add_speed_limit_req(lane_key, speed_limit_req_msgs); } else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Normal) { - // construct Speed Limit msg + add_speed_unlimit_req(lane_key, speed_limit_req_msgs); } else if (old_state == LaneState::SpeedLimited && new_state == LaneState::Closed) { - // construct Speed Limit msg + 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) { - // construct Speed Limit msg + add_lane_open_req(lane_key, lane_req_msgs); + add_speed_limit_req(lane_key, speed_limit_req_msgs); } // update lane state @@ -695,6 +656,161 @@ void LaneBlocker::transition_lane_state( } } +//============================================================================== +void LaneBlocker::add_lane_close_req(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(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( + 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( + 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() && msg->open_lanes.empty()) + { + RCLCPP_DEBUG( + this->get_logger(), + "None of the lanes for fleet %s need to be opened or closed", + msg->fleet_name.c_str() + ); + continue; + } + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to close for fleet %s", + msg->close_lanes.size(), msg->fleet_name.c_str() + ); + RCLCPP_INFO( + this->get_logger(), + "Requested %ld lanes to open for fleet %s", + msg->open_lanes.size(), msg->fleet_name.c_str() + ); + _lane_closure_pub->publish(std::move(msg)); + } +} + +//============================================================================== +void LaneBlocker::publish_speed_limit_req_msgs( + 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 @@ -747,7 +863,6 @@ void LaneBlocker::purge_obstacles( } } - //============================================================================== void LaneBlocker::cull() { @@ -783,9 +898,9 @@ void LaneBlocker::cull() // Cull purge_obstacles(obstacles_to_cull); - // Open lanes if needed - // A map to collate lanes per fleet that need to be opened + // 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) @@ -797,33 +912,22 @@ void LaneBlocker::cull() } const auto& obstacles = _lane_to_obstacles_map.at(lane_key); - if (obstacles.size() < _lane_closure_threshold && lane_state == LaneState::Closed) + 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); } - } - - // Publish lane opening - for (auto& [_, msg] : lane_req_msgs) - { - if (msg->open_lanes.empty()) + else if (obstacles.size() < _lane_closure_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::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)); } - + publish_lane_req_msgs(lane_req_msgs); + publish_speed_limit_req_msgs(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 f2df159e8..81041621c 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -145,6 +145,28 @@ class LaneBlocker : public rclcpp::Node std::unordered_map> &lane_req_msgs, std::unordered_map> &speed_limit_req_msgs); + void add_lane_close_req( + std::string lane_key, + std::unordered_map> &lane_req_msgs); + + void add_lane_open_req( + std::string lane_key, + std::unordered_map> &lane_req_msgs); + + void add_speed_limit_req( + std::string lane_key, + std::unordered_map> &speed_limit_req_msgs); + + void add_speed_unlimit_req( + 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); @@ -171,9 +193,6 @@ class LaneBlocker : public rclcpp::Node std::unordered_set> _lane_to_obstacles_map = {}; - std::unordered_set _currently_closed_lanes; - std::unordered_set _currently_speed_limited_lanes; - rclcpp::Subscription::SharedPtr _obstacle_sub; rclcpp::Subscription::SharedPtr _graph_sub; rclcpp::Subscription::SharedPtr _lane_states_sub; From 1b823e902bc33e670dd44815df5038b41a86a040 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Nov 2022 17:57:07 +0800 Subject: [PATCH 5/6] Fix style Signed-off-by: Yadunund --- .../src/lane_blocker/LaneBlocker.cpp | 88 ++++++++++++------- .../src/lane_blocker/LaneBlocker.hpp | 24 +++-- 2 files changed, 74 insertions(+), 38 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index ce53193b3..9ebf4834e 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -564,7 +564,8 @@ void LaneBlocker::request_lane_modifications( // 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::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) @@ -580,17 +581,23 @@ void LaneBlocker::request_lane_modifications( } const auto& obstacles = _lane_to_obstacles_map.at(lane_key); const auto& lane_state = _internal_lane_states.at(lane_key); - if (obstacles.size() >= _lane_closure_threshold && lane_state == LaneState::Normal) + 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, + lane_req_msgs, speed_limit_req_msgs); } - else if (obstacles.size() >= _speed_limit_threshold && lane_state == LaneState::Normal) + 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, + lane_req_msgs, speed_limit_req_msgs); } - else if (obstacles.size() >= _lane_closure_threshold && lane_state == LaneState::SpeedLimited) + 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, + lane_req_msgs, speed_limit_req_msgs); } else { @@ -598,7 +605,8 @@ void LaneBlocker::request_lane_modifications( this->get_logger(), "Lane %s has %ld obstacles in its vicinity but will not be closed or speed limited as " "the closure threshold is %ld and the speed limit threshold is %ld", - lane_key.c_str(), obstacles.size(), _lane_closure_threshold, _speed_limit_threshold + lane_key.c_str(), + obstacles.size(), _lane_closure_threshold, _speed_limit_threshold ); continue; } @@ -613,8 +621,9 @@ void LaneBlocker::transition_lane_state( LaneState old_state, LaneState new_state, std::string lane_key, - std::unordered_map> &lane_req_msgs, - std::unordered_map> &speed_limit_req_msgs) + std::unordered_map>& lane_req_msgs, + std::unordered_map>& speed_limit_req_msgs) { if (new_state == old_state) { @@ -629,20 +638,24 @@ void LaneBlocker::transition_lane_state( { add_lane_open_req(lane_key, lane_req_msgs); } - else if (old_state == LaneState::Normal && new_state == LaneState::SpeedLimited) + 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) + 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) + 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) + 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); @@ -657,7 +670,9 @@ void LaneBlocker::transition_lane_state( } //============================================================================== -void LaneBlocker::add_lane_close_req(std::string lane_key, std::unordered_map> &lane_req_msgs) +void LaneBlocker::add_lane_close_req(std::string lane_key, + std::unordered_map>& lane_req_msgs) { auto [fleet_name, lane_id] = deserialize_key(lane_key); // construct Lane Closure msg @@ -679,7 +694,9 @@ void LaneBlocker::add_lane_close_req(std::string lane_key, std::unordered_map> &lane_req_msgs) +void LaneBlocker::add_lane_open_req(std::string lane_key, + std::unordered_map>& lane_req_msgs) { auto [fleet_name, lane_id] = deserialize_key(lane_key); // construct Lane Open msg @@ -703,15 +720,17 @@ void LaneBlocker::add_lane_open_req(std::string lane_key, std::unordered_map> &speed_limit_req_msgs) + 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); + SpeedLimitedLane speed_limited_lane = + rmf_fleet_msgs::build() + .lane_index(std::move(lane_id)) + .speed_limit(_speed_limit); if (msg_it.second) { @@ -732,7 +751,8 @@ void LaneBlocker::add_speed_limit_req( //============================================================================== void LaneBlocker::add_speed_unlimit_req( std::string lane_key, - std::unordered_map> &speed_limit_req_msgs) + std::unordered_map>& speed_limit_req_msgs) { auto [fleet_name, lane_id] = deserialize_key(lane_key); // construct Speed Unlimit msg @@ -755,7 +775,7 @@ void LaneBlocker::add_speed_unlimit_req( //============================================================================== void LaneBlocker::publish_lane_req_msgs( - std::unordered_map> &lane_req_msgs) + std::unordered_map>& lane_req_msgs) { for (auto& [_, msg] : lane_req_msgs) { @@ -784,7 +804,8 @@ void LaneBlocker::publish_lane_req_msgs( //============================================================================== void LaneBlocker::publish_speed_limit_req_msgs( - std::unordered_map> &speed_limit_req_msgs) + std::unordered_map>& speed_limit_req_msgs) { for (auto& [_, msg] : speed_limit_req_msgs) { @@ -901,7 +922,8 @@ void LaneBlocker::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::unordered_map> speed_limit_req_msgs; for (const auto& [lane_key, lane_state] : _internal_lane_states) { @@ -912,17 +934,23 @@ void LaneBlocker::cull() } const auto& obstacles = _lane_to_obstacles_map.at(lane_key); - if (obstacles.size() < _speed_limit_threshold && lane_state == LaneState::Closed) + 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, + lane_req_msgs, speed_limit_req_msgs); } - else if (obstacles.size() < _lane_closure_threshold && lane_state == LaneState::Closed) + 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, + lane_req_msgs, speed_limit_req_msgs); } - else if (obstacles.size() < _speed_limit_threshold && lane_state == LaneState::SpeedLimited) + 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, + lane_req_msgs, speed_limit_req_msgs); } } diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp index 81041621c..aad9cfe14 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -142,30 +142,38 @@ class LaneBlocker : public rclcpp::Node LaneState old_state, LaneState new_state, std::string lane_key, - std::unordered_map> &lane_req_msgs, - std::unordered_map> &speed_limit_req_msgs); + std::unordered_map>& lane_req_msgs, + std::unordered_map>& speed_limit_req_msgs); void add_lane_close_req( std::string lane_key, - std::unordered_map> &lane_req_msgs); + std::unordered_map>& lane_req_msgs); void add_lane_open_req( std::string lane_key, - std::unordered_map> &lane_req_msgs); + std::unordered_map>& lane_req_msgs); void add_speed_limit_req( std::string lane_key, - std::unordered_map> &speed_limit_req_msgs); + std::unordered_map>& speed_limit_req_msgs); void add_speed_unlimit_req( std::string lane_key, - std::unordered_map> &speed_limit_req_msgs); + std::unordered_map>& speed_limit_req_msgs); void publish_lane_req_msgs( - std::unordered_map> &lane_req_msgs); + std::unordered_map>& lane_req_msgs); void publish_speed_limit_req_msgs( - std::unordered_map> &speed_limit_req_msgs); + std::unordered_map>& speed_limit_req_msgs); void purge_obstacles( const std::unordered_set& obstacle_keys, From 897cc3e776f504e13cff227652d1eeb182a8fda3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Nov 2022 18:29:52 +0800 Subject: [PATCH 6/6] Fix const correctness and do not pass unordered_map by ref to publisher Signed-off-by: Yadunund --- .../src/lane_blocker/LaneBlocker.cpp | 35 +++++++++++-------- .../src/lane_blocker/LaneBlocker.hpp | 29 +++++++++------ 2 files changed, 40 insertions(+), 24 deletions(-) diff --git a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp index 9ebf4834e..7bc7fa56a 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.cpp @@ -241,7 +241,12 @@ 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) { - std::string lane_key = get_lane_key(msg->name, 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}); @@ -612,15 +617,15 @@ void LaneBlocker::request_lane_modifications( } } - publish_lane_req_msgs(lane_req_msgs); - publish_speed_limit_req_msgs(speed_limit_req_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( - LaneState old_state, - LaneState new_state, - std::string lane_key, + 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) @@ -670,7 +675,8 @@ void LaneBlocker::transition_lane_state( } //============================================================================== -void LaneBlocker::add_lane_close_req(std::string lane_key, +void LaneBlocker::add_lane_close_req( + const std::string& lane_key, std::unordered_map>& lane_req_msgs) { @@ -694,7 +700,8 @@ void LaneBlocker::add_lane_close_req(std::string lane_key, } //============================================================================== -void LaneBlocker::add_lane_open_req(std::string lane_key, +void LaneBlocker::add_lane_open_req( + const std::string& lane_key, std::unordered_map>& lane_req_msgs) { @@ -719,7 +726,7 @@ void LaneBlocker::add_lane_open_req(std::string lane_key, //============================================================================== void LaneBlocker::add_speed_limit_req( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs) { @@ -750,7 +757,7 @@ void LaneBlocker::add_speed_limit_req( //============================================================================== void LaneBlocker::add_speed_unlimit_req( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs) { @@ -775,7 +782,7 @@ void LaneBlocker::add_speed_unlimit_req( //============================================================================== void LaneBlocker::publish_lane_req_msgs( - std::unordered_map>& lane_req_msgs) + std::unordered_map> lane_req_msgs) { for (auto& [_, msg] : lane_req_msgs) { @@ -805,7 +812,7 @@ void LaneBlocker::publish_lane_req_msgs( //============================================================================== void LaneBlocker::publish_speed_limit_req_msgs( std::unordered_map>& speed_limit_req_msgs) + std::unique_ptr> speed_limit_req_msgs) { for (auto& [_, msg] : speed_limit_req_msgs) { @@ -954,8 +961,8 @@ void LaneBlocker::cull() } } - publish_lane_req_msgs(lane_req_msgs); - publish_speed_limit_req_msgs(speed_limit_req_msgs); + 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 aad9cfe14..190a8d969 100644 --- a/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp +++ b/rmf_obstacle_ros2/src/lane_blocker/LaneBlocker.hpp @@ -132,48 +132,57 @@ class LaneBlocker : public rclcpp::Node void request_lane_modifications( const std::unordered_set& changes); - enum LaneState { Normal, Closed, SpeedLimited }; + 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( - LaneState old_state, - LaneState new_state, - std::string lane_key, + 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( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& lane_req_msgs); void add_lane_open_req( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& lane_req_msgs); void add_speed_limit_req( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs); void add_speed_unlimit_req( - std::string lane_key, + const std::string& lane_key, std::unordered_map>& speed_limit_req_msgs); void publish_lane_req_msgs( std::unordered_map>& lane_req_msgs); + std::unique_ptr> lane_req_msgs); void publish_speed_limit_req_msgs( std::unordered_map>& speed_limit_req_msgs); + std::unique_ptr> speed_limit_req_msgs); void purge_obstacles( const std::unordered_set& obstacle_keys,