Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
accc4d2b
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,发现更多精彩内容 >>
提交
accc4d2b
编写于
12月 30, 2017
作者:
D
Dong Li
提交者:
Jiangtao Hu
12月 30, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: added protected obstacles in lagged prediction
上级
ab9dfeae
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
47 addition
and
19 deletion
+47
-19
modules/planning/common/lag_prediction.cc
modules/planning/common/lag_prediction.cc
+43
-18
modules/planning/common/lag_prediction.h
modules/planning/common/lag_prediction.h
+1
-1
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/common/lag_prediction.cc
浏览文件 @
accc4d2b
...
...
@@ -23,6 +23,7 @@
#include <algorithm>
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
namespace
apollo
{
...
...
@@ -50,13 +51,36 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
AdapterManager
::
GetPrediction
()
->
Empty
())
{
return
;
}
const
auto
&
prediction
=
*
AdapterManager
::
GetPrediction
();
const
auto
&
prediction
=
*
(
AdapterManager
::
GetPrediction
());
if
(
!
AdapterManager
::
GetLocalization
()
||
AdapterManager
::
GetLocalization
()
->
Empty
())
{
// no localization
obstacles
->
CopyFrom
(
prediction
.
GetLatestObserved
());
return
;
}
const
auto
adc_position
=
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
().
pose
().
position
();
const
auto
latest_prediction
=
(
*
prediction
.
begin
());
const
double
timestamp
=
latest_prediction
->
header
().
timestamp_sec
();
std
::
unordered_set
<
int
>
protected_obstacles
;
for
(
const
auto
&
obstacle
:
latest_prediction
->
prediction_obstacle
())
{
double
distance
=
common
::
util
::
DistanceXY
(
obstacle
.
perception_obstacle
().
position
(),
adc_position
);
if
(
distance
<
FLAGS_lag_prediction_protection_distance
)
{
protected_obstacles
.
insert
(
obstacle
.
perception_obstacle
().
id
());
// add protected obstacle
AddObstacleToPrediction
(
0.0
,
obstacle
,
obstacles
);
}
}
std
::
unordered_map
<
int
,
LagInfo
>
obstacle_lag_info
;
int
index
=
0
;
// data in begin() is the most recent data
for
(
auto
it
=
prediction
.
begin
();
it
!=
prediction
.
end
();
++
it
,
++
index
)
{
for
(
const
auto
&
obstacle
:
(
*
it
)
->
prediction_obstacle
())
{
auto
id
=
obstacle
.
perception_obstacle
().
id
();
if
(
protected_obstacles
.
count
(
id
)
>
0
)
{
continue
;
// don't need to count the already added protected obstacle
}
auto
&
info
=
obstacle_lag_info
[
id
];
++
info
.
count
;
if
((
*
it
)
->
header
().
timestamp_sec
()
>
info
.
last_observed_time
)
{
...
...
@@ -67,7 +91,6 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
}
}
const
auto
latest_prediction
=
(
*
prediction
.
begin
());
obstacles
->
mutable_header
()
->
CopyFrom
(
latest_prediction
->
header
());
obstacles
->
mutable_header
()
->
set_module_name
(
"lag_prediction"
);
obstacles
->
mutable_prediction_obstacle
()
->
Clear
();
...
...
@@ -75,40 +98,41 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
latest_prediction
->
perception_error_code
());
obstacles
->
set_start_timestamp
(
latest_prediction
->
start_timestamp
());
obstacles
->
set_end_timestamp
(
latest_prediction
->
end_timestamp
());
bool
apply_lag
=
std
::
distance
(
prediction
.
begin
(),
prediction
.
end
())
>=
static_cast
<
int32_t
>
(
min_appear_num_
);
bool
enough_msg_in_queue
=
std
::
distance
(
prediction
.
begin
(),
prediction
.
end
())
>=
static_cast
<
int32_t
>
(
min_appear_num_
);
for
(
const
auto
&
iter
:
obstacle_lag_info
)
{
if
(
apply_lag
&&
iter
.
second
.
count
<
min_appear_num_
)
{
if
(
enough_msg_in_queue
&&
iter
.
second
.
count
<
min_appear_num_
)
{
continue
;
}
if
(
apply_lag
&&
iter
.
second
.
last_observed_seq
>
max_disappear_num_
)
{
if
(
enough_msg_in_queue
&&
iter
.
second
.
last_observed_seq
>
max_disappear_num_
)
{
continue
;
}
AddObstacleToPrediction
(
latest_prediction
->
header
().
timestamp_sec
()
,
iter
.
second
,
obstacles
);
AddObstacleToPrediction
(
timestamp
-
iter
.
second
.
last_observed_time
,
*
iter
.
second
.
obstacle_ptr
,
obstacles
);
}
}
void
LagPrediction
::
AddObstacleToPrediction
(
double
start_time
,
const
LagInfo
&
lag_info
,
double
delay_sec
,
const
PredictionObstacle
&
history_obstacle
,
prediction
::
PredictionObstacles
*
obstacles
)
const
{
double
time_diff
=
start_time
-
lag_info
.
last_observed_time
;
auto
*
obstacle
=
obstacles
->
add_prediction_obstacle
();
if
(
time_diff
<=
1e-6
)
{
obstacle
->
CopyFrom
(
*
lag_info
.
obstacle_ptr
);
if
(
delay_sec
<=
1e-6
)
{
obstacle
->
CopyFrom
(
history_obstacle
);
return
;
}
obstacle
->
mutable_perception_obstacle
()
->
CopyFrom
(
lag_info
.
obstacle_ptr
->
perception_obstacle
());
for
(
const
auto
&
hist_trajectory
:
lag_info
.
obstacle_ptr
->
trajectory
())
{
history_obstacle
.
perception_obstacle
());
for
(
const
auto
&
hist_trajectory
:
history_obstacle
.
trajectory
())
{
auto
*
traj
=
obstacle
->
add_trajectory
();
for
(
const
auto
&
hist_point
:
hist_trajectory
.
trajectory_point
())
{
if
(
hist_point
.
relative_time
()
<
time_diff
)
{
if
(
hist_point
.
relative_time
()
<
delay_sec
)
{
continue
;
}
auto
*
point
=
traj
->
add_trajectory_point
();
point
->
CopyFrom
(
hist_point
);
point
->
set_relative_time
(
hist_point
.
relative_time
()
-
time_diff
);
point
->
set_relative_time
(
hist_point
.
relative_time
()
-
delay_sec
);
}
if
(
traj
->
trajectory_point_size
()
<=
0
)
{
obstacle
->
mutable_trajectory
()
->
RemoveLast
();
...
...
@@ -120,8 +144,9 @@ void LagPrediction::AddObstacleToPrediction(
obstacles
->
mutable_prediction_obstacle
()
->
RemoveLast
();
return
;
}
obstacle
->
set_timestamp
(
lag_info
.
obstacle_ptr
->
timestamp
());
obstacle
->
set_predicted_period
(
lag_info
.
obstacle_ptr
->
predicted_period
());
obstacle
->
set_timestamp
(
history_obstacle
.
timestamp
());
obstacle
->
set_predicted_period
(
history_obstacle
.
predicted_period
()
-
delay_sec
);
}
}
// namespace planning
...
...
modules/planning/common/lag_prediction.h
浏览文件 @
accc4d2b
...
...
@@ -43,7 +43,7 @@ class LagPrediction {
private:
void
AddObstacleToPrediction
(
double
start_time
,
const
LagInfo
&
lag_info
,
double
delay_sec
,
const
prediction
::
PredictionObstacle
&
history_obstacle
,
prediction
::
PredictionObstacles
*
obstacles
)
const
;
uint32_t
min_appear_num_
=
0
;
...
...
modules/planning/common/planning_gflags.cc
浏览文件 @
accc4d2b
...
...
@@ -309,6 +309,8 @@ DEFINE_int32(lag_prediction_min_appear_num, 5,
DEFINE_double
(
lag_prediction_max_disappear_num
,
3
,
"In lagged prediction, ingnore obstacle disappeared for more "
"than this value"
);
DEFINE_double
(
lag_prediction_protection_distance
,
30
,
"Within this distance, we do not use lagged prediction"
);
DEFINE_bool
(
enable_traffic_light
,
true
,
"True to enable traffic light input."
);
// QpSt optimizer
...
...
modules/planning/common/planning_gflags.h
浏览文件 @
accc4d2b
...
...
@@ -127,6 +127,7 @@ DECLARE_bool(align_prediction_time);
DECLARE_bool
(
enable_lag_prediction
);
DECLARE_int32
(
lag_prediction_min_appear_num
);
DECLARE_double
(
lag_prediction_max_disappear_num
);
DECLARE_double
(
lag_prediction_protection_distance
);
DECLARE_int32
(
trajectory_point_num_for_debug
);
DECLARE_bool
(
enable_record_debug
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录