fix(scnenario_selector): prevent changing scneario when same uuid rou…

…te is received Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Aug 9, 2023 · 9c87223 · 9c87223
1 parent 9ac8faf
commit 9c87223
Showing 1 changed file with 6 additions and 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
@@ -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)