aem bootstrap start --plus

buildtool create --template plugin \
    --namespaces planning \
    --dependencies planning:binary:planning \
    --base_class_name apollo::planning::Scenario \
    --class_name BestParkingSpace \
    modules/planning/scenarios/best_parking_space

tree modules/planning/scenarios/best_parking_space/

modules/planning/scenarios/best_parking_space/
├── BUILD
├── best_parking_space.cc
├── best_parking_space.h
├── conf
│   ├── best_parking_approaching_parking_spot
│   │   └── open_space_pre_stop_decider.pb.txt
│   ├── best_parking_parking
│   │   ├── open_space_roi_decider.pb.txt
│   │   ├── open_space_trajectory_partition.pb.txt
│   │   └── open_space_trajectory_provider.pb.txt
│   ├── pipeline.pb.txt
│   └── scenario_conf.pb.txt
├── cyberfile.xml
├── plugins.xml
├── proto
│   ├── BUILD
│   └── best_parking_space.proto
├── stage_approaching_parking_spot.cc
├── stage_approaching_parking_spot.h
├── stage_parking.cc
└── stage_parking.h

/******************************************************************************
 * @file best_parking_space.cc
 *****************************************************************************/
#include "modules/planning/scenarios/best_parking_space/best_parking_space.h"
#include "modules/planning/planning_base/common/frame.h"
// #include "modules/planning/scenarios/best_parking_space/stage_approaching_parking_spot.h"
// #include "modules/planning/scenarios/best_parking_space/stage_parking.h"
namespace apollo {
namespace planning {
using apollo::common::VehicleState;
using apollo::common::math::Polygon2d;
using apollo::common::math::Vec2d;
using apollo::hdmap::ParkingSpaceInfoConstPtr;
using apollo::hdmap::Path;
using apollo::hdmap::PathOverlap;
bool BestParkingSpaceScenario::Init(std::shared_ptr<DependencyInjector> injector, const std::string& name) {
    if (init_) {
        return true;
    }
    if (!Scenario::Init(injector, name)) {
        AERROR << "failed to init scenario" << Name();
        return false;
    }
    if (!Scenario::LoadConfig<BestParkingSpaceConfig>(&context_.scenario_config)) {
        AERROR << "fail to get config of scenario" << Name();
        return false;
    }
    hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
    CHECK_NOTNULL(hdmap_);
    init_ = true;
    return true;
}
bool BestParkingSpaceScenario::IsTransferable(const Scenario* const other_scenario, const Frame& frame) {
    std::string target_parking_spot_id;
    double parking_spot_range_to_start = context_.scenario_config.parking_spot_range_to_start();
    auto parking_command = frame.local_view().planning_command->has_parking_command();
    auto parking_spot_id = frame.local_view().planning_command->parking_command().has_parking_spot_id();
    if (other_scenario == nullptr || frame.reference_line_info().empty()) {
        return false;
        if (parking_command && parking_spot_id) {
            target_parking_spot_id = frame.local_view().planning_command->parking_command().parking_spot_id();
        }
    }
    if (!parking_command) {
        const auto routing_end = frame.local_view().end_lane_way_point;
        common::PointENU end_postion;
        end_postion.set_x(routing_end->pose().x());
        end_postion.set_y(routing_end->pose().y());
        if (nullptr == routing_end) {
            return false;
        }
        common::SLPoint dest_sl;
        const auto& reference_line_info = frame.reference_line_info().front();
        const auto& reference_line = reference_line_info.reference_line();
        reference_line.XYToSL(routing_end->pose(), &dest_sl);
        const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
        const double adc_distance_to_dest = dest_sl.s() - adc_front_edge_s;
        if (adc_distance_to_dest > parking_spot_range_to_start) {
            return false;
        }
        if (target_parking_spot_id.empty()) {
            std::vector<apollo::hdmap::ParkingSpaceInfoConstPtr> parking_spaces;
            if (hdmap_->GetParkingSpaces(end_postion, parking_spot_range_to_start, &parking_spaces) == 0
                && parking_spaces.size() > 0) {
                for (auto parking_space : parking_spaces) {
                    target_parking_spot_id = parking_space->parking_space().id().id();
                }
            }
        }
    }
    if (target_parking_spot_id.empty()) {
        return false;
    }
    AINFO << "target_parking_spot_id" << target_parking_spot_id;
    const auto& nearby_path = frame.reference_line_info().front().reference_line().map_path();
    PathOverlap parking_space_overlap;
    const auto& vehicle_state = frame.vehicle_state();
    if (!SearchTargetParkingSpotOnPath(nearby_path, target_parking_spot_id, &parking_space_overlap)) {
        ADEBUG << "No such parking spot found after searching all path forward "
                  "possible"
               << target_parking_spot_id;
        return false;
    }
    if (!CheckDistanceToParkingSpot(
                frame, vehicle_state, nearby_path, parking_spot_range_to_start, parking_space_overlap)) {
        ADEBUG << "target parking spot found, but too far, distance larger than "
                  "pre-defined distance"
               << target_parking_spot_id;
        return false;
    }
    context_.target_parking_spot_id = target_parking_spot_id;
    return true;
}
bool BestParkingSpaceScenario::SearchTargetParkingSpotOnPath(
        const Path& nearby_path,
        const std::string& target_parking_id,
        PathOverlap* parking_space_overlap) {
    const auto& parking_space_overlaps = nearby_path.parking_space_overlaps();
    for (const auto& parking_overlap : parking_space_overlaps) {
        if (parking_overlap.object_id == target_parking_id) {
            *parking_space_overlap = parking_overlap;
            return true;
        }
    }
    return false;
}
bool BestParkingSpaceScenario::CheckDistanceToParkingSpot(
        const Frame& frame,
        const VehicleState& vehicle_state,
        const Path& nearby_path,
        const double parking_start_range,
        const PathOverlap& parking_space_overlap) {
    // TODO(Jinyun) parking overlap s are wrong on map, not usable
    const hdmap::HDMap* hdmap = hdmap::HDMapUtil::BaseMapPtr();
    hdmap::Id id;
    double center_point_s, center_point_l;
    id.set_id(parking_space_overlap.object_id);
    ParkingSpaceInfoConstPtr target_parking_spot_ptr = hdmap->GetParkingSpaceById(id);
    Vec2d left_bottom_point = target_parking_spot_ptr->polygon().points().at(0);
    Vec2d right_bottom_point = target_parking_spot_ptr->polygon().points().at(1);
    Vec2d right_top_point = target_parking_spot_ptr->polygon().points().at(2);
    Vec2d left_top_point = target_parking_spot_ptr->polygon().points().at(3);
    Vec2d center_point = (left_bottom_point + right_bottom_point + right_top_point + left_top_point) / 4.0;
    nearby_path.GetNearestPoint(center_point, &center_point_s, &center_point_l);
    double vehicle_point_s = 0.0;
    double vehicle_point_l = 0.0;
    Vec2d vehicle_vec(vehicle_state.x(), vehicle_state.y());
    nearby_path.GetNearestPoint(vehicle_vec, &vehicle_point_s, &vehicle_point_l);
    if (std::abs(center_point_s - vehicle_point_s) < parking_start_range) {
        return true;
    }
    return false;
}
}  // namespace planning
}  // namespace apollo

/******************************************************************************
 * @file best_parking_space.h
 *****************************************************************************/
#pragma once
#include <memory>
#include <string>
#include "modules/common_msgs/map_msgs/map_id.pb.h"
#include "modules/planning/scenarios/best_parking_space/proto/best_parking_space.pb.h"
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/pnc_map/path.h"
#include "modules/planning/planning_interface_base/scenario_base/scenario.h"
#include "gtest/gtest.h"
#include "cyber/common/file.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
namespace apollo {
namespace planning {
struct BestParkingSpaceContext : public ScenarioContext {
    BestParkingSpaceConfig scenario_config;
    std::string target_parking_spot_id;
    bool pre_stop_rightaway_flag = false;
    hdmap::MapPathPoint pre_stop_rightaway_point;
};
class BestParkingSpaceScenario : public Scenario {
public:
    bool Init(std::shared_ptr<DependencyInjector> injector, const std::string& name) override;
    /**
     * @brief Get the scenario context.
     */
    BestParkingSpaceContext* GetContext() override {
        return &context_;
    }
    bool IsTransferable(const Scenario* const other_scenario, const Frame& frame) override;
private:
    static bool SearchTargetParkingSpotOnPath(
            const hdmap::Path& nearby_path,
            const std::string& target_parking_id,
            hdmap::PathOverlap* parking_space_overlap);
    static bool CheckDistanceToParkingSpot(
            const Frame& frame,
            const common::VehicleState& vehicle_state,
            const hdmap::Path& nearby_path,
            const double parking_start_range,
            const hdmap::PathOverlap& parking_space_overlap);
private:
    bool init_ = false;
    BestParkingSpaceContext context_;
    const hdmap::HDMap* hdmap_ = nullptr;
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::BestParkingSpaceScenario, Scenario)
}  // namespace planning
}  // namespace apollo

#include <string>
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/scenarios/best_parking_space/stage_approaching_parking_spot.h"
namespace apollo {
namespace planning {
bool StageBestApproachingParkingSpot::Init(
        const StagePipeline& config,
        const std::shared_ptr<DependencyInjector>& injector,
        const std::string& config_dir,
        void* context) {
    if (!Stage::Init(config, injector, config_dir, context)) {
        return false;
    }
    scenario_config_.CopyFrom(GetContextAs<BestParkingSpaceContext>()->scenario_config);
    return true;
}
StageResult StageBestApproachingParkingSpot::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {
    ADEBUG << "stage: StageBestApproachingParkingSpot";
    CHECK_NOTNULL(frame);
    StageResult result;
    auto scenario_context = GetContextAs<BestParkingSpaceContext>();
    if (scenario_context->target_parking_spot_id.empty()) {
        return result.SetStageStatus(StageStatusType::ERROR);
    }
    *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) = scenario_context->target_parking_spot_id;
    frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(scenario_context->pre_stop_rightaway_flag);
    *(frame->mutable_open_space_info()->mutable_pre_stop_rightaway_point())
            = scenario_context->pre_stop_rightaway_point;
    auto* reference_lines = frame->mutable_reference_line_info();
    for (auto& reference_line : *reference_lines) {
        auto* path_decision = reference_line.path_decision();
        if (nullptr == path_decision) {
            continue;
        }
        auto* dest_obstacle = path_decision->Find(FLAGS_destination_obstacle_id);
        if (nullptr == dest_obstacle) {
            continue;
        }
        ObjectDecisionType decision;
        decision.mutable_ignore();
        dest_obstacle->EraseDecision();
        dest_obstacle->AddLongitudinalDecision("ignore-dest-in-valet-parking", decision);
    }
    result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    scenario_context->pre_stop_rightaway_flag = frame->open_space_info().pre_stop_rightaway_flag();
    scenario_context->pre_stop_rightaway_point = frame->open_space_info().pre_stop_rightaway_point();
    if (CheckADCStop(*frame)) {
        next_stage_ = "BEST_PARKING_PARKING";
        return StageResult(StageStatusType::FINISHED);
    }
    if (result.HasError()) {
        AERROR << "StopSignUnprotectedStagePreStop planning error";
        return result.SetStageStatus(StageStatusType::ERROR);
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
bool StageBestApproachingParkingSpot::CheckADCStop(const Frame& frame) {
    const auto& reference_line_info = frame.reference_line_info().front();
    const double adc_speed = injector_->vehicle_state()->linear_velocity();
    const double max_adc_stop_speed
            = common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param().max_abs_speed_when_stopped();
    if (adc_speed > max_adc_stop_speed) {
        ADEBUG << "ADC not stopped: speed[" << adc_speed << "]";
        return false;
    }
    // check stop close enough to stop line of the stop_sign
    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
    const double stop_fence_start_s = frame.open_space_info().open_space_pre_stop_fence_s();
    const double distance_stop_line_to_adc_front_edge = stop_fence_start_s - adc_front_edge_s;
    if (distance_stop_line_to_adc_front_edge > scenario_config_.max_valid_stop_distance()) {
        ADEBUG << "not a valid stop. too far from stop line.";
        return false;
    }
    return true;
}
}  // namespace planning
}  // namespace apollo

#pragma once
#include <memory>
#include <string>
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_interface_base/scenario_base/stage.h"
#include "modules/planning/scenarios/best_parking_space/best_parking_space.h"
#include "gtest/gtest.h"
#include "cyber/common/file.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
namespace apollo {
namespace planning {
class StageBestApproachingParkingSpot : public Stage {
public:
    bool Init(
            const StagePipeline& config,
            const std::shared_ptr<DependencyInjector>& injector,
            const std::string& config_dir,
            void* context);
    StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override;
private:
    bool CheckADCStop(const Frame& frame);
    BestParkingSpaceConfig scenario_config_;
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::StageBestApproachingParkingSpot, Stage)
}  // namespace planning
}  // namespace apollo

#include "modules/planning/scenarios/best_parking_space/stage_parking.h"
#include "modules/planning/planning_base/common/frame.h"
namespace apollo {
namespace planning {
StageResult StageBestParking::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {
    AERROR << "1";
    // Open space planning doesn't use planning_init_point from upstream because
    // of different stitching strategy
    auto scenario_context = GetContextAs<BestParkingSpaceContext>();
    frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);
    *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) = scenario_context->target_parking_spot_id;
    StageResult result = ExecuteTaskOnOpenSpace(frame);
    if (result.HasError()) {
        AERROR << "StageParking planning error";
        return result.SetStageStatus(StageStatusType::ERROR);
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}
StageResult StageBestParking::FinishStage() {
    return StageResult(StageStatusType::FINISHED);
}
}  // namespace planning
}  // namespace apollo

#pragma once
#include <memory>
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/planning/planning_interface_base/scenario_base/stage.h"
#include "modules/planning/scenarios/best_parking_space/best_parking_space.h"
namespace apollo {
namespace planning {
class StageBestParking : public Stage {
public:
    StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override;
private:
    StageResult FinishStage();
    BestParkingSpaceConfig scenario_config_;
};
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::planning::StageBestParking, Stage)
}  // namespace planning
}  // namespace apollo

stage: {
  name: "BEST_PARKING_APPROACHING_PARKING_SPOT"
  type: "StageBestApproachingParkingSpot"
  enabled: true
  task {
    name: "OPEN_SPACE_PRE_STOP_DECIDER"
    type: "OpenSpacePreStopDecider"
  }
  task {
    name: "LANE_FOLLOW_PATH"
    type: "LaneFollowPath"
  }
  task {
    name: "LANE_BORROW_PATH"
    type: "LaneBorrowPath"
  }
  task {
    name: "FALLBACK_PATH"
    type: "FallbackPath"
  }
  task {
    name: "PATH_DECIDER"
    type: "PathDecider"
  }
  task {
    name: "RULE_BASED_STOP_DECIDER"
    type: "RuleBasedStopDecider"
  }
  task {
    name: "SPEED_BOUNDS_PRIORI_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "SPEED_HEURISTIC_OPTIMIZER"
    type: "PathTimeHeuristicOptimizer"
  }
  task {
    name: "SPEED_DECIDER"
    type: "SpeedDecider"
  }
  task {
    name: "SPEED_BOUNDS_FINAL_DECIDER"
    type: "SpeedBoundsDecider"
  }
  task {
    name: "PIECEWISE_JERK_SPEED"
    type: "PiecewiseJerkSpeedOptimizer"
  }
}
stage: {
  name: "BEST_PARKING_PARKING"
  type: "StageBestParking"
  enabled: true
  task {
    name: "OPEN_SPACE_ROI_DECIDER"
    type: "OpenSpaceRoiDecider"
  }
  task {
    name: "OPEN_SPACE_TRAJECTORY_PROVIDER"
    type: "OpenSpaceTrajectoryProvider"
  }
  task {
    name: "OPEN_SPACE_TRAJECTORY_PARTITION"
    type: "OpenSpaceTrajectoryPartition"
  }
  task {
    name: "OPEN_SPACE_FALLBACK_DECIDER"
    type: "OpenSpaceFallbackDecider"
  }
}

syntax = "proto2";
package apollo.planning;
message BestParkingSpaceConfig {
      // Only when the distance between the vehicle and parking spot is smaller 
  // than this value, "ValetParkingScenario" can be entered
  optional double parking_spot_range_to_start = 1 [default = 20.0];
  // Only when the distance between the vehicle and pre-stop distance is smaller
  // than this value, "StageApproachingParkingSpot" can be finished
  optional double max_valid_stop_distance = 2 [default = 1.0];  // meter
}

parking_spot_range_to_start: 20.0
max_valid_stop_distance: 1.0

<library path="modules/planning/scenarios/best_parking_space/libbest_parking_space.so">
    <class type="apollo::planning::BestParkingSpaceScenario" base_class="apollo::planning::Scenario"></class>
    <class type="apollo::planning::StageBestApproachingParkingSpot" base_class="apollo::planning::Stage"></class>
    <class type="apollo::planning::StageBestParking" base_class="apollo::planning::Stage"></class>
</library>

buildtool profile config init --package planning --profile=default && aem profile use default

profiles/current/modules/planning/planning_component/conf/public_road_planner_config.pb.txt

scenario {
    name: "EMERGENCY_PULL_OVER"
    type: "EmergencyPullOverScenario"
}
scenario {
    name: "EMERGENCY_STOP"
    type: "EmergencyStopScenario"
}
scenario {
    name: "BEST_PARKING"
    type: "BestParkingSpaceScenario"
}
scenario {
    name: "VALET_PARKING"
    type: "ValetParkingScenario"
 }
scenario {
    name: "BARE_INTERSECTION_UNPROTECTED"
    type: "BareIntersectionUnprotectedScenario"
}
scenario {
    name: "STOP_SIGN_UNPROTECTED"
    type: "StopSignUnprotectedScenario"
}
scenario {
    name: "YIELD_SIGN"
    type: "YieldSignScenario"
}
scenario {
    name: "TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN"
    type: "TrafficLightUnprotectedLeftTurnScenario"
}
scenario {
    name: "TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN"
    type: "TrafficLightUnprotectedRightTurnScenario"
}
scenario {
    name: "TRAFFIC_LIGHT_PROTECTED"
    type: "TrafficLightProtectedScenario"
}
scenario {
    name: "PULL_OVER"
    type: "PullOverScenario"
}
scenario {
    name: "PARK_AND_GO"
    type: "ParkAndGoScenario"
}
scenario {
    name: "LANE_FOLLOW"
    type: "LaneFollowScenario"
}

load("//tools:apollo.bzl", "cyber_plugin_description")
load("//tools:apollo_package.bzl", "apollo_cc_test", "apollo_package", "apollo_plugin")
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
filegroup(
    name = "best_parking_space_files",
    srcs = glob([
        "conf/**",
    ]),
)
apollo_plugin(
    name = "libbest_parking_space.so",
    srcs = [
        "stage_approaching_parking_spot.cc",
        "stage_parking.cc",
        "best_parking_space.cc",
    ],
    hdrs = [
        "stage_approaching_parking_spot.h",
        "stage_parking.h",
        "best_parking_space.h",
    ],
    copts = ["-DMODULE_NAME=\\\"planning\\\""],
    description = ":plugins.xml",
    deps = [
        "//cyber",
        "//modules/common_msgs/planning_msgs:planning_cc_proto",
        "//modules/planning/planning_interface_base:apollo_planning_planning_interface_base",
        "//modules/planning/scenarios/best_parking_space/proto:best_parking_space_proto",
    ],
)
apollo_package()
cpplint()

modules/planning/scenarios/best_parking_space/conf/best_parking_approaching_parking_spot/open_space_pre_stop_decider.pb.txt

stop_type: PARKING

buildtool build -p modules/planning/