From ab0e7ea3e935e9f3b0595757d6fdfba18c8c008a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 9 Aug 2023 02:18:37 +0900 Subject: [PATCH] fix(scnenario_selector): prevent changing scneario when same uuid route is received 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)