Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
436fe6a6
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,发现更多精彩内容 >>
提交
436fe6a6
编写于
9月 05, 2017
作者:
A
Aaron Xiao
提交者:
Jiangtao Hu
9月 05, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: Extract PathDecider::MakeStaticObstacleDecision to shorter functions.
上级
160b7b4b
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
91 addition
and
84 deletion
+91
-84
modules/planning/proto/decision.proto
modules/planning/proto/decision.proto
+3
-2
modules/planning/proto/sl_boundary.proto
modules/planning/proto/sl_boundary.proto
+1
-1
modules/planning/tasks/path_decider/path_decider.cc
modules/planning/tasks/path_decider/path_decider.cc
+81
-81
modules/planning/tasks/path_decider/path_decider.h
modules/planning/tasks/path_decider/path_decider.h
+6
-0
未找到文件。
modules/planning/proto/decision.proto
浏览文件 @
436fe6a6
...
...
@@ -41,8 +41,9 @@ message ObjectStop {
// dodge the obstacle in lateral direction when driving
message
ObjectNudge
{
enum
Type
{
LEFT_NUDGE
=
1
;
// drive from the left side of the obstacle
RIGHT_NUDGE
=
2
;
// drive from the right side of the obstacle
LEFT_NUDGE
=
1
;
// drive from the left side of the obstacle
RIGHT_NUDGE
=
2
;
// drive from the right side of the obstacle
NO_NUDGE
=
3
;
// No nudge is set.
};
optional
Type
type
=
1
;
// minimum lateral distance in meters. positive if type = LEFT_NUDGE
...
...
modules/planning/proto/sl_boundary.proto
浏览文件 @
436fe6a6
...
...
@@ -14,7 +14,7 @@ package apollo.planning;
// start_s
//
// The start_l and end_l are lateral values.
// start_l <= end_l. Left side of the reference line is
is
positive,
// start_l <= end_l. Left side of the reference line is positive,
// and right side of the reference line is negative.
// end_l <-----L direction---- start_l
/////////////////////////////////////////////////////////////////
...
...
modules/planning/tasks/path_decider/path_decider.cc
浏览文件 @
436fe6a6
...
...
@@ -86,9 +86,8 @@ bool PathDecider::MakeStaticObstacleDecision(
AERROR
<<
"get_point_in_Frenet_frame error for ego vehicle "
<<
path_point
.
x
()
<<
" "
<<
path_point
.
y
();
return
false
;
}
else
{
adc_sl_points
.
push_back
(
std
::
move
(
adc_sl
));
}
adc_sl_points
.
push_back
(
std
::
move
(
adc_sl
));
}
const
double
adc_allowed_s_radius
=
...
...
@@ -96,8 +95,8 @@ bool PathDecider::MakeStaticObstacleDecision(
const
double
adc_allowed_l_radius
=
adc_max_edge_to_center_dist
+
FLAGS_static_decision_nudge_l_buffer
;
for
(
const
auto
*
path_obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
const
auto
*
obstacle
=
path_obstacle
->
obstacle
();
if
(
!
obstacle
->
IsStatic
())
{
const
auto
&
obstacle
=
*
path_obstacle
->
obstacle
();
if
(
!
obstacle
.
IsStatic
())
{
continue
;
}
...
...
@@ -121,95 +120,30 @@ bool PathDecider::MakeStaticObstacleDecision(
}
// check STOP/NUDGE
double
left_width
=
0.0
;
double
right_width
=
0.0
;
if
(
!
reference_line
.
GetLaneWidth
(
adc_sl
.
s
(),
&
left_width
,
&
right_width
))
{
left_width
=
right_width
=
FLAGS_default_reference_line_width
/
2
;
}
double
driving_width
;
bool
left_nudgable
=
false
;
bool
right_nudgable
=
false
;
if
(
sl_boundary
.
start_l
()
>=
0
)
{
// obstacle on the left, check RIGHT_NUDGE
driving_width
=
sl_boundary
.
start_l
()
-
FLAGS_static_decision_nudge_l_buffer
+
right_width
;
right_nudgable
=
(
driving_width
>=
FLAGS_min_driving_width
)
?
true
:
false
;
}
else
if
(
sl_boundary
.
end_l
()
<=
0
)
{
// obstacle on the right, check LEFT_NUDGE
driving_width
=
std
::
fabs
(
sl_boundary
.
end_l
())
-
FLAGS_static_decision_nudge_l_buffer
+
left_width
;
left_nudgable
=
(
driving_width
>=
FLAGS_min_driving_width
)
?
true
:
false
;
}
else
{
// obstacle across the central line, decide RIGHT_NUDGE/LEFT_NUDGE
double
driving_width_left
=
left_width
-
sl_boundary
.
end_l
()
-
FLAGS_static_decision_nudge_l_buffer
;
double
driving_width_right
=
right_width
-
std
::
fabs
(
sl_boundary
.
start_l
())
-
FLAGS_static_decision_nudge_l_buffer
;
if
(
std
::
max
(
driving_width_right
,
driving_width_left
)
>=
FLAGS_min_driving_width
)
{
// nudgable
left_nudgable
=
driving_width_left
>
driving_width_right
?
true
:
false
;
right_nudgable
=
!
left_nudgable
;
}
}
if
(
!
left_nudgable
&&
!
right_nudgable
)
{
const
auto
nudge_type
=
DecideNudge
(
sl_boundary
,
adc_sl
);
if
(
nudge_type
==
ObjectNudge
::
NO_NUDGE
)
{
// STOP: and break
ObjectStop
*
object_stop_ptr
=
object_decision
.
mutable_stop
();
double
stop_distance
=
0
;
if
(
obstacle
->
Id
()
==
FLAGS_destination_obstacle_id
)
{
// destination
object_stop_ptr
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_DESTINATION
);
stop_distance
=
FLAGS_stop_distance_destination
;
}
else
{
// static obstacle
object_stop_ptr
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_OBSTACLE
);
stop_distance
=
FLAGS_stop_distance_obstacle
;
}
object_stop_ptr
->
set_distance_s
(
-
stop_distance
);
double
stop_ref_s
=
sl_boundary
.
start_s
()
-
stop_distance
;
auto
stop_ref_point
=
reference_line
.
GetReferencePoint
(
stop_ref_s
);
object_stop_ptr
->
mutable_stop_point
()
->
set_x
(
stop_ref_point
.
x
());
object_stop_ptr
->
mutable_stop_point
()
->
set_y
(
stop_ref_point
.
y
());
object_stop_ptr
->
set_stop_heading
(
stop_ref_point
.
heading
());
*
object_decision
.
mutable_stop
()
=
GenerateObjectStopDecision
(
*
path_obstacle
);
has_stop
=
true
;
break
;
}
else
{
}
else
if
(
FLAGS_enable_nudge_decision
&&
!
object_decision
.
has_nudge
())
{
// NUDGE: and continue to check potential STOP along the ref line
if
(
!
FLAGS_enable_nudge_decision
)
{
continue
;
}
if
(
!
object_decision
.
has_nudge
())
{
ObjectNudge
*
object_nudge_ptr
=
object_decision
.
mutable_nudge
();
if
(
left_nudgable
)
{
// LEFT_NUDGE
object_nudge_ptr
->
set_type
(
ObjectNudge
::
LEFT_NUDGE
);
object_nudge_ptr
->
set_distance_l
(
FLAGS_nudge_distance_obstacle
);
}
else
{
// RIGHT_NUDGE
object_nudge_ptr
->
set_type
(
ObjectNudge
::
RIGHT_NUDGE
);
object_nudge_ptr
->
set_distance_l
(
-
FLAGS_nudge_distance_obstacle
);
}
}
ObjectNudge
*
object_nudge_ptr
=
object_decision
.
mutable_nudge
();
object_nudge_ptr
->
set_type
(
nudge_type
);
object_nudge_ptr
->
set_distance_l
(
nudge_type
==
ObjectNudge
::
LEFT_NUDGE
?
FLAGS_nudge_distance_obstacle
:
-
FLAGS_nudge_distance_obstacle
);
}
}
if
(
has_stop
)
{
// STOP
path_decision
->
AddLongitudinalDecision
(
"PathDecider"
,
obstacle
->
Id
(),
path_decision
->
AddLongitudinalDecision
(
"PathDecider"
,
obstacle
.
Id
(),
object_decision
);
}
else
{
// NUDGE / IGNORE
path_decision
->
AddLateralDecision
(
"PathDecider"
,
obstacle
->
Id
(),
path_decision
->
AddLateralDecision
(
"PathDecider"
,
obstacle
.
Id
(),
object_decision
);
}
}
...
...
@@ -217,5 +151,71 @@ bool PathDecider::MakeStaticObstacleDecision(
return
true
;
}
ObjectNudge
::
Type
PathDecider
::
DecideNudge
(
const
SLBoundary
&
obstacle_boundary
,
const
common
::
SLPoint
&
adc_sl
)
{
double
left_width
=
0.0
;
double
right_width
=
0.0
;
if
(
!
reference_line_info_
->
reference_line
().
GetLaneWidth
(
adc_sl
.
s
(),
&
left_width
,
&
right_width
))
{
left_width
=
right_width
=
FLAGS_default_reference_line_width
/
2
;
}
// obstacle on the left, check RIGHT_NUDGE
if
(
obstacle_boundary
.
start_l
()
>=
0
)
{
const
double
driving_width
=
obstacle_boundary
.
start_l
()
-
FLAGS_static_decision_nudge_l_buffer
+
right_width
;
return
driving_width
>=
FLAGS_min_driving_width
?
ObjectNudge
::
RIGHT_NUDGE
:
ObjectNudge
::
NO_NUDGE
;
}
// obstacle on the right, check LEFT_NUDGE
if
(
obstacle_boundary
.
end_l
()
<=
0
)
{
const
double
driving_width
=
std
::
fabs
(
obstacle_boundary
.
end_l
())
-
FLAGS_static_decision_nudge_l_buffer
+
left_width
;
return
driving_width
>=
FLAGS_min_driving_width
?
ObjectNudge
::
LEFT_NUDGE
:
ObjectNudge
::
NO_NUDGE
;
}
// obstacle across the central line, decide RIGHT_NUDGE/LEFT_NUDGE
double
driving_width_left
=
left_width
-
obstacle_boundary
.
end_l
()
-
FLAGS_static_decision_nudge_l_buffer
;
double
driving_width_right
=
right_width
-
std
::
fabs
(
obstacle_boundary
.
start_l
())
-
FLAGS_static_decision_nudge_l_buffer
;
if
(
std
::
max
(
driving_width_right
,
driving_width_left
)
>=
FLAGS_min_driving_width
)
{
// Nudge is possible, then select the wider side.
return
driving_width_left
>
driving_width_right
?
ObjectNudge
::
LEFT_NUDGE
:
ObjectNudge
::
RIGHT_NUDGE
;
}
// Nudge is impossible.
return
ObjectNudge
::
NO_NUDGE
;
}
ObjectStop
PathDecider
::
GenerateObjectStopDecision
(
const
PathObstacle
&
path_obstacle
)
const
{
ObjectStop
object_stop
;
double
stop_distance
=
0
;
if
(
path_obstacle
.
obstacle
()
->
Id
()
==
FLAGS_destination_obstacle_id
)
{
// destination
object_stop
.
set_reason_code
(
StopReasonCode
::
STOP_REASON_DESTINATION
);
stop_distance
=
FLAGS_stop_distance_destination
;
}
else
{
// static obstacle
object_stop
.
set_reason_code
(
StopReasonCode
::
STOP_REASON_OBSTACLE
);
stop_distance
=
FLAGS_stop_distance_obstacle
;
}
object_stop
.
set_distance_s
(
-
stop_distance
);
const
double
stop_ref_s
=
path_obstacle
.
perception_sl_boundary
().
start_s
()
-
stop_distance
;
const
auto
stop_ref_point
=
reference_line_info_
->
reference_line
().
GetReferencePoint
(
stop_ref_s
);
object_stop
.
mutable_stop_point
()
->
set_x
(
stop_ref_point
.
x
());
object_stop
.
mutable_stop_point
()
->
set_y
(
stop_ref_point
.
y
());
object_stop
.
set_stop_heading
(
stop_ref_point
.
heading
());
return
object_stop
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/tasks/path_decider/path_decider.h
浏览文件 @
436fe6a6
...
...
@@ -44,6 +44,12 @@ class PathDecider : public Task {
bool
MakeStaticObstacleDecision
(
const
PathData
&
path_data
,
PathDecision
*
const
path_decision
);
ObjectNudge
::
Type
DecideNudge
(
const
SLBoundary
&
obstacle_boundary
,
const
common
::
SLPoint
&
adc_sl
);
ObjectStop
GenerateObjectStopDecision
(
const
PathObstacle
&
path_obstacle
)
const
;
};
}
// namespace planning
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录