feat(lane_change): cancel lane change on red traffic light

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

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -156,7 +156,7 @@ struct PlannerData
   std::optional<PoseWithUuidStamped> prev_modified_goal{};
   std::optional<UUID> prev_route_id{};
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
-  std::map<int, TrafficSignalStamped> traffic_light_id_map;
+  std::map<int64_t, TrafficSignalStamped> traffic_light_id_map;
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
 
@@ -174,7 +174,7 @@ struct PlannerData
       route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data);
   }
 
-  std::optional<TrafficSignalStamped> getTrafficSignal(const int id) const
+  std::optional<TrafficSignalStamped> getTrafficSignal(const int64_t id) const
   {
     if (traffic_light_id_map.count(id) == 0) {
       return std::nullopt;

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

@@ -107,6 +107,8 @@ class LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const double threshold) const = 0;
 
+  virtual bool isStoppedAtRedTrafficLight() const = 0;
+
   virtual bool calcAbortPath() = 0;
 
   virtual bool specialRequiredCheck() const { return false; }

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

@@ -96,6 +96,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isLaneChangeRequired() const override;
 
+  bool isStoppedAtRedTrafficLight() const override;
+
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 

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

@@ -27,6 +27,7 @@
 #include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
+#include <limits>
 #include <memory>
 
 namespace behavior_path_planner::utils::traffic_light
@@ -107,6 +108,27 @@ double getDistanceToNextTrafficLight(
 std::optional<double> calcDistanceToRedTrafficLight(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data);
+
+/**
+ * @brief Checks if the vehicle is stationary within a specified distance from a red traffic light.
+ *
+ * This function first checks if the vehicle's velocity is below a minimum threshold, indicating it
+ * is stopped. It then calculates the distance to the nearest red traffic light using the
+ * calcDistanceToRedTrafficLight function. If the vehicle is within the specified distance threshold
+ * from the red traffic light, the function returns true, otherwise false.
+ *
+ * @param lanelets The lanelets to search for traffic lights.
+ * @param path The path along which to measure the distance to the traffic light.
+ * @param planner_data Shared pointer to the planner data containing vehicle state information.
+ * @param distance_threshold The maximum allowable distance from a red traffic light to consider the
+ * vehicle stopped.
+ * @return True if the vehicle is stopped within the distance threshold from a red traffic light,
+ * false otherwise.
+ */
+bool isStoppedAtRedTrafficLightWithinDistance(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const double distance_threshold = std::numeric_limits<double>::infinity());
 }  // namespace behavior_path_planner::utils::traffic_light
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_

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

@@ -108,6 +108,14 @@ ModuleStatus LaneChangeInterface::updateState()
     return ModuleStatus::SUCCESS;
   }
 
+  if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
+      "Ego stopped at traffic light. Canceling lane change");
+    module_type_->toCancelState();
+    return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS;
+  }
+
   const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe();
 
   setObjectDebugVisualization();

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

@@ -127,6 +127,13 @@ bool NormalLaneChange::isLaneChangeRequired() const
   return !target_lanes.empty();
 }
 
+bool NormalLaneChange::isStoppedAtRedTrafficLight() const
+{
+  return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance(
+    status_.current_lanes, status_.lane_change_path.path, planner_data_,
+    status_.lane_change_path.info.length.sum());
+}
+
 LaneChangePath NormalLaneChange::getLaneChangePath() const
 {
   return status_.lane_change_path;
@@ -1363,9 +1370,16 @@ bool NormalLaneChange::getLaneChangePaths(
         if (
           lane_change_parameters_->regulate_on_traffic_light &&
           !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) {
+          debug_print("Reject: regulate on traffic light!!");
           continue;
         }
 
+        if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance(
+              lane_change_info.current_lanes, candidate_path.value().path, planner_data_,
+              lane_change_info.length.sum())) {
+          debug_print("Ego is stopping near traffic light. Do not allow lane change");
+          continue;
+        }
         candidate_paths->push_back(*candidate_path);
 
         std::vector<ExtendedPredictedObject> filtered_objects =

planning/behavior_path_planner/src/utils/traffic_light_utils.cpp

@@ -154,4 +154,27 @@ std::optional<double> calcDistanceToRedTrafficLight(
 
   return std::nullopt;
 }
+
+bool isStoppedAtRedTrafficLightWithinDistance(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data, const double distance_threshold)
+{
+  const auto ego_velocity = std::hypot(
+    planner_data->self_odometry->twist.twist.linear.x,
+    planner_data->self_odometry->twist.twist.linear.y);
+  constexpr double minimum_speed = 0.1;
+  if (ego_velocity > minimum_speed) {
+    return false;
+  }
+
+  const auto distance_to_red_traffic_light =
+    calcDistanceToRedTrafficLight(lanelets, path, planner_data);
+
+  if (!distance_to_red_traffic_light) {
+    return false;
+  }
+
+  return (distance_to_red_traffic_light < distance_threshold);
+}
+
 }  // namespace behavior_path_planner::utils::traffic_light