diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto index 3288d93f113fd2eb5e2e5327db503783ff557bd7..54a77a1f8367a62f5821a6657a7ee521d74b59b5 100644 --- a/modules/planning/proto/planning_config.proto +++ b/modules/planning/proto/planning_config.proto @@ -136,7 +136,9 @@ message ScenarioTrafficLightUnprotectedRightTurnConfig { optional float creep_timeout = 6 [default = 10.0]; // sec } -message ScenarioValetParkingConfig {} +message ScenarioValetParkingConfig { + optional double parking_spot_range_to_start = 1 [default = 20.0]; +} message ScenarioNarrowStreetUTurnConfig {} diff --git a/modules/planning/scenarios/valet_parking/valet_parking_scenario.cc b/modules/planning/scenarios/valet_parking/valet_parking_scenario.cc index 11d7f078e8c8c9e1e0c374e8d81afee7e5842580..6baad93b2873adb21dbee4653f938a8f3284c9f6 100644 --- a/modules/planning/scenarios/valet_parking/valet_parking_scenario.cc +++ b/modules/planning/scenarios/valet_parking/valet_parking_scenario.cc @@ -17,21 +17,31 @@ /** * @file **/ -#include "modules/planning/scenarios/valet_parking/valet_parking_scenario.h" -#include "modules/planning/scenarios/valet_parking/stage_parking.h" -#include "modules/planning/scenarios/valet_parking/stage_approaching_parking_spot.h" +#include +#include "modules/planning/scenarios/valet_parking/valet_parking_scenario.h" +#include "modules/planning/scenarios/valet_parking/stage_approaching_parking_spot.h" +#include "modules/planning/scenarios/valet_parking/stage_parking.h" namespace apollo { namespace planning { namespace scenario { namespace valet_parking { +using apollo::common::Status; +using apollo::common::VehicleState; +using apollo::common::math::Box2d; +using apollo::common::math::Vec2d; +using apollo::hdmap::HDMapUtil; +using apollo::hdmap::LaneSegment; +using apollo::hdmap::ParkingSpaceInfoConstPtr; +using apollo::hdmap::Path; + apollo::common::util::Factory< - ScenarioConfig::StageType, Stage, - Stage* (*)(const ScenarioConfig::StageConfig& stage_config)> - ValetParkingScenario::s_stage_factory_; + ScenarioConfig::StageType, Stage, + Stage* (*)(const ScenarioConfig::StageConfig& stage_config)> + ValetParkingScenario::s_stage_factory_; void ValetParkingScenario::Init() { if (init_) { @@ -44,6 +54,9 @@ void ValetParkingScenario::Init() { AERROR << "fail to get scenario specific config"; return; } + + hdmap_ = hdmap::HDMapUtil::BaseMapPtr(); + CHECK_NOTNULL(hdmap_); } void ValetParkingScenario::RegisterStages() { @@ -86,9 +99,124 @@ bool ValetParkingScenario::GetScenarioConfig() { bool ValetParkingScenario::IsTransferable(const Scenario& current_scenario, const Frame& frame) { + // TODO(all) Implement avaliable parking spot detection by preception results + context_.target_parking_spot_id.clear(); + if (frame.local_view().routing->routing_request().has_parking_space() && + frame.local_view().routing->routing_request().parking_space().has_id()) { + context_.target_parking_spot_id = + frame.local_view().routing->routing_request().parking_space().id().id(); + } else { + const std::string msg = "No parking space id from routing"; + AERROR << msg; + return false; + } + + if (context_.target_parking_spot_id.empty()) { + return false; + } + + ParkingSpaceInfoConstPtr target_parking_spot = nullptr; + Path nearby_path; + const auto& vehicle_state = frame.vehicle_state(); + auto point = common::util::MakePointENU(vehicle_state.x(), vehicle_state.y(), + vehicle_state.z()); + hdmap::LaneInfoConstPtr nearest_lane; + double vehicle_lane_s = 0.0; + double vehicle_lane_l = 0.0; + int status = HDMapUtil::BaseMap().GetNearestLaneWithHeading( + point, 5.0, vehicle_state.heading(), M_PI / 3.0, &nearest_lane, + &vehicle_lane_s, &vehicle_lane_l); + if (status != 0) { + AERROR << "GetNearestLaneWithHeading failed at IsTransferable() of valet " + "parking scenario"; + return false; + } + // TODO(Jinyun) Take path from reference line + LaneSegment nearest_lanesegment = + LaneSegment(nearest_lane, nearest_lane->accumulate_s().front(), + nearest_lane->accumulate_s().back()); + std::vector segments_vector; + int next_lanes_num = nearest_lane->lane().successor_id_size(); + if (next_lanes_num != 0) { + for (int i = 0; i < next_lanes_num; ++i) { + auto next_lane_id = nearest_lane->lane().successor_id(i); + segments_vector.push_back(nearest_lanesegment); + auto next_lane = hdmap_->GetLaneById(next_lane_id); + LaneSegment next_lanesegment = + LaneSegment(next_lane, next_lane->accumulate_s().front(), + next_lane->accumulate_s().back()); + segments_vector.emplace_back(next_lanesegment); + nearby_path = Path(segments_vector); + SearchTargetParkingSpotOnPath(nearby_path, &target_parking_spot); + if (target_parking_spot != nullptr) { + break; + } + } + } else { + segments_vector.push_back(nearest_lanesegment); + nearby_path = Path(segments_vector); + SearchTargetParkingSpotOnPath(nearby_path, &target_parking_spot); + } + + if (target_parking_spot == nullptr) { + std::string msg( + "No such parking spot found after searching all path forward possible"); + AERROR << msg << context_.target_parking_spot_id; + return false; + } + + if (!CheckDistanceToParkingSpot(vehicle_state, nearby_path, + target_parking_spot)) { + std::string msg( + "target parking spot found, but too far, distance larger than " + "pre-defined distance"); + AERROR << msg << context_.target_parking_spot_id; + return false; + } + return true; } +void ValetParkingScenario::SearchTargetParkingSpotOnPath( + const Path& nearby_path, ParkingSpaceInfoConstPtr* target_parking_spot) { + const auto& parking_space_overlaps = nearby_path.parking_space_overlaps(); + if (parking_space_overlaps.size() != 0) { + for (const auto& parking_overlap : parking_space_overlaps) { + if (parking_overlap.object_id == context_.target_parking_spot_id) { + hdmap::Id id; + id.set_id(parking_overlap.object_id); + *target_parking_spot = hdmap_->GetParkingSpaceById(id); + } + } + } +} + +bool ValetParkingScenario::CheckDistanceToParkingSpot( + const VehicleState& vehicle_state, const Path& nearby_path, + const ParkingSpaceInfoConstPtr& target_parking_spot) { + Vec2d left_bottom_point = target_parking_spot->polygon().points().at(0); + Vec2d right_bottom_point = target_parking_spot->polygon().points().at(1); + double left_bottom_point_s = 0.0; + double left_bottom_point_l = 0.0; + double right_bottom_point_s = 0.0; + double right_bottom_point_l = 0.0; + double vehicle_point_s = 0.0; + double vehicle_point_l = 0.0; + nearby_path.GetNearestPoint(left_bottom_point, &left_bottom_point_s, + &left_bottom_point_l); + nearby_path.GetNearestPoint(right_bottom_point, &right_bottom_point_s, + &right_bottom_point_l); + Vec2d vehicle_vec(vehicle_state.x(), vehicle_state.y()); + nearby_path.GetNearestPoint(vehicle_vec, &vehicle_point_s, &vehicle_point_l); + if (std::abs((left_bottom_point_s + right_bottom_point_s) / 2 - + vehicle_point_s) < + context_.scenario_config.parking_spot_range_to_start()) { + return true; + } else { + return false; + } +} + } // namespace valet_parking } // namespace scenario } // namespace planning diff --git a/modules/planning/scenarios/valet_parking/valet_parking_scenario.h b/modules/planning/scenarios/valet_parking/valet_parking_scenario.h index 96d4ca3fbba322157a1c1993767c556007bc29e4..8ae94603044984272a6c541308b2a607067c4976 100644 --- a/modules/planning/scenarios/valet_parking/valet_parking_scenario.h +++ b/modules/planning/scenarios/valet_parking/valet_parking_scenario.h @@ -23,6 +23,10 @@ #include #include +#include "modules/map/hdmap/hdmap_util.h" +#include "modules/map/pnc_map/path.h" +#include "modules/map/pnc_map/pnc_map.h" +#include "modules/map/proto/map_id.pb.h" #include "modules/planning/scenarios/scenario.h" namespace apollo { @@ -54,6 +58,12 @@ class ValetParkingScenario : public Scenario { private: static void RegisterStages(); bool GetScenarioConfig(); + void SearchTargetParkingSpotOnPath( + const hdmap::Path& nearby_path, + hdmap::ParkingSpaceInfoConstPtr* target_parking_spot); + bool CheckDistanceToParkingSpot( + const common::VehicleState& vehicle_state, const hdmap::Path& nearby_path, + const hdmap::ParkingSpaceInfoConstPtr& target_parking_spot); private: bool init_ = false; @@ -62,6 +72,7 @@ class ValetParkingScenario : public Scenario { Stage* (*)(const ScenarioConfig::StageConfig& stage_config)> s_stage_factory_; ValetParkingContext context_; + const hdmap::HDMap* hdmap_ = nullptr; }; } // namespace valet_parking diff --git a/modules/planning/tasks/deciders/open_space_roi_decider.cc b/modules/planning/tasks/deciders/open_space_roi_decider.cc index 2549f11bb511fbb610c707592d1c288fa7a11769..4251d86b44c512786ef66532bb8ec78497c6f31c 100644 --- a/modules/planning/tasks/deciders/open_space_roi_decider.cc +++ b/modules/planning/tasks/deciders/open_space_roi_decider.cc @@ -101,9 +101,10 @@ bool OpenSpaceRoiDecider::VPresentationObstacle() { // last to form closed convex hull) for (const auto &obstacle : obstacles_by_frame_->Items()) { Box2d original_box = obstacle->PerceptionBoundingBox(); - original_box.Shift(-1.0 * - *(frame_->mutable_open_space_info()->mutable_origin_point())); - original_box.RotateFromCenter(-1.0 * + original_box.Shift( + -1.0 * *(frame_->mutable_open_space_info()->mutable_origin_point())); + original_box.RotateFromCenter( + -1.0 * *(frame_->mutable_open_space_info()->mutable_origin_heading())); std::vector vertices_ccw = original_box.GetAllCorners(); std::vector vertices_cw; @@ -164,8 +165,7 @@ bool OpenSpaceRoiDecider::HPresentationObstacle() { } bool OpenSpaceRoiDecider::ObsHRep( - const size_t &obstacles_num, - const Eigen::MatrixXi &obstacles_edges_num, + const size_t &obstacles_num, const Eigen::MatrixXi &obstacles_edges_num, const std::vector> &obstacles_vertices_vec, Eigen::MatrixXd *A_all, Eigen::MatrixXd *b_all) { if (obstacles_num != obstacles_vertices_vec.size()) { @@ -464,6 +464,7 @@ void OpenSpaceRoiDecider::SearchTargetParkingSpotOnPath( } } +// TODO(Jinyun) Deprecate because of duplicate code in valet parking scenario bool OpenSpaceRoiDecider::GetMapInfo( ParkingSpaceInfoConstPtr *target_parking_spot, std::shared_ptr *nearby_path) { @@ -473,16 +474,14 @@ bool OpenSpaceRoiDecider::GetMapInfo( double vehicle_lane_s = 0.0; double vehicle_lane_l = 0.0; int status = HDMapUtil::BaseMap().GetNearestLaneWithHeading( - point, 10.0, vehicle_state_.heading(), - M_PI / 2.0, &nearest_lane, + point, 10.0, vehicle_state_.heading(), M_PI / 2.0, &nearest_lane, &vehicle_lane_s, &vehicle_lane_l); if (status != 0) { AERROR << "Getlane failed at OpenSpaceRoiDecider::GetOpenSpaceROI()"; return false; } LaneSegment nearest_lanesegment = - LaneSegment(nearest_lane, - nearest_lane->accumulate_s().front(), + LaneSegment(nearest_lane, nearest_lane->accumulate_s().front(), nearest_lane->accumulate_s().back()); std::vector segments_vector; int next_lanes_num = nearest_lane->lane().successor_id_size(); @@ -492,8 +491,7 @@ bool OpenSpaceRoiDecider::GetMapInfo( segments_vector.push_back(nearest_lanesegment); auto next_lane = hdmap_->GetLaneById(next_lane_id); LaneSegment next_lanesegment = - LaneSegment(next_lane, - next_lane->accumulate_s().front(), + LaneSegment(next_lane, next_lane->accumulate_s().front(), next_lane->accumulate_s().back()); segments_vector.emplace_back(next_lanesegment); (*nearby_path).reset(new Path(segments_vector));