feat(avoidance): use traffic light signal info (#5395)

* feat(utils): add function to calculate shift start/end point Signed-off-by: satoshi-ota <[email protected]> * feat(avoidance): add new parameter Signed-off-by: satoshi-ota <[email protected]> * feat(avoidance): set shift start/end point with module taking traffic signal into account Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Oct 31, 2023 · 1998880 · 1998880
1 parent 9628276
commit 1998880
Show file tree

Hide file tree

Showing 6 changed files with 228 additions and 24 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -191,11 +191,18 @@
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]
-          remain_buffer_distance: 30.0                  # [m]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]
           nominal_avoidance_speed: 8.33                 # [m/s]
+        # return dead line
+        return_dead_line:
+          goal:
+            enable: true                                # [-]
+            buffer: 30.0                                # [m]
+          traffic_light:
+            enable: true                                # [-]
+            buffer: 3.0                                 # [m]
 
       # For yield maneuver
       yield:

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -112,6 +112,14 @@ struct AvoidanceParameters
   // use intersection area for avoidance
   bool use_intersection_areas{false};
 
+  // consider avoidance return dead line
+  bool enable_dead_line_for_goal{false};
+  bool enable_dead_line_for_traffic_light{false};
+
+  // module try to return original path to keep this distance from edge point of the path.
+  double dead_line_buffer_for_goal{0.0};
+  double dead_line_buffer_for_traffic_light{0.0};
+
   // max deceleration for
   double max_deceleration{0.0};
 
@@ -217,9 +225,6 @@ struct AvoidanceParameters
   // nominal avoidance sped
   double nominal_avoidance_speed{0.0};
 
-  // module try to return original path to keep this distance from edge point of the path.
-  double remain_buffer_distance{0.0};
-
   // The margin is configured so that the generated avoidance trajectory does not come near to the
   // road shoulder.
   double soft_road_shoulder_margin{1.0};
@@ -517,6 +522,10 @@ struct AvoidancePlanningData
   bool found_avoidance_path{false};
 
   double to_stop_line{std::numeric_limits<double>::max()};
+
+  double to_start_point{std::numeric_limits<double>::lowest()};
+
+  double to_return_point{std::numeric_limits<double>::max()};
 };
 
 /*

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -172,6 +172,16 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
 DrivableLanes generateExpandDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
+
+double calcDistanceToReturnDeadLine(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
+
+double calcDistanceToAvoidStartLine(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -288,6 +288,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     data.reference_path, 0, data.reference_path.points.size(),
     calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0));
 
+  data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine(
+    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
+
+  data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine(
+    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
+
   // target objects for avoidance
   fillAvoidanceTargetObjects(data, debug);
 
@@ -1066,18 +1072,35 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
           : 0.0;
       const auto offset =
         object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
+      const auto to_shift_end = o.longitudinal - offset;
       const auto path_front_to_ego =
         avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index);
 
-      al_avoid.start_longitudinal =
-        std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3);
+      // start point (use previous linear shift length as start shift length.)
+      al_avoid.start_longitudinal = [&]() {
+        const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3);
+
+        if (data.to_start_point > to_shift_end) {
+          return nearest_avoid_distance;
+        }
+
+        const auto minimum_avoid_distance =
+          helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
+        const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
+
+        return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
+      }();
+
       al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength(
         avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
       al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose;
       al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);
 
+      // end point
       al_avoid.end_shift_length = feasible_shift_length.get();
-      al_avoid.end_longitudinal = o.longitudinal - offset;
+      al_avoid.end_longitudinal = to_shift_end;
+
+      // misc
       al_avoid.id = getOriginalShiftLineUniqueId();
       al_avoid.object = o;
       al_avoid.object_on_right = utils::avoidance::isOnRight(o);
@@ -1086,18 +1109,24 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     AvoidLine al_return;
     {
       const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
-      // The end_margin also has the purpose of preventing the return path from NOT being
-      // triggered at the end point.
-      const auto return_remaining_distance = std::max(
-        data.arclength_from_ego.back() - o.longitudinal - offset -
-          parameters_->remain_buffer_distance,
-        0.0);
+      const auto to_shift_start = o.longitudinal + offset;
 
+      // start point
       al_return.start_shift_length = feasible_shift_length.get();
+      al_return.start_longitudinal = to_shift_start;
+
+      // end point
+      al_return.end_longitudinal = [&]() {
+        if (data.to_return_point > to_shift_start) {
+          return std::clamp(
+            data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start);
+        }
+
+        return to_shift_start + feasible_return_distance;
+      }();
       al_return.end_shift_length = 0.0;
-      al_return.start_longitudinal = o.longitudinal + offset;
-      al_return.end_longitudinal =
-        o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance);
+
+      // misc
       al_return.id = getOriginalShiftLineUniqueId();
       al_return.object = o;
       al_return.object_on_right = utils::avoidance::isOnRight(o);
@@ -1795,7 +1824,9 @@ AvoidLineArray AvoidanceModule::addReturnShiftLine(
     return ret;
   }
 
-  const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance;
+  const auto remaining_distance = std::min(
+    arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal,
+    avoid_data_.to_return_point);
 
   // If the avoidance point has already been set, the return shift must be set after the point.
   const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx);
@@ -2859,8 +2890,8 @@ void AvoidanceModule::insertReturnDeadLine(
 {
   const auto & data = avoid_data_;
 
-  if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) {
-    RCLCPP_DEBUG(getLogger(), "goal is far enough.");
+  if (data.to_return_point > planner_data_->parameters.forward_path_length) {
+    RCLCPP_DEBUG(getLogger(), "return dead line is far enough.");
     return;
   }
 
@@ -2872,10 +2903,7 @@ void AvoidanceModule::insertReturnDeadLine(
   }
 
   const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length);
-
-  const auto to_goal = calcSignedArcLength(
-    shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1);
-  const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance;
+  const auto to_stop_line = data.to_return_point - min_return_distance;
 
   // If we don't need to consider deceleration constraints, insert a deceleration point
   // and return immediately

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -223,13 +223,23 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     std::string ns = "avoidance.avoidance.longitudinal.";
     p.prepare_time = getOrDeclareParameter<double>(*node, ns + "prepare_time");
     p.min_prepare_distance = getOrDeclareParameter<double>(*node, ns + "min_prepare_distance");
-    p.remain_buffer_distance = getOrDeclareParameter<double>(*node, ns + "remain_buffer_distance");
     p.min_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "min_slow_down_speed");
     p.buf_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "buf_slow_down_speed");
     p.nominal_avoidance_speed =
       getOrDeclareParameter<double>(*node, ns + "nominal_avoidance_speed");
   }
 
+  // avoidance maneuver (return shift dead line)
+  {
+    std::string ns = "avoidance.avoidance.return_dead_line.";
+    p.enable_dead_line_for_goal = getOrDeclareParameter<bool>(*node, ns + "goal.enable");
+    p.enable_dead_line_for_traffic_light =
+      getOrDeclareParameter<bool>(*node, ns + "traffic_light.enable");
+    p.dead_line_buffer_for_goal = getOrDeclareParameter<double>(*node, ns + "goal.buffer");
+    p.dead_line_buffer_for_traffic_light =
+      getOrDeclareParameter<double>(*node, ns + "traffic_light.buffer");
+  }
+
   // yield
   {
     std::string ns = "avoidance.yield.";

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -48,6 +48,7 @@
 namespace behavior_path_planner::utils::avoidance
 {
 
+using autoware_perception_msgs::msg::TrafficSignalElement;
 using motion_utils::calcLongitudinalOffsetPoint;
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
@@ -225,6 +226,86 @@ 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)
@@ -1902,4 +1983,63 @@ DrivableLanes generateExpandDrivableLanes(
 
   return current_drivable_lanes;
 }
+
+double calcDistanceToAvoidStartLine(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  if (lanelets.empty()) {
+    return std::numeric_limits<double>::lowest();
+  }
+
+  double distance_to_return_dead_line = std::numeric_limits<double>::lowest();
+
+  // dead line stop factor(traffic light)
+  if (parameters->enable_dead_line_for_traffic_light) {
+    const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data);
+    if (to_traffic_light.has_value()) {
+      distance_to_return_dead_line = std::max(
+        distance_to_return_dead_line,
+        to_traffic_light.value() + parameters->dead_line_buffer_for_traffic_light);
+    }
+  }
+
+  return distance_to_return_dead_line;
+}
+
+double calcDistanceToReturnDeadLine(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  if (lanelets.empty()) {
+    return std::numeric_limits<double>::max();
+  }
+
+  double distance_to_return_dead_line = std::numeric_limits<double>::max();
+
+  // dead line stop factor(traffic light)
+  if (parameters->enable_dead_line_for_traffic_light) {
+    const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data);
+    if (to_traffic_light.has_value()) {
+      distance_to_return_dead_line = std::min(
+        distance_to_return_dead_line,
+        to_traffic_light.value() - parameters->dead_line_buffer_for_traffic_light);
+    }
+  }
+
+  // dead line for goal
+  if (parameters->enable_dead_line_for_goal) {
+    if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
+      const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
+      const auto to_goal_distance =
+        calcSignedArcLength(path.points, ego_pos, path.points.size() - 1);
+      distance_to_return_dead_line = std::min(
+        distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal);
+    }
+  }
+
+  return distance_to_return_dead_line;
+}
 }  // namespace behavior_path_planner::utils::avoidance