BaiduApollo代码解析之碰撞检测

//construct a bounding box according to obstacle's length and width got from Perception module Box2d obstacle_box = obstacle->GetBoundingBox(obtacle_point); //if there is overlap between obstacle and ego vehicle, return if (ego_rlap(obstacle_box)) { return true; } } } return false;} /** * @brief Check if there are overlaps between obstacles and ego vehicle according to the predicted obstacles environment * @pre BuildPredictedEnvironment() is called before this, so that predicted_bounding_rectangles_ is ready * @param discretized_trajectory ego vehicle's trajectory sampling points * @return true if collision */bool CollisionChecker::InCollision( const DiscretizedTrajectory& discretized_trajectory) { CHECK_LE(discretized_oints(), predicted_bounding_rectangles_.size()); const auto& vehicle_config = common::VehicleConfigHelper::Instance()->GetConfig(); double ego_length = vehicle_e_param().length(); double ego_width = vehicle_e_param().width(); //traverse every point on ego vehicle's trajectory for (size_t i = 0; i < discretized_oints(); ++i) { const auto& trajectory_point = discretized_toryPointAt(static_cast(i)); double ego_theta = trajectory__point().theta(); //construct a bounding box according to ego vehicle's length and width got from config Box2d ego_box( {trajectory__point().x(), trajectory__point().y()}, ego_theta, ego_length, ego_width); double shift_distance = ego_length / 2.0 - vehicle_e_param().back_edge_to_center(); Vec2d shift_vec{shift_distance * std::cos(ego_theta), shift_distance * std::sin(ego_theta)}; ego_(shift_vec); //traverse every obstacle's bounding box stored in predicted_bounding_rectangles_ for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) { if (ego_rlap(obstacle_box)) { return true; } } } return false;} /** * @brief choose obstacles of interest and build bounding boxes for every obstacle's every trajectory point * @param obstacles * @param ego_vehicle_s * @param ego_vehicle_d * @param discretized_reference_line */void CollisionChecker::BuildPredictedEnvironment( const std::vector& obstacles, const double ego_vehicle_s, const double ego_vehicle_d, const std::vector& discretized_reference_line) { CHECK(predicted_bounding_rectangles_.empty()); // If the ego vehicle is in lane, // then, ignore all obstacles from the same lane behind of the ego vehicle. bool ego_vehicle_in_lane = IsEgoVehicleInLane(ego_vehicle_s, ego_vehicle_d); std::vector obstacles_considered;