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); + } + } } } } @@ -516,8 +566,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) @@ -531,47 +584,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; @@ -581,10 +800,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 @@ -637,7 +891,6 @@ void LaneBlocker::purge_obstacles( } } - //============================================================================== void LaneBlocker::cull() { @@ -673,58 +926,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;