Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
352ac4e8
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,发现更多精彩内容 >>
提交
352ac4e8
编写于
12月 24, 2017
作者:
D
Dong Li
提交者:
Liangliang Zhang
12月 25, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: add history st graph decision
上级
7df7a923
变更
13
隐藏空白更改
内联
并排
Showing
13 changed file
with
275 addition
and
101 deletion
+275
-101
modules/planning/common/frame.h
modules/planning/common/frame.h
+7
-0
modules/planning/common/path_obstacle.h
modules/planning/common/path_obstacle.h
+5
-9
modules/planning/common/planning_gflags.cc
modules/planning/common/planning_gflags.cc
+2
-0
modules/planning/common/planning_gflags.h
modules/planning/common/planning_gflags.h
+1
-0
modules/planning/planning.cc
modules/planning/planning.cc
+22
-20
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc
+88
-41
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h
+15
-0
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
...s/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
+1
-1
modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
.../tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
+1
-1
modules/planning/tasks/speed_optimizer.cc
modules/planning/tasks/speed_optimizer.cc
+2
-2
modules/planning/tasks/speed_optimizer.h
modules/planning/tasks/speed_optimizer.h
+1
-1
modules/planning/tasks/st_graph/st_boundary_mapper.cc
modules/planning/tasks/st_graph/st_boundary_mapper.cc
+117
-22
modules/planning/tasks/st_graph/st_boundary_mapper.h
modules/planning/tasks/st_graph/st_boundary_mapper.h
+13
-4
未找到文件。
modules/planning/common/frame.h
浏览文件 @
352ac4e8
...
...
@@ -72,6 +72,7 @@ class Frame {
Obstacle
*
Find
(
const
std
::
string
&
id
);
const
ReferenceLineInfo
*
FindDriveReferenceLineInfo
();
const
ReferenceLineInfo
*
DriveReferenceLineInfo
()
const
;
const
std
::
vector
<
const
Obstacle
*>
obstacles
()
const
;
...
...
@@ -87,6 +88,10 @@ class Frame {
const
double
planning_start_time
,
prediction
::
PredictionObstacles
*
prediction_obstacles
);
ADCTrajectory
*
mutable_trajectory
()
{
return
&
trajectory_
;
}
const
ADCTrajectory
&
trajectory
()
const
{
return
trajectory_
;
}
private:
bool
InitReferenceLineInfo
();
...
...
@@ -127,6 +132,8 @@ class Frame {
ChangeLaneDecider
change_lane_decider_
;
ADCTrajectory
trajectory_
;
// last published trajectory
std
::
unique_ptr
<
LagPrediction
>
lag_predictor_
;
};
...
...
modules/planning/common/path_obstacle.h
浏览文件 @
352ac4e8
...
...
@@ -26,8 +26,6 @@
#include <unordered_map>
#include <vector>
#include "gtest/gtest_prod.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/planning/proto/decision.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
...
...
@@ -123,19 +121,17 @@ class PathObstacle {
void
SetPerceptionSlBoundary
(
const
SLBoundary
&
sl_boundary
);
private:
/**
* @brief check if a ObjectDecisionType is a l
ater
al decision.
* @brief check if a ObjectDecisionType is a l
ongitudin
al decision.
**/
FRIEND_TEST
(
IsLateralDecision
,
AllDecisions
);
static
bool
IsLateralDecision
(
const
ObjectDecisionType
&
decision
);
static
bool
IsLongitudinalDecision
(
const
ObjectDecisionType
&
decision
);
/**
* @brief check if a ObjectDecisionType is
longitudin
al decision.
* @brief check if a ObjectDecisionType is
a later
al decision.
**/
FRIEND_TEST
(
IsLongitudinalDecision
,
AllDecisions
);
static
bool
IsLongitudinalDecision
(
const
ObjectDecisionType
&
decision
);
static
bool
IsLateralDecision
(
const
ObjectDecisionType
&
decision
);
private:
FRIEND_TEST
(
MergeLongitudinalDecision
,
AllDecisions
);
static
ObjectDecisionType
MergeLongitudinalDecision
(
const
ObjectDecisionType
&
lhs
,
const
ObjectDecisionType
&
rhs
);
...
...
modules/planning/common/planning_gflags.cc
浏览文件 @
352ac4e8
...
...
@@ -178,6 +178,8 @@ DEFINE_bool(enable_nudge_decision, true, "enable nudge decision");
DEFINE_bool
(
enable_nudge_slowdown
,
true
,
"True to slow down when nudge obstacles."
);
DEFINE_bool
(
try_history_decision
,
false
,
"try history decision first"
);
DEFINE_double
(
static_decision_nudge_l_buffer
,
0.3
,
"l buffer for nudge"
);
DEFINE_double
(
longitudinal_ignore_buffer
,
10.0
,
"If an obstacle's longitudinal distance is further away "
...
...
modules/planning/common/planning_gflags.h
浏览文件 @
352ac4e8
...
...
@@ -101,6 +101,7 @@ DECLARE_double(st_max_t);
DECLARE_double
(
static_obstacle_speed_threshold
);
DECLARE_bool
(
enable_nudge_decision
);
DECLARE_bool
(
enable_nudge_slowdown
);
DECLARE_bool
(
try_history_decision
);
DECLARE_double
(
static_decision_nudge_l_buffer
);
DECLARE_double
(
longitudinal_ignore_buffer
);
DECLARE_double
(
lateral_ignore_buffer
);
...
...
modules/planning/planning.cc
浏览文件 @
352ac4e8
...
...
@@ -249,11 +249,15 @@ void Planning::RunOnce() {
const
uint32_t
frame_num
=
AdapterManager
::
GetPlanning
()
->
GetSeqNum
()
+
1
;
status
=
InitFrame
(
frame_num
,
stitching_trajectory
.
back
(),
start_timestamp
,
vehicle_state
);
ADCTrajectory
trajectory_pb
;
if
(
!
frame_
)
{
AERROR
<<
"Failed to init frame"
;
return
;
}
auto
*
trajectory_pb
=
frame_
->
mutable_trajectory
();
if
(
FLAGS_enable_record_debug
)
{
frame_
->
RecordInputDebug
(
trajectory_pb
.
mutable_debug
());
frame_
->
RecordInputDebug
(
trajectory_pb
->
mutable_debug
());
}
trajectory_pb
.
mutable_latency_stats
()
->
set_init_frame_time_ms
(
trajectory_pb
->
mutable_latency_stats
()
->
set_init_frame_time_ms
(
Clock
::
NowInSeconds
()
-
start_timestamp
);
if
(
!
status
.
ok
())
{
AERROR
<<
"Init frame failed"
;
...
...
@@ -263,43 +267,41 @@ void Planning::RunOnce() {
status
.
Save
(
estop
.
mutable_header
()
->
mutable_status
());
PublishPlanningPb
(
&
estop
,
start_timestamp
);
}
if
(
frame_
)
{
auto
seq_num
=
frame_
->
SequenceNum
();
FrameHistory
::
instance
()
->
Add
(
seq_num
,
std
::
move
(
frame_
));
}
auto
seq_num
=
frame_
->
SequenceNum
();
FrameHistory
::
instance
()
->
Add
(
seq_num
,
std
::
move
(
frame_
));
return
;
}
status
=
Plan
(
start_timestamp
,
stitching_trajectory
,
&
trajectory_pb
);
status
=
Plan
(
start_timestamp
,
stitching_trajectory
,
trajectory_pb
);
const
auto
time_diff_ms
=
(
Clock
::
NowInSeconds
()
-
start_timestamp
)
*
1000
;
ADEBUG
<<
"total planning time spend: "
<<
time_diff_ms
<<
" ms."
;
trajectory_pb
.
mutable_latency_stats
()
->
set_total_time_ms
(
time_diff_ms
);
ADEBUG
<<
"Planning latency: "
<<
trajectory_pb
.
latency_stats
().
DebugString
();
trajectory_pb
->
mutable_latency_stats
()
->
set_total_time_ms
(
time_diff_ms
);
ADEBUG
<<
"Planning latency: "
<<
trajectory_pb
->
latency_stats
().
DebugString
();
auto
*
ref_line_task
=
trajectory_pb
.
mutable_latency_stats
()
->
add_task_stats
();
auto
*
ref_line_task
=
trajectory_pb
->
mutable_latency_stats
()
->
add_task_stats
();
ref_line_task
->
set_time_ms
(
ReferenceLineProvider
::
instance
()
->
LastTimeDelay
()
*
1000.0
);
ref_line_task
->
set_name
(
"ReferenceLineProvider"
);
if
(
!
status
.
ok
())
{
status
.
Save
(
trajectory_pb
.
mutable_header
()
->
mutable_status
());
status
.
Save
(
trajectory_pb
->
mutable_header
()
->
mutable_status
());
AERROR
<<
"Planning failed:"
<<
status
.
ToString
();
if
(
FLAGS_publish_estop
)
{
AERROR
<<
"Planning failed and set estop"
;
trajectory_pb
.
mutable_estop
();
trajectory_pb
->
mutable_estop
();
}
}
trajectory_pb
.
set_is_replan
(
is_replan
);
PublishPlanningPb
(
&
trajectory_pb
,
start_timestamp
);
ADEBUG
<<
"Planning pb:"
<<
trajectory_pb
.
header
().
DebugString
();
trajectory_pb
->
set_is_replan
(
is_replan
);
PublishPlanningPb
(
trajectory_pb
,
start_timestamp
);
ADEBUG
<<
"Planning pb:"
<<
trajectory_pb
->
header
().
DebugString
();
if
(
frame_
)
{
auto
seq_num
=
frame_
->
SequenceNum
();
FrameHistory
::
instance
()
->
Add
(
seq_num
,
std
::
move
(
frame_
));
}
auto
seq_num
=
frame_
->
SequenceNum
();
FrameHistory
::
instance
()
->
Add
(
seq_num
,
std
::
move
(
frame_
));
}
void
Planning
::
Stop
()
{
...
...
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @
352ac4e8
...
...
@@ -38,6 +38,7 @@ namespace planning {
using
apollo
::
common
::
ErrorCode
;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
VehicleConfigHelper
;
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
planning_internal
::
STGraphDebug
;
...
...
@@ -52,38 +53,11 @@ bool DpStSpeedOptimizer::Init(const PlanningConfig& config) {
return
true
;
}
Status
DpStSpeedOptimizer
::
Process
(
const
SLBoundary
&
adc_sl_boundary
,
const
PathData
&
path_data
,
const
common
::
TrajectoryPoint
&
init_point
,
const
ReferenceLine
&
reference_line
,
const
SpeedData
&
reference_speed_data
,
PathDecision
*
const
path_decision
,
SpeedData
*
const
speed_data
)
{
if
(
!
is_init_
)
{
AERROR
<<
"Please call Init() before process DpStSpeedOptimizer."
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Not inited."
);
}
if
(
path_data
.
discretized_path
().
NumOfPoints
()
==
0
)
{
std
::
string
msg
(
"Empty path data"
);
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
StBoundaryMapper
boundary_mapper
(
adc_sl_boundary
,
st_boundary_config_
,
reference_line
,
path_data
,
dp_st_speed_config_
.
total_path_length
(),
dp_st_speed_config_
.
total_time
(),
reference_line_info_
->
IsChangeLanePath
());
// step 1 get boundaries
path_decision
->
EraseStBoundaries
();
if
(
boundary_mapper
.
GetGraphBoundary
(
path_decision
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
const
std
::
string
msg
=
"Mapping obstacle for dp st speed optimizer failed."
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
bool
DpStSpeedOptimizer
::
SearchStGraph
(
const
StBoundaryMapper
&
boundary_mapper
,
const
PathData
&
path_data
,
SpeedData
*
speed_data
,
PathDecision
*
path_decision
,
STGraphDebug
*
st_graph_debug
)
const
{
std
::
vector
<
const
StBoundary
*>
boundaries
;
for
(
const
auto
*
obstacle
:
path_decision
->
path_obstacles
().
Items
())
{
if
(
!
obstacle
->
st_boundary
().
IsEmpty
())
{
...
...
@@ -99,27 +73,100 @@ Status DpStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
const
std
::
string
msg
=
"Getting speed limits for dp st speed optimizer failed!"
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
)
;
return
false
;
}
const
double
path_length
=
path_data
.
discretized_path
().
Length
();
StGraphData
st_graph_data
(
boundaries
,
init_point
,
speed_limit
,
path_length
);
StGraphData
st_graph_data
(
boundaries
,
init_point
_
,
speed_limit
,
path_length
);
DpStGraph
st_graph
(
reference_line
,
st_graph_data
,
dp_st_speed_config_
,
path_data
,
adc_sl_boundary
);
auto
*
debug
=
reference_line_info_
->
mutable_debug
();
STGraphDebug
*
st_graph_debug
=
debug
->
mutable_planning_data
()
->
add_st_graph
();
DpStGraph
st_graph
(
*
reference_line_
,
st_graph_data
,
dp_st_speed_config_
,
path_data
,
adc_sl_boundary_
);
if
(
!
st_graph
.
Search
(
path_decision
,
speed_data
).
ok
())
{
const
std
::
string
msg
(
Name
()
+
":Failed to search graph with dynamic programming."
);
const
std
::
string
msg
(
"With history decision: failed to search graph with dynamic "
"programming."
);
AERROR
<<
msg
;
RecordSTGraphDebug
(
st_graph_data
,
st_graph_debug
);
return
false
;
}
RecordSTGraphDebug
(
st_graph_data
,
st_graph_debug
);
return
true
;
}
bool
DpStSpeedOptimizer
::
CreateStBoundaryWithHistoryDecision
(
const
StBoundaryMapper
&
boundary_mapper
,
const
PathData
&
path_data
,
SpeedData
*
speed_data
,
PathDecision
*
path_decision
)
{
if
(
!
FLAGS_try_history_decision
)
{
return
false
;
}
path_decision
->
EraseStBoundaries
();
const
auto
*
last_frame
=
FrameHistory
::
instance
()
->
Latest
();
if
(
!
last_frame
)
{
return
false
;
}
if
(
boundary_mapper
.
CreateStBoundaryWithHistory
(
last_frame
->
trajectory
().
decision
().
object_decision
(),
path_decision
)
.
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
const
std
::
string
msg
=
"With history decision: decision for dp st speed optimizer "
"failed."
;
AERROR
<<
msg
;
return
false
;
}
return
SearchStGraph
(
boundary_mapper
,
path_data
,
speed_data
,
path_decision
,
nullptr
);
}
Status
DpStSpeedOptimizer
::
Process
(
const
SLBoundary
&
adc_sl_boundary
,
const
PathData
&
path_data
,
const
TrajectoryPoint
&
init_point
,
const
ReferenceLine
&
reference_line
,
const
SpeedData
&
reference_speed_data
,
PathDecision
*
const
path_decision
,
SpeedData
*
const
speed_data
)
{
if
(
!
is_init_
)
{
AERROR
<<
"Please call Init() before process DpStSpeedOptimizer."
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Not inited."
);
}
init_point_
=
init_point
;
adc_sl_boundary_
=
adc_sl_boundary
;
reference_line_
=
&
reference_line
;
if
(
path_data
.
discretized_path
().
NumOfPoints
()
==
0
)
{
std
::
string
msg
(
"Empty path data"
);
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
RecordSTGraphDebug
(
st_graph_data
,
st_graph_debug
);
StBoundaryMapper
boundary_mapper
(
adc_sl_boundary
,
st_boundary_config_
,
*
reference_line_
,
path_data
,
dp_st_speed_config_
.
total_path_length
(),
dp_st_speed_config_
.
total_time
(),
reference_line_info_
->
IsChangeLanePath
());
auto
*
debug
=
reference_line_info_
->
mutable_debug
();
STGraphDebug
*
st_graph_debug
=
debug
->
mutable_planning_data
()
->
add_st_graph
();
if
(
!
CreateStBoundaryWithHistoryDecision
(
boundary_mapper
,
path_data
,
speed_data
,
path_decision
))
{
path_decision
->
EraseStBoundaries
();
if
(
boundary_mapper
.
CreateStBoundary
(
path_decision
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
const
std
::
string
msg
=
"Mapping obstacle for dp st speed optimizer failed."
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
if
(
!
SearchStGraph
(
boundary_mapper
,
path_data
,
speed_data
,
path_decision
,
st_graph_debug
))
{
const
std
::
string
msg
(
Name
()
+
":Failed to search graph with dynamic programming."
);
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
}
return
Status
::
OK
();
}
...
...
modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.h
浏览文件 @
352ac4e8
...
...
@@ -24,6 +24,7 @@
#include <string>
#include "modules/planning/proto/dp_st_speed_config.pb.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/proto/st_boundary_config.pb.h"
#include "modules/planning/tasks/speed_optimizer.h"
...
...
@@ -46,6 +47,20 @@ class DpStSpeedOptimizer : public SpeedOptimizer {
const
SpeedData
&
reference_speed_data
,
PathDecision
*
const
path_decision
,
SpeedData
*
const
speed_data
)
override
;
bool
CreateStBoundaryWithHistoryDecision
(
const
StBoundaryMapper
&
boundary_mapper
,
const
PathData
&
path_data
,
SpeedData
*
speed_data
,
PathDecision
*
path_decision
);
bool
SearchStGraph
(
const
StBoundaryMapper
&
boundary_mapper
,
const
PathData
&
path_data
,
SpeedData
*
speed_data
,
PathDecision
*
path_decision
,
planning_internal
::
STGraphDebug
*
debug
)
const
;
private:
common
::
TrajectoryPoint
init_point_
;
const
ReferenceLine
*
reference_line_
=
nullptr
;
SLBoundary
adc_sl_boundary_
;
DpStSpeedConfig
dp_st_speed_config_
;
StBoundaryConfig
st_boundary_config_
;
};
...
...
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
浏览文件 @
352ac4e8
...
...
@@ -88,7 +88,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
}
// step 1 get boundaries
path_decision
->
EraseStBoundaries
();
if
(
boundary_mapper
.
GetGraph
Boundary
(
path_decision
).
code
()
==
if
(
boundary_mapper
.
CreateSt
Boundary
(
path_decision
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Mapping obstacle for qp st speed optimizer failed!"
);
...
...
modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @
352ac4e8
...
...
@@ -85,7 +85,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
}
// step 1 get boundaries
path_decision
->
EraseStBoundaries
();
if
(
boundary_mapper
.
GetGraph
Boundary
(
path_decision
).
code
()
==
if
(
boundary_mapper
.
CreateSt
Boundary
(
path_decision
).
code
()
==
ErrorCode
::
PLANNING_ERROR
)
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Mapping obstacle for qp st speed optimizer failed!"
);
...
...
modules/planning/tasks/speed_optimizer.cc
浏览文件 @
352ac4e8
...
...
@@ -110,8 +110,8 @@ void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
}
void
SpeedOptimizer
::
RecordSTGraphDebug
(
const
StGraphData
&
st_graph_data
,
STGraphDebug
*
st_graph_debug
)
{
if
(
!
FLAGS_enable_record_debug
)
{
STGraphDebug
*
st_graph_debug
)
const
{
if
(
!
FLAGS_enable_record_debug
||
!
st_graph_debug
)
{
ADEBUG
<<
"Skip record debug info"
;
return
;
}
...
...
modules/planning/tasks/speed_optimizer.h
浏览文件 @
352ac4e8
...
...
@@ -51,7 +51,7 @@ class SpeedOptimizer : public Task {
const
double
init_acc
)
const
;
void
RecordSTGraphDebug
(
const
StGraphData
&
st_graph_data
,
planning_internal
::
STGraphDebug
*
stGraphDebug
);
planning_internal
::
STGraphDebug
*
stGraphDebug
)
const
;
void
RecordDebugInfo
(
const
SpeedData
&
speed_data
);
};
...
...
modules/planning/tasks/st_graph/st_boundary_mapper.cc
浏览文件 @
352ac4e8
...
...
@@ -22,6 +22,7 @@
#include <algorithm>
#include <limits>
#include <unordered_map>
#include <utility>
#include "modules/common/proto/pnc_point.pb.h"
...
...
@@ -72,7 +73,7 @@ StBoundaryMapper::StBoundaryMapper(const SLBoundary& adc_sl_boundary,
planning_time_
(
planning_time
),
is_change_lane_
(
is_change_lane
)
{}
Status
StBoundaryMapper
::
GetGraph
Boundary
(
PathDecision
*
path_decision
)
const
{
Status
StBoundaryMapper
::
CreateSt
Boundary
(
PathDecision
*
path_decision
)
const
{
const
auto
&
path_obstacles
=
path_decision
->
path_obstacles
();
if
(
planning_time_
<
0.0
)
{
const
std
::
string
msg
=
"Fail to get params since planning_time_ < 0."
;
...
...
@@ -94,8 +95,6 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
for
(
const
auto
*
const_path_obstacle
:
path_obstacles
.
Items
())
{
auto
*
path_obstacle
=
path_decision
->
Find
(
const_path_obstacle
->
Id
());
StBoundary
boundary
;
boundary
.
SetId
(
path_obstacle
->
Id
());
if
(
!
path_obstacle
->
HasLongitudinalDecision
())
{
if
(
!
MapWithoutDecision
(
path_obstacle
).
ok
())
{
std
::
string
msg
=
StrCat
(
"Fail to map obstacle "
,
path_obstacle
->
Id
(),
...
...
@@ -126,7 +125,7 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
}
}
else
if
(
decision
.
has_follow
()
||
decision
.
has_overtake
()
||
decision
.
has_yield
())
{
if
(
!
MapWith
PredictionTrajectory
(
path_obstacle
).
ok
())
{
if
(
!
MapWith
Decision
(
path_obstacle
,
decision
).
ok
())
{
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
<<
" with decision: "
<<
decision
.
DebugString
();
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
...
...
@@ -138,7 +137,7 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
}
if
(
stop_obstacle
)
{
bool
success
=
MapStopDecision
(
stop_obstacle
);
bool
success
=
MapStopDecision
(
stop_obstacle
,
stop_decision
);
if
(
!
success
)
{
std
::
string
msg
=
"Fail to MapStopDecision."
;
AERROR
<<
msg
;
...
...
@@ -148,8 +147,105 @@ Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
return
Status
::
OK
();
}
bool
StBoundaryMapper
::
MapStopDecision
(
PathObstacle
*
stop_obstacle
)
const
{
const
auto
&
stop_decision
=
stop_obstacle
->
LongitudinalDecision
();
Status
StBoundaryMapper
::
CreateStBoundaryWithHistory
(
const
ObjectDecisions
&
history_decisions
,
PathDecision
*
path_decision
)
const
{
const
auto
&
path_obstacles
=
path_decision
->
path_obstacles
();
if
(
planning_time_
<
0.0
)
{
const
std
::
string
msg
=
"Fail to get params since planning_time_ < 0."
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
if
(
path_data_
.
discretized_path
().
NumOfPoints
()
<
2
)
{
AERROR
<<
"Fail to get params because of too few path points. path points "
"size: "
<<
path_data_
.
discretized_path
().
NumOfPoints
()
<<
"."
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Fail to get params because of too few path points"
);
}
std
::
unordered_map
<
std
::
string
,
ObjectDecisionType
>
prev_decision_map
;
for
(
const
auto
&
history_decision
:
history_decisions
.
decision
())
{
for
(
const
auto
&
decision
:
history_decision
.
object_decision
())
{
if
(
PathObstacle
::
IsLongitudinalDecision
(
decision
)
&&
!
decision
.
has_ignore
())
{
prev_decision_map
[
history_decision
.
id
()]
=
decision
;
break
;
}
}
}
PathObstacle
*
stop_obstacle
=
nullptr
;
ObjectDecisionType
stop_decision
;
double
min_stop_s
=
std
::
numeric_limits
<
double
>::
max
();
for
(
const
auto
*
const_path_obstacle
:
path_obstacles
.
Items
())
{
auto
*
path_obstacle
=
path_decision
->
Find
(
const_path_obstacle
->
Id
());
auto
iter
=
prev_decision_map
.
find
(
path_obstacle
->
Id
());
ObjectDecisionType
decision
;
if
(
iter
==
prev_decision_map
.
end
())
{
decision
.
mutable_ignore
();
}
else
{
decision
=
iter
->
second
;
}
if
(
!
path_obstacle
->
HasLongitudinalDecision
()
&&
decision
.
has_ignore
())
{
if
(
!
MapWithoutDecision
(
path_obstacle
).
ok
())
{
std
::
string
msg
=
StrCat
(
"Fail to map obstacle "
,
path_obstacle
->
Id
(),
" without decision."
);
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
continue
;
}
if
(
path_obstacle
->
HasLongitudinalDecision
())
{
decision
=
path_obstacle
->
LongitudinalDecision
();
}
if
(
decision
.
has_stop
())
{
const
double
stop_s
=
path_obstacle
->
PerceptionSLBoundary
().
start_s
()
+
decision
.
stop
().
distance_s
();
// this is a rough estimation based on reference line s, so that a large
// buffer is used.
constexpr
double
stop_buff
=
1.0
;
if
(
stop_s
+
stop_buff
<
adc_sl_boundary_
.
end_s
())
{
AERROR
<<
"Invalid stop decision. not stop at behind of current "
"position. stop_s : "
<<
stop_s
<<
", and current adc_s is; "
<<
adc_sl_boundary_
.
end_s
();
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"invalid decision"
);
}
if
(
stop_s
<
min_stop_s
)
{
stop_obstacle
=
path_obstacle
;
min_stop_s
=
stop_s
;
stop_decision
=
decision
;
}
}
else
if
(
decision
.
has_follow
()
||
decision
.
has_overtake
()
||
decision
.
has_yield
())
{
if
(
!
MapWithDecision
(
path_obstacle
,
decision
).
ok
())
{
AERROR
<<
"Fail to map obstacle "
<<
path_obstacle
->
Id
()
<<
" with decision: "
<<
decision
.
DebugString
();
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Fail to map overtake/yield decision"
);
}
}
else
{
AWARN
<<
"No mapping for decision: "
<<
decision
.
DebugString
();
}
}
if
(
stop_obstacle
)
{
bool
success
=
MapStopDecision
(
stop_obstacle
,
stop_decision
);
if
(
!
success
)
{
std
::
string
msg
=
"Fail to MapStopDecision."
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
}
return
Status
::
OK
();
}
bool
StBoundaryMapper
::
MapStopDecision
(
PathObstacle
*
stop_obstacle
,
const
ObjectDecisionType
&
stop_decision
)
const
{
DCHECK
(
stop_decision
.
has_stop
())
<<
"Must have stop decision"
;
if
(
stop_obstacle
->
PerceptionSLBoundary
().
start_s
()
>
planning_distance_
)
{
...
...
@@ -347,13 +443,12 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
return
(
lower_points
->
size
()
>
1
&&
upper_points
->
size
()
>
1
);
}
Status
StBoundaryMapper
::
MapWithPredictionTrajectory
(
PathObstacle
*
path_obstacle
)
const
{
const
auto
&
obj_decision
=
path_obstacle
->
LongitudinalDecision
();
DCHECK
(
obj_decision
.
has_follow
()
||
obj_decision
.
has_yield
()
||
obj_decision
.
has_overtake
())
<<
"obj_decision must be follow or yield or overtake.
\n
"
<<
obj_decision
.
DebugString
();
Status
StBoundaryMapper
::
MapWithDecision
(
PathObstacle
*
path_obstacle
,
const
ObjectDecisionType
&
decision
)
const
{
DCHECK
(
decision
.
has_follow
()
||
decision
.
has_yield
()
||
decision
.
has_overtake
())
<<
"decision is "
<<
decision
.
DebugString
()
<<
", but it must be follow or yield or overtake."
;
std
::
vector
<
STPoint
>
lower_points
;
std
::
vector
<
STPoint
>
upper_points
;
...
...
@@ -364,7 +459,7 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
return
Status
::
OK
();
}
if
(
obj_
decision
.
has_follow
()
&&
lower_points
.
back
().
t
()
<
planning_time_
)
{
if
(
decision
.
has_follow
()
&&
lower_points
.
back
().
t
()
<
planning_time_
)
{
const
double
diff_s
=
lower_points
.
back
().
s
()
-
lower_points
.
front
().
s
();
const
double
diff_t
=
lower_points
.
back
().
t
()
-
lower_points
.
front
().
t
();
double
extend_lower_s
=
...
...
@@ -384,20 +479,20 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
// get characteristic_length and boundary_type.
StBoundary
::
BoundaryType
b_type
=
StBoundary
::
BoundaryType
::
UNKNOWN
;
double
characteristic_length
=
0.0
;
if
(
obj_
decision
.
has_follow
())
{
characteristic_length
=
std
::
fabs
(
obj_
decision
.
follow
().
distance_s
());
if
(
decision
.
has_follow
())
{
characteristic_length
=
std
::
fabs
(
decision
.
follow
().
distance_s
());
b_type
=
StBoundary
::
BoundaryType
::
FOLLOW
;
}
else
if
(
obj_
decision
.
has_yield
())
{
characteristic_length
=
std
::
fabs
(
obj_
decision
.
yield
().
distance_s
());
}
else
if
(
decision
.
has_yield
())
{
characteristic_length
=
std
::
fabs
(
decision
.
yield
().
distance_s
());
boundary
=
StBoundary
::
GenerateStBoundary
(
lower_points
,
upper_points
)
.
ExpandByS
(
characteristic_length
);
b_type
=
StBoundary
::
BoundaryType
::
YIELD
;
}
else
if
(
obj_
decision
.
has_overtake
())
{
characteristic_length
=
std
::
fabs
(
obj_
decision
.
overtake
().
distance_s
());
}
else
if
(
decision
.
has_overtake
())
{
characteristic_length
=
std
::
fabs
(
decision
.
overtake
().
distance_s
());
b_type
=
StBoundary
::
BoundaryType
::
OVERTAKE
;
}
else
{
DCHECK
(
false
)
<<
"Obj decision should be either yield or overtake: "
<<
obj_
decision
.
DebugString
();
<<
decision
.
DebugString
();
}
boundary
.
SetBoundaryType
(
b_type
);
boundary
.
SetId
(
path_obstacle
->
obstacle
()
->
Id
());
...
...
modules/planning/tasks/st_graph/st_boundary_mapper.h
浏览文件 @
352ac4e8
...
...
@@ -47,7 +47,15 @@ class StBoundaryMapper {
virtual
~
StBoundaryMapper
()
=
default
;
apollo
::
common
::
Status
GetGraphBoundary
(
PathDecision
*
path_decision
)
const
;
apollo
::
common
::
Status
CreateStBoundary
(
PathDecision
*
path_decision
)
const
;
apollo
::
common
::
Status
CreateStBoundaryWithHistory
(
const
ObjectDecisions
&
history_decisions
,
PathDecision
*
path_decision
)
const
;
apollo
::
common
::
Status
CreateStBoundary
(
PathObstacle
*
path_obstacle
,
const
ObjectDecisionType
&
external_decision
)
const
;
virtual
apollo
::
common
::
Status
GetSpeedLimits
(
const
IndexedList
<
std
::
string
,
PathObstacle
>&
path_obstacles
,
...
...
@@ -71,10 +79,11 @@ class StBoundaryMapper {
apollo
::
common
::
Status
MapWithoutDecision
(
PathObstacle
*
path_obstacle
)
const
;
bool
MapStopDecision
(
PathObstacle
*
stop_obstacle
)
const
;
bool
MapStopDecision
(
PathObstacle
*
stop_obstacle
,
const
ObjectDecisionType
&
decision
)
const
;
apollo
::
common
::
Status
MapWith
PredictionTrajectory
(
PathObstacle
*
path_obstacle
)
const
;
apollo
::
common
::
Status
MapWith
Decision
(
PathObstacle
*
path_obstacle
,
const
ObjectDecisionType
&
decision
)
const
;
FRIEND_TEST
(
StBoundaryMapperTest
,
get_centric_acc_limit
);
double
GetCentricAccLimit
(
const
double
kappa
)
const
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录