fix: traffic light module (#695)

* fix: traffic light arrow Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * change function name Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
tier4 · Apr 14, 2022 · fd5b9d3 · fd5b9d3
1 parent 1da3e61
commit fd5b9d3
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 6 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
@@ -101,7 +101,7 @@ class TrafficLightModule : public SceneModuleInterface
 
   bool isPassthrough(const double & signed_arc_length) const;
 
-  bool hasTrafficLightColor(
+  bool hasTrafficLightCircleColor(
     const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
     const uint8_t & lamp_color) const;
 

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
@@ -364,7 +364,8 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
 bool TrafficLightModule::isTrafficSignalStop(
   const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const
 {
-  if (hasTrafficLightColor(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) {
+  if (hasTrafficLightCircleColor(
+        tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) {
     return false;
   }
 
@@ -485,13 +486,16 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   return modified_path;
 }
 
-bool TrafficLightModule::hasTrafficLightColor(
+bool TrafficLightModule::hasTrafficLightCircleColor(
   const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
   const uint8_t & lamp_color) const
 {
-  const auto it_lamp = std::find_if(
-    tl_state.lights.begin(), tl_state.lights.end(),
-    [&lamp_color](const auto & x) { return x.color == lamp_color; });
+  using autoware_auto_perception_msgs::msg::TrafficLight;
+
+  const auto it_lamp =
+    std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) {
+      return x.shape == TrafficLight::CIRCLE && x.color == lamp_color;
+    });
 
   return it_lamp != tl_state.lights.end();
 }