Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
824d12ac
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,发现更多精彩内容 >>
提交
824d12ac
编写于
8月 14, 2017
作者:
Z
Zhang Liangliang
提交者:
Dong Li
8月 14, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: Fixed bugs when calculating the index range in dp_st_graph.
TODO: fix boundary_byte not supported issue.
上级
575cf7d0
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
30 addition
and
18 deletion
+30
-18
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
+24
-14
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
...s/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
+2
-1
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
...imizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
+4
-3
未找到文件。
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
浏览文件 @
824d12ac
...
...
@@ -106,6 +106,7 @@ Status DpStGraph::InitCostTable() {
for
(
uint32_t
i
=
0
;
i
<
cost_table_
.
size
();
++
i
)
{
for
(
uint32_t
j
=
0
;
j
<
cost_table_
[
i
].
size
();
++
j
)
{
cost_table_
[
i
][
j
].
init
(
i
,
j
,
STPoint
(
unit_s_
*
j
,
unit_t_
*
i
));
cost_table_
[
i
][
j
].
set_total_cost
(
std
::
numeric_limits
<
double
>::
infinity
());
}
}
...
...
@@ -137,6 +138,13 @@ void DpStGraph::CalculatePointwiseCost(
Status
DpStGraph
::
CalculateTotalCost
(
const
StGraphData
&
st_graph_data
)
{
// s corresponding to row
// time corresponding to col
/*
for (uint32_t c = 0; c < cost_table_.size(); ++c) {
for (uint32_t r = 0; r < cost_table_.back().size(); ++r) {
CalculateCostAt(st_graph_data, c, r);
}
}
*/
uint32_t
c
=
0
;
uint32_t
next_highest_row
=
0
;
uint32_t
next_lowest_row
=
0
;
...
...
@@ -145,15 +153,15 @@ Status DpStGraph::CalculateTotalCost(const StGraphData& st_graph_data) {
uint32_t
highest_row
=
0
;
uint32_t
lowest_row
=
cost_table_
.
back
().
size
()
-
1
;
for
(
uint32_t
r
=
next_lowest_row
;
r
<=
next_highest_row
;
++
r
)
{
if
(
cost_table_
.
at
(
c
).
at
(
r
).
pre_point
()
==
nullptr
)
{
continue
;
}
CalculateCostAt
(
st_graph_data
,
c
,
r
);
uint32_t
h_r
=
0
;
uint32_t
l_r
=
0
;
GetRowRange
(
r
,
c
,
&
h_r
,
&
l_r
);
highest_row
=
std
::
max
(
highest_row
,
h_r
);
lowest_row
=
std
::
min
(
lowest_row
,
l_r
);
if
(
cost_table_
.
at
(
c
).
at
(
r
).
total_cost
()
<
std
::
numeric_limits
<
double
>::
infinity
())
{
GetRowRange
(
r
,
c
,
&
h_r
,
&
l_r
);
highest_row
=
std
::
max
(
highest_row
,
h_r
);
lowest_row
=
std
::
min
(
lowest_row
,
l_r
);
}
}
++
c
;
next_highest_row
=
highest_row
;
...
...
@@ -167,29 +175,31 @@ void DpStGraph::GetRowRange(const uint32_t curr_row, const uint32_t curr_col,
uint32_t
*
next_highest_row
,
uint32_t
*
next_lowest_row
)
{
// TODO: finish the implementation of this function.
*
next_highest_row
=
cost_table_
.
back
().
size
()
-
1
;
*
next_lowest_row
=
0
;
const
auto
&
curr_point
=
cost_table_
[
curr_col
][
curr_row
];
*
next_highest_row
=
cost_table_
.
back
().
size
()
-
1
;
*
next_lowest_row
=
curr_point
.
index_s
();
double
v0
=
0.0
;
if
(
curr_col
==
0
)
{
v0
=
init_point_
.
v
();
}
else
{
const
auto
*
pre_point
=
curr_point
.
pre_point
();
DCHECK_NOTNULL
(
pre_point
);
v0
=
(
curr_point
.
index_s
()
-
pre_point
->
index_s
())
/
unit_t_
;
v0
=
(
curr_point
.
index_s
()
-
pre_point
->
index_s
())
*
unit_s_
/
unit_t_
;
}
const
double
s_upper_bound
=
const
double
delta_
s_upper_bound
=
v0
*
unit_t_
+
0.5
*
vehicle_param_
.
max_acceleration
()
*
unit_t_
*
unit_t_
;
*
next_highest_row
=
std
::
min
(
static_cast
<
uint32_t
>
(
cost_table_
.
back
().
size
()
-
1
),
*
next_lowest_row
+
static_cast
<
uint32_t
>
(
s_upper_bound
/
unit_s_
));
*
next_lowest_row
+
static_cast
<
uint32_t
>
(
delta_
s_upper_bound
/
unit_s_
));
const
double
s_lower_bound
=
const
double
delta_
s_lower_bound
=
v0
*
unit_t_
+
0.5
*
vehicle_param_
.
max_deceleration
()
*
unit_t_
*
unit_t_
;
*
next_lowest_row
=
*
next_lowest_row
+
std
::
max
(
0
,
static_cast
<
int32_t
>
(
s_lower_bound
/
unit_s_
));
*
next_lowest_row
=
*
next_lowest_row
+
std
::
max
(
0
,
static_cast
<
int32_t
>
(
delta_s_lower_bound
/
unit_s_
));
}
void
DpStGraph
::
CalculateCostAt
(
const
StGraphData
&
st_graph_data
,
...
...
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @
824d12ac
...
...
@@ -74,7 +74,8 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
// step 1 get boundaries
std
::
vector
<
StGraphBoundary
>
boundaries
;
if
(
!
boundary_mapper
.
GetGraphBoundary
(
*
path_decision
,
&
boundaries
).
ok
())
{
if
(
boundary_mapper
.
GetGraphBoundary
(
*
path_decision
,
&
boundaries
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
const
std
::
string
msg
=
"Mapping obstacle for dp st speed optimizer failed."
;
AERROR
<<
msg
;
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @
824d12ac
...
...
@@ -77,14 +77,15 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
// step 1 get boundaries
std
::
vector
<
StGraphBoundary
>
boundaries
;
if
(
!
boundary_mapper
.
GetGraphBoundary
(
*
path_decision
,
&
boundaries
).
ok
())
{
if
(
boundary_mapper
.
GetGraphBoundary
(
*
path_decision
,
&
boundaries
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Mapping obstacle for
d
p st speed optimizer failed!"
);
"Mapping obstacle for
q
p st speed optimizer failed!"
);
}
SpeedLimit
speed_limits
;
if
(
boundary_mapper
.
GetSpeedLimits
(
&
speed_limits
)
!=
Status
::
OK
())
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"
Mapping obstacle
for dp st speed optimizer failed!"
);
"
GetSpeedLimits
for dp st speed optimizer failed!"
);
}
// step 2 perform graph search
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录