Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
87ff64e9
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,发现更多精彩内容 >>
提交
87ff64e9
编写于
8月 03, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
8月 03, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
remove qp_spline_path_sampler.
上级
deb40355
变更
5
隐藏空白更改
内联
并排
Showing
5 changed file
with
49 addition
and
185 deletion
+49
-185
modules/planning/optimizer/qp_spline_path/BUILD
modules/planning/optimizer/qp_spline_path/BUILD
+1
-3
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
...ning/optimizer/qp_spline_path/qp_spline_path_generator.cc
+43
-56
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h
...nning/optimizer/qp_spline_path/qp_spline_path_generator.h
+5
-4
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
...anning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
+0
-75
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h
...lanning/optimizer/qp_spline_path/qp_spline_path_sampler.h
+0
-47
未找到文件。
modules/planning/optimizer/qp_spline_path/BUILD
浏览文件 @
87ff64e9
...
...
@@ -8,13 +8,11 @@ cc_library(
"qp_frenet_frame.cc"
,
"qp_spline_path_generator.cc"
,
"qp_spline_path_optimizer.cc"
,
"qp_spline_path_sampler.cc"
,
],
hdrs
=
[
"qp_frenet_frame.h"
,
"qp_spline_path_generator.h"
,
"qp_spline_path_optimizer.h"
,
"qp_spline_path_sampler.h"
,
],
deps
=
[
"//modules/common/configs:vehicle_config_helper"
,
...
...
@@ -26,7 +24,7 @@ cc_library(
"//modules/planning/common:decision_data"
,
"//modules/planning/common:obstacle"
,
"//modules/planning/common:planning_data"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/common/path:discretized_path"
,
"//modules/planning/common/path:frenet_frame_path"
,
"//modules/planning/common/path:path_data"
,
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
浏览文件 @
87ff64e9
...
...
@@ -32,7 +32,6 @@
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"
#include "modules/planning/math/sl_analytic_transformation.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h"
namespace
apollo
{
namespace
planning
{
...
...
@@ -50,16 +49,30 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line,
const
SpeedData
&
speed_data
,
const
common
::
TrajectoryPoint
&
init_point
,
PathData
*
const
path_data
)
{
if
(
!
calculate_sl_point
(
reference_line
,
init_point
,
&
_init_point
))
{
if
(
!
calculate_sl_point
(
reference_line
,
init_point
,
&
init_point_
))
{
AERROR
<<
"Fail to map init point: "
<<
init_point
.
ShortDebugString
();
return
false
;
}
double
start_s
=
_init_point
.
s
();
double
start_s
=
init_point_
.
s
();
double
end_s
=
std
::
min
(
reference_line
.
length
(),
_init_point
.
s
()
+
FLAGS_planning_distance
);
init_point_
.
s
()
+
FLAGS_planning_distance
);
if
(
qp_spline_path_config_
.
number_of_knots
()
<=
1
)
{
AERROR
<<
"Two few number of knots: "
<<
qp_spline_path_config_
.
number_of_knots
();
return
false
;
}
const
double
delta_s
=
(
end_s
-
start_s
)
/
qp_spline_path_config_
.
number_of_knots
();
double
curr_knot_s
=
start_s
+
delta_s
;
for
(
uint32_t
i
=
0
;
i
<=
qp_spline_path_config_
.
number_of_knots
();
++
i
)
{
fx_knots_
.
push_back
(
curr_knot_s
);
curr_knot_s
+=
delta_s
;
}
QpFrenetFrame
qp_frenet_frame
(
reference_line
,
decision_data
,
speed_data
,
_init_point
,
start_s
,
end_s
,
init_point_
,
start_s
,
end_s
,
qp_spline_path_config_
.
time_resolution
());
if
(
!
qp_frenet_frame
.
Init
(
qp_spline_path_config_
.
num_output
()))
{
AERROR
<<
"Fail to initialize qp frenet frame"
;
...
...
@@ -72,7 +85,7 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line,
}
AINFO
<<
"pss path start with "
<<
start_s
<<
", end with "
<<
end_s
;
if
(
!
init_smoothing_spline
(
reference_line
,
start_s
,
end_s
))
{
if
(
!
init_smoothing_spline
())
{
AERROR
<<
"Init smoothing spline failed with ("
<<
start_s
<<
", end_s "
<<
end_s
;
return
false
;
...
...
@@ -91,16 +104,16 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line,
return
false
;
}
AINFO
<<
common
::
util
::
StrCat
(
"Spline dl:"
,
_init_point
.
dl
(),
", ddl:"
,
_init_point
.
ddl
());
AINFO
<<
common
::
util
::
StrCat
(
"Spline dl:"
,
init_point_
.
dl
(),
", ddl:"
,
init_point_
.
ddl
());
// extract data
const
Spline1d
&
spline
=
_spline_generator
->
spline
();
const
Spline1d
&
spline
=
spline_generator_
->
spline
();
std
::
vector
<
common
::
PathPoint
>
path_points
;
double
start_l
=
spline
(
_init_point
.
s
());
double
start_l
=
spline
(
init_point_
.
s
());
ReferencePoint
ref_point
=
reference_line
.
get_reference_point
(
_init_point
.
s
());
reference_line
.
get_reference_point
(
init_point_
.
s
());
common
::
math
::
Vec2d
xy_point
=
SLAnalyticTransformation
::
calculate_xypoint
(
ref_point
.
heading
(),
common
::
math
::
Vec2d
(
ref_point
.
x
(),
ref_point
.
y
()),
start_l
);
...
...
@@ -108,9 +121,9 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line,
double
x_diff
=
xy_point
.
x
()
-
init_point
.
path_point
().
x
();
double
y_diff
=
xy_point
.
y
()
-
init_point
.
path_point
().
y
();
double
s
=
_init_point
.
s
();
double
s
=
init_point_
.
s
();
double
s_resolution
=
(
end_s
-
_init_point
.
s
())
/
qp_spline_path_config_
.
num_output
();
(
end_s
-
init_point_
.
s
())
/
qp_spline_path_config_
.
num_output
();
while
(
Double
::
compare
(
s
,
end_s
)
<
0
)
{
double
l
=
spline
(
s
);
double
dl
=
spline
.
derivative
(
s
);
...
...
@@ -179,7 +192,7 @@ bool QpSplinePathGenerator::init_coord_range(
const
QpFrenetFrame
&
qp_frenet_frame
,
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
);
double
start_point
=
std
::
max
(
init_point_
.
s
()
-
5.0
,
0.0
);
const
ReferenceLine
&
reference_line
=
qp_frenet_frame
.
GetReferenceLine
();
...
...
@@ -193,40 +206,28 @@ bool QpSplinePathGenerator::init_coord_range(
return
true
;
}
bool
QpSplinePathGenerator
::
init_smoothing_spline
(
const
ReferenceLine
&
reference_line
,
const
double
start_s
,
const
double
end_s
)
{
QpSplinePathSampler
sampler
(
qp_spline_path_config_
);
// TODO(all): refine here, add end_s tolorence here
std
::
vector
<
double
>
sampling_point
;
if
(
!
sampler
.
Sample
(
_init_point
,
reference_line
,
start_s
,
end_s
-
0.1
,
&
sampling_point
))
{
AERROR
<<
"Qp spline sampler failed!"
;
return
false
;
}
_spline_generator
.
reset
(
new
Spline1dGenerator
(
sampling_point
,
qp_spline_path_config_
.
spline_order
()));
bool
QpSplinePathGenerator
::
init_smoothing_spline
()
{
spline_generator_
.
reset
(
new
Spline1dGenerator
(
fx_knots_
,
qp_spline_path_config_
.
spline_order
()));
return
true
;
}
bool
QpSplinePathGenerator
::
setup_constraint
(
const
QpFrenetFrame
&
qp_frenet_frame
)
{
Spline1dConstraint
*
spline_constraint
=
_spline_generator
->
mutable_spline_constraint
();
spline_generator_
->
mutable_spline_constraint
();
// add init status constraint
spline_constraint
->
add_point_fx_constraint
(
_init_point
.
s
(),
_init_point
.
l
());
spline_constraint
->
add_point_derivative_constraint
(
_init_point
.
s
(),
_init_point
.
dl
());
spline_constraint
->
add_point_second_derivative_constraint
(
_init_point
.
s
(),
_init_point
.
ddl
());
AINFO
<<
"init frenet point: "
<<
_init_point
.
ShortDebugString
();
spline_constraint
->
add_point_fx_constraint
(
init_point_
.
s
(),
init_point_
.
l
());
spline_constraint
->
add_point_derivative_constraint
(
init_point_
.
s
(),
init_point_
.
dl
());
spline_constraint
->
add_point_second_derivative_constraint
(
init_point_
.
s
(),
init_point_
.
ddl
());
AINFO
<<
"init frenet point: "
<<
init_point_
.
ShortDebugString
();
// add end point constraint
const
std
::
vector
<
double
>
spline_knots
=
_spline_generator
->
spline
().
x_knots
();
spline_generator_
->
spline
().
x_knots
();
if
(
spline_knots
.
size
()
<
2
)
{
AERROR
<<
common
::
util
::
StrCat
(
"Smoothing spline knot size("
,
spline_knots
.
size
(),
") < 2"
);
...
...
@@ -289,7 +290,7 @@ bool QpSplinePathGenerator::setup_constraint(
}
bool
QpSplinePathGenerator
::
setup_kernel
()
{
Spline1dKernel
*
spline_kernel
=
_spline_generator
->
mutable_spline_kernel
();
Spline1dKernel
*
spline_kernel
=
spline_generator_
->
mutable_spline_kernel
();
if
(
qp_spline_path_config_
.
regularization_weight
()
>
0
)
{
spline_kernel
->
add_regularization
(
...
...
@@ -312,30 +313,16 @@ bool QpSplinePathGenerator::setup_kernel() {
}
// reference line kernel
if
(
qp_spline_path_config_
.
number_of_fx_constraint_knots
()
>
1
)
{
std
::
vector
<
double
>
fx_knots
;
std
::
vector
<
double
>
l_vec
;
const
std
::
vector
<
double
>
spline_knots
=
_spline_generator
->
spline
().
x_knots
();
double
ds
=
(
spline_knots
.
back
()
-
spline_knots
.
front
())
/
qp_spline_path_config_
.
number_of_fx_constraint_knots
();
double
s
=
spline_knots
.
front
();
for
(
std
::
uint32_t
i
=
0
;
i
<
qp_spline_path_config_
.
number_of_fx_constraint_knots
()
+
1
;
++
i
)
{
fx_knots
.
push_back
(
s
);
l_vec
.
push_back
(
0.0
);
s
+=
ds
;
}
if
(
qp_spline_path_config_
.
number_of_knots
()
>
1
)
{
std
::
vector
<
double
>
l_vec
(
fx_knots_
.
size
(),
0.0
);
spline_kernel
->
add_reference_line_kernel_matrix
(
fx_knots
,
l_vec
,
qp_spline_path_config_
.
reference_line_weight
());
fx_knots
_
,
l_vec
,
qp_spline_path_config_
.
reference_line_weight
());
}
return
true
;
}
bool
QpSplinePathGenerator
::
solve
()
{
if
(
!
_spline_generator
->
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
浏览文件 @
87ff64e9
...
...
@@ -53,8 +53,7 @@ class QpSplinePathGenerator {
bool
init_coord_range
(
const
QpFrenetFrame
&
qp_frenet_frame
,
double
*
const
start_s
,
double
*
const
end_s
);
bool
init_smoothing_spline
(
const
ReferenceLine
&
reference_line
,
const
double
start_s
,
const
double
end_s
);
bool
init_smoothing_spline
();
bool
setup_constraint
(
const
QpFrenetFrame
&
qp_frenet_frame
);
...
...
@@ -70,8 +69,10 @@ class QpSplinePathGenerator {
private:
QpSplinePathConfig
qp_spline_path_config_
;
common
::
FrenetFramePoint
_init_point
;
std
::
unique_ptr
<
Spline1dGenerator
>
_spline_generator
;
common
::
FrenetFramePoint
init_point_
;
std
::
unique_ptr
<
Spline1dGenerator
>
spline_generator_
;
std
::
vector
<
double
>
fx_knots_
;
};
}
// namespace planning
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.cc
已删除
100644 → 0
浏览文件 @
deb40355
/******************************************************************************
* Copyright 2017 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.
*****************************************************************************/
/**
* @file qp_spline_path_simple_sampler.cc
**/
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h"
#include <algorithm>
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"
namespace
apollo
{
namespace
planning
{
QpSplinePathSampler
::
QpSplinePathSampler
(
const
QpSplinePathConfig
&
config
)
:
config_
(
config
)
{}
bool
QpSplinePathSampler
::
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
)
{
double
sampling_distance
=
std
::
min
(
reference_line
.
map_path
().
length
(),
s_upper_bound
)
-
init_point
.
s
();
sampling_distance
=
std
::
min
(
sampling_distance
,
FLAGS_planning_distance
);
if
(
Double
::
compare
(
sampling_distance
,
0.0
)
<=
0
)
{
AERROR
<<
"Failed to allocate init trajectory point SL("
<<
init_point
.
ShortDebugString
()
<<
") on target lane with length "
<<
reference_line
.
map_path
().
length
();
return
false
;
}
if
(
s_lower_bound
>
init_point
.
s
())
{
AERROR
<<
"The coordinate system s lower bound "
<<
s_lower_bound
<<
" is greater than projected car position "
<<
init_point
.
s
();
return
false
;
}
double
start_s
=
init_point
.
s
();
sampling_point
->
emplace_back
(
start_s
);
// map sampling point
if
(
config_
.
number_of_knots
()
==
0
)
{
AERROR
<<
"sampling point shall greater than 0"
;
return
false
;
}
const
double
delta_s
=
sampling_distance
/
config_
.
number_of_knots
();
double
s
=
start_s
+
delta_s
;
for
(
uint32_t
i
=
1
;
i
<=
config_
.
number_of_knots
();
++
i
)
{
sampling_point
->
emplace_back
(
s
);
s
+=
delta_s
;
}
return
true
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/optimizer/qp_spline_path/qp_spline_path_sampler.h
已删除
100644 → 0
浏览文件 @
deb40355
/******************************************************************************
* Copyright 2017 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.
*****************************************************************************/
/**
* @file qp_spline_path_sampler.h
**/
#ifndef MODULES_PLANNING_OPTIMIZER_QP_SPLINE_PATH_QP_SPLINE_PATH_SAMPLER_H_
#define MODULES_PLANNING_OPTIMIZER_QP_SPLINE_PATH_QP_SPLINE_PATH_SAMPLER_H_
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/qp_spline_path_config.pb.h"
#include "modules/planning/reference_line/reference_line.h"
namespace
apollo
{
namespace
planning
{
class
QpSplinePathSampler
{
public:
explicit
QpSplinePathSampler
(
const
QpSplinePathConfig
&
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
QpSplinePathConfig
config_
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_OPTIMIZER_QP_SPLINE_PATH_QP_SPLINE_PATH_SAMPLER_H_
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录