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] 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}); + } } } },