traffic_light

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -95,6 +95,8 @@ class LaneChangeBase
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
+  virtual bool isStoppedAtTrafficSignal() const = 0;
+
   virtual LaneChangePath getLaneChangePath() const = 0;
 
   virtual bool isEgoOnPreparePhase() const = 0;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -90,6 +90,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbleToStopSafely() const override;
 
+  bool isStoppedAtTrafficSignal() const override;
+
   bool hasFinishedAbort() const override;
 
   bool isAbortState() const override;
@@ -135,6 +137,10 @@ class NormalLaneChange : public LaneChangeBase
   bool hasEnoughLengthToIntersection(
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
 
+  bool hasEnoughLengthToTrafficLight(
+    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
+    lanelet::Id & traffic_light_id) const;
+
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     Direction direction, LaneChangePaths * candidate_paths,

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -65,6 +65,7 @@ struct LaneChangeParameters
   // regulatory elements
   bool regulate_on_crosswalk{false};
   bool regulate_on_intersection{false};
+  bool regulate_on_traffic_light{false};
 
   // ego vehicle stuck detection
   double stop_velocity_threshold{0.1};

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -130,7 +130,9 @@ double l2Norm(const Vector3 vector);
 
 double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
 
-double getDistanceToNextTrafficLight(
+bool isRedTrafficSignal(const TrafficSignalStamped & traffic_signal);
+
+std::pair<lanelet::Id, double> getDistanceToNextTrafficLight(
   const Pose & current_pose, const lanelet::ConstLanelets & lanelets);
 
 double getDistanceToNextIntersection(

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -125,6 +125,26 @@ bool NormalLaneChange::isLaneChangeRequired() const
   return !target_lanes.empty();
 }
 
+bool NormalLaneChange::isStoppedAtTrafficSignal() const
+{
+  lanelet::Id next_traffic_light_id = lanelet::InvalId;
+  const auto has_enough_length_to_traffic_light = hasEnoughLengthToTrafficLight(
+    status_.lane_change_path, status_.current_lanes, next_traffic_light_id);
+  if (has_enough_length_to_traffic_light) {
+    return false;
+  }
+
+  const auto traffic_light_id_map = planner_data_->traffic_light_id_map;
+
+  if (traffic_light_id_map.find(next_traffic_light_id) == traffic_light_id_map.end()) {
+    return false;
+  }
+
+  const auto traffic_signal = planner_data_->traffic_light_id_map.at(next_traffic_light_id);
+
+  return utils::isRedTrafficSignal(traffic_signal);
+}
+
 LaneChangePath NormalLaneChange::getLaneChangePath() const
 {
   return isAbortState() ? *abort_path_ : status_.lane_change_path;
@@ -1063,6 +1083,25 @@ bool NormalLaneChange::hasEnoughLengthToIntersection(
   return true;
 }
 
+bool NormalLaneChange::hasEnoughLengthToTrafficLight(
+  const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
+  lanelet::Id & traffic_light_id) const
+{
+  const auto current_pose = getEgoPose();
+  const auto [next_traffic_light_id, dist_to_next_traffic_light] =
+    utils::getDistanceToNextTrafficLight(current_pose, current_lanes);
+  traffic_light_id = next_traffic_light_id;
+  const auto dist_to_next_traffic_light_from_lc_start_pose =
+    dist_to_next_traffic_light - path.info.length.prepare;
+
+  std::cerr << __func__ << "dist to traffic light = " << dist_to_next_traffic_light
+            << "\tdist from start pose " << dist_to_next_traffic_light_from_lc_start_pose
+            << "\tlength.prapare " << path.info.length.prepare << '\n';
+
+  return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 ||
+         dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing;
+}
+
 bool NormalLaneChange::getLaneChangePaths(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
   Direction direction, LaneChangePaths * candidate_paths,

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -426,7 +426,8 @@ bool isForceAvoidanceTarget(
   bool not_parked_object = true;
 
   // check traffic light
-  const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
+  const auto [next_traffic_light_id, to_traffic_light] =
+    utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
   {
     not_parked_object =
       to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;

planning/behavior_path_planner/src/utils/utils.cpp

@@ -26,6 +26,8 @@
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 
+#include "autoware_perception_msgs/msg/traffic_signal_element.hpp"
+
 #include <boost/geometry/algorithms/is_valid.hpp>
 
 #include <lanelet2_core/geometry/Point.h>
@@ -136,6 +138,7 @@ namespace behavior_path_planner::utils
 {
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::Shape;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 using geometry_msgs::msg::PoseWithCovarianceStamped;
 using tf2::fromMsg;
 using tier4_autoware_utils::Point2d;
@@ -2289,12 +2292,20 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan
   return lanelet_length - arc_coordinates.length;
 }
 
-double getDistanceToNextTrafficLight(
+bool isRedTrafficSignal(const TrafficSignalStamped & traffic_signal)
+{
+  if (traffic_signal.signal.elements.empty()) {
+    return false;
+  }
+  return traffic_signal.signal.elements.front().color == TrafficSignalElement::RED;
+}
+
+std::pair<lanelet::Id, double> getDistanceToNextTrafficLight(
   const Pose & current_pose, const lanelet::ConstLanelets & lanelets)
 {
   lanelet::ConstLanelet current_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, &current_lanelet)) {
-    return std::numeric_limits<double>::infinity();
+    return {lanelet::InvalId, std::numeric_limits<double>::infinity()};
   }
 
   const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
@@ -2312,7 +2323,7 @@ double getDistanceToNextTrafficLight(
     const auto distance_object_to_stop_line = to_stop_line.length - to_object.length;
 
     if (distance_object_to_stop_line > 0.0) {
-      return distance_object_to_stop_line;
+      return {element->id(), distance_object_to_stop_line};
     }
   }
 
@@ -2336,13 +2347,13 @@ double getDistanceToNextTrafficLight(
         lanelet::utils::to2D(llt.centerline()),
         lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint());
 
-      return distance + to_stop_line.length - to_object.length;
+      return {element->id(), distance + to_stop_line.length - to_object.length};
     }
 
     distance += lanelet::utils::getLaneletLength3d(llt);
   }
 
-  return std::numeric_limits<double>::infinity();
+  return {lanelet::InvalId, std::numeric_limits<double>::infinity()};
 }
 
 double getDistanceToNextIntersection(