Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
6742e057
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,发现更多精彩内容 >>
提交
6742e057
编写于
8月 14, 2017
作者:
D
Dong Li
提交者:
lianglia-apollo
8月 14, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
added decision category: use the merged longitutional or lateral decision
上级
3cec2f64
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
214 addition
and
200 deletion
+214
-200
modules/planning/common/path_obstacle.cc
modules/planning/common/path_obstacle.cc
+16
-4
modules/planning/common/path_obstacle.h
modules/planning/common/path_obstacle.h
+6
-6
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
+2
-3
modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc
modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc
+103
-98
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
+42
-45
modules/planning/planner/em/decider.cc
modules/planning/planner/em/decider.cc
+45
-44
未找到文件。
modules/planning/common/path_obstacle.cc
浏览文件 @
6742e057
...
@@ -131,6 +131,14 @@ ObjectDecisionType PathObstacle::MergeLongitutionalDecision(
...
@@ -131,6 +131,14 @@ ObjectDecisionType PathObstacle::MergeLongitutionalDecision(
<<
" and decision: "
<<
rhs
.
ShortDebugString
();
<<
" and decision: "
<<
rhs
.
ShortDebugString
();
}
}
const
ObjectDecisionType
&
PathObstacle
::
LongitutionalDecision
()
const
{
return
longitutional_decision_
;
}
const
ObjectDecisionType
&
PathObstacle
::
LateralDecision
()
const
{
return
lateral_decision_
;
}
ObjectDecisionType
PathObstacle
::
MergeLateralDecision
(
ObjectDecisionType
PathObstacle
::
MergeLateralDecision
(
const
ObjectDecisionType
&
lhs
,
const
ObjectDecisionType
&
rhs
)
{
const
ObjectDecisionType
&
lhs
,
const
ObjectDecisionType
&
rhs
)
{
auto
lhs_iter
=
s_lateral_decision_safety_sorter_
.
find
(
lhs
.
object_tag_case
());
auto
lhs_iter
=
s_lateral_decision_safety_sorter_
.
find
(
lhs
.
object_tag_case
());
...
@@ -167,6 +175,12 @@ ObjectDecisionType PathObstacle::MergeLateralDecision(
...
@@ -167,6 +175,12 @@ ObjectDecisionType PathObstacle::MergeLateralDecision(
<<
" and decision: "
<<
rhs
.
ShortDebugString
();
<<
" and decision: "
<<
rhs
.
ShortDebugString
();
}
}
bool
PathObstacle
::
HasLateralDecision
()
const
{
return
has_lateral_decision_
;
}
bool
PathObstacle
::
HasLongitutionalDecision
()
const
{
return
has_longitutional_decision_
;
}
const
planning
::
Obstacle
*
PathObstacle
::
Obstacle
()
const
{
return
obstacle_
;
}
const
planning
::
Obstacle
*
PathObstacle
::
Obstacle
()
const
{
return
obstacle_
;
}
void
PathObstacle
::
AddDecision
(
const
std
::
string
&
decider_tag
,
void
PathObstacle
::
AddDecision
(
const
std
::
string
&
decider_tag
,
...
@@ -179,10 +193,12 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
...
@@ -179,10 +193,12 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
MergeLongitutionalDecision
(
longitutional_decision_
,
merged_decision
);
MergeLongitutionalDecision
(
longitutional_decision_
,
merged_decision
);
}
else
{
}
else
{
lateral_decision_
=
merged_decision
;
lateral_decision_
=
merged_decision
;
has_lateral_decision_
=
true
;
}
}
}
else
if
(
IsLongitutionalDecision
(
decision
))
{
}
else
if
(
IsLongitutionalDecision
(
decision
))
{
longitutional_decision_
=
longitutional_decision_
=
MergeLongitutionalDecision
(
longitutional_decision_
,
decision
);
MergeLongitutionalDecision
(
longitutional_decision_
,
decision
);
has_longitutional_decision_
=
true
;
}
else
{
}
else
{
DCHECK
(
false
)
<<
"Unkown decision type: "
<<
decision
.
DebugString
();
DCHECK
(
false
)
<<
"Unkown decision type: "
<<
decision
.
DebugString
();
}
}
...
@@ -190,10 +206,6 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
...
@@ -190,10 +206,6 @@ void PathObstacle::AddDecision(const std::string& decider_tag,
decider_tags_
.
push_back
(
decider_tag
);
decider_tags_
.
push_back
(
decider_tag
);
}
}
const
std
::
vector
<
ObjectDecisionType
>&
PathObstacle
::
Decisions
()
const
{
return
decisions_
;
}
const
std
::
string
PathObstacle
::
DebugString
()
const
{
const
std
::
string
PathObstacle
::
DebugString
()
const
{
std
::
stringstream
ss
;
std
::
stringstream
ss
;
for
(
std
::
size_t
i
=
0
;
i
<
decisions_
.
size
();
++
i
)
{
for
(
std
::
size_t
i
=
0
;
i
<
decisions_
.
size
();
++
i
)
{
...
...
modules/planning/common/path_obstacle.h
浏览文件 @
6742e057
...
@@ -70,8 +70,6 @@ class PathObstacle {
...
@@ -70,8 +70,6 @@ class PathObstacle {
const
planning
::
Obstacle
*
Obstacle
()
const
;
const
planning
::
Obstacle
*
Obstacle
()
const
;
bool
MergeObstacle
()
const
;
/**
/**
* @class Add decision to this obstacle. This function will
* @class Add decision to this obstacle. This function will
* also calculate the merged decision. i.e., if there are multiple lateral
* also calculate the merged decision. i.e., if there are multiple lateral
...
@@ -87,19 +85,19 @@ class PathObstacle {
...
@@ -87,19 +85,19 @@ class PathObstacle {
void
AddDecision
(
const
std
::
string
&
decider_tag
,
void
AddDecision
(
const
std
::
string
&
decider_tag
,
const
ObjectDecisionType
&
decision
);
const
ObjectDecisionType
&
decision
);
const
std
::
vector
<
ObjectDecisionType
>&
Decisions
()
const
;
bool
HasLateralDecision
()
const
;
bool
HasLongitutionalDecision
()
const
;
/**
/**
* return the merged lateral decision
* return the merged lateral decision
* Lateral decision is one of {Nudge, Ignore}
* Lateral decision is one of {Nudge, Ignore}
**/
**/
const
ObjectDecisionType
&
Merged
LateralDecision
()
const
;
const
ObjectDecisionType
&
LateralDecision
()
const
;
/**
/**
* @brief return the merged longitutional decision
* @brief return the merged longitutional decision
* Longitutional decision is one of {Stop, Yield, Follow, Overtake, Ignore}
* Longitutional decision is one of {Stop, Yield, Follow, Overtake, Ignore}
**/
**/
const
ObjectDecisionType
&
Merged
LongitutionalDecision
()
const
;
const
ObjectDecisionType
&
LongitutionalDecision
()
const
;
const
std
::
string
DebugString
()
const
;
const
std
::
string
DebugString
()
const
;
...
@@ -137,7 +135,9 @@ class PathObstacle {
...
@@ -137,7 +135,9 @@ class PathObstacle {
// StGraphBoundary st_boundary_;
// StGraphBoundary st_boundary_;
ObjectDecisionType
lateral_decision_
;
ObjectDecisionType
lateral_decision_
;
bool
has_lateral_decision_
=
false
;
ObjectDecisionType
longitutional_decision_
;
ObjectDecisionType
longitutional_decision_
;
bool
has_longitutional_decision_
=
false
;
struct
ObjectTagCaseHash
{
struct
ObjectTagCaseHash
{
std
::
size_t
operator
()(
std
::
size_t
operator
()(
...
...
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
浏览文件 @
6742e057
...
@@ -409,7 +409,7 @@ Status DpStGraph::GetObjectDecision(const StGraphData& st_graph_data,
...
@@ -409,7 +409,7 @@ Status DpStGraph::GetObjectDecision(const StGraphData& st_graph_data,
}
}
}
}
for
(
const
auto
*
path_obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
for
(
const
auto
*
path_obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
if
(
path_obstacle
->
Decisions
().
empty
())
{
if
(
!
path_obstacle
->
HasLongitutionalDecision
())
{
ObjectDecisionType
ignore_decision
;
ObjectDecisionType
ignore_decision
;
ignore_decision
.
mutable_ignore
();
ignore_decision
.
mutable_ignore
();
path_decision
->
AddDecision
(
"dp_st_graph"
,
path_obstacle
->
Id
(),
path_decision
->
AddDecision
(
"dp_st_graph"
,
path_obstacle
->
Id
(),
...
@@ -428,8 +428,7 @@ bool DpStGraph::CreateFollowDecision(
...
@@ -428,8 +428,7 @@ bool DpStGraph::CreateFollowDecision(
const
double
follow_distance_s
=
-
FLAGS_follow_min_distance
;
const
double
follow_distance_s
=
-
FLAGS_follow_min_distance
;
follow
->
set_distance_s
(
follow_distance_s
);
follow
->
set_distance_s
(
follow_distance_s
);
const
double
reference_line_fence_s
=
const
double
reference_line_fence_s
=
boundary
.
min_s
()
+
follow_distance_s
;
boundary
.
min_s
()
+
follow_distance_s
;
common
::
PathPoint
path_point
;
common
::
PathPoint
path_point
;
if
(
!
path_data_
.
GetPathPointWithRefS
(
reference_line_fence_s
,
&
path_point
))
{
if
(
!
path_data_
.
GetPathPointWithRefS
(
reference_line_fence_s
,
&
path_point
))
{
AERROR
<<
"Failed to get path point from reference line s "
AERROR
<<
"Failed to get path point from reference line s "
...
...
modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc
浏览文件 @
6742e057
...
@@ -112,8 +112,7 @@ bool QpFrenetFrame::GetOverallBound(
...
@@ -112,8 +112,7 @@ bool QpFrenetFrame::GetOverallBound(
return
false
;
return
false
;
}
}
std
::
pair
<
double
,
double
>
obs_bound
(
std
::
pair
<
double
,
double
>
obs_bound
(
-
std
::
numeric_limits
<
double
>::
infinity
(),
-
std
::
numeric_limits
<
double
>::
infinity
(),
std
::
numeric_limits
<
double
>::
infinity
());
std
::
numeric_limits
<
double
>::
infinity
());
if
(
!
GetStaticObstacleBound
(
s
,
&
obs_bound
))
{
if
(
!
GetStaticObstacleBound
(
s
,
&
obs_bound
))
{
...
@@ -168,12 +167,15 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
...
@@ -168,12 +167,15 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
bool
QpFrenetFrame
::
MapDynamicObstacleWithDecision
(
bool
QpFrenetFrame
::
MapDynamicObstacleWithDecision
(
const
PathObstacle
&
path_obstacle
)
{
const
PathObstacle
&
path_obstacle
)
{
const
std
::
vector
<
ObjectDecisionType
>&
decisions
=
path_obstacle
.
Decisions
();
const
Obstacle
*
ptr_obstacle
=
path_obstacle
.
Obstacle
();
const
Obstacle
*
ptr_obstacle
=
path_obstacle
.
Obstacle
();
if
(
!
path_obstacle
.
HasLateralDecision
())
{
for
(
const
auto
&
decision
:
decisions
)
{
AERROR
<<
"object has no lateral decision"
;
return
false
;
}
const
auto
&
decision
=
path_obstacle
.
LateralDecision
();
if
(
!
decision
.
has_nudge
())
{
if
(
!
decision
.
has_nudge
())
{
continue
;
AWARN
<<
"only support nudge now"
;
return
true
;
}
}
const
auto
&
nudge
=
decision
.
nudge
();
const
auto
&
nudge
=
decision
.
nudge
();
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
...
@@ -195,8 +197,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
...
@@ -195,8 +197,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
common
::
SLPoint
cur_point
;
common
::
SLPoint
cur_point
;
if
(
!
reference_line_
.
get_point_in_frenet_frame
(
if
(
!
reference_line_
.
get_point_in_frenet_frame
(
{
corners
[
i
].
x
(),
corners
[
i
].
y
()},
&
cur_point
))
{
{
corners
[
i
].
x
(),
corners
[
i
].
y
()},
&
cur_point
))
{
AERROR
<<
"Fail to map xy point "
<<
corners
[
i
].
DebugString
()
AERROR
<<
"Fail to map xy point "
<<
corners
[
i
].
DebugString
()
<<
" to "
<<
" to "
<<
cur_point
.
ShortDebugString
();
<<
cur_point
.
ShortDebugString
();
return
false
;
return
false
;
}
}
...
@@ -230,8 +232,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
...
@@ -230,8 +232,8 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
std
::
pair
<
uint32_t
,
uint32_t
>
update_index_range
=
std
::
pair
<
uint32_t
,
uint32_t
>
update_index_range
=
FindInterval
(
updated_start_s
,
updated_end_s
);
FindInterval
(
updated_start_s
,
updated_end_s
);
for
(
uint32_t
j
=
update_index_range
.
first
;
for
(
uint32_t
j
=
update_index_range
.
first
;
j
<
update_index_range
.
second
;
j
<
update_index_range
.
second
;
++
j
)
{
++
j
)
{
dynamic_obstacle_bound_
[
j
].
first
=
dynamic_obstacle_bound_
[
j
].
first
=
std
::
max
(
bound
.
first
,
dynamic_obstacle_bound_
[
j
].
first
);
std
::
max
(
bound
.
first
,
dynamic_obstacle_bound_
[
j
].
first
);
dynamic_obstacle_bound_
[
j
].
second
=
dynamic_obstacle_bound_
[
j
].
second
=
...
@@ -239,19 +241,20 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
...
@@ -239,19 +241,20 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
}
}
}
}
}
}
}
return
true
;
return
true
;
}
}
bool
QpFrenetFrame
::
MapStaticObstacleWithDecision
(
bool
QpFrenetFrame
::
MapStaticObstacleWithDecision
(
const
PathObstacle
&
path_obstacle
)
{
const
PathObstacle
&
path_obstacle
)
{
const
std
::
vector
<
ObjectDecisionType
>&
object_decisions
=
path_obstacle
.
Decisions
();
const
auto
ptr_obstacle
=
path_obstacle
.
Obstacle
();
const
auto
ptr_obstacle
=
path_obstacle
.
Obstacle
();
if
(
!
path_obstacle
.
HasLateralDecision
())
{
for
(
const
auto
&
decision
:
object_decisions
)
{
AERROR
<<
"obstacle has no lateral decision"
;
return
false
;
}
const
auto
&
decision
=
path_obstacle
.
LateralDecision
();
if
(
!
decision
.
has_nudge
())
{
if
(
!
decision
.
has_nudge
())
{
continue
;
AWARN
<<
"only support nudge decision now"
;
return
true
;
}
}
const
auto
&
nudge
=
decision
.
nudge
();
const
auto
&
nudge
=
decision
.
nudge
();
const
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
const
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
...
@@ -274,13 +277,11 @@ bool QpFrenetFrame::MapStaticObstacleWithDecision(
...
@@ -274,13 +277,11 @@ bool QpFrenetFrame::MapStaticObstacleWithDecision(
return
false
;
return
false
;
}
}
}
}
}
return
true
;
return
true
;
}
}
bool
QpFrenetFrame
::
MapPolygon
(
bool
QpFrenetFrame
::
MapPolygon
(
const
std
::
vector
<
common
::
math
::
Vec2d
>&
corners
,
const
std
::
vector
<
common
::
math
::
Vec2d
>&
corners
,
const
double
buffer
,
const
double
buffer
,
const
int
nudge_side
,
const
int
nudge_side
,
std
::
vector
<
std
::
pair
<
double
,
double
>>*
const
bound_map
)
{
std
::
vector
<
std
::
pair
<
double
,
double
>>*
const
bound_map
)
{
std
::
vector
<
common
::
SLPoint
>
sl_corners
;
std
::
vector
<
common
::
SLPoint
>
sl_corners
;
...
@@ -434,6 +435,9 @@ void QpFrenetFrame::CalculateHDMapBound() {
...
@@ -434,6 +435,9 @@ void QpFrenetFrame::CalculateHDMapBound() {
bool
QpFrenetFrame
::
CalculateObstacleBound
()
{
bool
QpFrenetFrame
::
CalculateObstacleBound
()
{
for
(
const
auto
ptr_path_obstacle
:
path_obstacles_
)
{
for
(
const
auto
ptr_path_obstacle
:
path_obstacles_
)
{
if
(
!
ptr_path_obstacle
->
HasLateralDecision
())
{
continue
;
}
if
(
ptr_path_obstacle
->
Obstacle
()
->
IsStatic
())
{
if
(
ptr_path_obstacle
->
Obstacle
()
->
IsStatic
())
{
if
(
!
MapStaticObstacleWithDecision
(
*
ptr_path_obstacle
))
{
if
(
!
MapStaticObstacleWithDecision
(
*
ptr_path_obstacle
))
{
AERROR
<<
"mapping obstacle with id ["
<<
ptr_path_obstacle
->
Id
()
AERROR
<<
"mapping obstacle with id ["
<<
ptr_path_obstacle
->
Id
()
...
@@ -501,7 +505,8 @@ uint32_t QpFrenetFrame::FindIndex(const double s) const {
...
@@ -501,7 +505,8 @@ uint32_t QpFrenetFrame::FindIndex(const double s) const {
std
::
lower_bound
(
evaluated_knots_
.
begin
()
+
1
,
evaluated_knots_
.
end
(),
s
);
std
::
lower_bound
(
evaluated_knots_
.
begin
()
+
1
,
evaluated_knots_
.
end
(),
s
);
return
std
::
min
(
return
std
::
min
(
static_cast
<
uint32_t
>
(
evaluated_knots_
.
size
()
-
1
),
static_cast
<
uint32_t
>
(
evaluated_knots_
.
size
()
-
1
),
static_cast
<
uint32_t
>
(
lower_bound
-
evaluated_knots_
.
begin
()))
-
1
;
static_cast
<
uint32_t
>
(
lower_bound
-
evaluated_knots_
.
begin
()))
-
1
;
}
}
}
// namespace planning
}
// namespace planning
...
...
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
浏览文件 @
6742e057
...
@@ -103,7 +103,7 @@ Status StBoundaryMapper::GetGraphBoundary(
...
@@ -103,7 +103,7 @@ Status StBoundaryMapper::GetGraphBoundary(
double
min_stop_s
=
std
::
numeric_limits
<
double
>::
max
();
double
min_stop_s
=
std
::
numeric_limits
<
double
>::
max
();
for
(
const
auto
*
path_obstacle
:
path_obstacles
.
Items
())
{
for
(
const
auto
*
path_obstacle
:
path_obstacles
.
Items
())
{
if
(
path_obstacle
->
Decisions
().
empty
())
{
if
(
!
path_obstacle
->
HasLongitutionalDecision
())
{
const
auto
ret
=
const
auto
ret
=
MapObstacleWithoutDecision
(
*
path_obstacle
,
st_graph_boundaries
);
MapObstacleWithoutDecision
(
*
path_obstacle
,
st_graph_boundaries
);
if
(
ret
.
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
if
(
ret
.
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
...
@@ -113,20 +113,18 @@ Status StBoundaryMapper::GetGraphBoundary(
...
@@ -113,20 +113,18 @@ Status StBoundaryMapper::GetGraphBoundary(
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
}
}
}
for
(
const
auto
&
decision
:
path_obstacle
->
Decisions
())
{
const
auto
&
decision
=
path_obstacle
->
LongitutionalDecision
();
StGraphBoundary
boundary
(
path_obstacle
);
StGraphBoundary
boundary
(
path_obstacle
);
if
(
decision
.
has_follow
())
{
if
(
decision
.
has_follow
())
{
const
auto
ret
=
MapFollowDecision
(
*
path_obstacle
,
decision
,
&
boundary
);
const
auto
ret
=
MapFollowDecision
(
*
path_obstacle
,
decision
,
&
boundary
);
if
(
!
ret
.
ok
())
{
if
(
!
ret
.
ok
())
{
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
<<
" with follow decision: "
<<
decision
.
DebugString
();
<<
" with follow decision: "
<<
decision
.
DebugString
();
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Fail to map follow decision"
);
"Fail to map follow decision"
);
}
}
}
else
if
(
decision
.
has_stop
())
{
}
else
if
(
decision
.
has_stop
())
{
// TODO(all) change start_s() to st_boundary.min_s()
// TODO(all) change start_s() to st_boundary.min_s()
const
double
stop_s
=
const
double
stop_s
=
path_obstacle
->
perception_sl_boundary
().
start_s
()
+
path_obstacle
->
perception_sl_boundary
().
start_s
()
+
decision
.
stop
().
distance_s
();
decision
.
stop
().
distance_s
();
if
(
stop_s
<
adc_front_s_
)
{
if
(
stop_s
<
adc_front_s_
)
{
AERROR
<<
"Invalid stop decision. not stop at ahead of current "
AERROR
<<
"Invalid stop decision. not stop at ahead of current "
...
@@ -159,7 +157,6 @@ Status StBoundaryMapper::GetGraphBoundary(
...
@@ -159,7 +157,6 @@ Status StBoundaryMapper::GetGraphBoundary(
}
}
boundary
.
set_id
(
path_obstacle
->
Id
());
boundary
.
set_id
(
path_obstacle
->
Id
());
}
}
}
if
(
stop_obstacle
)
{
if
(
stop_obstacle
)
{
StGraphBoundary
stop_boundary
(
stop_obstacle
);
StGraphBoundary
stop_boundary
(
stop_obstacle
);
...
...
modules/planning/planner/em/decider.cc
浏览文件 @
6742e057
...
@@ -18,8 +18,8 @@
...
@@ -18,8 +18,8 @@
* @file decider.cpp
* @file decider.cpp
**/
**/
#include "modules/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/log.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/planner/em/decider.h"
#include "modules/planning/planner/em/decider.h"
...
@@ -31,8 +31,7 @@ using common::Status;
...
@@ -31,8 +31,7 @@ using common::Status;
using
common
::
VehicleState
;
using
common
::
VehicleState
;
Decider
::
Decider
(
DecisionResult
*
decision_result
)
Decider
::
Decider
(
DecisionResult
*
decision_result
)
:
decision_
(
decision_result
)
{
:
decision_
(
decision_result
)
{}
}
const
DecisionResult
&
Decider
::
Decision
()
const
{
return
*
decision_
;
}
const
DecisionResult
&
Decider
::
Decision
()
const
{
return
*
decision_
;
}
...
@@ -79,7 +78,7 @@ int Decider::MakeMainStopDecision(Frame* frame,
...
@@ -79,7 +78,7 @@ int Decider::MakeMainStopDecision(Frame* frame,
const
auto
&
path_obstacles
=
path_decision
->
path_obstacles
();
const
auto
&
path_obstacles
=
path_decision
->
path_obstacles
();
for
(
const
auto
path_obstacle
:
path_obstacles
.
Items
())
{
for
(
const
auto
path_obstacle
:
path_obstacles
.
Items
())
{
const
auto
&
obstacle
=
path_obstacle
->
Obstacle
();
const
auto
&
obstacle
=
path_obstacle
->
Obstacle
();
for
(
const
auto
&
object_decision
:
path_obstacle
->
Decisions
())
{
const
auto
&
object_decision
=
path_obstacle
->
LongitutionalDecision
();
if
(
not
object_decision
.
has_stop
())
{
if
(
not
object_decision
.
has_stop
())
{
continue
;
continue
;
}
}
...
@@ -102,14 +101,12 @@ int Decider::MakeMainStopDecision(Frame* frame,
...
@@ -102,14 +101,12 @@ int Decider::MakeMainStopDecision(Frame* frame,
common
::
SLPoint
adc_sl
;
common
::
SLPoint
adc_sl
;
auto
&
adc_position
=
VehicleState
::
instance
()
->
pose
().
position
();
auto
&
adc_position
=
VehicleState
::
instance
()
->
pose
().
position
();
reference_line
.
get_point_in_frenet_frame
(
reference_line
.
get_point_in_frenet_frame
(
{
adc_position
.
x
(),
adc_position
.
y
()},
{
adc_position
.
x
(),
adc_position
.
y
()},
&
adc_sl
);
&
adc_sl
);
const
auto
&
vehicle_param
=
const
auto
&
vehicle_param
=
common
::
VehicleConfigHelper
::
instance
()
->
GetConfig
().
vehicle_param
();
common
::
VehicleConfigHelper
::
instance
()
->
GetConfig
().
vehicle_param
();
if
(
stop_line_s
<=
adc_sl
.
s
()
+
vehicle_param
.
front_edge_to_center
())
{
if
(
stop_line_s
<=
adc_sl
.
s
()
+
vehicle_param
.
front_edge_to_center
())
{
AERROR
<<
"object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
AERROR
<<
"object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
<<
stop_line_s
<<
stop_line_s
<<
"] behind adc route_s["
<<
adc_sl
.
s
()
<<
"] behind adc route_s["
<<
adc_sl
.
s
()
<<
"]"
;
<<
"]"
;
continue
;
continue
;
}
}
...
@@ -119,7 +116,6 @@ int Decider::MakeMainStopDecision(Frame* frame,
...
@@ -119,7 +116,6 @@ int Decider::MakeMainStopDecision(Frame* frame,
stop_decision
=
&
(
object_decision
.
stop
());
stop_decision
=
&
(
object_decision
.
stop
());
}
}
}
}
}
if
(
stop_obstacle
!=
nullptr
)
{
if
(
stop_obstacle
!=
nullptr
)
{
MainStop
*
main_stop
=
decision_
->
mutable_main_decision
()
->
mutable_stop
();
MainStop
*
main_stop
=
decision_
->
mutable_main_decision
()
->
mutable_stop
();
...
@@ -150,8 +146,13 @@ int Decider::SetObjectDecisions(PathDecision* const path_decision) {
...
@@ -150,8 +146,13 @@ int Decider::SetObjectDecisions(PathDecision* const path_decision) {
const
auto
&
obstacle
=
path_obstacle
->
Obstacle
();
const
auto
&
obstacle
=
path_obstacle
->
Obstacle
();
object_decision
->
set_id
(
obstacle
->
Id
());
object_decision
->
set_id
(
obstacle
->
Id
());
object_decision
->
set_perception_id
(
obstacle
->
PerceptionId
());
object_decision
->
set_perception_id
(
obstacle
->
PerceptionId
());
for
(
const
auto
&
decision
:
path_obstacle
->
Decisions
())
{
if
(
path_obstacle
->
HasLateralDecision
())
{
object_decision
->
add_object_decision
()
->
CopyFrom
(
decision
);
object_decision
->
add_object_decision
()
->
CopyFrom
(
path_obstacle
->
LateralDecision
());
}
if
(
path_obstacle
->
HasLongitutionalDecision
())
{
object_decision
->
add_object_decision
()
->
CopyFrom
(
path_obstacle
->
LongitutionalDecision
());
}
}
}
}
return
0
;
return
0
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录