refactor(behavior_path_planner): traffic light utils (autowarefoundat…

…ion#5503) * refactor(behavior_path_planner): traffic light utils Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/src/utils/traffic_light_utils.cpp Co-authored-by: Satoshi OTA <[email protected]> * Update planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp Co-authored-by: Satoshi OTA <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Co-authored-by: Satoshi OTA <[email protected]>
zulfaqar-azmi-t4 · Nov 7, 2023 · c9e0b91 · c9e0b91
1 parent f662ac7
commit c9e0b91
Show file tree

Hide file tree

Showing 6 changed files with 274 additions and 139 deletions.
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -30,6 +30,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/turn_signal_decider.cpp
   src/utils/utils.cpp
   src/utils/path_utils.cpp
+  src/utils/traffic_light_utils.cpp
   src/utils/path_safety_checker/safety_check.cpp
   src/utils/path_safety_checker/objects_filtering.cpp
   src/utils/start_goal_planner_common/utils.cpp

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp
@@ -0,0 +1,112 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_
+
+#include <behavior_path_planner/data_manager.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+
+#include <autoware_perception_msgs/msg/traffic_signal.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_element.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_core/geometry/Polygon.h>
+
+#include <memory>
+
+namespace behavior_path_planner::utils::traffic_light
+{
+
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalElement;
+using geometry_msgs::msg::Pose;
+
+/**
+ * @brief Checks if a traffic light state includes a circle-shaped light with the specified color.
+ *
+ * Iterates through the traffic light elements to find a circle-shaped light that matches the given
+ * color.
+ *
+ * @param tl_state The traffic light state to check.
+ * @param lamp_color The color to look for in the traffic light's circle-shaped lamps.
+ * @return True if a circle-shaped light with the specified color is found, false otherwise.
+ */
+bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color);
+
+/**
+ * @brief Checks if a traffic light state includes a light with the specified shape.
+ *
+ * Searches through the traffic light elements to find a light that matches the given shape.
+ *
+ * @param tl_state The traffic light state to check.
+ * @param shape The shape to look for in the traffic light's lights.
+ * @return True if a light with the specified shape is found, false otherwise.
+ */
+bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape);
+
+/**
+ * @brief Determines if a traffic signal indicates a stop for the given lanelet.
+ *
+ * Evaluates the current state of the traffic light, considering if it's green or unknown,
+ * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet
+ * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can
+ * proceed based on allowed turn directions.
+ *
+ * @param lanelet The lanelet to check for a stop signal at its traffic light.
+ * @param tl_state The current state of the traffic light associated with the lanelet.
+ * @return True if the traffic signal indicates a stop is required, false otherwise.
+ */
+bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state);
+
+/**
+ * @brief Computes the distance from the current position to the next traffic light along a set of
+ * lanelets.
+ *
+ * This function finds the closest lanelet to the current position and then searches for the
+ * next traffic light within the lanelet sequence. If a traffic light is found, it calculates
+ * the distance to the stop line of that traffic light from the current position.
+ *
+ * @param current_pose The current position of ego vehicle.
+ * @param lanelets A collection of lanelets representing the road ahead.
+ * @return The distance to the next traffic light's stop line or infinity if no traffic light is
+ * found ahead.
+ */
+double getDistanceToNextTrafficLight(
+  const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
+
+/**
+ * @brief Calculates the signed distance from the ego vehicle to the stop line of the nearest red
+ * traffic light.
+ *
+ * Iterates over lanelets to find traffic lights. If a red traffic light is found,
+ * computes the distance to its stop line from the ego vehicle's position along a specified path.
+ *
+ * @param lanelets Collection of lanelets to inspect for traffic light regulatory elements.
+ * @param path The path along which the distance from ego current position to the stop line is
+ * measured.
+ * @param planner_data Shared pointer to planner data with vehicle odometry and traffic signal
+ * information.
+ * @return Optional double value with the signed distance to the stop line, or std::nullopt if no
+ * red traffic light is detected.
+ */
+std::optional<double> calcDistanceToRedTrafficLight(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data);
+}  // namespace behavior_path_planner::utils::traffic_light
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -130,9 +130,6 @@ double l2Norm(const Vector3 vector);
 
 double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
 
-double getDistanceToNextTrafficLight(
-  const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
-
 double getDistanceToNextIntersection(
   const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
 

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -18,6 +18,7 @@
 #include "behavior_path_planner/utils/avoidance/utils.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/traffic_light_utils.hpp"
 #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
 #include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
@@ -49,6 +50,8 @@ namespace behavior_path_planner::utils::avoidance
 {
 
 using autoware_perception_msgs::msg::TrafficSignalElement;
+using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight;
+using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight;
 using motion_utils::calcLongitudinalOffsetPoint;
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
@@ -227,85 +230,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
   base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
 }
 
-bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color)
-{
-  const auto it_lamp =
-    std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) {
-      return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color;
-    });
-
-  return it_lamp != tl_state.elements.end();
-}
-
-bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape)
-{
-  const auto it_lamp = std::find_if(
-    tl_state.elements.begin(), tl_state.elements.end(),
-    [&lamp_shape](const auto & x) { return x.shape == lamp_shape; });
-
-  return it_lamp != tl_state.elements.end();
-}
-
-bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state)
-{
-  if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) {
-    return false;
-  }
-
-  if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) {
-    return false;
-  }
-
-  const std::string turn_direction = lanelet.attributeOr("turn_direction", "else");
-
-  if (turn_direction == "else") {
-    return true;
-  }
-  if (
-    turn_direction == "right" &&
-    hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) {
-    return false;
-  }
-  if (
-    turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) {
-    return false;
-  }
-  if (
-    turn_direction == "straight" &&
-    hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) {
-    return false;
-  }
-
-  return true;
-}
-
-std::optional<double> calcDistanceToRedTrafficLight(
-  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
-  const std::shared_ptr<const PlannerData> & planner_data)
-{
-  for (const auto & lanelet : lanelets) {
-    for (const auto & element : lanelet.regulatoryElementsAs<TrafficLight>()) {
-      const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id());
-      if (!traffic_signal_stamped.has_value()) {
-        continue;
-      }
-
-      if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) {
-        continue;
-      }
-
-      const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
-      lanelet::ConstLineString3d stop_line = *(element->stopLine());
-      const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x());
-      const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y());
-      const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z());
-
-      return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z));
-    }
-  }
-
-  return std::nullopt;
-}
 }  // namespace
 
 bool isOnRight(const ObjectData & obj)
@@ -426,7 +350,7 @@ bool isForceAvoidanceTarget(
   bool not_parked_object = true;
 
   // check traffic light
-  const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
+  const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, extend_lanelets);
   {
     not_parked_object =
       to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;