Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e5998ae8
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,发现更多精彩内容 >>
提交
e5998ae8
编写于
12月 26, 2017
作者:
L
Liangliang Zhang
提交者:
Dong Li
12月 26, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: added unblocking obstacles in poly_st_speed. (#2076)
上级
3db178a0
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
24 addition
and
5 deletion
+24
-5
modules/planning/common/path_obstacle.h
modules/planning/common/path_obstacle.h
+5
-0
modules/planning/conf/planning_config.pb.txt
modules/planning/conf/planning_config.pb.txt
+2
-0
modules/planning/proto/poly_st_speed_config.proto
modules/planning/proto/poly_st_speed_config.proto
+2
-1
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
...s/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
+8
-2
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
+6
-1
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
+1
-1
未找到文件。
modules/planning/common/path_obstacle.h
浏览文件 @
e5998ae8
...
@@ -131,6 +131,9 @@ class PathObstacle {
...
@@ -131,6 +131,9 @@ class PathObstacle {
**/
**/
static
bool
IsLateralDecision
(
const
ObjectDecisionType
&
decision
);
static
bool
IsLateralDecision
(
const
ObjectDecisionType
&
decision
);
void
SetBlockingObstacle
(
bool
blocking
)
{
is_blocking_obstacle_
=
blocking
;
}
bool
IsBlockingObstacle
()
const
{
return
is_blocking_obstacle_
;
}
private:
private:
FRIEND_TEST
(
MergeLongitudinalDecision
,
AllDecisions
);
FRIEND_TEST
(
MergeLongitudinalDecision
,
AllDecisions
);
static
ObjectDecisionType
MergeLongitudinalDecision
(
static
ObjectDecisionType
MergeLongitudinalDecision
(
...
@@ -155,6 +158,8 @@ class PathObstacle {
...
@@ -155,6 +158,8 @@ class PathObstacle {
ObjectDecisionType
lateral_decision_
;
ObjectDecisionType
lateral_decision_
;
ObjectDecisionType
longitudinal_decision_
;
ObjectDecisionType
longitudinal_decision_
;
bool
is_blocking_obstacle_
=
false
;
struct
ObjectTagCaseHash
{
struct
ObjectTagCaseHash
{
std
::
size_t
operator
()(
std
::
size_t
operator
()(
const
planning
::
ObjectDecisionType
::
ObjectTagCase
tag
)
const
{
const
planning
::
ObjectDecisionType
::
ObjectTagCase
tag
)
const
{
...
...
modules/planning/conf/planning_config.pb.txt
浏览文件 @
e5998ae8
...
@@ -158,6 +158,8 @@ em_planner_config {
...
@@ -158,6 +158,8 @@ em_planner_config {
jerk_weight: 1.0
jerk_weight: 1.0
obstacle_weight: 10.0
obstacle_weight: 10.0
unblocking_obstacle_cost: 1e3
st_boundary_config {
st_boundary_config {
boundary_buffer: 0.1
boundary_buffer: 0.1
high_speed_centric_acceleration_limit: 1.2
high_speed_centric_acceleration_limit: 1.2
...
...
modules/planning/proto/poly_st_speed_config.proto
浏览文件 @
e5998ae8
...
@@ -16,5 +16,6 @@ message PolyStSpeedConfig {
...
@@ -16,5 +16,6 @@ message PolyStSpeedConfig {
optional
double
speed_weight
=
8
;
optional
double
speed_weight
=
8
;
optional
double
jerk_weight
=
9
;
optional
double
jerk_weight
=
9
;
optional
double
obstacle_weight
=
10
;
optional
double
obstacle_weight
=
10
;
optional
apollo.planning.StBoundaryConfig
st_boundary_config
=
11
;
optional
double
unblocking_obstacle_cost
=
11
;
optional
apollo.planning.StBoundaryConfig
st_boundary_config
=
12
;
}
}
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
浏览文件 @
e5998ae8
...
@@ -87,6 +87,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
...
@@ -87,6 +87,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
DCHECK
(
path_obstacle
->
HasLongitudinalDecision
());
DCHECK
(
path_obstacle
->
HasLongitudinalDecision
());
}
}
// step 1 get boundaries
// step 1 get boundaries
auto
path_decision_copy
=
*
path_decision
;
path_decision
->
EraseStBoundaries
();
path_decision
->
EraseStBoundaries
();
if
(
boundary_mapper
.
CreateStBoundary
(
path_decision
).
code
()
==
if
(
boundary_mapper
.
CreateStBoundary
(
path_decision
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
ErrorCode
::
PLANNING_ERROR
)
{
...
@@ -94,10 +95,15 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
...
@@ -94,10 +95,15 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
"Mapping obstacle for qp st speed optimizer failed!"
);
"Mapping obstacle for qp st speed optimizer failed!"
);
}
}
std
::
vector
<
const
StBoundary
*>
boundaries
;
for
(
const
auto
*
obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
for
(
const
auto
*
obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
auto
id
=
obstacle
->
Id
();
auto
*
mutable_obstacle
=
path_decision
->
Find
(
id
);
if
(
!
obstacle
->
st_boundary
().
IsEmpty
())
{
if
(
!
obstacle
->
st_boundary
().
IsEmpty
())
{
boundaries
.
push_back
(
&
obstacle
->
st_boundary
());
mutable_obstacle
->
SetBlockingObstacle
(
true
);
}
else
{
path_decision
->
SetStBoundary
(
id
,
path_decision_copy
.
Find
(
id
)
->
st_boundary
());
}
}
}
}
...
...
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
浏览文件 @
e5998ae8
...
@@ -85,7 +85,8 @@ double SpeedProfileCost::CalculatePointCost(
...
@@ -85,7 +85,8 @@ double SpeedProfileCost::CalculatePointCost(
if
(
t
<
boundary
.
min_t
()
||
t
>
boundary
.
max_t
())
{
if
(
t
<
boundary
.
min_t
()
||
t
>
boundary
.
max_t
())
{
continue
;
continue
;
}
}
if
(
boundary
.
IsPointInBoundary
(
STPoint
(
s
,
t
)))
{
if
(
obstacle
->
IsBlockingObstacle
()
&&
boundary
.
IsPointInBoundary
(
STPoint
(
s
,
t
)))
{
return
kInfCost
;
return
kInfCost
;
}
}
double
s_upper
=
0.0
;
double
s_upper
=
0.0
;
...
@@ -106,6 +107,10 @@ double SpeedProfileCost::CalculatePointCost(
...
@@ -106,6 +107,10 @@ double SpeedProfileCost::CalculatePointCost(
cost
+=
config_
.
obstacle_weight
()
*
cost
+=
config_
.
obstacle_weight
()
*
std
::
pow
((
kSafeDistance
+
s_upper
-
s
),
2
);
std
::
pow
((
kSafeDistance
+
s_upper
-
s
),
2
);
}
}
}
else
{
if
(
!
obstacle
->
IsBlockingObstacle
())
{
cost
+=
config_
.
unblocking_obstacle_cost
();
}
}
}
}
}
cost
+=
config_
.
speed_weight
()
*
std
::
pow
((
v
-
speed_limit
),
2
);
cost
+=
config_
.
speed_weight
()
*
std
::
pow
((
v
-
speed_limit
),
2
);
...
...
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
浏览文件 @
e5998ae8
...
@@ -15,7 +15,7 @@
...
@@ -15,7 +15,7 @@
*****************************************************************************/
*****************************************************************************/
/**
/**
* @file
trajectory_cost.h
* @file
**/
**/
#ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
#ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录