From 9f22504f4562c826b372fdf1ebec712001f6b9bc Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 10 Aug 2023 12:26:36 +0900 Subject: [PATCH] fix(scnenario_selector): prevent changing scneario when same uuid route is received (#4569) Signed-off-by: kosuke55 --- .../src/scenario_selector_node/scenario_selector_node.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index dfba038dee898..cffb490464de0 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -197,8 +197,13 @@ void ScenarioSelectorNode::onMap( void ScenarioSelectorNode::onRoute( const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg) { + // When the route id is the same (e.g. reporting with modified goal) keep the current scenario. + // Otherwise, reset the scenario. + if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) { + current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; + } + route_ = msg; - current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; } void ScenarioSelectorNode::onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg)