planning: add nudge buffer for non-static obstacles when constructing…

… ST-boundary
qxdxx · Aug 15, 2019 · 8dec3e3 · 8dec3e3
1 parent 201e63c
commit 8dec3e3
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 10 deletions.
diff --git a/modules/planning/common/obstacle.cc b/modules/planning/common/obstacle.cc
@@ -459,13 +459,15 @@ bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
       if (!has_low) {
         auto low_ref = reference_line.GetReferencePoint(low_s);
         has_low = object_moving_box.HasOverlap(
-            {low_ref, low_ref.heading(), adc_length, adc_width});
+            {low_ref, low_ref.heading(),
+             adc_length, adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});
         low_s += st_boundary_delta_s;
       }
       if (!has_high) {
         auto high_ref = reference_line.GetReferencePoint(high_s);
         has_high = object_moving_box.HasOverlap(
-            {high_ref, high_ref.heading(), adc_length, adc_width});
+            {high_ref, high_ref.heading(),
+             adc_length, adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});
         high_s -= st_boundary_delta_s;
       }
     }

diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc
@@ -235,7 +235,9 @@ DEFINE_bool(enable_alwasy_stop_for_pedestrian, true,
 DEFINE_bool(enable_side_radar, false,
             "If there is no radar on the side,ignore it");
 DEFINE_double(static_obstacle_nudge_l_buffer, 0.3,
-              "minimum l-distance to nudge an obstacle (meters)");
+              "minimum l-distance to nudge a static obstacle (meters)");
+DEFINE_double(nonstatic_obstacle_nudge_l_buffer, 0.4,
+              "minimum l-distance to nudge a non-static obstacle (meters)");
 DEFINE_double(lateral_ignore_buffer, 3.0,
               "If an obstacle's lateral distance is further away than this "
               "distance, ignore it");

diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h
@@ -117,6 +117,7 @@ DECLARE_bool(enable_nudge_decision);
 DECLARE_bool(enable_nudge_slowdown);
 DECLARE_bool(enable_alwasy_stop_for_pedestrian);
 DECLARE_double(static_obstacle_nudge_l_buffer);
+DECLARE_double(nonstatic_obstacle_nudge_l_buffer);
 DECLARE_double(lateral_ignore_buffer);
 DECLARE_double(min_stop_distance_obstacle);
 DECLARE_double(max_stop_distance_obstacle);

diff --git a/modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.cc b/modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.cc
@@ -207,7 +207,7 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints(
       const Box2d& obs_box = obstacle.PerceptionBoundingBox();
 
       if (CheckOverlap(curr_point_on_path, obs_box,
-                       speed_bounds_config_.boundary_buffer())) {
+                       FLAGS_nonstatic_obstacle_nudge_l_buffer)) {
         const double backward_distance = -vehicle_param_.front_edge_to_center();
         const double forward_distance = obs_box.length();
         double low_s =
@@ -253,7 +253,7 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints(
         const auto curr_adc_path_point =
             discretized_path.Evaluate(path_s + discretized_path.front().s());
         if (CheckOverlap(curr_adc_path_point, obs_box,
-                         speed_bounds_config_.boundary_buffer())) {
+                         FLAGS_nonstatic_obstacle_nudge_l_buffer)) {
           // found overlap, start searching with higher resolution
           const double backward_distance = -step_length;
           const double forward_distance = vehicle_param_.length() +
@@ -277,7 +277,7 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints(
               const auto& point_low = discretized_path.Evaluate(
                   low_s + discretized_path.front().s());
               if (!CheckOverlap(point_low, obs_box,
-                                speed_bounds_config_.boundary_buffer())) {
+                                FLAGS_nonstatic_obstacle_nudge_l_buffer)) {
                 low_s += fine_tuning_step_length;
               } else {
                 find_low = true;
@@ -287,7 +287,7 @@ bool STBoundaryMapper::GetOverlapBoundaryPoints(
               const auto& point_high = discretized_path.Evaluate(
                   high_s + discretized_path.front().s());
               if (!CheckOverlap(point_high, obs_box,
-                                speed_bounds_config_.boundary_buffer())) {
+                                FLAGS_nonstatic_obstacle_nudge_l_buffer)) {
                 high_s -= fine_tuning_step_length;
               } else {
                 find_high = true;
@@ -366,7 +366,7 @@ void STBoundaryMapper::ComputeSTBoundaryWithDecision(
 
 bool STBoundaryMapper::CheckOverlap(const PathPoint& path_point,
                                     const Box2d& obs_box,
-                                    const double buffer) const {
+                                    const double l_buffer) const {
   Vec2d ego_center_map_frame((vehicle_param_.front_edge_to_center() -
                               vehicle_param_.back_edge_to_center()) *
                                  0.5,
@@ -379,7 +379,7 @@ bool STBoundaryMapper::CheckOverlap(const PathPoint& path_point,
   ego_center_map_frame.set_y(ego_center_map_frame.y() + path_point.y());
 
   Box2d adc_box(ego_center_map_frame, path_point.theta(),
-                vehicle_param_.length(), vehicle_param_.width());
+                vehicle_param_.length(), vehicle_param_.width() + l_buffer);
   return obs_box.HasOverlap(adc_box);
 }
 

diff --git a/modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.h b/modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.h
@@ -52,7 +52,7 @@ class STBoundaryMapper {
   FRIEND_TEST(StBoundaryMapperTest, check_overlap_test);
   bool CheckOverlap(const common::PathPoint& path_point,
                     const common::math::Box2d& obs_box,
-                    const double buffer) const;
+                    const double l_buffer) const;
 
   /**
    * Creates valid st boundary upper_points and lower_points