Skip to content

Commit

Permalink
planning: naming changes in task confs for consistency
Browse files Browse the repository at this point in the history
  • Loading branch information
jmtao committed May 13, 2020
1 parent 35e1f75 commit c56f6c7
Show file tree
Hide file tree
Showing 7 changed files with 39 additions and 36 deletions.
6 changes: 3 additions & 3 deletions modules/planning/conf/planning_config.pb.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ standard_planning_config {
}
default_task_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
piecewise_jerk_path_config {
piecewise_jerk_path_optimizer_config {
default_path_config {
l_weight: 1.0
dl_weight: 20.0
Expand All @@ -22,7 +22,7 @@ default_task_config: {
}
default_task_config: {
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
piecewise_jerk_speed_config {
piecewise_jerk_speed_optimizer_config {
acc_weight: 1.0
jerk_weight: 3.0
kappa_penalty_weight: 2000.0
Expand All @@ -32,7 +32,7 @@ default_task_config: {
}
default_task_config: {
task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
piecewise_jerk_nonlinear_speed_config {
piecewise_jerk_nonlinear_speed_optimizer_config {
acc_weight: 2.0
jerk_weight: 3.0
lat_acc_weight: 1000.0
Expand Down
4 changes: 2 additions & 2 deletions modules/planning/proto/planner_open_space_config.proto
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ message WarmStartConfig {
// Grid a star for heuristic
optional double grid_a_star_xy_resolution = 15 [default = 0.1];
optional double node_radius = 16 [default = 0.5];
optional PiecewiseJerkSpeedConfig s_curve_config = 17;
optional PiecewiseJerkSpeedOptimizerConfig s_curve_config = 17;
}

message DualVariableWarmStartConfig {
Expand Down Expand Up @@ -162,7 +162,7 @@ message IterativeAnchoringConfig {
optional double max_reverse_acc = 13 [default = 2.0];
optional double max_acc_jerk = 14 [default = 4.0];
optional double delta_t = 15 [default = 0.2];
optional PiecewiseJerkSpeedConfig s_curve_config = 16;
optional PiecewiseJerkSpeedOptimizerConfig s_curve_config = 16;
}

message TrajectoryPartitionConfig {
Expand Down
8 changes: 4 additions & 4 deletions modules/planning/proto/planning_config.proto
Original file line number Diff line number Diff line change
Expand Up @@ -64,10 +64,10 @@ message TaskConfig {
OpenSpaceTrajectoryProviderConfig
open_space_trajectory_provider_config = 16;
SpeedHeuristicConfig speed_heuristic_config = 17;
PiecewiseJerkNonlinearSpeedConfig
piecewise_jerk_nonlinear_speed_config = 18;
PiecewiseJerkPathConfig piecewise_jerk_path_config = 19;
PiecewiseJerkSpeedConfig piecewise_jerk_speed_config = 20;
PiecewiseJerkNonlinearSpeedOptimizerConfig
piecewise_jerk_nonlinear_speed_optimizer_config = 18;
PiecewiseJerkPathOptimizerConfig piecewise_jerk_path_optimizer_config = 19;
PiecewiseJerkSpeedOptimizerConfig piecewise_jerk_speed_optimizer_config = 20;
// other tasks
LearningBasedTaskConfig learning_based_task_config = 21;
}
Expand Down
12 changes: 6 additions & 6 deletions modules/planning/proto/task_config.proto
Original file line number Diff line number Diff line change
Expand Up @@ -210,9 +210,9 @@ message PathReuseDeciderConfig {
}

//////////////////////////////////
// PiecewiseJerkNonlinearSpeedConfig
// PiecewiseJerkNonlinearSpeedOptimizerConfig

message PiecewiseJerkNonlinearSpeedConfig {
message PiecewiseJerkNonlinearSpeedOptimizerConfig {
// Driving comfort weights
optional double acc_weight = 1 [default = 500.0];
optional double jerk_weight = 2 [default = 100.0];
Expand Down Expand Up @@ -240,9 +240,9 @@ message PiecewiseJerkNonlinearSpeedConfig {
}

//////////////////////////////////
// PiecewiseJerkPathConfig
// PiecewiseJerkPathOptimizerConfig

message PiecewiseJerkPathConfig {
message PiecewiseJerkPathOptimizerConfig {
optional PiecewiseJerkPathWeights default_path_config = 1;
optional PiecewiseJerkPathWeights lane_change_path_config = 2;
}
Expand All @@ -254,9 +254,9 @@ message PiecewiseJerkPathWeights {
}

//////////////////////////////////
// PiecewiseJerkSpeedConfig
// PiecewiseJerkSpeedOptimizerConfig

message PiecewiseJerkSpeedConfig {
message PiecewiseJerkSpeedOptimizerConfig {
optional double acc_weight = 1 [default = 1.0];
optional double jerk_weight = 2 [default = 10.0];
optional double kappa_penalty_weight = 3 [default = 1000.0];
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ using apollo::common::VehicleConfigHelper;

PiecewiseJerkPathOptimizer::PiecewiseJerkPathOptimizer(const TaskConfig& config)
: PathOptimizer(config) {
ACHECK(config_.has_piecewise_jerk_path_config());
ACHECK(config_.has_piecewise_jerk_path_optimizer_config());
}

common::Status PiecewiseJerkPathOptimizer::Process(
Expand All @@ -61,18 +61,21 @@ common::Status PiecewiseJerkPathOptimizer::Process(

// Choose lane_change_path_config for lane-change cases
// Otherwise, choose default_path_config for normal path planning
const auto& piecewise_jerk_path_config =
const auto& config =
reference_line_info_->IsChangeLanePath()
? config_.piecewise_jerk_path_config().lane_change_path_config()
: config_.piecewise_jerk_path_config().default_path_config();
? config_.piecewise_jerk_path_optimizer_config()
.lane_change_path_config()
: config_.piecewise_jerk_path_optimizer_config()
.default_path_config();

std::array<double, 5> w = {
piecewise_jerk_path_config.l_weight(),
piecewise_jerk_path_config.dl_weight() *
config.l_weight(),
config.dl_weight() *
std::fmax(init_frenet_state.first[1] * init_frenet_state.first[1],
5.0),
piecewise_jerk_path_config.ddl_weight(),
piecewise_jerk_path_config.dddl_weight(), 0.0};
config.ddl_weight(),
config.dddl_weight(),
0.0};

const auto& path_boundaries =
reference_line_info_->GetCandidatePathBoundaries();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ PiecewiseJerkSpeedNonlinearOptimizer::PiecewiseJerkSpeedNonlinearOptimizer(
: SpeedOptimizer(config),
smoothed_speed_limit_(0.0, 0.0, 0.0),
smoothed_path_curvature_(0.0, 0.0, 0.0) {
ACHECK(config_.has_piecewise_jerk_nonlinear_speed_config());
ACHECK(config_.has_piecewise_jerk_nonlinear_speed_optimizer_config());
}

Status PiecewiseJerkSpeedNonlinearOptimizer::Process(
Expand Down Expand Up @@ -438,7 +438,8 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP(
piecewise_jerk_problem.set_x_bounds(s_bounds_);

// TODO(Jinyun): parameter tunnings
const auto& config = config_.piecewise_jerk_nonlinear_speed_config();
const auto& config =
config_.piecewise_jerk_nonlinear_speed_optimizer_config();
piecewise_jerk_problem.set_weight_x(0.0);
piecewise_jerk_problem.set_weight_dx(0.0);
piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
Expand Down Expand Up @@ -480,7 +481,8 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByNLP(
ptr_interface->set_safety_bounds(s_bounds_);

// Set weights and reference values
const auto& config = config_.piecewise_jerk_nonlinear_speed_config();
const auto& config =
config_.piecewise_jerk_nonlinear_speed_optimizer_config();

ptr_interface->set_curvature_curve(smoothed_path_curvature_);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ using apollo::common::TrajectoryPoint;
PiecewiseJerkSpeedOptimizer::PiecewiseJerkSpeedOptimizer(
const TaskConfig& config)
: SpeedOptimizer(config) {
ACHECK(config_.has_piecewise_jerk_speed_config());
ACHECK(config_.has_piecewise_jerk_speed_optimizer_config());
}

Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data,
Expand Down Expand Up @@ -77,12 +77,10 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data,
PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t,
init_s);

const auto& piecewise_jerk_speed_config =
config_.piecewise_jerk_speed_config();
piecewise_jerk_problem.set_weight_ddx(
piecewise_jerk_speed_config.acc_weight());
piecewise_jerk_problem.set_weight_dddx(
piecewise_jerk_speed_config.jerk_weight());
const auto& config =
config_.piecewise_jerk_speed_optimizer_config();
piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
piecewise_jerk_problem.set_weight_dddx(config.jerk_weight());

piecewise_jerk_problem.set_x_bounds(0.0, total_length);
piecewise_jerk_problem.set_dx_bounds(
Expand All @@ -93,7 +91,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data,
piecewise_jerk_problem.set_dddx_bound(FLAGS_longitudinal_jerk_lower_bound,
FLAGS_longitudinal_jerk_upper_bound);

piecewise_jerk_problem.set_dx_ref(piecewise_jerk_speed_config.ref_v_weight(),
piecewise_jerk_problem.set_dx_ref(config.ref_v_weight(),
reference_line_info_->GetCruiseSpeed());

// Update STBoundary
Expand Down Expand Up @@ -149,14 +147,14 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data,
// get curvature
PathPoint path_point = path_data.GetPathPointWithPathS(path_s);
penalty_dx.push_back(std::fabs(path_point.kappa()) *
piecewise_jerk_speed_config.kappa_penalty_weight());
config.kappa_penalty_weight());
// get v_upper_bound
const double v_lower_bound = 0.0;
double v_upper_bound = FLAGS_planning_upper_speed_limit;
v_upper_bound = speed_limit.GetSpeedLimitByS(path_s);
s_dot_bounds.emplace_back(v_lower_bound, std::fmax(v_upper_bound, 0.0));
}
piecewise_jerk_problem.set_x_ref(piecewise_jerk_speed_config.ref_s_weight(),
piecewise_jerk_problem.set_x_ref(config.ref_s_weight(),
x_ref);
piecewise_jerk_problem.set_penalty_dx(penalty_dx);
piecewise_jerk_problem.set_dx_bounds(std::move(s_dot_bounds));
Expand Down

0 comments on commit c56f6c7

Please sign in to comment.