Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e3f099ce
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
前往新版Gitcode,体验更适合开发者的 AI 搜索 >>
提交
e3f099ce
编写于
7月 27, 2017
作者:
Z
Zhang Liangliang
提交者:
Kecheng Xu
7月 27, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
uniform the naming of 'QP'
上级
afa43c7b
变更
17
隐藏空白更改
内联
并排
Showing
17 changed file
with
61 addition
and
61 deletion
+61
-61
modules/common/math/mpc_solver.cc
modules/common/math/mpc_solver.cc
+3
-3
modules/common/math/qp_solver/active_set_qp_solver.cc
modules/common/math/qp_solver/active_set_qp_solver.cc
+20
-20
modules/common/math/qp_solver/active_set_qp_solver.h
modules/common/math/qp_solver/active_set_qp_solver.h
+2
-2
modules/common/math/qp_solver/qp_solver.cc
modules/common/math/qp_solver/qp_solver.cc
+8
-8
modules/common/math/qp_solver/qp_solver.h
modules/common/math/qp_solver/qp_solver.h
+2
-2
modules/planning/math/smoothing_spline/spline_1d_generator.cc
...les/planning/math/smoothing_spline/spline_1d_generator.cc
+1
-1
modules/planning/math/smoothing_spline/spline_1d_generator.h
modules/planning/math/smoothing_spline/spline_1d_generator.h
+1
-1
modules/planning/math/smoothing_spline/spline_2d_solver.cc
modules/planning/math/smoothing_spline/spline_2d_solver.cc
+1
-1
modules/planning/math/smoothing_spline/spline_2d_solver.h
modules/planning/math/smoothing_spline/spline_2d_solver.h
+1
-1
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
...ning/optimizer/qp_spline_path/qp_spline_path_generator.cc
+8
-8
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h
...nning/optimizer/qp_spline_path/qp_spline_path_generator.h
+3
-3
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc
...ning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc
+3
-3
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h
...nning/optimizer/qp_spline_path/qp_spline_path_optimizer.h
+3
-3
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
...anning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
+1
-1
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h
...lanning/optimizer/qp_spline_path/qp_spline_path_sampler.h
+2
-2
modules/planning/planner/em/em_planner.cc
modules/planning/planner/em/em_planner.cc
+1
-1
modules/planning/proto/qp_spline_path_config.proto
modules/planning/proto/qp_spline_path_config.proto
+1
-1
未找到文件。
modules/common/math/mpc_solver.cc
浏览文件 @
e3f099ce
...
...
@@ -24,8 +24,8 @@ namespace common {
namespace
math
{
using
Matrix
=
Eigen
::
MatrixXd
;
using
::
apollo
::
common
::
math
::
Q
P
Solver
;
using
::
apollo
::
common
::
math
::
ActiveSetQ
P
Solver
;
using
::
apollo
::
common
::
math
::
Q
p
Solver
;
using
::
apollo
::
common
::
math
::
ActiveSetQ
p
Solver
;
// discrete linear predictive control solver, with control format
// x(i + 1) = A * x(i) + B * u (i) + C
...
...
@@ -130,7 +130,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
Eigen
::
MatrixXd
matrix_equality_boundary
=
Eigen
::
MatrixXd
::
Zero
(
matrix_ll
.
rows
()
+
matrix_uu
.
rows
(),
matrix_ll
.
cols
());
std
::
unique_ptr
<
Q
PSolver
>
qp_solver
(
new
ActiveSetQP
Solver
(
matrix_m1
,
std
::
unique_ptr
<
Q
pSolver
>
qp_solver
(
new
ActiveSetQp
Solver
(
matrix_m1
,
matrix_m2
,
matrix_inequality_constrain
,
matrix_inequality_boundary
,
...
...
modules/common/math/qp_solver/active_set_qp_solver.cc
浏览文件 @
e3f099ce
...
...
@@ -29,13 +29,13 @@ namespace apollo {
namespace
common
{
namespace
math
{
ActiveSetQ
PSolver
::
ActiveSetQP
Solver
(
ActiveSetQ
pSolver
::
ActiveSetQp
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
const
Eigen
::
MatrixXd
&
offset
,
const
Eigen
::
MatrixXd
&
affine_inequality_matrix
,
const
Eigen
::
MatrixXd
&
affine_inequality_boundary
,
const
Eigen
::
MatrixXd
&
affine_equality_matrix
,
const
Eigen
::
MatrixXd
&
affine_equality_boundary
)
:
Q
P
Solver
(
kernel_matrix
,
offset
,
affine_inequality_matrix
,
:
Q
p
Solver
(
kernel_matrix
,
offset
,
affine_inequality_matrix
,
affine_inequality_boundary
,
affine_equality_matrix
,
affine_equality_boundary
)
{
num_param_
=
kernel_matrix
.
rows
();
...
...
@@ -48,7 +48,7 @@ ActiveSetQPSolver::ActiveSetQPSolver(
debug_info_
=
FLAGS_default_enable_active_set_debug_info
;
}
bool
ActiveSetQ
P
Solver
::
solve
()
{
bool
ActiveSetQ
p
Solver
::
solve
()
{
::
qpOASES
::
QProblem
qp_problem
(
num_param_
,
num_constraint_
);
::
qpOASES
::
Options
my_options
;
my_options
.
epsNum
=
qp_eps_num_
;
...
...
@@ -138,54 +138,54 @@ bool ActiveSetQPSolver::solve() {
return
false
;
}
void
ActiveSetQ
P
Solver
::
set_qp_eps_num
(
const
double
eps
)
{
qp_eps_num_
=
eps
;
}
void
ActiveSetQ
p
Solver
::
set_qp_eps_num
(
const
double
eps
)
{
qp_eps_num_
=
eps
;
}
void
ActiveSetQ
P
Solver
::
set_qp_eps_den
(
const
double
eps
)
{
qp_eps_den_
=
eps
;
}
void
ActiveSetQ
p
Solver
::
set_qp_eps_den
(
const
double
eps
)
{
qp_eps_den_
=
eps
;
}
void
ActiveSetQ
P
Solver
::
set_qp_eps_iter_ref
(
const
double
eps
)
{
void
ActiveSetQ
p
Solver
::
set_qp_eps_iter_ref
(
const
double
eps
)
{
qp_eps_iter_ref_
=
eps
;
}
void
ActiveSetQ
P
Solver
::
set_debug_info
(
const
bool
enable
)
{
void
ActiveSetQ
p
Solver
::
set_debug_info
(
const
bool
enable
)
{
debug_info_
=
enable
;
}
void
ActiveSetQ
P
Solver
::
set_l_lower_bound
(
const
double
l_lower_bound
)
{
void
ActiveSetQ
p
Solver
::
set_l_lower_bound
(
const
double
l_lower_bound
)
{
l_lower_bound_
=
l_lower_bound
;
}
void
ActiveSetQ
P
Solver
::
set_l_upper_bound
(
const
double
l_upper_bound
)
{
void
ActiveSetQ
p
Solver
::
set_l_upper_bound
(
const
double
l_upper_bound
)
{
l_upper_bound_
=
l_upper_bound
;
}
void
ActiveSetQ
P
Solver
::
set_constraint_upper_bound
(
void
ActiveSetQ
p
Solver
::
set_constraint_upper_bound
(
const
double
la_upper_bound
)
{
constraint_upper_bound_
=
la_upper_bound
;
}
void
ActiveSetQ
P
Solver
::
set_max_iteration
(
const
int
max_iter
)
{
void
ActiveSetQ
p
Solver
::
set_max_iteration
(
const
int
max_iter
)
{
max_iteration_
=
max_iter
;
}
double
ActiveSetQ
P
Solver
::
qp_eps_num
()
const
{
return
qp_eps_num_
;
}
double
ActiveSetQ
p
Solver
::
qp_eps_num
()
const
{
return
qp_eps_num_
;
}
double
ActiveSetQ
P
Solver
::
qp_eps_den
()
const
{
return
qp_eps_den_
;
}
double
ActiveSetQ
p
Solver
::
qp_eps_den
()
const
{
return
qp_eps_den_
;
}
double
ActiveSetQ
P
Solver
::
qp_eps_iter_ref
()
const
{
return
qp_eps_iter_ref_
;
}
double
ActiveSetQ
p
Solver
::
qp_eps_iter_ref
()
const
{
return
qp_eps_iter_ref_
;
}
bool
ActiveSetQ
P
Solver
::
debug_info
()
const
{
return
debug_info_
;
}
bool
ActiveSetQ
p
Solver
::
debug_info
()
const
{
return
debug_info_
;
}
double
ActiveSetQ
P
Solver
::
l_lower_bound
()
const
{
return
l_lower_bound_
;
}
double
ActiveSetQ
p
Solver
::
l_lower_bound
()
const
{
return
l_lower_bound_
;
}
double
ActiveSetQ
P
Solver
::
l_upper_bound
()
const
{
return
l_upper_bound_
;
}
double
ActiveSetQ
p
Solver
::
l_upper_bound
()
const
{
return
l_upper_bound_
;
}
double
ActiveSetQ
P
Solver
::
constraint_upper_bound
()
const
{
double
ActiveSetQ
p
Solver
::
constraint_upper_bound
()
const
{
return
constraint_upper_bound_
;
}
int
ActiveSetQ
P
Solver
::
max_iteration
()
const
{
return
max_iteration_
;
}
int
ActiveSetQ
p
Solver
::
max_iteration
()
const
{
return
max_iteration_
;
}
// pure virtual
bool
ActiveSetQ
P
Solver
::
sanity_check
()
{
bool
ActiveSetQ
p
Solver
::
sanity_check
()
{
return
kernel_matrix_
.
rows
()
==
kernel_matrix_
.
cols
()
&&
kernel_matrix_
.
rows
()
==
affine_inequality_matrix_
.
cols
()
&&
kernel_matrix_
.
rows
()
==
affine_equality_matrix_
.
cols
()
&&
...
...
modules/common/math/qp_solver/active_set_qp_solver.h
浏览文件 @
e3f099ce
...
...
@@ -29,9 +29,9 @@ namespace apollo {
namespace
common
{
namespace
math
{
class
ActiveSetQ
PSolver
:
public
QP
Solver
{
class
ActiveSetQ
pSolver
:
public
Qp
Solver
{
public:
ActiveSetQ
P
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
ActiveSetQ
p
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
const
Eigen
::
MatrixXd
&
offset
,
const
Eigen
::
MatrixXd
&
affine_inequality_matrix
,
const
Eigen
::
MatrixXd
&
affine_inequality_boundary
,
...
...
modules/common/math/qp_solver/qp_solver.cc
浏览文件 @
e3f099ce
...
...
@@ -23,7 +23,7 @@ namespace apollo {
namespace
common
{
namespace
math
{
Q
PSolver
::
QP
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
Q
pSolver
::
Qp
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
const
Eigen
::
MatrixXd
&
offset
,
const
Eigen
::
MatrixXd
&
affine_inequality_matrix
,
const
Eigen
::
MatrixXd
&
affine_inequality_boundary
,
...
...
@@ -36,27 +36,27 @@ QPSolver::QPSolver(const Eigen::MatrixXd& kernel_matrix,
affine_equality_matrix_
(
affine_equality_matrix
),
affine_equality_boundary_
(
affine_equality_boundary
)
{}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
params
()
const
{
return
params_
;
}
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
params
()
const
{
return
params_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
kernel_matrix
()
const
{
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
kernel_matrix
()
const
{
return
kernel_matrix_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
offset
()
const
{
return
offset_
;
}
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
offset
()
const
{
return
offset_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
affine_equality_matrix
()
const
{
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
affine_equality_matrix
()
const
{
return
affine_equality_matrix_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
affine_equality_boundary
()
const
{
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
affine_equality_boundary
()
const
{
return
affine_equality_boundary_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
affine_inequality_matrix
()
const
{
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
affine_inequality_matrix
()
const
{
return
affine_inequality_matrix_
;
}
const
Eigen
::
MatrixXd
&
Q
P
Solver
::
affine_inequality_boundary
()
const
{
const
Eigen
::
MatrixXd
&
Q
p
Solver
::
affine_inequality_boundary
()
const
{
return
affine_inequality_boundary_
;
}
...
...
modules/common/math/qp_solver/qp_solver.h
浏览文件 @
e3f099ce
...
...
@@ -33,9 +33,9 @@ namespace apollo {
namespace
common
{
namespace
math
{
class
Q
P
Solver
{
class
Q
p
Solver
{
public:
Q
P
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
const
Eigen
::
MatrixXd
&
offset
,
Q
p
Solver
(
const
Eigen
::
MatrixXd
&
kernel_matrix
,
const
Eigen
::
MatrixXd
&
offset
,
const
Eigen
::
MatrixXd
&
affine_inequality_matrix
,
const
Eigen
::
MatrixXd
&
affine_inequality_boundary
,
const
Eigen
::
MatrixXd
&
affine_equality_matrix
,
...
...
modules/planning/math/smoothing_spline/spline_1d_generator.cc
浏览文件 @
e3f099ce
...
...
@@ -64,7 +64,7 @@ bool Spline1dGenerator::solve() {
const
Eigen
::
MatrixXd
&
equality_constraint_boundary
=
spline_constraint_
.
equality_constraint
().
constraint_boundary
();
qp_solver_
.
reset
(
new
apollo
::
common
::
math
::
ActiveSetQ
P
Solver
(
qp_solver_
.
reset
(
new
apollo
::
common
::
math
::
ActiveSetQ
p
Solver
(
kernel_matrix
,
offset
,
inequality_constraint_matrix
,
inequality_constraint_boundary
,
equality_constraint_matrix
,
equality_constraint_boundary
));
...
...
modules/planning/math/smoothing_spline/spline_1d_generator.h
浏览文件 @
e3f099ce
...
...
@@ -88,7 +88,7 @@ class Spline1dGenerator {
Spline1dConstraint
spline_constraint_
;
Spline1dKernel
spline_kernel_
;
std
::
unique_ptr
<
apollo
::
common
::
math
::
Q
P
Solver
>
qp_solver_
=
nullptr
;
std
::
unique_ptr
<
apollo
::
common
::
math
::
Q
p
Solver
>
qp_solver_
=
nullptr
;
Eigen
::
MatrixXd
init_x_
;
Eigen
::
MatrixXd
init_y_
;
Eigen
::
MatrixXd
init_z_
;
...
...
modules/planning/math/smoothing_spline/spline_2d_solver.cc
浏览文件 @
e3f099ce
...
...
@@ -52,7 +52,7 @@ bool Spline2dSolver::solve() {
const
Eigen
::
MatrixXd
&
equality_constraint_boundary
=
constraint_
.
equality_constraint
().
constraint_boundary
();
qp_solver_
.
reset
(
new
apollo
::
common
::
math
::
ActiveSetQ
P
Solver
(
qp_solver_
.
reset
(
new
apollo
::
common
::
math
::
ActiveSetQ
p
Solver
(
kernel_matrix
,
offset
,
inequality_constraint_matrix
,
inequality_constraint_boundary
,
equality_constraint_matrix
,
equality_constraint_boundary
));
...
...
modules/planning/math/smoothing_spline/spline_2d_solver.h
浏览文件 @
e3f099ce
...
...
@@ -51,7 +51,7 @@ class Spline2dSolver {
Spline2d
spline_
;
Spline2dKernel
kernel_
;
Spline2dConstraint
constraint_
;
std
::
unique_ptr
<
apollo
::
common
::
math
::
Q
P
Solver
>
qp_solver_
=
nullptr
;
std
::
unique_ptr
<
apollo
::
common
::
math
::
Q
p
Solver
>
qp_solver_
=
nullptr
;
};
}
// namespace planning
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
浏览文件 @
e3f099ce
...
...
@@ -36,7 +36,7 @@
namespace
apollo
{
namespace
planning
{
bool
Q
P
SplinePathGenerator
::
SetConfig
(
const
std
::
string
&
config_file
)
{
bool
Q
p
SplinePathGenerator
::
SetConfig
(
const
std
::
string
&
config_file
)
{
if
(
!
common
::
util
::
GetProtoFromFile
(
config_file
,
&
_qp_spline_path_config
))
{
AERROR
<<
"Failed to load config file "
<<
config_file
;
return
false
;
...
...
@@ -44,7 +44,7 @@ bool QPSplinePathGenerator::SetConfig(const std::string& config_file) {
return
true
;
}
bool
Q
P
SplinePathGenerator
::
generate
(
const
ReferenceLine
&
reference_line
,
bool
Q
p
SplinePathGenerator
::
generate
(
const
ReferenceLine
&
reference_line
,
const
DecisionData
&
decision_data
,
const
SpeedData
&
speed_data
,
const
common
::
TrajectoryPoint
&
init_point
,
...
...
@@ -140,7 +140,7 @@ bool QPSplinePathGenerator::generate(const ReferenceLine& reference_line,
return
true
;
}
bool
Q
P
SplinePathGenerator
::
calculate_sl_point
(
bool
Q
p
SplinePathGenerator
::
calculate_sl_point
(
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
traj_point
,
common
::
FrenetFramePoint
*
const
frenet_frame_point
)
{
...
...
@@ -173,7 +173,7 @@ bool QPSplinePathGenerator::calculate_sl_point(
return
true
;
}
bool
Q
P
SplinePathGenerator
::
init_coord_range
(
double
*
const
start_s
,
bool
Q
p
SplinePathGenerator
::
init_coord_range
(
double
*
const
start_s
,
double
*
const
end_s
)
{
// TODO(all): step 1 get current sl coordinate - with init coordinate point
double
start_point
=
std
::
max
(
_init_point
.
s
()
-
5.0
,
0.0
);
...
...
@@ -194,7 +194,7 @@ bool QPSplinePathGenerator::init_coord_range(double* const start_s,
return
true
;
}
bool
Q
P
SplinePathGenerator
::
init_smoothing_spline
(
bool
Q
p
SplinePathGenerator
::
init_smoothing_spline
(
const
ReferenceLine
&
reference_line
,
const
double
start_s
,
const
double
end_s
)
{
QpSplinePathSampler
sampler
(
_qp_spline_path_config
);
...
...
@@ -212,7 +212,7 @@ bool QPSplinePathGenerator::init_smoothing_spline(
return
true
;
}
bool
Q
P
SplinePathGenerator
::
setup_constraint
()
{
bool
Q
p
SplinePathGenerator
::
setup_constraint
()
{
Spline1dConstraint
*
spline_constraint
=
_spline_generator
->
mutable_spline_constraint
();
// add init status constraint
...
...
@@ -284,7 +284,7 @@ bool QPSplinePathGenerator::setup_constraint() {
return
true
;
}
bool
Q
P
SplinePathGenerator
::
setup_kernel
()
{
bool
Q
p
SplinePathGenerator
::
setup_kernel
()
{
Spline1dKernel
*
spline_kernel
=
_spline_generator
->
mutable_spline_kernel
();
if
(
_qp_spline_path_config
.
regularization_weight
()
>
0
)
{
...
...
@@ -311,7 +311,7 @@ bool QPSplinePathGenerator::setup_kernel() {
return
true
;
}
bool
Q
P
SplinePathGenerator
::
solve
()
{
bool
Q
p
SplinePathGenerator
::
solve
()
{
if
(
!
_spline_generator
->
solve
())
{
AERROR
<<
"Could not solve the qp problem in spline path generator."
;
return
false
;
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h
浏览文件 @
e3f099ce
...
...
@@ -36,9 +36,9 @@
namespace
apollo
{
namespace
planning
{
class
Q
P
SplinePathGenerator
{
class
Q
p
SplinePathGenerator
{
public:
Q
P
SplinePathGenerator
()
=
default
;
Q
p
SplinePathGenerator
()
=
default
;
bool
SetConfig
(
const
std
::
string
&
config_file
);
bool
generate
(
const
ReferenceLine
&
reference_line
,
const
DecisionData
&
decision_data
,
const
SpeedData
&
speed_data
,
...
...
@@ -68,7 +68,7 @@ class QPSplinePathGenerator {
double
*
const
y_diff
)
const
;
private:
Q
P
SplinePathConfig
_qp_spline_path_config
;
Q
p
SplinePathConfig
_qp_spline_path_config
;
common
::
FrenetFramePoint
_init_point
;
QpFrenetFrame
_qp_frenet_frame
;
std
::
unique_ptr
<
Spline1dGenerator
>
_spline_generator
;
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.cc
浏览文件 @
e3f099ce
...
...
@@ -27,10 +27,10 @@ namespace planning {
using
apollo
::
common
::
ErrorCode
;
using
apollo
::
common
::
Status
;
Q
PSplinePathOptimizer
::
QP
SplinePathOptimizer
(
const
std
::
string
&
name
)
Q
pSplinePathOptimizer
::
Qp
SplinePathOptimizer
(
const
std
::
string
&
name
)
:
PathOptimizer
(
name
)
{}
bool
Q
P
SplinePathOptimizer
::
Init
()
{
bool
Q
p
SplinePathOptimizer
::
Init
()
{
if
(
!
_path_generator
.
SetConfig
(
FLAGS_qp_spline_path_config_file
))
{
AERROR
<<
"Fail to set config file for path generator."
;
return
false
;
...
...
@@ -39,7 +39,7 @@ bool QPSplinePathOptimizer::Init() {
return
true
;
}
Status
Q
P
SplinePathOptimizer
::
Process
(
const
SpeedData
&
speed_data
,
Status
Q
p
SplinePathOptimizer
::
Process
(
const
SpeedData
&
speed_data
,
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
init_point
,
DecisionData
*
const
decision_data
,
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h
浏览文件 @
e3f099ce
...
...
@@ -30,9 +30,9 @@
namespace
apollo
{
namespace
planning
{
class
Q
P
SplinePathOptimizer
:
public
PathOptimizer
{
class
Q
p
SplinePathOptimizer
:
public
PathOptimizer
{
public:
explicit
Q
P
SplinePathOptimizer
(
const
std
::
string
&
name
);
explicit
Q
p
SplinePathOptimizer
(
const
std
::
string
&
name
);
bool
Init
()
override
;
private:
...
...
@@ -43,7 +43,7 @@ class QPSplinePathOptimizer : public PathOptimizer {
PathData
*
const
path_data
)
override
;
private:
Q
P
SplinePathGenerator
_path_generator
;
Q
p
SplinePathGenerator
_path_generator
;
};
}
// namespace planning
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
浏览文件 @
e3f099ce
...
...
@@ -28,7 +28,7 @@
namespace
apollo
{
namespace
planning
{
QpSplinePathSampler
::
QpSplinePathSampler
(
const
Q
P
SplinePathConfig
&
config
)
QpSplinePathSampler
::
QpSplinePathSampler
(
const
Q
p
SplinePathConfig
&
config
)
:
config_
(
config
)
{}
bool
QpSplinePathSampler
::
Sample
(
const
common
::
FrenetFramePoint
&
init_point
,
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h
浏览文件 @
e3f099ce
...
...
@@ -31,14 +31,14 @@ namespace planning {
class
QpSplinePathSampler
{
public:
explicit
QpSplinePathSampler
(
const
Q
P
SplinePathConfig
&
config
);
explicit
QpSplinePathSampler
(
const
Q
p
SplinePathConfig
&
config
);
bool
Sample
(
const
common
::
FrenetFramePoint
&
init_point
,
const
ReferenceLine
&
reference_line
,
const
double
s_lower_bound
,
const
double
s_upper_bound
,
std
::
vector
<
double
>*
const
sampling_point
);
private:
const
Q
P
SplinePathConfig
config_
;
const
Q
p
SplinePathConfig
config_
;
};
}
// namespace planning
...
...
modules/planning/planner/em/em_planner.cc
浏览文件 @
e3f099ce
...
...
@@ -55,7 +55,7 @@ void EMPlanner::RegisterOptimizers() {
return
new
DpStSpeedOptimizer
(
"DpStSpeedOptimizer"
);
});
optimizer_factory_
.
Register
(
QP_SPLINE_PATH_OPTIMIZER
,
[]()
->
Optimizer
*
{
return
new
Q
PSplinePathOptimizer
(
"QP
SplinePathOptimizer"
);
return
new
Q
pSplinePathOptimizer
(
"Qp
SplinePathOptimizer"
);
});
optimizer_factory_
.
Register
(
QP_SPLINE_ST_SPEED_OPTIMIZER
,
[]()
->
Optimizer
*
{
return
new
QpSplineStSpeedOptimizer
(
"QpSplineStSpeedOptimizer"
);
...
...
modules/planning/proto/qp_spline_path_config.proto
浏览文件 @
e3f099ce
...
...
@@ -2,7 +2,7 @@ syntax = "proto2";
package
apollo
.
planning
;
message
Q
P
SplinePathConfig
{
message
Q
p
SplinePathConfig
{
optional
uint32
spline_order
=
1
[
default
=
6
];
optional
uint32
number_of_spline
=
2
[
default
=
5
];
optional
uint32
number_of_fx_constraint_knots
=
3
[
default
=
6
];
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录