fix(traffic_light): fix stop reason (autowarefoundation#1864)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
satoshi-ota · Oct 28, 2022 · 0c3d634 · 0c3d634
1 parent 804201e
commit 0c3d634
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp
@@ -16,6 +16,7 @@
 #define SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <tuple>
 #include <vector>
@@ -52,7 +53,8 @@ class TrafficLightModule : public SceneModuleInterface
     geometry_msgs::msg::Pose first_stop_pose;
     std::vector<geometry_msgs::msg::Pose> dead_line_poses;
     std::vector<geometry_msgs::msg::Point> traffic_light_points;
-    geometry_msgs::msg::Point highest_confidence_traffic_light_point;
+    std::optional<geometry_msgs::msg::Point> highest_confidence_traffic_light_point = {
+      std::nullopt};
   };
 
   struct PlannerParam

diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp
@@ -441,8 +441,9 @@ bool TrafficLightModule::getHighestConfidenceTrafficSignal(
       highest_confidence = tl_state.lights.front().confidence;
       highest_confidence_tl_state = *tl_state_stamped;
       try {
-        debug_data_.traffic_light_points.push_back(getTrafficLightPosition(traffic_light));
-        debug_data_.highest_confidence_traffic_light_point = getTrafficLightPosition(traffic_light);
+        auto tl_position = getTrafficLightPosition(traffic_light);
+        debug_data_.traffic_light_points.push_back(tl_position);
+        debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position);
       } catch (const std::invalid_argument & ex) {
         RCLCPP_WARN_STREAM(logger_, ex.what());
         continue;
@@ -486,10 +487,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   // Get stop point and stop factor
   tier4_planning_msgs::msg::StopFactor stop_factor;
   stop_factor.stop_pose = debug_data_.first_stop_pose;
-  stop_factor.stop_factor_points =
-    std::vector<geometry_msgs::msg::Point>{debug_data_.highest_confidence_traffic_light_point};
-  planning_utils::appendStopReason(stop_factor, stop_reason);
-
+  if (debug_data_.highest_confidence_traffic_light_point != std::nullopt) {
+    stop_factor.stop_factor_points = std::vector<geometry_msgs::msg::Point>{
+      debug_data_.highest_confidence_traffic_light_point.value()};
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+  }
   return modified_path;
 }