diff --git a/modules/planning/common/open_space_info.h b/modules/planning/common/open_space_info.h index 481b7843985c30d27b7d2895402dc71863b70a4f..16e449231ed818fd4991808de18596828f47cc8f 100644 --- a/modules/planning/common/open_space_info.h +++ b/modules/planning/common/open_space_info.h @@ -65,14 +65,6 @@ class OpenSpaceInfo { OpenSpaceInfo() = default; ~OpenSpaceInfo() = default; - bool open_space_pre_stop_finished() const { - return open_space_pre_stop_finished_; - } - - void set_open_space_pre_stop_finished(const bool flag) { - open_space_pre_stop_finished_ = flag; - } - const std::string target_parking_spot_id() const { return target_parking_spot_id_; } @@ -308,9 +300,6 @@ class OpenSpaceInfo { void RecordDebug(apollo::planning_internal::Debug *ptr_debug); private: - // @brief vehicle needs to stop first in open space related scenarios - bool open_space_pre_stop_finished_ = true; - std::string target_parking_spot_id_; hdmap::ParkingSpaceInfoConstPtr target_parking_spot_ = nullptr; diff --git a/modules/planning/conf/scenario/valet_parking_config.pb.txt b/modules/planning/conf/scenario/valet_parking_config.pb.txt index 73405d088f5541de569ac50cdc41292927da9926..b7bd2781d28c178a99a4918520616cfd9808dc18 100644 --- a/modules/planning/conf/scenario/valet_parking_config.pb.txt +++ b/modules/planning/conf/scenario/valet_parking_config.pb.txt @@ -25,6 +25,9 @@ stage_config: { task_type: PIECEWISE_JERK_SPEED_OPTIMIZER task_config: { task_type: OPEN_SPACE_PRE_STOP_DECIDER + open_space_pre_stop_decider_config { + stop_type: PARKING + } } task_config: { task_type: PATH_LANE_BORROW_DECIDER diff --git a/modules/planning/proto/BUILD b/modules/planning/proto/BUILD index 5857532923009c4dab3c37488aec02152a2dee74..8fcfd0aca37fdd091a50e9228aaa47c48ff0bd17 100644 --- a/modules/planning/proto/BUILD +++ b/modules/planning/proto/BUILD @@ -159,6 +159,7 @@ proto_library( ":navi_path_decider_config_proto_lib", ":navi_speed_decider_config_proto_lib", ":open_space_fallback_decider_config_proto_lib", + ":open_space_pre_stop_decider_config_proto_lib", ":open_space_roi_decider_config_proto_lib", ":open_space_trajectory_partition_config_proto_lib", ":open_space_trajectory_provider_config_proto_lib", @@ -537,6 +538,20 @@ cc_proto_library( ], ) +proto_library( + name = "open_space_pre_stop_decider_config_proto_lib", + srcs = [ + "open_space_pre_stop_decider_config.proto", + ], +) + +cc_proto_library( + name = "open_space_pre_stop_decider_config_proto", + deps = [ + ":open_space_pre_stop_decider_config_proto_lib", + ], +) + cc_proto_library( name = "path_decider_info_proto", deps = [ diff --git a/modules/planning/proto/decider_config.proto b/modules/planning/proto/decider_config.proto index 8e768dd99fbf1389eea5c5a59f342e8f2498fc00..d3f02f2f05ed8591069f972128902218048d3e60 100644 --- a/modules/planning/proto/decider_config.proto +++ b/modules/planning/proto/decider_config.proto @@ -17,11 +17,6 @@ message CreepDeciderConfig { optional double ignore_min_st_min_s = 6 [default = 15.0]; // meter } -message OpenSpacePreStopDeciderConfig { - optional double rightaway_stop_distance = 1 [default = 2.0]; // meter - optional double stop_distance_to_target = 2 [default = 5.0]; // second -} - message SidePassSafetyConfig { // min lateral distance(m) for safe obstacle optional double min_obstacle_lateral_distance = 1 [default = 1.0]; // meter diff --git a/modules/planning/proto/open_space_fallback_decider_config.proto b/modules/planning/proto/open_space_fallback_decider_config.proto index d53fa2a05c48dd291498bbe53795dc02cca14247..23994370d8daa042abb40b3f6c07e4051b427712 100644 --- a/modules/planning/proto/open_space_fallback_decider_config.proto +++ b/modules/planning/proto/open_space_fallback_decider_config.proto @@ -9,4 +9,4 @@ message OpenSpaceFallBackDeciderConfig { optional double open_space_fall_back_collision_distance = 2 [default = 5.0]; // fallback stop trajectory safety gap to obstacles optional double open_space_fall_back_stop_distance = 3 [default = 2.0]; -} \ No newline at end of file +} diff --git a/modules/planning/proto/open_space_pre_stop_decider_config.proto b/modules/planning/proto/open_space_pre_stop_decider_config.proto new file mode 100644 index 0000000000000000000000000000000000000000..3a9118f27d2c15e98d96233f0a27d4f7d9d78d38 --- /dev/null +++ b/modules/planning/proto/open_space_pre_stop_decider_config.proto @@ -0,0 +1,16 @@ +syntax = "proto2"; + +package apollo.planning; + +message OpenSpacePreStopDeciderConfig { + // roi scenario definitions + enum StopType { + NOT_DEFINED = 0; + PARKING = 1; + PULL_OVER = 2; + NARROW_STREET_U_TURN = 3; + } + optional StopType stop_type = 1; + optional double rightaway_stop_distance = 2 [default = 2.0]; // meter + optional double stop_distance_to_target = 3 [default = 5.0]; // second +} diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto index 13ffbd1409ba9f3ee64fef8d7172d3568db4ea33..2dea29d8cd1d2de261bc3db27428412e30822a26 100644 --- a/modules/planning/proto/planning_config.proto +++ b/modules/planning/proto/planning_config.proto @@ -6,6 +6,7 @@ import "modules/planning/proto/decider_config.proto"; import "modules/planning/proto/dp_st_speed_config.proto"; import "modules/planning/proto/lane_change_decider_config.proto"; import "modules/planning/proto/open_space_fallback_decider_config.proto"; +import "modules/planning/proto/open_space_pre_stop_decider_config.proto"; import "modules/planning/proto/open_space_roi_decider_config.proto"; import "modules/planning/proto/open_space_trajectory_provider_config.proto"; import "modules/planning/proto/open_space_trajectory_partition_config.proto"; diff --git a/modules/planning/scenarios/park/valet_parking/stage_approaching_parking_spot.cc b/modules/planning/scenarios/park/valet_parking/stage_approaching_parking_spot.cc index a9fdfce32553a1cc5bdc0640dcae69ae74ae2144..76780bb61723bc983509002468b9bebb50c73333 100644 --- a/modules/planning/scenarios/park/valet_parking/stage_approaching_parking_spot.cc +++ b/modules/planning/scenarios/park/valet_parking/stage_approaching_parking_spot.cc @@ -50,8 +50,6 @@ Stage::StageStatus StageApproachingParkingSpot::Process( return StageStatus::ERROR; } - frame->mutable_open_space_info()->set_open_space_pre_stop_finished( - GetContext()->valet_parking_pre_stop_finished); *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) = GetContext()->target_parking_spot_id; frame->mutable_open_space_info()->set_pre_stop_rightaway_flag( @@ -100,8 +98,6 @@ bool StageApproachingParkingSpot::CheckADCStop(const Frame& frame) { ADEBUG << "not a valid stop. too far from stop line."; return false; } - - GetContext()->valet_parking_pre_stop_finished = true; return true; } diff --git a/modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h b/modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h index 76b4925abdb45a46a168dd004ca8784c3534cb42..52043a98ea7350746661a83a360fc3f4d52045d8 100644 --- a/modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h +++ b/modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h @@ -37,7 +37,6 @@ namespace valet_parking { struct ValetParkingContext { ScenarioValetParkingConfig scenario_config; std::string target_parking_spot_id; - bool valet_parking_pre_stop_finished = false; bool pre_stop_rightaway_flag = false; hdmap::MapPathPoint pre_stop_rightaway_point; }; diff --git a/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.cc b/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.cc index e492ecc40e4e7f7bc4147fb029b92489ff224c74..2a99ab9b72f69d00c89133ea3140d154e2e994b8 100644 --- a/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.cc +++ b/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.cc @@ -26,12 +26,14 @@ #include "modules/common/vehicle_state/vehicle_state_provider.h" #include "modules/map/pnc_map/path.h" +#include "modules/planning/common/planning_context.h" #include "modules/planning/common/util/common.h" namespace apollo { namespace planning { using apollo::common::Status; +using apollo::common::ErrorCode; using apollo::common::VehicleState; using apollo::common::math::Vec2d; using apollo::hdmap::ParkingSpaceInfoConstPtr; @@ -46,27 +48,54 @@ Status OpenSpacePreStopDecider::Process( Frame* frame, ReferenceLineInfo* reference_line_info) { CHECK_NOTNULL(frame); CHECK_NOTNULL(reference_line_info); - - CheckOpenSpacePreStop(frame, reference_line_info); - + open_space_pre_stop_decider_config_ = + config_.open_space_pre_stop_decider_config(); + double target_s = 0.0; + const auto& stop_type = open_space_pre_stop_decider_config_.stop_type(); + switch (stop_type) { + case OpenSpacePreStopDeciderConfig::PARKING: + if (!CheckParkingSpotPreStop(frame, reference_line_info, &target_s)) { + const std::string msg = "Checking parking spot pre stop fails"; + AERROR << msg; + return Status(ErrorCode::PLANNING_ERROR, msg); + } + break; + case OpenSpacePreStopDeciderConfig::PULL_OVER: + if (!CheckPullOverPreStop(frame, reference_line_info, &target_s)) { + const std::string msg = "Checking pull over pre stop fails"; + AERROR << msg; + return Status(ErrorCode::PLANNING_ERROR, msg); + } + break; + default: + const std::string msg = "This stop type not implemented"; + AERROR << msg; + return Status(ErrorCode::PLANNING_ERROR, msg); + } + SetStopFence(target_s, frame, reference_line_info); return Status::OK(); } -void OpenSpacePreStopDecider::CheckOpenSpacePreStop( - Frame* const frame, ReferenceLineInfo* const reference_line_info) { - const auto& open_space_config = config_.open_space_pre_stop_decider_config(); - - if (frame->open_space_info().open_space_pre_stop_finished()) { - return; - } +bool OpenSpacePreStopDecider::CheckPullOverPreStop( + Frame* const frame, ReferenceLineInfo* const reference_line_info, + double* target_s) { + const auto& pull_over_info = + PlanningContext::Instance()->planning_status().pull_over(); + const auto& pull_over_s = pull_over_info.pull_over_s(); + *target_s = pull_over_s; + return true; +} - const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); - const VehicleState& vehicle_state = frame->vehicle_state(); +bool OpenSpacePreStopDecider::CheckParkingSpotPreStop( + Frame* const frame, ReferenceLineInfo* const reference_line_info, + double* target_s) { const auto& target_parking_spot_id = frame->open_space_info().target_parking_spot_id(); const auto& nearby_path = reference_line_info->reference_line().map_path(); - AERROR_IF(target_parking_spot_id.empty()) - << "no target parking spot id found when setting pre stop fence"; + if (target_parking_spot_id.empty()) { + AERROR << "no target parking spot id found when setting pre stop fence"; + return false; + } double target_area_center_s = 0.0; bool target_area_found = false; @@ -97,24 +126,38 @@ void OpenSpacePreStopDecider::CheckOpenSpacePreStop( target_area_found = true; } } - AERROR_IF(!target_area_found) - << "no target parking spot found on reference line"; + if (!target_area_found) { + AERROR << "no target parking spot found on reference line"; + return false; + } + *target_s = target_area_center_s; + return true; +} + +void OpenSpacePreStopDecider::SetStopFence( + const double target_s, Frame* const frame, + ReferenceLineInfo* const reference_line_info) { + const auto& nearby_path = reference_line_info->reference_line().map_path(); + const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); + const VehicleState& vehicle_state = frame->vehicle_state(); double stop_line_s = 0.0; - double stop_distance_to_target = open_space_config.stop_distance_to_target(); + double stop_distance_to_target = + open_space_pre_stop_decider_config_.stop_distance_to_target(); double static_linear_velocity_epsilon = 1.0e-2; CHECK_GE(stop_distance_to_target, 1.0e-8); - double target_vehicle_offset = target_area_center_s - adc_front_edge_s; + double target_vehicle_offset = target_s - adc_front_edge_s; if (target_vehicle_offset > stop_distance_to_target) { - stop_line_s = target_area_center_s - stop_distance_to_target; + stop_line_s = target_s - stop_distance_to_target; } else if (std::abs(target_vehicle_offset) < stop_distance_to_target) { - stop_line_s = target_area_center_s + stop_distance_to_target; + stop_line_s = target_s + stop_distance_to_target; } else if (target_vehicle_offset < -stop_distance_to_target) { if (!frame->open_space_info().pre_stop_rightaway_flag()) { // TODO(Jinyun) Use constant comfortable deacceleration rather than // distance by config to set stop fence stop_line_s = - adc_front_edge_s + open_space_config.rightaway_stop_distance(); + adc_front_edge_s + + open_space_pre_stop_decider_config_.rightaway_stop_distance(); if (std::abs(vehicle_state.linear_velocity()) < static_linear_velocity_epsilon) { stop_line_s = adc_front_edge_s; @@ -132,8 +175,7 @@ void OpenSpacePreStopDecider::CheckOpenSpacePreStop( } } - const std::string stop_wall_id = - OPEN_SPACE_VO_ID_PREFIX + target_parking_spot_id; + const std::string stop_wall_id = OPEN_SPACE_STOP_ID; std::vector wait_for_obstacles; frame->mutable_open_space_info()->set_open_space_pre_stop_fence_s( stop_line_s); diff --git a/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.h b/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.h index 6dab28b1070036b541a22953739903da4f3b60e3..112eccce94c2c14627a9d0779011f6437780f9db 100644 --- a/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.h +++ b/modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.h @@ -20,7 +20,7 @@ #pragma once -#include "modules/planning/proto/decider_config.pb.h" +#include "modules/planning/proto/open_space_pre_stop_decider_config.pb.h" #include "cyber/common/macros.h" @@ -39,11 +39,20 @@ class OpenSpacePreStopDecider : public Decider { apollo::common::Status Process( Frame* frame, ReferenceLineInfo* reference_line_info) override; - void CheckOpenSpacePreStop(Frame* const frame, - ReferenceLineInfo* const reference_line_info); + bool CheckParkingSpotPreStop(Frame* const frame, + ReferenceLineInfo* const reference_line_info, + double* target_s); + + bool CheckPullOverPreStop(Frame* const frame, + ReferenceLineInfo* const reference_line_info, + double* target_s); + + void SetStopFence(const double target_s, Frame* const frame, + ReferenceLineInfo* const reference_line_info); private: - static constexpr const char* OPEN_SPACE_VO_ID_PREFIX = "OP_"; + static constexpr const char* OPEN_SPACE_STOP_ID = "OPEN_SPACE_PRE_STOP"; + OpenSpacePreStopDeciderConfig open_space_pre_stop_decider_config_; }; } // namespace planning diff --git a/modules/planning/testdata/conf/scenario/valet_parking_config.pb.txt b/modules/planning/testdata/conf/scenario/valet_parking_config.pb.txt index c9fd5f1d43c9a97d8dbbab3a38621e13b2f24e28..b7bd2781d28c178a99a4918520616cfd9808dc18 100644 --- a/modules/planning/testdata/conf/scenario/valet_parking_config.pb.txt +++ b/modules/planning/testdata/conf/scenario/valet_parking_config.pb.txt @@ -25,6 +25,9 @@ stage_config: { task_type: PIECEWISE_JERK_SPEED_OPTIMIZER task_config: { task_type: OPEN_SPACE_PRE_STOP_DECIDER + open_space_pre_stop_decider_config { + stop_type: PARKING + } } task_config: { task_type: PATH_LANE_BORROW_DECIDER @@ -73,6 +76,9 @@ stage_config: { task_type: OPEN_SPACE_FALLBACK_DECIDER task_config: { task_type: OPEN_SPACE_ROI_DECIDER + open_space_roi_decider_config { + roi_type: PARKING + } } task_config: { task_type: OPEN_SPACE_TRAJECTORY_PROVIDER