Planning: record whether an st-obstacle starts at a adc's low road-ri…

…ght segment and related info.

modules/planning/common/speed/st_boundary.cc

@@ -20,6 +20,8 @@
 
 #include "modules/planning/common/speed/st_boundary.h"
 
+#include <limits>
+
 #include "cyber/common/log.h"
 #include "modules/common/math/math_utils.h"
 #include "modules/planning/common/planning_gflags.h"
@@ -61,6 +63,8 @@ STBoundary::STBoundary(
   }
   min_t_ = lower_points_.front().t();
   max_t_ = lower_points_.back().t();
+
+  obstacle_road_right_ending_t_ = std::numeric_limits<double>::lowest();
 }
 
 STBoundary::STBoundary(
@@ -97,6 +101,8 @@ STBoundary::STBoundary(
   }
   min_t_ = lower_points_.front().t();
   max_t_ = lower_points_.back().t();
+
+  obstacle_road_right_ending_t_ = std::numeric_limits<double>::lowest();
 }
 
 STBoundary STBoundary::CreateInstance(

modules/planning/common/speed/st_boundary.h

@@ -128,6 +128,13 @@ class STBoundary : public common::math::Polygon2d {
   void set_bottom_left_point(STPoint st_point);
   void set_bottom_right_point(STPoint st_point);
 
+  void set_obstacle_road_right_ending_t(double road_right_ending_t) {
+    obstacle_road_right_ending_t_ = road_right_ending_t;
+  }
+  double obstacle_road_right_ending_t() const {
+    return obstacle_road_right_ending_t_;
+  }
+
  private:
   /** @brief The sanity check function for a vector of ST-point pairs.
    */
@@ -175,6 +182,8 @@ class STBoundary : public common::math::Polygon2d {
   STPoint bottom_right_point_;
   STPoint upper_left_point_;
   STPoint upper_right_point_;
+
+  double obstacle_road_right_ending_t_;
 };
 
 }  // namespace planning

modules/planning/tasks/deciders/st_bounds_decider/st_obstacles_processor.cc

@@ -91,6 +91,27 @@ Status STObstaclesProcessor::MapObstaclesToSTBoundaries(
   }
   obs_id_to_st_boundary_.clear();
 
+  // Some preprocessing to save the adc_low_road_right segments.
+  bool is_adc_low_road_right_beginning = true;
+  for (const auto& path_pt_info : path_data_.path_point_decision_guide()) {
+    double path_pt_s = 0.0;
+    PathData::PathPointType path_pt_type;
+    std::tie(path_pt_s, path_pt_type, std::ignore) = path_pt_info;
+    if (path_pt_type == PathData::PathPointType::OUT_ON_FORWARD_LANE ||
+        path_pt_type == PathData::PathPointType::OUT_ON_REVERSE_LANE) {
+      if (is_adc_low_road_right_beginning) {
+        adc_low_road_right_segments_.emplace_back(path_pt_s, path_pt_s);
+        is_adc_low_road_right_beginning = false;
+      } else {
+        adc_low_road_right_segments_.back().second = path_pt_s;
+      }
+    } else if (path_pt_type == PathData::PathPointType::IN_LANE) {
+      if (!is_adc_low_road_right_beginning) {
+        is_adc_low_road_right_beginning = true;
+      }
+    }
+  }
+
   // Map obstacles into ST-graph.
   // Go through every obstacle and plot them in ST-graph.
   std::tuple<std::string, STBoundary, Obstacle*> closest_stop_obstacle;
@@ -104,13 +125,19 @@ Status STObstaclesProcessor::MapObstaclesToSTBoundaries(
 
     std::vector<STPoint> lower_points;
     std::vector<STPoint> upper_points;
-    if (!ComputeObstacleSTBoundary(*obs_ptr, &lower_points, &upper_points)) {
+    bool is_caution_obstacle = false;
+    double obs_caution_end_t = 0.0;
+    if (!ComputeObstacleSTBoundary(*obs_ptr, &lower_points, &upper_points,
+                                   &is_caution_obstacle, &obs_caution_end_t)) {
       // Obstacle doesn't appear on ST-Graph.
       continue;
     }
     auto boundary =
         STBoundary::CreateInstanceAccurate(lower_points, upper_points);
     boundary.set_id(obs_ptr->Id());
+    if (is_caution_obstacle) {
+      boundary.set_obstacle_road_right_ending_t(obs_caution_end_t);
+    }
     if (obs_ptr->Trajectory().trajectory_point().empty()) {
       // Obstacle is static.
       if (std::get<0>(closest_stop_obstacle) == "NULL" ||
@@ -332,9 +359,11 @@ void STObstaclesProcessor::SetObstacleDecision(
 
 bool STObstaclesProcessor::ComputeObstacleSTBoundary(
     const Obstacle& obstacle, std::vector<STPoint>* const lower_points,
-    std::vector<STPoint>* const upper_points) {
+    std::vector<STPoint>* const upper_points,
+    bool* const is_caution_obstacle, double* const obs_caution_end_t) {
   lower_points->clear();
   upper_points->clear();
+  *is_caution_obstacle = false;
   const auto& adc_path_points = path_data_.discretized_path();
   const auto& obs_trajectory = obstacle.Trajectory();
 
@@ -360,6 +389,7 @@ bool STObstaclesProcessor::ComputeObstacleSTBoundary(
     // Processing a dynamic obstacle.
     // Go through every occurrence of the obstacle at all timesteps, and
     // figure out the overlapping s-max and s-min one by one.
+    bool is_obs_first_traj_pt = true;
     for (const auto& obs_traj_pt : obs_trajectory.trajectory_point()) {
       // TODO(jiacheng): Currently, if the obstacle overlaps with ADC at
       // disjoint segments (happens very rarely), we merge them into one.
@@ -373,7 +403,20 @@ bool STObstaclesProcessor::ComputeObstacleSTBoundary(
                                    obs_traj_pt.relative_time());
         upper_points->emplace_back(overlapping_s.second,
                                    obs_traj_pt.relative_time());
+        if (is_obs_first_traj_pt) {
+          if (IsSWithinADCLowRoadRightSegment(overlapping_s.first) ||
+              IsSWithinADCLowRoadRightSegment(overlapping_s.second)) {
+            *is_caution_obstacle = true;
+          }
+        }
+        if ((*is_caution_obstacle)) {
+          if (IsSWithinADCLowRoadRightSegment(overlapping_s.first) ||
+              IsSWithinADCLowRoadRightSegment(overlapping_s.second)) {
+            *obs_caution_end_t = obs_traj_pt.relative_time();
+          }
+        }
       }
+      is_obs_first_traj_pt = false;
     }
     if (lower_points->size() == 1) {
       lower_points->emplace_back(lower_points->front().s(),
@@ -578,5 +621,15 @@ ObjectDecisionType STObstaclesProcessor::DetermineObstacleDecision(
   return decision;
 }
 
+bool STObstaclesProcessor::IsSWithinADCLowRoadRightSegment(
+    const double s) const {
+  for (const auto& seg : adc_low_road_right_segments_) {
+    if (s >= seg.first && s <= seg.second) {
+      return true;
+    }
+  }
+  return false;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/deciders/st_bounds_decider/st_obstacles_processor.h

@@ -43,6 +43,7 @@ namespace planning {
 
 constexpr double kADCSafetyLBuffer = 0.1;
 constexpr double kSIgnoreThreshold = 0.01;
+constexpr double kOvertakenObsCautionTime = 0.5;
 
 class STObstaclesProcessor {
  public:
@@ -103,7 +104,9 @@ class STObstaclesProcessor {
    */
   bool ComputeObstacleSTBoundary(const Obstacle& obstacle,
                                  std::vector<STPoint>* const lower_points,
-                                 std::vector<STPoint>* const upper_points);
+                                 std::vector<STPoint>* const upper_points,
+                                 bool* const is_caution_obstacle,
+                                 double* const obs_caution_end_t);
 
   /** @brief Given ADC's path and an obstacle instance at a certain timestep,
    * get the upper and lower s that ADC might overlap with the obs instance.
@@ -182,6 +185,12 @@ class STObstaclesProcessor {
                                                const double obs_s_max,
                                                const double s) const;
 
+  /** @brief Check if a given s falls within adc's low road right segment.
+    * @param A certain S.
+    * @return True if within; false otherwise.
+    */
+  bool IsSWithinADCLowRoadRightSegment(const double s) const;
+
  private:
   double planning_time_;
   double planning_distance_;
@@ -197,6 +206,8 @@ class STObstaclesProcessor {
 
   std::unordered_map<std::string, STBoundary> obs_id_to_st_boundary_;
   std::unordered_map<std::string, ObjectDecisionType> obs_id_to_decision_;
+
+  std::vector<std::pair<double, double>> adc_low_road_right_segments_;
 };
 
 }  // namespace planning