Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 747e659f 编写于 作者: J JasonZhou404 提交者： HongyiSun
浏览文件
操作

Planning: seperate rule-based stop to a task

上级 b1ba11e8
隐藏空白更改
内联 并排
Showing with 682 addition and 331 deletion
modules/planning/conf/planning_config.pb.txt
浏览文件 @ 747e659f
......@@ -64,6 +64,12 @@ default_task_config: {
default_task_config: {
task_type: SPEED_DECIDER
}
default_task_config: {
task_type: RULE_BASED_STOP_DECIDER
rule_based_stop_decider_config {
max_valid_stop_distance: 0.5
}
}
default_task_config: {
task_type: SPEED_BOUNDS_PRIORI_DECIDER
speed_bounds_decider_config {
......@@ -81,7 +87,6 @@ default_task_config: {
static_obs_nudge_speed_ratio: 0.6
dynamic_obs_nudge_speed_ratio: 0.8
centri_jerk_speed_coeff: 1.0
max_valid_stop_distance: 0.5
}
}
default_task_config: {
......@@ -101,7 +106,6 @@ default_task_config: {
static_obs_nudge_speed_ratio: 0.6
dynamic_obs_nudge_speed_ratio: 0.8
centri_jerk_speed_coeff: 1.0
max_valid_stop_distance: 0.25
}
}
default_task_config: {
......
modules/planning/conf/scenario/bare_intersection_unprotected_config.pb.txt
浏览文件 @ 747e659f
......@@ -20,6 +20,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -66,6 +67,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -115,6 +117,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -125,6 +130,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -163,4 +169,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/lane_follow_config.pb.txt
浏览文件 @ 747e659f
......@@ -9,6 +9,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -58,4 +59,7 @@ stage_config: {
task_config: {
task_type: DECIDER_RSS
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/pull_over_config.pb.txt
浏览文件 @ 747e659f
......@@ -18,6 +18,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -62,6 +63,9 @@ stage_config: {
task_config: {
task_type: DECIDER_RSS
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......
modules/planning/conf/scenario/side_pass_config.pb.txt 0 → 100644
浏览文件 @ 747e659f
scenario_type: SIDE_PASS
stage_type: SIDE_PASS_DEFAULT_STAGE
side_pass_config: {
side_pass_exit_distance: 5.0
approach_obstacle_max_stop_speed: 1.0e-2
approach_obstacle_min_stop_distance: 2.5
block_obstacle_min_speed: 0.1
enable_obstacle_blocked_check: true
max_backup_stage_cycle_num: 30
min_l_nudge_buffer: 0.3
min_front_obstacle_distance: 1.0
max_front_obstacle_distance: 15.0
stop_fence_distance_to_blocking_obstacle: 6.0
}
stage_config: {
stage_type: SIDE_PASS_DEFAULT_STAGE
enabled: false
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
# task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
task_config: {
task_type: PATH_BOUNDS_DECIDER
path_bounds_decider_config {
is_lane_borrowing: true
}
}
task_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
}
task_config: {
task_type: PATH_ASSESSMENT_DECIDER
}
task_config: {
task_type: PATH_DECIDER
}
task_config: {
task_type: DP_ST_SPEED_OPTIMIZER
}
task_config: {
task_type: SPEED_DECIDER
}
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: SPEED_BOUNDS_PRIORI_DECIDER
}
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/stop_sign_unprotected_config.pb.txt
浏览文件 @ 747e659f
......@@ -22,6 +22,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -57,6 +58,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -67,6 +71,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -102,6 +107,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -113,6 +121,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -166,6 +175,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -176,6 +188,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -214,4 +227,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/traffic_light_protected_config.pb.txt
浏览文件 @ 747e659f
......@@ -16,6 +16,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -51,6 +52,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -61,6 +65,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -99,4 +104,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/traffic_light_unprotected_left_turn_config.pb.txt
浏览文件 @ 747e659f
......@@ -19,6 +19,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -65,6 +66,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -75,6 +79,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -113,4 +118,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/traffic_light_unprotected_right_turn_config.pb.txt
浏览文件 @ 747e659f
......@@ -20,6 +20,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -55,6 +56,9 @@ stage_config: {
task_config: {
task_type: SPEED_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -66,6 +70,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -112,6 +117,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -122,6 +130,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -160,4 +169,7 @@ stage_config: {
task_config: {
task_type: SPEED_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/conf/scenario/valet_parking_config.pb.txt
浏览文件 @ 747e659f
......@@ -16,8 +16,9 @@ stage_config: {
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
......@@ -58,6 +59,10 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
stage_type: VALET_PARKING_PARKING
......
modules/planning/proto/BUILD
浏览文件 @ 747e659f
......@@ -170,6 +170,7 @@ proto_library(
":planner_open_space_config_proto_lib",
":proceed_with_caution_speed_config_proto_lib",
":reference_line_smoother_config_proto_lib",
":rule_based_stop_decider_config_proto_lib",
":speed_bounds_decider_config_proto_lib",
],
)
......@@ -550,4 +551,18 @@ proto_library(
],
)
cc_proto_library(
name = "rule_based_stop_decider_config_proto",
deps = [
":rule_based_stop_decider_config_proto_lib",
],
)
proto_library(
name = "rule_based_stop_decider_config_proto_lib",
srcs = [
"rule_based_stop_decider_config.proto",
],
)
cpplint()
modules/planning/proto/planning_config.proto
浏览文件 @ 747e659f
......@@ -16,6 +16,7 @@ import "modules/planning/proto/piecewise_jerk_path_config.proto";
import "modules/planning/proto/piecewise_jerk_speed_config.proto";
import "modules/planning/proto/proceed_with_caution_speed_config.proto";
import "modules/planning/proto/planner_open_space_config.proto";
import "modules/planning/proto/rule_based_stop_decider_config.proto";
import "modules/planning/proto/speed_bounds_decider_config.proto";
import "modules/planning/proto/navi_path_decider_config.proto";
import "modules/planning/proto/navi_speed_decider_config.proto";
......@@ -54,6 +55,7 @@ message TaskConfig {
PATH_LANE_BORROW_DECIDER = 26;
PIECEWISE_JERK_SPEED_OPTIMIZER = 27;
LANE_CHANGE_DECIDER = 28;
RULE_BASED_STOP_DECIDER = 29;
};
optional TaskType task_type = 1;
oneof task_config {
......@@ -73,6 +75,7 @@ message TaskConfig {
PiecewiseJerkSpeedConfig piecewise_jerk_speed_config = 23;
PathLaneBorrowDeciderConfig path_lane_borrow_decider_config = 24;
LaneChangeDeciderConfig lane_change_decider_config = 25;
RuleBasedStopDeciderConfig rule_based_stop_decider_config = 26;
}
}
......
modules/planning/proto/rule_based_stop_decider_config.proto 0 → 100644
浏览文件 @ 747e659f
syntax = "proto2";
package apollo.planning;
message RuleBasedStopDeciderConfig {
optional double max_adc_stop_speed = 1 [default = 0.3];
optional double max_valid_stop_distance = 2 [default = 0.5];
optional double approach_distance_for_lane_change = 3 [default = 80.0];
optional double urgent_distance_for_lane_change = 4 [default = 50.0];
}
\ No newline at end of file
modules/planning/proto/speed_bounds_decider_config.proto
浏览文件 @ 747e659f
......@@ -18,9 +18,4 @@ message SpeedBoundsDeciderConfig {
optional double static_obs_nudge_speed_ratio = 13;
optional double dynamic_obs_nudge_speed_ratio = 14;
optional double centri_jerk_speed_coeff = 15;
//Rule based side pass stop
optional double max_adc_stop_speed = 16 [default = 0.3];
optional double max_valid_stop_distance = 17 [default = 0.5];
optional double approach_distance_for_lane_change = 18 [default = 80.0];
optional double urgent_distance_for_lane_change = 19 [default = 50.0];
}
modules/planning/tasks/BUILD
浏览文件 @ 747e659f
......@@ -44,6 +44,7 @@ cc_library(
"//modules/planning/tasks/deciders/path_assessment_decider",
"//modules/planning/tasks/deciders/path_bounds_decider",
"//modules/planning/tasks/deciders/path_lane_borrow_decider",
"//modules/planning/tasks/deciders/rule_based_stop_decider",
"//modules/planning/tasks/optimizers/open_space_trajectory_generation:open_space_trajectory_provider",
"//modules/planning/tasks/optimizers/open_space_trajectory_partition",
"//modules/planning/tasks/optimizers/path_decider",
......
modules/planning/tasks/deciders/rule_based_stop_decider/BUILD 0 → 100644
浏览文件 @ 747e659f
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "rule_based_stop_decider",
srcs = [
"rule_based_stop_decider.cc",
],
hdrs = [
"rule_based_stop_decider.h",
],
copts = ["-DMODULE_NAME=\\\"planning\\\""],
deps = [
"//modules/common/status",
"//modules/planning/common:frame",
"//modules/planning/common:planning_gflags",
"//modules/planning/common/util:common_lib",
"//modules/planning/tasks:task",
"//modules/planning/tasks/deciders:decider_base",
"//modules/planning/tasks/deciders/lane_change_decider",
],
)
cpplint()
modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.cc 0 → 100644
浏览文件 @ 747e659f
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.h"
#include <string>
#include <tuple>
#include <vector>
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/util/common.h"
#include "modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.h"
namespace apollo {
namespace planning {
using apollo::common::ErrorCode;
using apollo::common::SLPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Vec2d;
RuleBasedStopDecider::RuleBasedStopDecider(const TaskConfig &config)
: Decider(config) {
CHECK(config.has_rule_based_stop_decider_config());
rule_based_stop_decider_config_ = config.rule_based_stop_decider_config();
SetName("RuleBasedStopDecider");
}
apollo::common::Status RuleBasedStopDecider::Process(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// 1. Rule_based stop for side pass onto reverse lane
if (FLAGS_enable_nonscenario_side_pass) {
StopOnSidePass(frame, reference_line_info);
}
// 2. Rule_based stop for urgent lane change
if (FLAGS_enable_lane_change_urgency_checking) {
CheckLaneChangeUrgency(frame);
}
// 3. Rule_based stop at path end position
AddPathEndStop(frame, reference_line_info);
return Status::OK();
}
void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
// Check if the target lane is blocked or not
if (reference_line_info.IsChangeLanePath()) {
is_clear_to_change_lane_ =
LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
continue;
}
// If it's not in lane-change scenario or target lane is not blocked, skip
if (frame->reference_line_info().size() <= 1 || is_clear_to_change_lane_) {
continue;
}
// When the target lane is blocked in change-lane case, check the urgency
// Get the end point of current routing
const auto &route_end_waypoint =
reference_line_info.Lanes().RouteEndWaypoint();
// If can't get lane from the route's end waypoint, then skip
if (!route_end_waypoint.lane) {
continue;
}
auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
auto *reference_line = reference_line_info.mutable_reference_line();
common::SLPoint sl_point;
// Project the end point to sl_point on current reference lane
if (reference_line->XYToSL({point.x(), point.y()}, &sl_point) &&
reference_line->IsOnLane(sl_point)) {
// Check the distance from ADC to the end point of current routing
double distance_to_passage_end =
sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
// If ADC is still far from the end of routing, no need to stop, skip
if (distance_to_passage_end >
rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
continue;
}
// In urgent case, set a temporary stop fence and wait to change lane
// TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
const std::string stop_wall_id = "lane_change_stop";
std::vector<std::string> wait_for_obstacles;
util::BuildStopDecision(
stop_wall_id, sl_point.s(),
rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
"RuleBasedStopDecider", frame, &reference_line_info);
}
}
}
void RuleBasedStopDecider::AddPathEndStop(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
const std::string stop_wall_id = "path_end_stop";
std::vector<std::string> wait_for_obstacles;
if (!reference_line_info->path_data().path_label().empty() &&
reference_line_info->path_data().frenet_frame_path().back().s() -
reference_line_info->path_data().frenet_frame_path().front().s() <
FLAGS_short_path_length_threshold) {
util::BuildStopDecision(
stop_wall_id,
reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
0.0, StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY,
wait_for_obstacles, "RuleBasedStopDecider", frame, reference_line_info);
}
}
void RuleBasedStopDecider::StopOnSidePass(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
const PathData &path_data = reference_line_info->path_data();
double stop_s_on_pathdata = 0.0;
bool set_stop_fence = false;
const auto &side_pass_info = PlanningContext::Instance()->side_pass_info();
auto *mutable_side_pass_info =
PlanningContext::Instance()->mutable_side_pass_info();
if (path_data.path_label().find("self") != std::string::npos) {
mutable_side_pass_info->check_clear_flag = false;
mutable_side_pass_info->change_lane_stop_flag = false;
mutable_side_pass_info->change_lane_stop_path_point.Clear();
return;
}
if (side_pass_info.change_lane_stop_flag) {
// Check if stop at last frame stop fence
ADEBUG << "stop fence is set due to side pass on reverse lane";
if (CheckADCStop(*reference_line_info,
side_pass_info.change_lane_stop_path_point)) {
ADEBUG << "ADV Stopped due to change lane in side pass";
if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
ADEBUG << "Environment clear for ADC to change lane in side pass";
mutable_side_pass_info->check_clear_flag = true;
} else {
if (CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata) &&
BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info)) {
set_stop_fence = true;
} else {
set_stop_fence =
BuildSidePassStopFence(side_pass_info.change_lane_stop_path_point,
frame, reference_line_info);
}
}
} else {
if (CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata) &&
BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info)) {
set_stop_fence = true;
} else {
set_stop_fence =
BuildSidePassStopFence(side_pass_info.change_lane_stop_path_point,
frame, reference_line_info);
}
}
} else {
if (!side_pass_info.check_clear_flag &&
CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata)) {
set_stop_fence = BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info);
}
if (side_pass_info.check_clear_flag &&
CheckClearDone(*reference_line_info,
side_pass_info.change_lane_stop_path_point)) {
mutable_side_pass_info->check_clear_flag = false;
}
}
mutable_side_pass_info->change_lane_stop_flag = set_stop_fence;
}
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vector<std::tuple<double, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
for (const auto &point_guide : path_point_decision_guide) {
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get<1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get<0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
const Vec2d stop_fence_pose =
shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
stop_s_on_pathdata -= nearby_path.GetNearestPoint(
stop_fence_pose, stop_s_on_pathdata, &stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get<1>(point_guide);
}
return false;
}
// @brief Set stop fence for side pass
bool RuleBasedStopDecider::BuildSidePassStopFence(
const PathData &path_data, const double stop_s_on_pathdata,
common::PathPoint *stop_pathpoint, Frame *const frame,
ReferenceLineInfo *const reference_line_info) {
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
return BuildSidePassStopFence(*stop_pathpoint, frame, reference_line_info);
}
bool RuleBasedStopDecider::BuildSidePassStopFence(
const common::PathPoint &stop_point, Frame *const frame,
ReferenceLineInfo *const reference_line_info) {
const std::string stop_wall_id = "Side_Pass_Stop";
// TODO(Jinyun) load relavent surrounding obstacles
std::vector<std::string> wait_for_obstacles;
const auto &nearby_path = reference_line_info->reference_line().map_path();
double stop_point_s = 0.0;
double stop_point_l = 0.0;
nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
&stop_point_l);
// TODO(Jinyun) move to confs
constexpr double stop_buffer = 0.25;
util::BuildStopDecision(stop_wall_id, stop_point_s - stop_buffer, 0.0,
StopReasonCode::STOP_REASON_SIDEPASS_SAFETY,
wait_for_obstacles, "RuleBasedStopDecider", frame,
reference_line_info);
return true;
}
// @brief Check if ADV stop at a stop fence
bool RuleBasedStopDecider::CheckADCStop(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
const double adc_speed =
common::VehicleStateProvider::Instance()->linear_velocity();
if (adc_speed > rule_based_stop_decider_config_.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 auto &nearby_path = reference_line_info.reference_line().map_path();
double stop_point_s = 0.0;
double stop_point_l = 0.0;
nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
&stop_point_l);
const double distance_stop_line_to_adc_front_edge =
stop_point_s - adc_front_edge_s;
if (distance_stop_line_to_adc_front_edge >
rule_based_stop_decider_config_.max_valid_stop_distance()) {
ADEBUG << "not a valid stop. too far from stop line.";
return false;
}
return true;
}
bool RuleBasedStopDecider::CheckClearDone(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line_info.reference_line().GetLaneWidth(
(adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
&lane_right_width);
SLPoint stop_sl_point;
reference_line_info.reference_line().XYToSL({stop_point.x(), stop_point.y()},
&stop_sl_point);
// use distance to last stop point to determine if needed to check clear
// again
if (adc_back_edge_s > stop_sl_point.s()) {
if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
return true;
}
}
return false;
}
} // namespace planning
} // namespace apollo
modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.h 0 → 100644
浏览文件 @ 747e659f
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include "modules/planning/proto/decider_config.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
#include "modules/planning/proto/rule_based_stop_decider_config.pb.h"
#include "modules/planning/tasks/deciders/decider.h"
namespace apollo {
namespace planning {
class RuleBasedStopDecider : public Decider {
public:
explicit RuleBasedStopDecider(const TaskConfig& config);
private:
apollo::common::Status Process(
Frame* const frame,
ReferenceLineInfo* const reference_line_info) override;
// @brief Rule-based stop at path end
void AddPathEndStop(Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Rule-based stop for urgent lane change
void CheckLaneChangeUrgency(Frame* const frame);
// @brief Rule-based stop for side pass on reverse lane
void StopOnSidePass(Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool CheckSidePassStop(const PathData& path_data,
const ReferenceLineInfo& reference_line_info,
double* stop_s_on_pathdata);
// @brief Set stop fence for side pass
bool BuildSidePassStopFence(const PathData& path_data,
const double stop_s_on_pathdata,
common::PathPoint* stop_pathpoint,
Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Set stop fence for side pass
bool BuildSidePassStopFence(const common::PathPoint& stop_point,
Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Check if ADV stop at a stop fence
bool CheckADCStop(const ReferenceLineInfo& reference_line_info,
const common::PathPoint& stop_point);
// @brief Check if needed to check clear again for side pass
bool CheckClearDone(const ReferenceLineInfo& reference_line_info,
const common::PathPoint& stop_point);
private:
RuleBasedStopDeciderConfig rule_based_stop_decider_config_;
bool is_clear_to_change_lane_ = false;
};
} // namespace planning
} // namespace apollo
modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.cc
浏览文件 @ 747e659f
......@@ -28,7 +28,6 @@
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/common/util/common.h"
#include "modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.h"
#include "modules/planning/tasks/deciders/speed_bounds_decider/speed_limit_decider.h"
#include "modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.h"
......@@ -59,19 +58,7 @@ Status SpeedBoundsDecider::Process(
const ReferenceLine &reference_line = reference_line_info->reference_line();
PathDecision *const path_decision = reference_line_info->path_decision();
AddPathEndStop(frame, reference_line_info);
// 1. Rule_based speed planning configurations for different traffic scenarios
if (FLAGS_enable_nonscenario_side_pass) {
StopOnSidePass(frame, reference_line_info);
}
// Check the lane change urgency at current frame
if (FLAGS_enable_lane_change_urgency_checking) {
CheckLaneChangeUrgency(frame);
}
// 2. Map obstacles into st graph
// 1. Map obstacles into st graph
STBoundaryMapper boundary_mapper(adc_sl_boundary, speed_bounds_config_,
reference_line, path_data,
speed_bounds_config_.total_path_length(),
......@@ -101,7 +88,7 @@ Status SpeedBoundsDecider::Process(
const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision);
// 3. Create speed limit along path
// 2. Create speed limit along path
SpeedLimitDecider speed_limit_decider(adc_sl_boundary, speed_bounds_config_,
reference_line, path_data);
......@@ -139,69 +126,6 @@ Status SpeedBoundsDecider::Process(
return Status::OK();
}
void SpeedBoundsDecider::CheckLaneChangeUrgency(Frame *const frame) {
for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
// Check if the target lane is blocked or not
if (reference_line_info.IsChangeLanePath()) {
is_clear_to_change_lane_ =
LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
continue;
}
// If it's not in lane-change scenario or target lane is not blocked, skip
if (frame->reference_line_info().size() <= 1 || is_clear_to_change_lane_) {
continue;
}
// When the target lane is blocked in change-lane case, check the urgency
// Get the end point of current routing
const auto &route_end_waypoint =
reference_line_info.Lanes().RouteEndWaypoint();
// If can't get lane from the route's end waypoint, then skip
if (!route_end_waypoint.lane) {
continue;
}
auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
auto *reference_line = reference_line_info.mutable_reference_line();
common::SLPoint sl_point;
// Project the end point to sl_point on current reference lane
if (reference_line->XYToSL({point.x(), point.y()}, &sl_point) &&
reference_line->IsOnLane(sl_point)) {
// Check the distance from ADC to the end point of current routing
double distance_to_passage_end =
sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
// If ADC is still far from the end of routing, no need to stop, skip
if (distance_to_passage_end >
speed_bounds_config_.approach_distance_for_lane_change()) {
continue;
}
// In urgent case, set a temporary stop fence and wait to change lane
// TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
const std::string stop_wall_id = "lane_change_stop";
std::vector<std::string> wait_for_obstacles;
util::BuildStopDecision(
stop_wall_id, sl_point.s(),
speed_bounds_config_.urgent_distance_for_lane_change(),
StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
"SpeedBoundsDecider", frame, &reference_line_info);
}
}
}
void SpeedBoundsDecider::AddPathEndStop(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
const std::string stop_wall_id = "path_end_stop";
std::vector<std::string> wait_for_obstacles;
if (!reference_line_info->path_data().path_label().empty() &&
reference_line_info->path_data().frenet_frame_path().back().s() -
reference_line_info->path_data().frenet_frame_path().front().s() <
FLAGS_short_path_length_threshold) {
util::BuildStopDecision(
stop_wall_id,
reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
0.0, StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY,
wait_for_obstacles, "SpeedBoundsDecider", frame, reference_line_info);
}
}
double SpeedBoundsDecider::SetSpeedFallbackDistance(
PathDecision *const path_decision) {
// Set min_s_on_st_boundaries to guide speed fallback. Different stop distance
......@@ -249,212 +173,6 @@ double SpeedBoundsDecider::SetSpeedFallbackDistance(
return min_s_non_reverse > min_s_reverse ? 0.0 : min_s_non_reverse;
}
void SpeedBoundsDecider::StopOnSidePass(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
const PathData &path_data = reference_line_info->path_data();
double stop_s_on_pathdata = 0.0;
bool set_stop_fence = false;
const auto &side_pass_info = PlanningContext::Instance()->side_pass_info();
auto *mutable_side_pass_info =
PlanningContext::Instance()->mutable_side_pass_info();
if (path_data.path_label().find("self") != std::string::npos) {
mutable_side_pass_info->check_clear_flag = false;
mutable_side_pass_info->change_lane_stop_flag = false;
mutable_side_pass_info->change_lane_stop_path_point.Clear();
return;
}
if (side_pass_info.change_lane_stop_flag) {
// Check if stop at last frame stop fence
ADEBUG << "stop fence is set due to side pass on reverse lane";
if (CheckADCStop(*reference_line_info,
side_pass_info.change_lane_stop_path_point)) {
ADEBUG << "ADV Stopped due to change lane in side pass";
if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
ADEBUG << "Environment clear for ADC to change lane in side pass";
mutable_side_pass_info->check_clear_flag = true;
} else {
if (CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata) &&
BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info)) {
set_stop_fence = true;
} else {
set_stop_fence =
BuildSidePassStopFence(side_pass_info.change_lane_stop_path_point,
frame, reference_line_info);
}
}
} else {
if (CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata) &&
BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info)) {
set_stop_fence = true;
} else {
set_stop_fence =
BuildSidePassStopFence(side_pass_info.change_lane_stop_path_point,
frame, reference_line_info);
}
}
} else {
if (!side_pass_info.check_clear_flag &&
CheckSidePassStop(path_data, *reference_line_info,
&stop_s_on_pathdata)) {
set_stop_fence = BuildSidePassStopFence(
path_data, stop_s_on_pathdata,
&(mutable_side_pass_info->change_lane_stop_path_point), frame,
reference_line_info);
}
if (side_pass_info.check_clear_flag &&
CheckClearDone(*reference_line_info,
side_pass_info.change_lane_stop_path_point)) {
mutable_side_pass_info->check_clear_flag = false;
}
}
mutable_side_pass_info->change_lane_stop_flag = set_stop_fence;
}
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool SpeedBoundsDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vector<std::tuple<double, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
for (const auto &point_guide : path_point_decision_guide) {
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get<1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get<0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
const Vec2d stop_fence_pose =
shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
stop_s_on_pathdata -= nearby_path.GetNearestPoint(
stop_fence_pose, stop_s_on_pathdata, &stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get<1>(point_guide);
}
return false;
}
// @brief Set stop fence for side pass
bool SpeedBoundsDecider::BuildSidePassStopFence(
const PathData &path_data, const double stop_s_on_pathdata,
common::PathPoint *stop_pathpoint, Frame *const frame,
ReferenceLineInfo *const reference_line_info) {
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
return BuildSidePassStopFence(*stop_pathpoint, frame, reference_line_info);
}
bool SpeedBoundsDecider::BuildSidePassStopFence(
const common::PathPoint &stop_point, Frame *const frame,
ReferenceLineInfo *const reference_line_info) {
const std::string stop_wall_id = "Side_Pass_Stop";
// TODO(Jinyun) load relavent surrounding obstacles
std::vector<std::string> wait_for_obstacles;
const auto &nearby_path = reference_line_info->reference_line().map_path();
double stop_point_s = 0.0;
double stop_point_l = 0.0;
nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
&stop_point_l);
// TODO(Jinyun) move to confs
constexpr double stop_buffer = 0.25;
util::BuildStopDecision(stop_wall_id, stop_point_s - stop_buffer, 0.0,
StopReasonCode::STOP_REASON_SIDEPASS_SAFETY,
wait_for_obstacles, "SpeedBoundsDecider", frame,
reference_line_info);
return true;
}
// @brief Check if ADV stop at a stop fence
bool SpeedBoundsDecider::CheckADCStop(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
const double adc_speed =
common::VehicleStateProvider::Instance()->linear_velocity();
if (adc_speed > speed_bounds_config_.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 auto &nearby_path = reference_line_info.reference_line().map_path();
double stop_point_s = 0.0;
double stop_point_l = 0.0;
nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
&stop_point_l);
const double distance_stop_line_to_adc_front_edge =
stop_point_s - adc_front_edge_s;
if (distance_stop_line_to_adc_front_edge >
speed_bounds_config_.max_valid_stop_distance()) {
ADEBUG << "not a valid stop. too far from stop line.";
return false;
}
return true;
}
bool SpeedBoundsDecider::CheckClearDone(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line_info.reference_line().GetLaneWidth(
(adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
&lane_right_width);
SLPoint stop_sl_point;
reference_line_info.reference_line().XYToSL({stop_point.x(), stop_point.y()},
&stop_sl_point);
// use distance to last stop point to determine if needed to check clear
// again
if (adc_back_edge_s > stop_sl_point.s()) {
if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
return true;
}
}
return false;
}
void SpeedBoundsDecider::RecordSTGraphDebug(
const StGraphData &st_graph_data, STGraphDebug *st_graph_debug) const {
if (!FLAGS_enable_record_debug || !st_graph_debug) {
......@@ -503,5 +221,6 @@ void SpeedBoundsDecider::RecordSTGraphDebug(
st_graph_debug->add_speed_limit()->CopyFrom(speed_point);
}
}
} // namespace planning
} // namespace apollo
modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.h
浏览文件 @ 747e659f
......@@ -38,48 +38,14 @@ class SpeedBoundsDecider : public Decider {
Frame* const frame,
ReferenceLineInfo* const reference_line_info) override;
void AddPathEndStop(Frame* const frame,
ReferenceLineInfo* const reference_line_info);
void CheckLaneChangeUrgency(Frame* const frame);
double SetSpeedFallbackDistance(PathDecision* const path_decision);
// @brief Rule-based stop for side pass on reverse lane
void StopOnSidePass(Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool CheckSidePassStop(const PathData& path_data,
const ReferenceLineInfo& reference_line_info,
double* stop_s_on_pathdata);
// @brief Set stop fence for side pass
bool BuildSidePassStopFence(const PathData& path_data,
const double stop_s_on_pathdata,
common::PathPoint* stop_pathpoint,
Frame* const frame,
ReferenceLineInfo* const reference_line_info);
bool BuildSidePassStopFence(const common::PathPoint& stop_point,
Frame* const frame,
ReferenceLineInfo* const reference_line_info);
// @brief Check if ADV stop at a stop fence
bool CheckADCStop(const ReferenceLineInfo& reference_line_info,
const common::PathPoint& stop_point);
// @brief Check if needed to check clear again for side pass
bool CheckClearDone(const ReferenceLineInfo& reference_line_info,
const common::PathPoint& stop_point);
void RecordSTGraphDebug(
const StGraphData& st_graph_data,
planning_internal::STGraphDebug* st_graph_debug) const;
private:
SpeedBoundsDeciderConfig speed_bounds_config_;
bool is_clear_to_change_lane_ = false;
};
} // namespace planning
......
modules/planning/tasks/task_factory.cc
浏览文件 @ 747e659f
......@@ -31,6 +31,7 @@
#include "modules/planning/tasks/deciders/path_assessment_decider/path_assessment_decider.h"
#include "modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.h"
#include "modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.h"
#include "modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.h"
#include "modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.h"
#include "modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h"
#include "modules/planning/tasks/optimizers/open_space_trajectory_partition/open_space_trajectory_partition.h"
......@@ -99,10 +100,9 @@ void TaskFactory::Init(const PlanningConfig& config) {
[](const TaskConfig& config) -> Task* {
return new OpenSpacePreStopDecider(config);
});
task_factory_.Register(TaskConfig::DECIDER_RSS,
[](const TaskConfig& config) -> Task* {
return new RssDecider(config);
});
task_factory_.Register(
TaskConfig::DECIDER_RSS,
[](const TaskConfig& config) -> Task* { return new RssDecider(config); });
task_factory_.Register(TaskConfig::SPEED_BOUNDS_PRIORI_DECIDER,
[](const TaskConfig& config) -> Task* {
return new SpeedBoundsDecider(config);
......@@ -131,6 +131,10 @@ void TaskFactory::Init(const PlanningConfig& config) {
[](const TaskConfig& config) -> Task* {
return new PathAssessmentDecider(config);
});
task_factory_.Register(TaskConfig::RULE_BASED_STOP_DECIDER,
[](const TaskConfig& config) -> Task* {
return new RuleBasedStopDecider(config);
});
for (const auto& default_task_config : config.default_task_config()) {
default_task_configs_[default_task_config.task_type()] =
default_task_config;
......
modules/planning/testdata/conf/scenario/bare_intersection_unprotected_config.pb.txt
浏览文件 @ 747e659f
......@@ -20,6 +20,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -55,6 +56,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -66,6 +70,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -115,6 +120,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -125,6 +133,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -163,4 +172,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/lane_follow_config.pb.txt
浏览文件 @ 747e659f
......@@ -8,6 +8,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -51,4 +52,7 @@ stage_config: {
task_config: {
task_type: DECIDER_RSS
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/pull_over_config.pb.txt
浏览文件 @ 747e659f
......@@ -18,6 +18,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -62,6 +63,9 @@ stage_config: {
task_config: {
task_type: DECIDER_RSS
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......
modules/planning/testdata/conf/scenario/side_pass_config.pb.txt
浏览文件 @ 747e659f
......@@ -19,6 +19,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
# task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -54,4 +55,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/stop_sign_unprotected_config.pb.txt
浏览文件 @ 747e659f
......@@ -22,6 +22,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -57,6 +58,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -67,6 +71,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -102,6 +107,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -113,6 +121,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -166,6 +175,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -176,6 +188,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -214,4 +227,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/traffic_light_protected_config.pb.txt
浏览文件 @ 747e659f
......@@ -16,6 +16,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -51,6 +52,9 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -61,6 +65,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -99,4 +104,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/traffic_light_unprotected_left_turn_config.pb.txt
浏览文件 @ 747e659f
......@@ -19,6 +19,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -72,6 +73,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -82,6 +86,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -120,4 +125,7 @@ stage_config: {
task_config: {
task_type: SPEED_BOUNDS_FINAL_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/traffic_light_unprotected_right_turn_config.pb.txt
浏览文件 @ 747e659f
......@@ -20,6 +20,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -55,6 +56,9 @@ stage_config: {
task_config: {
task_type: SPEED_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -66,6 +70,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -119,6 +124,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
......@@ -129,6 +137,7 @@ stage_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
......@@ -167,4 +176,7 @@ stage_config: {
task_config: {
task_type: SPEED_DECIDER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
modules/planning/testdata/conf/scenario/valet_parking_config.pb.txt
浏览文件 @ 747e659f
......@@ -16,8 +16,9 @@ stage_config: {
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
......@@ -58,6 +59,9 @@ stage_config: {
task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
}
task_config: {
task_type: RULE_BASED_STOP_DECIDER
}
}
stage_config: {
stage_type: VALET_PARKING_PARKING
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或