Planning: enabled collision detection when adc is stopped.

modules/planning/common/ego_info.cc

@@ -30,19 +30,33 @@ using common::math::Vec2d;
 using common::math::Box2d;
 
 EgoInfo::EgoInfo() {
-  common::VehicleConfig ego_vehicle_config_ =
-      common::VehicleConfigHelper::GetConfig();
+  ego_vehicle_config_ = common::VehicleConfigHelper::GetConfig();
 }
 
 bool EgoInfo::Update(const common::TrajectoryPoint& start_point,
-                     const common::VehicleState& vehicle_state,
-                     const std::vector<const Obstacle*>& obstacles) {
+                     const common::VehicleState& vehicle_state) {
   set_start_point(start_point);
   set_vehicle_state(vehicle_state);
-  CalculateFrontObstacleClearDistance(obstacles);
+  CalculateEgoBox(vehicle_state);
   return true;
 }
 
+void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) {
+  const auto& param = ego_vehicle_config_.vehicle_param();
+  AINFO << "param: " << param.DebugString();
+
+  Vec2d vec_to_center(
+      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
+      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);
+
+  Vec2d position(vehicle_state.x(), vehicle_state.y());
+  Vec2d center(position + vec_to_center.rotate(vehicle_state.heading()));
+
+  const double buffer = 0.1;  // in meters
+  ego_box_ = Box2d(center, vehicle_state.heading(), param.length() + buffer,
+                   param.width() + buffer);
+}
+
 void EgoInfo::Clear() {
   start_point_.Clear();
   vehicle_state_.Clear();
@@ -60,15 +74,11 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
 
   Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));
 
-  const double buffer = 0.1;  // in meters
-  Box2d ego_box(center, vehicle_state_.heading(), param.length() + buffer,
-                param.width() + buffer);
-  const double adc_half_diagnal = ego_box.diagonal() / 2.0;
-
   Vec2d unit_vec_heading = Vec2d::CreateUnitVec2d(vehicle_state_.heading());
 
   // Due to the error of ego heading, only short range distance is meaningful
-  const double kDistanceThreshold = 50.0;
+  constexpr double kDistanceThreshold = 50.0;
+  constexpr double buffer = 0.1;  // in meters
   const double impact_region_length =
       param.length() + buffer + kDistanceThreshold;
   Box2d ego_front_region(center + unit_vec_heading * kDistanceThreshold / 2.0,
@@ -81,9 +91,9 @@ void EgoInfo::CalculateFrontObstacleClearDistance(
       continue;
     }
 
-    double dist = ego_box.center().DistanceTo(
+    double dist = ego_box_.center().DistanceTo(
                       obstacle->PerceptionBoundingBox().center()) -
-                  adc_half_diagnal;
+                  ego_box_.diagonal() / 2.0;
 
     if (front_clear_distance_ < 0.0 || dist < front_clear_distance_) {
       front_clear_distance_ = dist;

modules/planning/common/ego_info.h

@@ -44,8 +44,8 @@ class EgoInfo {
   ~EgoInfo() = default;
 
   bool Update(const common::TrajectoryPoint& start_point,
-              const common::VehicleState& vehicle_state,
-              const std::vector<const Obstacle*>& obstacles);
+              const common::VehicleState& vehicle_state);
+
   void Clear();
 
   common::TrajectoryPoint start_point() const { return start_point_; }
@@ -54,6 +54,11 @@ class EgoInfo {
 
   double front_clear_distance() const { return front_clear_distance_; }
 
+  common::math::Box2d ego_box() const { return ego_box_; }
+
+  void CalculateFrontObstacleClearDistance(
+      const std::vector<const Obstacle*>& obstacles);
+
  private:
   FRIEND_TEST(EgoInfoTest, EgoInfoSimpleTest);
 
@@ -65,8 +70,7 @@ class EgoInfo {
     start_point_ = start_point;
   }
 
-  void CalculateFrontObstacleClearDistance(
-      const std::vector<const Obstacle*>& obstacles);
+  void CalculateEgoBox(const common::VehicleState& vehicle_state);
 
   // stitched point (at stitching mode)
   // or real vehicle point (at non-stitching mode)
@@ -79,6 +83,8 @@ class EgoInfo {
 
   common::VehicleConfig ego_vehicle_config_;
 
+  common::math::Box2d ego_box_;
+
   DECLARE_SINGLETON(EgoInfo);
 };
 

modules/planning/common/frame.cc

@@ -36,6 +36,7 @@
 #include "modules/map/hdmap/hdmap_util.h"
 #include "modules/map/pnc_map/path.h"
 #include "modules/map/pnc_map/pnc_map.h"
+#include "modules/planning/common/ego_info.h"
 #include "modules/planning/common/planning_context.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/reference_line/reference_line_provider.h"
@@ -48,6 +49,7 @@ using apollo::common::Status;
 using apollo::common::VehicleStateProvider;
 using apollo::common::math::Box2d;
 using apollo::common::math::Vec2d;
+using apollo::common::math::Polygon2d;
 using apollo::hdmap::HDMapUtil;
 using apollo::hdmap::LaneSegment;
 using apollo::hdmap::ParkingSpaceInfoConstPtr;
@@ -393,10 +395,9 @@ Status Frame::InitFrameData() {
        Obstacle::CreateObstacles(*local_view_.prediction_obstacles)) {
     AddObstacle(*ptr);
   }
-  if (FLAGS_enable_collision_detection) {
-    const auto *collision_obstacle =
-        FindCollisionObstacleWithInExtraRadius(FLAGS_max_collision_distance);
-    if (collision_obstacle) {
+  if (FLAGS_enable_collision_detection && planning_start_point_.v() < 1e-3) {
+    const auto *collision_obstacle = FindCollisionObstacle();
+    if (collision_obstacle != nullptr) {
       std::string err_str =
           "Found collision with obstacle: " + collision_obstacle->Id();
       monitor_logger_buffer_.ERROR(err_str);
@@ -406,53 +407,19 @@ Status Frame::InitFrameData() {
   return Status::OK();
 }
 
-const Obstacle *Frame::FindCollisionObstacleWithInExtraRadius(
-    const double radius) const {
-  CHECK_GT(radius, kMathEpsilon)
-      << "Extra safety radius must be positive number!";
+const Obstacle *Frame::FindCollisionObstacle() const {
   if (obstacles_.Items().empty()) {
     return nullptr;
   }
-  const auto &param =
-      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
-  Vec2d position(vehicle_state_.x(), vehicle_state_.y());
-  Vec2d vec_to_center(
-      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
-      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);
-  Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));
-  Box2d adc_box(center, vehicle_state_.heading(), param.length(),
-                param.width());
-  const double adc_half_diagnal = adc_box.diagonal() / 2.0;
+
+  const auto &adc_polygon = Polygon2d(EgoInfo::Instance()->ego_box());
   for (const auto &obstacle : obstacles_.Items()) {
     if (obstacle->IsVirtual()) {
       continue;
     }
 
-    double center_dist =
-        adc_box.center().DistanceTo(obstacle->PerceptionBoundingBox().center());
-    if (center_dist > obstacle->PerceptionBoundingBox().diagonal() / 2.0 +
-                          adc_half_diagnal + radius) {
-      ADEBUG << "Obstacle : " << obstacle->Id() << " is too far to collide";
-      continue;
-    }
-    double distance = obstacle->PerceptionPolygon().DistanceTo(adc_box);
-    if (FLAGS_ignore_overlapped_obstacle && distance < kMathEpsilon) {
-      bool all_points_in = true;
-      for (const auto &point : obstacle->PerceptionPolygon().points()) {
-        if (!adc_box.IsPointIn(point)) {
-          all_points_in = false;
-          break;
-        }
-      }
-      if (all_points_in) {
-        ADEBUG << "Skip overlapped obstacle, which is often caused by lidar "
-                  "calibration error";
-        continue;
-      }
-    }
-    if (distance < radius) {
-      AERROR << "Found collision with obstacle " << obstacle->Id()
-             << "Within extra safety radius of : " << radius;
+    const auto &obstacle_polygon = obstacle->PerceptionPolygon();
+    if (obstacle_polygon.HasOverlap(adc_polygon)) {
       return obstacle;
     }
   }

modules/planning/common/frame.h

@@ -166,8 +166,7 @@ class Frame {
    * @return pointer to the obstacle if such obstacle exists, otherwise
    * @return false if no colliding obstacle.
    */
-  const Obstacle *FindCollisionObstacleWithInExtraRadius(
-      const double radius) const;
+  const Obstacle *FindCollisionObstacle() const;
 
   /**
    * @brief create a static virtual obstacle

modules/planning/common/speed_profile_generator_test.cc

@@ -36,9 +36,8 @@ TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
   adc_planning_point.set_v(FLAGS_polynomial_speed_fallback_velocity + 0.1);
 
   common::VehicleState vs;
-  const std::vector<const Obstacle*> obstacles;
 
-  EgoInfo::Instance()->Update(adc_planning_point, vs, obstacles);
+  EgoInfo::Instance()->Update(adc_planning_point, vs);
   auto speed_data2 = SpeedProfileGenerator::GenerateFallbackSpeedProfile();
   EXPECT_FALSE(speed_data2.Empty());
 }

modules/planning/conf/planning.conf

@@ -15,6 +15,7 @@
 --enable_scenario_side_pass
 --enable_scenario_stop_sign_unprotected
 --noenable_scenario_traffic_light_right_turn_unprotected
+--enable_collision_detection
 
 # --smoother_config_filename=/apollo/modules/planning/conf/spiral_smoother_config.pb.txt
 # --smoother_config_filename=/apollo/modules/planning/conf/cosTheta_smoother_config.pb.txt

modules/planning/navi_planning.cc

@@ -109,8 +109,7 @@ Status NaviPlanning::InitFrame(const uint32_t sequence_num,
                                ADCTrajectory* output_trajectory) {
   frame_.reset(new Frame(sequence_num, local_view_, planning_start_point,
                          start_time, vehicle_state,
-                         reference_line_provider_.get(),
-                         output_trajectory));
+                         reference_line_provider_.get(), output_trajectory));
 
   std::list<ReferenceLine> reference_lines;
   std::list<hdmap::RouteSegments> segments;
@@ -220,8 +219,7 @@ void NaviPlanning::RunOnce(const LocalView& local_view,
     return;
   }
 
-  EgoInfo::Instance()->Update(stitching_trajectory.back(), vehicle_state,
-                              frame_->obstacles());
+  EgoInfo::Instance()->Update(stitching_trajectory.back(), vehicle_state);
 
   if (FLAGS_enable_record_debug) {
     frame_->RecordInputDebug(trajectory_pb->mutable_debug());

modules/planning/std_planning.cc

@@ -247,10 +247,15 @@ void StdPlanning::RunOnce(const LocalView& local_view,
       last_publishable_trajectory_.get());
 
   const uint32_t frame_num = seq_num_++;
+  bool update_ego_info =
+      EgoInfo::Instance()->Update(stitching_trajectory.back(), vehicle_state);
   status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp,
                      vehicle_state, trajectory_pb);
-  bool update_ego_info = EgoInfo::Instance()->Update(
-      stitching_trajectory.back(), vehicle_state, frame_->obstacles());
+
+  if (update_ego_info && status.ok()) {
+    EgoInfo::Instance()->CalculateFrontObstacleClearDistance(
+        frame_->obstacles());
+  }
 
   if (FLAGS_enable_record_debug) {
     frame_->RecordInputDebug(trajectory_pb->mutable_debug());