Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f97b7be8
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,发现更多精彩内容 >>
提交
f97b7be8
编写于
1月 10, 2019
作者:
Y
Yajia Zhang
提交者:
Kecheng Xu
1月 10, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
prediction: obstacle.cc refactor round2
上级
4e68104f
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
107 addition
and
79 deletion
+107
-79
modules/prediction/container/obstacles/obstacle.cc
modules/prediction/container/obstacles/obstacle.cc
+58
-51
modules/prediction/container/obstacles/obstacle.h
modules/prediction/container/obstacles/obstacle.h
+17
-10
modules/prediction/container/obstacles/obstacles_container.cc
...les/prediction/container/obstacles/obstacles_container.cc
+28
-17
modules/prediction/container/obstacles/obstacles_container.h
modules/prediction/container/obstacles/obstacles_container.h
+4
-1
未找到文件。
modules/prediction/container/obstacles/obstacle.cc
浏览文件 @
f97b7be8
...
...
@@ -52,18 +52,13 @@ bool IsClosed(const double x0, const double y0, const double theta0,
}
// namespace
Obstacle
::
Obstacle
()
{}
PerceptionObstacle
::
Type
Obstacle
::
type
()
const
{
return
type_
;
}
int
Obstacle
::
id
()
const
{
return
id_
;
}
double
Obstacle
::
timestamp
()
const
{
if
(
feature_history_
.
size
()
>
0
)
{
return
feature_history_
.
front
().
timestamp
();
}
else
{
return
0.0
;
}
CHECK
(
!
feature_history_
.
empty
());
return
feature_history_
.
front
().
timestamp
();
}
const
Feature
&
Obstacle
::
feature
(
const
size_t
i
)
const
{
...
...
@@ -72,6 +67,7 @@ const Feature& Obstacle::feature(const size_t i) const {
}
Feature
*
Obstacle
::
mutable_feature
(
const
size_t
i
)
{
CHECK
(
!
feature_history_
.
empty
());
CHECK
(
i
<
feature_history_
.
size
());
return
&
feature_history_
[
i
];
}
...
...
@@ -132,28 +128,32 @@ bool Obstacle::IsNearJunction() {
FLAGS_junction_search_radius
);
}
void
Obstacle
::
Insert
(
const
PerceptionObstacle
&
perception_obstacle
,
bool
Obstacle
::
Insert
(
const
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
const
int
prediction_obstacle_id
)
{
if
(
feature_history_
.
size
()
>
0
&&
timestamp
<=
feature_history_
.
front
().
timestamp
())
{
if
(
!
perception_obstacle
.
has_id
()
||
!
perception_obstacle
.
has_type
())
{
AERROR
<<
"Perception obstacle has incomplete information; "
"skip insertion"
;
return
false
;
}
if
(
ReceivedNewerMessage
(
timestamp
))
{
AERROR
<<
"Obstacle ["
<<
id_
<<
"] received an older frame ["
<<
std
::
setprecision
(
20
)
<<
timestamp
<<
"] than the most recent timestamp [ "
<<
feature_history_
.
front
().
timestamp
()
<<
"]."
;
return
;
<<
this
->
timestamp
()
<<
"]."
;
return
false
;
}
// Set ID, Type, and Status of the feature.
Feature
feature
;
if
(
SetId
(
perception_obstacle
,
&
feature
,
prediction_obstacle_id
)
==
ErrorCode
::
PREDICTION_ERROR
)
{
return
;
if
(
!
SetId
(
perception_obstacle
,
&
feature
,
prediction_obstacle_id
))
{
return
false
;
}
if
(
SetType
(
perception_obstacle
,
&
feature
)
==
ErrorCode
::
PREDICTION_ERROR
)
{
return
;
if
(
!
SetType
(
perception_obstacle
,
&
feature
)
)
{
return
false
;
}
SetStatus
(
perception_obstacle
,
timestamp
,
&
feature
,
prediction_obstacle_id
);
SetStatus
(
perception_obstacle
,
timestamp
,
&
feature
);
// Set obstacle observation for KF tracking
if
(
!
FLAGS_use_navigation_mode
)
{
...
...
@@ -197,10 +197,12 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
// Trim historical features
Trim
();
return
true
;
}
void
Obstacle
::
InsertFeature
(
const
Feature
&
feature
)
{
bool
Obstacle
::
InsertFeature
(
const
Feature
&
feature
)
{
InsertFeatureToHistory
(
feature
);
return
true
;
}
bool
Obstacle
::
IsInJunction
(
const
std
::
string
&
junction_id
)
{
...
...
@@ -303,13 +305,7 @@ void Obstacle::SetJunctionFeatureWithoutEnterLane(
}
void
Obstacle
::
SetStatus
(
const
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
Feature
*
feature
,
int
pred_id
)
{
if
(
SetId
(
perception_obstacle
,
feature
,
pred_id
)
==
ErrorCode
::
PREDICTION_ERROR
)
{
AERROR
<<
"Obstacle has no ID"
;
return
;
}
const
double
timestamp
,
Feature
*
feature
)
{
SetTimestamp
(
perception_obstacle
,
timestamp
,
feature
);
SetPolygonPoints
(
perception_obstacle
,
feature
);
SetPosition
(
perception_obstacle
,
feature
);
...
...
@@ -395,13 +391,8 @@ void Obstacle::UpdateStatus(Feature* feature) {
}
}
ErrorCode
Obstacle
::
SetId
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
,
int
prediction_obstacle_id
)
{
if
(
!
perception_obstacle
.
has_id
())
{
AERROR
<<
"Obstacle has no ID."
;
return
ErrorCode
::
PREDICTION_ERROR
;
}
bool
Obstacle
::
SetId
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
,
int
prediction_obstacle_id
)
{
int
id
=
prediction_obstacle_id
>
0
?
prediction_obstacle_id
:
perception_obstacle
.
id
();
if
(
id_
<
0
)
{
...
...
@@ -411,24 +402,19 @@ ErrorCode Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
if
(
id_
!=
id
)
{
AERROR
<<
"Obstacle ["
<<
id_
<<
"] has a mismatched ID ["
<<
id
<<
"] from perception obstacle."
;
return
ErrorCode
::
PREDICTION_ERROR
;
return
false
;
}
}
feature
->
set_id
(
id
);
return
ErrorCode
::
OK
;
return
true
;
}
ErrorCode
Obstacle
::
SetType
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
)
{
if
(
perception_obstacle
.
has_type
())
{
type_
=
perception_obstacle
.
type
();
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has type ["
<<
type_
<<
"]."
;
feature
->
set_type
(
type_
);
}
else
{
AERROR
<<
"Obstacle ["
<<
id_
<<
"] has no type."
;
return
ErrorCode
::
PREDICTION_ERROR
;
}
return
ErrorCode
::
OK
;
bool
Obstacle
::
SetType
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
)
{
type_
=
perception_obstacle
.
type
();
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has type ["
<<
type_
<<
"]."
;
feature
->
set_type
(
type_
);
return
true
;
}
void
Obstacle
::
SetTimestamp
(
const
PerceptionObstacle
&
perception_obstacle
,
...
...
@@ -1355,10 +1341,7 @@ void Obstacle::SetLaneSequencePath(LaneGraph* const lane_graph) {
void
Obstacle
::
SetNearbyObstacles
()
{
// This function runs after all basic features have been set up
Feature
*
feature_ptr
=
mutable_latest_feature
();
if
(
feature_ptr
==
nullptr
)
{
AERROR
<<
"Null feature received."
;
return
;
}
LaneGraph
*
lane_graph
=
feature_ptr
->
mutable_lane
()
->
mutable_lane_graph
();
for
(
int
i
=
0
;
i
<
lane_graph
->
lane_sequence_size
();
++
i
)
{
...
...
@@ -1452,7 +1435,7 @@ void Obstacle::SetMotionStatus() {
}
void
Obstacle
::
SetMotionStatusBySpeed
()
{
int
history_size
=
static_cast
<
int
>
(
feature_history_
.
size
()
);
auto
history_size
=
feature_history_
.
size
(
);
if
(
history_size
<
2
)
{
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has no history and "
<<
"is considered moving."
;
...
...
@@ -1479,6 +1462,30 @@ void Obstacle::InsertFeatureToHistory(const Feature& feature) {
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] inserted a frame into the history."
;
}
std
::
unique_ptr
<
Obstacle
>
Obstacle
::
Create
(
const
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
const
int
prediction_id
)
{
std
::
unique_ptr
<
Obstacle
>
ptr_obstacle
(
new
Obstacle
());
if
(
!
ptr_obstacle
->
Insert
(
perception_obstacle
,
timestamp
,
prediction_id
))
{
return
nullptr
;
}
return
ptr_obstacle
;
}
std
::
unique_ptr
<
Obstacle
>
Obstacle
::
Create
(
const
Feature
&
feature
)
{
std
::
unique_ptr
<
Obstacle
>
ptr_obstacle
(
new
Obstacle
());
ptr_obstacle
->
InsertFeatureToHistory
(
feature
);
return
ptr_obstacle
;
}
bool
Obstacle
::
ReceivedNewerMessage
(
const
double
timestamp
)
const
{
if
(
feature_history_
.
empty
())
{
return
false
;
}
auto
last_timestamp_received
=
feature_history_
.
front
().
timestamp
();
return
timestamp
<=
last_timestamp_received
;
}
void
Obstacle
::
Trim
()
{
if
(
feature_history_
.
size
()
<
2
)
{
return
;
...
...
modules/prediction/container/obstacles/obstacle.h
浏览文件 @
f97b7be8
...
...
@@ -47,7 +47,12 @@ class Obstacle {
/**
* @brief Constructor
*/
Obstacle
();
static
std
::
unique_ptr
<
Obstacle
>
Create
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
const
int
prediction_id
);
static
std
::
unique_ptr
<
Obstacle
>
Create
(
const
Feature
&
feature
);
/**
* @brief Destructor
...
...
@@ -59,14 +64,14 @@ class Obstacle {
* @param perception_obstacle The obstacle from perception.
* @param timestamp The timestamp when the perception obstacle was detected.
*/
void
Insert
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
bool
Insert
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
const
int
prediction_id
);
/**
* @brief Insert a feature proto message.
* @param feature proto message.
*/
void
InsertFeature
(
const
Feature
&
feature
);
bool
InsertFeature
(
const
Feature
&
feature
);
/**
* @brief Get the type of perception obstacle's type.
...
...
@@ -86,6 +91,8 @@ class Obstacle {
*/
double
timestamp
()
const
;
bool
ReceivedNewerMessage
(
const
double
timestamp
)
const
;
/**
* @brief Get the ith feature from latest to earliest.
* @param i The index of the feature.
...
...
@@ -219,18 +226,18 @@ class Obstacle {
bool
RNNEnabled
()
const
;
private:
Obstacle
()
=
default
;
void
SetStatus
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
double
timestamp
,
Feature
*
feature
,
int
prediction_id
);
double
timestamp
,
Feature
*
feature
);
void
UpdateStatus
(
Feature
*
feature
);
common
::
ErrorCode
SetId
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
,
int
prediction_id
=
-
1
);
bool
SetId
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
,
int
prediction_id
=
-
1
);
common
::
ErrorCode
SetType
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
);
bool
SetType
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
);
void
SetIsNearJunction
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
,
...
...
modules/prediction/container/obstacles/obstacles_container.cc
浏览文件 @
f97b7be8
...
...
@@ -31,9 +31,8 @@ namespace prediction {
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacles
;
ObstaclesContainer
::
ObstaclesContainer
()
:
obstacles_
(
FLAGS_max_num_obstacles
),
:
ptr_
obstacles_
(
FLAGS_max_num_obstacles
),
id_mapping_
(
FLAGS_max_num_obstacles
)
{}
// This is called by Perception module at every frame to insert all
...
...
@@ -58,7 +57,7 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
timestamp
=
perception_obstacles
.
header
().
timestamp_sec
();
}
if
(
std
::
fabs
(
timestamp
-
timestamp_
)
>
FLAGS_replay_timestamp_gap
)
{
obstacles_
.
Clear
();
ptr_
obstacles_
.
Clear
();
ADEBUG
<<
"Replay mode is enabled."
;
}
else
if
(
timestamp
<=
timestamp_
)
{
AERROR
<<
"Invalid timestamp curr ["
<<
timestamp
<<
"] v.s. prev ["
...
...
@@ -82,6 +81,7 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
// Set up the ObstacleClusters:
// 1. Initialize ObstacleClusters
ObstacleClusters
::
Init
();
// 2. Insert the Obstacles one by one
for
(
const
PerceptionObstacle
&
perception_obstacle
:
perception_obstacles
.
perception_obstacle
())
{
...
...
@@ -109,7 +109,10 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
Obstacle
*
ObstaclesContainer
::
GetObstacle
(
const
int
id
)
{
return
obstacles_
.
GetSilently
(
id
);
auto
ptr_obstacle
=
ptr_obstacles_
.
GetSilently
(
id
);
if
(
ptr_obstacle
!=
nullptr
)
return
ptr_obstacles_
.
GetSilently
(
id
)
->
get
();
return
nullptr
;
}
...
...
@@ -120,7 +123,7 @@ ObstaclesContainer::GetCurrentFramePredictableObstacleIds() const {
void
ObstaclesContainer
::
Clear
()
{
obstacles_
.
Clear
();
ptr_
obstacles_
.
Clear
();
id_mapping_
.
Clear
();
timestamp_
=
-
1.0
;
}
...
...
@@ -151,14 +154,18 @@ void ObstaclesContainer::InsertPerceptionObstacle(
// Insert the obstacle and also update the LRUCache.
curr_frame_predictable_obstacle_ids_
.
push_back
(
prediction_id
);
Obstacle
*
obstacle_ptr
=
obstacles_
.
Get
(
prediction_id
);
auto
obstacle_ptr
=
GetObstacle
(
prediction_id
);
if
(
obstacle_ptr
!=
nullptr
)
{
obstacle_ptr
->
Insert
(
perception_obstacle
,
timestamp
,
prediction_id
);
ADEBUG
<<
"Refresh obstacle ["
<<
prediction_id
<<
"]"
;
}
else
{
Obstacle
obstacle
;
obstacle
.
Insert
(
perception_obstacle
,
timestamp
,
prediction_id
);
obstacles_
.
Put
(
prediction_id
,
std
::
move
(
obstacle
));
auto
ptr_obstacle
=
Obstacle
::
Create
(
perception_obstacle
,
timestamp
,
prediction_id
);
if
(
ptr_obstacle
==
nullptr
)
{
AERROR
<<
"Failed to insert obstacle into container"
;
return
;
}
ptr_obstacles_
.
Put
(
prediction_id
,
std
::
move
(
ptr_obstacle
));
ADEBUG
<<
"Insert obstacle ["
<<
prediction_id
<<
"]"
;
}
}
...
...
@@ -169,13 +176,16 @@ void ObstaclesContainer::InsertFeatureProto(const Feature& feature) {
return
;
}
int
id
=
feature
.
id
();
Obstacle
*
obstacle_ptr
=
obstacles_
.
Get
(
id
);
auto
obstacle_ptr
=
GetObstacle
(
id
);
if
(
obstacle_ptr
!=
nullptr
)
{
obstacle_ptr
->
InsertFeature
(
feature
);
}
else
{
Obstacle
obstacle
;
obstacle
.
InsertFeature
(
feature
);
obstacles_
.
Put
(
id
,
std
::
move
(
obstacle
));
auto
ptr_obstacle
=
Obstacle
::
Create
(
feature
);
if
(
ptr_obstacle
==
nullptr
)
{
AERROR
<<
"Failed to insert obstacle into container"
;
return
;
}
ptr_obstacles_
.
Put
(
id
,
std
::
move
(
ptr_obstacle
));
}
}
...
...
@@ -213,7 +223,8 @@ void ObstaclesContainer::BuildCurrentFrameIdMapping(
seen_prediction_ids
.
insert
(
prediction_id
);
}
}
else
{
// process adaption
common
::
util
::
Node
<
int
,
Obstacle
>*
curr
=
obstacles_
.
First
();
common
::
util
::
Node
<
int
,
std
::
unique_ptr
<
Obstacle
>>*
curr
=
ptr_obstacles_
.
First
();
while
(
curr
!=
nullptr
)
{
int
obs_id
=
curr
->
key
;
curr
=
curr
->
next
;
...
...
@@ -223,7 +234,7 @@ void ObstaclesContainer::BuildCurrentFrameIdMapping(
// this obs_id has already been processed
continue
;
}
Obstacle
*
obstacle_ptr
=
obstacles_
.
GetSilently
(
obs_id
);
Obstacle
*
obstacle_ptr
=
GetObstacle
(
obs_id
);
if
(
obstacle_ptr
==
nullptr
)
{
AERROR
<<
"Obstacle id ["
<<
obs_id
<<
"] with empty obstacle_ptr."
;
break
;
...
...
@@ -247,7 +258,7 @@ void ObstaclesContainer::BuildLaneGraph() {
// Go through every obstacle in the current frame, after some
// sanity checks, build lane graph for non-junction cases.
for
(
const
int
id
:
curr_frame_predictable_obstacle_ids_
)
{
Obstacle
*
obstacle_ptr
=
obstacles_
.
GetSilently
(
id
);
Obstacle
*
obstacle_ptr
=
GetObstacle
(
id
);
if
(
obstacle_ptr
==
nullptr
)
{
AERROR
<<
"Null obstacle found."
;
continue
;
...
...
@@ -265,7 +276,7 @@ void ObstaclesContainer::BuildJunctionFeature() {
// Go through every obstacle in the current frame, after some
// sanit checks, build junction features for those that are in junction.
for
(
const
int
id
:
curr_frame_predictable_obstacle_ids_
)
{
Obstacle
*
obstacle_ptr
=
obstacles_
.
GetSilently
(
id
);
Obstacle
*
obstacle_ptr
=
GetObstacle
(
id
);
if
(
obstacle_ptr
==
nullptr
)
{
AERROR
<<
"Null obstacle found."
;
continue
;
...
...
modules/prediction/container/obstacles/obstacles_container.h
浏览文件 @
f97b7be8
...
...
@@ -21,6 +21,7 @@
#pragma once
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
...
...
@@ -99,6 +100,8 @@ class ObstaclesContainer : public Container {
*/
void
Clear
();
size_t
NumOfObstacles
()
{
return
ptr_obstacles_
.
size
();
}
private:
/**
* @brief Check if a perception_obstacle is an old existed obstacle
...
...
@@ -118,7 +121,7 @@ class ObstaclesContainer : public Container {
private:
double
timestamp_
=
-
1.0
;
common
::
util
::
LRUCache
<
int
,
Obstacle
>
obstacles_
;
common
::
util
::
LRUCache
<
int
,
std
::
unique_ptr
<
Obstacle
>>
ptr_
obstacles_
;
// an id_mapping from perception_id to prediction_id
common
::
util
::
LRUCache
<
int
,
int
>
id_mapping_
;
std
::
vector
<
int
>
curr_frame_predictable_obstacle_ids_
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录