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,9 +112,8 @@ bool QpFrenetFrame::GetOverallBound(
...
@@ -112,9 +112,8 @@ 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
))
{
return
false
;
return
false
;
...
@@ -168,75 +167,77 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
...
@@ -168,75 +167,77 @@ 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
())
{
AERROR
<<
"object has no lateral decision"
;
return
false
;
}
const
auto
&
decision
=
path_obstacle
.
LateralDecision
();
if
(
!
decision
.
has_nudge
())
{
AWARN
<<
"only support nudge now"
;
return
true
;
}
const
auto
&
nudge
=
decision
.
nudge
();
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
int
nudge_side
=
(
nudge
.
type
()
==
ObjectNudge
::
RIGHT_NUDGE
)
?
1
:
-
1
;
for
(
const
SpeedPoint
&
veh_point
:
discretized_vehicle_location_
)
{
double
time
=
veh_point
.
t
();
common
::
TrajectoryPoint
trajectory_point
=
ptr_obstacle
->
GetPointAtTime
(
time
);
common
::
math
::
Box2d
obs_box
=
ptr_obstacle
->
GetBoundingBox
(
trajectory_point
);
// project obs_box on reference line
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
obs_box
.
GetAllCorners
(
&
corners
);
std
::
vector
<
common
::
SLPoint
>
sl_corners
;
for
(
uint32_t
i
=
0
;
i
<
corners
.
size
();
++
i
)
{
common
::
SLPoint
cur_point
;
if
(
!
reference_line_
.
get_point_in_frenet_frame
(
{
corners
[
i
].
x
(),
corners
[
i
].
y
()},
&
cur_point
))
{
AERROR
<<
"Fail to map xy point "
<<
corners
[
i
].
DebugString
()
<<
" to "
<<
cur_point
.
ShortDebugString
();
for
(
const
auto
&
decision
:
decisions
)
{
return
false
;
if
(
!
decision
.
has_nudge
())
{
}
continue
;
// shift box base on buffer
cur_point
.
set_l
(
cur_point
.
l
()
-
buffer
*
nudge_side
);
sl_corners
.
push_back
(
std
::
move
(
cur_point
));
}
}
const
auto
&
nudge
=
decision
.
nudge
();
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
for
(
uint32_t
i
=
0
;
i
<
sl_corners
.
size
();
++
i
)
{
common
::
SLPoint
sl_first
=
sl_corners
[
i
%
sl_corners
.
size
()];
int
nudge_side
=
(
nudge
.
type
()
==
ObjectNudge
::
RIGHT_NUDGE
)
?
1
:
-
1
;
common
::
SLPoint
sl_second
=
sl_corners
[(
i
+
1
)
%
sl_corners
.
size
()];
if
(
sl_first
.
s
()
<
sl_second
.
s
())
{
for
(
const
SpeedPoint
&
veh_point
:
discretized_vehicle_location_
)
{
std
::
swap
(
sl_first
,
sl_second
);
double
time
=
veh_point
.
t
();
common
::
TrajectoryPoint
trajectory_point
=
ptr_obstacle
->
GetPointAtTime
(
time
);
common
::
math
::
Box2d
obs_box
=
ptr_obstacle
->
GetBoundingBox
(
trajectory_point
);
// project obs_box on reference line
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
obs_box
.
GetAllCorners
(
&
corners
);
std
::
vector
<
common
::
SLPoint
>
sl_corners
;
for
(
uint32_t
i
=
0
;
i
<
corners
.
size
();
++
i
)
{
common
::
SLPoint
cur_point
;
if
(
!
reference_line_
.
get_point_in_frenet_frame
(
{
corners
[
i
].
x
(),
corners
[
i
].
y
()},
&
cur_point
))
{
AERROR
<<
"Fail to map xy point "
<<
corners
[
i
].
DebugString
()
<<
" to "
<<
cur_point
.
ShortDebugString
();
return
false
;
}
// shift box base on buffer
cur_point
.
set_l
(
cur_point
.
l
()
-
buffer
*
nudge_side
);
sl_corners
.
push_back
(
std
::
move
(
cur_point
));
}
}
for
(
uint32_t
i
=
0
;
i
<
sl_corners
.
size
();
++
i
)
{
std
::
pair
<
double
,
double
>
bound
=
MapLateralConstraint
(
common
::
SLPoint
sl_first
=
sl_corners
[
i
%
sl_corners
.
size
()];
sl_first
,
sl_second
,
nudge_side
,
common
::
SLPoint
sl_second
=
sl_corners
[(
i
+
1
)
%
sl_corners
.
size
()];
veh_point
.
s
()
-
vehicle_param_
.
back_edge_to_center
(),
if
(
sl_first
.
s
()
<
sl_second
.
s
())
{
veh_point
.
s
()
+
vehicle_param_
.
front_edge_to_center
());
std
::
swap
(
sl_first
,
sl_second
);
}
// update bound map
double
s_resolution
=
std
::
fabs
(
veh_point
.
v
()
*
time_resolution_
);
std
::
pair
<
double
,
double
>
bound
=
MapLateralConstraint
(
double
updated_start_s
=
sl_first
,
sl_second
,
nudge_side
,
init_frenet_point_
.
s
()
+
veh_point
.
s
()
-
s_resolution
;
veh_point
.
s
()
-
vehicle_param_
.
back_edge_to_center
(),
double
updated_end_s
=
veh_point
.
s
()
+
vehicle_param_
.
front_edge_to_center
());
init_frenet_point_
.
s
()
+
veh_point
.
s
()
+
s_resolution
;
if
(
updated_end_s
>
evaluated_knots_
.
back
()
||
// update bound map
updated_start_s
<
evaluated_knots_
.
front
())
{
double
s_resolution
=
std
::
fabs
(
veh_point
.
v
()
*
time_resolution_
);
continue
;
double
updated_start_s
=
}
init_frenet_point_
.
s
()
+
veh_point
.
s
()
-
s_resolution
;
std
::
pair
<
uint32_t
,
uint32_t
>
update_index_range
=
double
updated_end_s
=
FindInterval
(
updated_start_s
,
updated_end_s
);
init_frenet_point_
.
s
()
+
veh_point
.
s
()
+
s_resolution
;
if
(
updated_end_s
>
evaluated_knots_
.
back
()
||
for
(
uint32_t
j
=
update_index_range
.
first
;
j
<
update_index_range
.
second
;
updated_start_s
<
evaluated_knots_
.
front
())
{
++
j
)
{
continue
;
dynamic_obstacle_bound_
[
j
].
first
=
}
std
::
max
(
bound
.
first
,
dynamic_obstacle_bound_
[
j
].
first
);
std
::
pair
<
uint32_t
,
uint32_t
>
update_index_range
=
dynamic_obstacle_bound_
[
j
].
second
=
FindInterval
(
updated_start_s
,
updated_end_s
);
std
::
min
(
bound
.
second
,
dynamic_obstacle_bound_
[
j
].
second
);
for
(
uint32_t
j
=
update_index_range
.
first
;
j
<
update_index_range
.
second
;
++
j
)
{
dynamic_obstacle_bound_
[
j
].
first
=
std
::
max
(
bound
.
first
,
dynamic_obstacle_bound_
[
j
].
first
);
dynamic_obstacle_bound_
[
j
].
second
=
std
::
min
(
bound
.
second
,
dynamic_obstacle_bound_
[
j
].
second
);
}
}
}
}
}
}
}
...
@@ -245,42 +246,42 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
...
@@ -245,42 +246,42 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision(
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"
;
if
(
!
decision
.
has_nudge
())
{
return
false
;
continue
;
}
const
auto
&
decision
=
path_obstacle
.
LateralDecision
();
if
(
!
decision
.
has_nudge
())
{
AWARN
<<
"only support nudge decision now"
;
return
true
;
}
const
auto
&
nudge
=
decision
.
nudge
();
const
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
if
(
nudge
.
type
()
==
ObjectNudge
::
RIGHT_NUDGE
)
{
const
auto
&
box
=
ptr_obstacle
->
PerceptionBoundingBox
();
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
box
.
GetAllCorners
(
&
corners
);
if
(
!
MapPolygon
(
corners
,
buffer
,
-
1
,
&
static_obstacle_bound_
))
{
AERROR
<<
"fail to map polygon with id "
<<
path_obstacle
.
Id
()
<<
" in qp frenet frame"
;
return
false
;
}
}
const
auto
&
nudge
=
decision
.
nudge
();
}
else
{
const
double
buffer
=
std
::
fabs
(
nudge
.
distance_l
());
const
auto
&
box
=
ptr_obstacle
->
PerceptionBoundingBox
();
if
(
nudge
.
type
()
==
ObjectNudge
::
RIGHT_NUDGE
)
{
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
const
auto
&
box
=
ptr_obstacle
->
PerceptionBoundingBox
();
box
.
GetAllCorners
(
&
corners
);
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
if
(
!
MapPolygon
(
corners
,
buffer
,
1
,
&
static_obstacle_bound_
))
{
box
.
GetAllCorners
(
&
corners
);
AERROR
<<
"fail to map polygon with id "
<<
path_obstacle
.
Id
()
if
(
!
MapPolygon
(
corners
,
buffer
,
-
1
,
&
static_obstacle_bound_
))
{
<<
"in qp frenet frame"
;
AERROR
<<
"fail to map polygon with id "
<<
path_obstacle
.
Id
()
return
false
;
<<
" in qp frenet frame"
;
return
false
;
}
}
else
{
const
auto
&
box
=
ptr_obstacle
->
PerceptionBoundingBox
();
std
::
vector
<
common
::
math
::
Vec2d
>
corners
;
box
.
GetAllCorners
(
&
corners
);
if
(
!
MapPolygon
(
corners
,
buffer
,
1
,
&
static_obstacle_bound_
))
{
AERROR
<<
"fail to map polygon with id "
<<
path_obstacle
.
Id
()
<<
"in qp frenet frame"
;
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
()
...
@@ -467,8 +471,8 @@ bool QpFrenetFrame::GetBound(
...
@@ -467,8 +471,8 @@ bool QpFrenetFrame::GetBound(
const
double
s_low
=
evaluated_knots_
[
lower_index
];
const
double
s_low
=
evaluated_knots_
[
lower_index
];
const
double
s_high
=
evaluated_knots_
[
lower_index
+
1
];
const
double
s_high
=
evaluated_knots_
[
lower_index
+
1
];
double
weight
=
Double
::
Compare
(
s_low
,
s_high
,
1e-8
)
<
0
double
weight
=
Double
::
Compare
(
s_low
,
s_high
,
1e-8
)
<
0
?
(
s
-
s_low
)
/
(
s_high
-
s_low
)
?
(
s
-
s_low
)
/
(
s_high
-
s_low
)
:
0.0
;
:
0.0
;
double
low_first
=
map_bound
[
lower_index
].
first
;
double
low_first
=
map_bound
[
lower_index
].
first
;
double
low_second
=
map_bound
[
lower_index
].
second
;
double
low_second
=
map_bound
[
lower_index
].
second
;
...
@@ -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,52 +113,49 @@ Status StBoundaryMapper::GetGraphBoundary(
...
@@ -113,52 +113,49 @@ 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
=
path_obstacle
->
perception_sl_boundary
().
start_s
()
+
const
double
stop_s
=
decision
.
stop
().
distance_s
();
path_obstacle
->
perception_sl_boundary
().
start_s
()
+
if
(
stop_s
<
adc_front_s_
)
{
decision
.
stop
().
distance_s
();
AERROR
<<
"Invalid stop decision. not stop at ahead of current "
if
(
stop_s
<
adc_front_s_
)
{
"position. stop_s : "
AERROR
<<
"Invalid stop decision. not stop at ahead of current "
<<
stop_s
<<
", and current adc_s is; "
<<
adc_front_s_
;
"position. stop_s : "
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"invalid decision"
);
<<
stop_s
<<
", and current adc_s is; "
<<
adc_front_s_
;
}
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"invalid decision"
);
if
(
!
stop_obstacle
)
{
}
stop_obstacle
=
path_obstacle
;
if
(
!
stop_obstacle
)
{
stop_decision
=
decision
;
stop_obstacle
=
path_obstacle
;
min_stop_s
=
stop_s
;
stop_decision
=
decision
;
}
else
if
(
stop_s
<
min_stop_s
)
{
min_stop_s
=
stop_s
;
stop_obstacle
=
path_obstacle
;
}
else
if
(
stop_s
<
min_stop_s
)
{
min_stop_s
=
stop_s
;
stop_obstacle
=
path_obstacle
;
stop_decision
=
decision
;
min_stop_s
=
stop_s
;
}
stop_decision
=
decision
;
}
else
if
(
decision
.
has_overtake
()
||
decision
.
has_yield
())
{
}
const
auto
ret
=
MapObstacleWithPredictionTrajectory
(
}
else
if
(
decision
.
has_overtake
()
||
decision
.
has_yield
())
{
*
path_obstacle
,
decision
,
st_graph_boundaries
);
const
auto
ret
=
MapObstacleWithPredictionTrajectory
(
if
(
!
ret
.
ok
())
{
*
path_obstacle
,
decision
,
st_graph_boundaries
);
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
if
(
!
ret
.
ok
())
{
<<
" with decision: "
<<
decision
.
DebugString
();
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
<<
" with decision: "
<<
decision
.
DebugString
();
"Fail to map overtake/yield decision"
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Fail to map overtake/yield decision"
);
}
}
else
{
std
::
string
msg
=
common
::
util
::
StrCat
(
"No mapping for decision: "
,
decision
.
DebugString
());
return
Status
(
ErrorCode
::
PLANNING_SKIP
,
msg
);
}
}
boundary
.
set_id
(
path_obstacle
->
Id
());
}
else
{
std
::
string
msg
=
common
::
util
::
StrCat
(
"No mapping for decision: "
,
decision
.
DebugString
());
return
Status
(
ErrorCode
::
PLANNING_SKIP
,
msg
);
}
}
boundary
.
set_id
(
path_obstacle
->
Id
());
}
}
if
(
stop_obstacle
)
{
if
(
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,45 +78,42 @@ int Decider::MakeMainStopDecision(Frame* frame,
...
@@ -79,45 +78,42 @@ 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
;
}
}
const
auto
&
reference_line
=
frame
->
reference_line
();
const
auto
&
reference_line
=
frame
->
reference_line
();
apollo
::
common
::
PointENU
stop_point
=
object_decision
.
stop
().
stop_point
();
apollo
::
common
::
PointENU
stop_point
=
object_decision
.
stop
().
stop_point
();
common
::
SLPoint
stop_line_sl
;
common
::
SLPoint
stop_line_sl
;
reference_line
.
get_point_in_frenet_frame
({
stop_point
.
x
(),
stop_point
.
y
()},
reference_line
.
get_point_in_frenet_frame
({
stop_point
.
x
(),
stop_point
.
y
()},
&
stop_line_sl
);
&
stop_line_sl
);
double
stop_line_s
=
stop_line_sl
.
s
();
double
stop_line_s
=
stop_line_sl
.
s
();
if
(
stop_line_s
<
0
||
stop_line_s
>
reference_line
.
length
())
{
if
(
stop_line_s
<
0
||
stop_line_s
>
reference_line
.
length
())
{
AERROR
<<
"Ignore object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
AERROR
<<
"Ignore object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
<<
stop_line_s
<<
"] not in range[0, "
<<
reference_line
.
length
()
<<
stop_line_s
<<
"] not in range[0, "
<<
reference_line
.
length
()
<<
"]"
;
<<
"]"
;
continue
;
continue
;
}
}
// check stop_line_s vs adc_s
// check stop_line_s vs adc_s
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["
<<
stop_line_s
AERROR
<<
"object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
<<
"] behind adc route_s["
<<
adc_sl
.
s
()
<<
"]"
;
<<
stop_line_s
<<
"] behind adc route_s["
<<
adc_sl
.
s
()
continue
;
<<
"]"
;
}
continue
;
}
if
(
stop_line_s
<
min_stop_line_s
)
{
min_stop_line_s
=
stop_line_s
;
if
(
stop_line_s
<
min_stop_line_s
)
{
stop_obstacle
=
obstacle
;
min_stop_line_s
=
stop_line_s
;
stop_decision
=
&
(
object_decision
.
stop
());
stop_obstacle
=
obstacle
;
stop_decision
=
&
(
object_decision
.
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.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录