Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
66157d14
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,发现更多精彩内容 >>
提交
66157d14
编写于
7月 13, 2017
作者:
K
kechxu
提交者:
Calvin Miao
7月 14, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
add some basic setters in obstacle class
上级
8dbbba4f
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
133 addition
and
13 deletion
+133
-13
modules/prediction/container/obstacles/BUILD
modules/prediction/container/obstacles/BUILD
+5
-1
modules/prediction/container/obstacles/obstacle.cc
modules/prediction/container/obstacles/obstacle.cc
+109
-11
modules/prediction/container/obstacles/obstacle.h
modules/prediction/container/obstacles/obstacle.h
+19
-1
未找到文件。
modules/prediction/container/obstacles/BUILD
浏览文件 @
66157d14
...
@@ -15,6 +15,10 @@ cc_library(
...
@@ -15,6 +15,10 @@ cc_library(
srcs
=
[
"obstacle.cc"
],
srcs
=
[
"obstacle.cc"
],
hdrs
=
[
"obstacle.h"
],
hdrs
=
[
"obstacle.h"
],
deps
=
[
deps
=
[
"//modules/common/proto:error_code_proto"
,
"//modules/perception/proto:perception_proto"
,
"//modules/perception/proto:perception_proto"
,
]
"//modules/prediction/proto:feature_proto"
,
"//modules/common:log"
,
"//modules/common/math:kalman_filter"
,
],
)
)
\ No newline at end of file
modules/prediction/container/obstacles/obstacle.cc
浏览文件 @
66157d14
...
@@ -16,13 +16,18 @@
...
@@ -16,13 +16,18 @@
#include "modules/prediction/container/obstacles/obstacle.h"
#include "modules/prediction/container/obstacles/obstacle.h"
#include <iomanip>
#include "modules/common/log.h"
namespace
apollo
{
namespace
apollo
{
namespace
prediction
{
namespace
prediction
{
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
common
::
math
::
KalmanFilter
;
using
apollo
::
common
::
math
::
KalmanFilter
;
using
apollo
::
common
::
ErrorCode
;
std
::
mutex
Obstacle
::
_mutex
;
std
::
mutex
Obstacle
::
mutex_
;
Obstacle
::
Obstacle
()
:
Obstacle
::
Obstacle
()
:
id_
(
-
1
),
id_
(
-
1
),
...
@@ -44,12 +49,12 @@ Obstacle::~Obstacle() {
...
@@ -44,12 +49,12 @@ Obstacle::~Obstacle() {
}
}
int
Obstacle
::
id
()
const
{
int
Obstacle
::
id
()
const
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
return
id_
;
return
id_
;
}
}
double
Obstacle
::
timestamp
()
const
{
double
Obstacle
::
timestamp
()
const
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
if
(
feature_history_
.
size
()
>
0
)
{
if
(
feature_history_
.
size
()
>
0
)
{
return
feature_history_
.
front
().
timestamp
();
return
feature_history_
.
front
().
timestamp
();
}
else
{
}
else
{
...
@@ -58,32 +63,32 @@ double Obstacle::timestamp() const {
...
@@ -58,32 +63,32 @@ double Obstacle::timestamp() const {
}
}
const
Feature
&
Obstacle
::
feature
(
size_t
i
)
{
const
Feature
&
Obstacle
::
feature
(
size_t
i
)
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
CHECK
(
i
<
feature_history_
.
size
());
CHECK
(
i
<
feature_history_
.
size
());
return
feature_history_
[
i
];
return
feature_history_
[
i
];
}
}
Feature
*
Obstacle
::
mutable_feature
(
size_t
i
)
{
Feature
*
Obstacle
::
mutable_feature
(
size_t
i
)
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
CHECK
(
i
<
feature_history_
.
size
());
CHECK
(
i
<
feature_history_
.
size
());
return
&
feature_history_
[
i
];
return
&
feature_history_
[
i
];
}
}
const
Feature
&
Obstacle
::
latest_feature
()
{
const
Feature
&
Obstacle
::
latest_feature
()
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
CHECK
(
feature_history_
.
size
()
>
0
);
CHECK
(
feature_history_
.
size
()
>
0
);
return
feature_history_
.
front
();
return
feature_history_
.
front
();
}
}
Feature
*
Obstacle
::
mutable_latest_feature
()
{
Feature
*
Obstacle
::
mutable_latest_feature
()
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
CHECK
(
feature_history_
.
size
()
>
0
);
CHECK
(
feature_history_
.
size
()
>
0
);
return
&
(
feature_history_
.
front
());
return
&
(
feature_history_
.
front
());
}
}
size_t
Obstacle
::
history_size
()
const
{
size_t
Obstacle
::
history_size
()
const
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
return
feature_history_
.
size
();
return
feature_history_
.
size
();
}
}
...
@@ -93,11 +98,104 @@ const KalmanFilter<double, 4, 2, 0>& Obstacle::kf_lane_tracker(
...
@@ -93,11 +98,104 @@ const KalmanFilter<double, 4, 2, 0>& Obstacle::kf_lane_tracker(
return
kf_lane_tracker_map_
[
lane_id
];
return
kf_lane_tracker_map_
[
lane_id
];
}
}
void
Obstacle
::
Insert
(
void
Obstacle
::
Insert
(
const
PerceptionObstacle
&
perception_obstacle
,
const
apollo
::
perception
::
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
)
{
const
double
timestamp
)
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
mutex_
);
if
(
feature_history_
.
size
()
>
0
&&
timestamp
<=
feature_history_
.
front
().
timestamp
())
{
AINFO
<<
"Obstacle ["
<<
id_
<<
"] received an older frame ["
<<
timestamp
<<
"] than the most recent timestamp [ "
<<
feature_history_
.
front
().
timestamp
()
<<
"]."
;
return
;
}
Feature
feature
;
if
(
SetId
(
perception_obstacle
,
&
feature
)
==
ErrorCode
::
PREDICTION_ERROR
)
{
return
;
}
if
(
SetType
(
perception_obstacle
)
==
ErrorCode
::
PREDICTION_ERROR
)
{
return
;
}
SetTimestamp
(
perception_obstacle
,
timestamp
,
&
feature
);
SetPosition
(
perception_obstacle
,
&
feature
);
}
ErrorCode
Obstacle
::
SetId
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
)
{
if
(
!
perception_obstacle
.
has_id
())
{
AERROR
<<
"Obstacle has no ID."
;
return
ErrorCode
::
PREDICTION_ERROR
;
}
int
id
=
perception_obstacle
.
id
();
if
(
id_
<
0
)
{
id_
=
id
;
AINFO
<<
"Obstacle set id ["
<<
id_
<<
"]."
;
}
else
{
if
(
id_
!=
id
)
{
AERROR
<<
"Obstacle ["
<<
id_
<<
"] has a mismatched ID ["
<<
id
<<
"] from perception obstacle."
;
return
ErrorCode
::
PREDICTION_ERROR
;
}
else
{
feature
->
set_id
(
id
);
}
}
return
ErrorCode
::
OK
;
}
ErrorCode
Obstacle
::
SetType
(
const
PerceptionObstacle
&
perception_obstacle
)
{
if
(
perception_obstacle
.
has_type
())
{
type_
=
perception_obstacle
.
type
();
AINFO
<<
"Obstacle ["
<<
id_
<<
"] set type ["
<<
type_
<<
"]."
;
}
else
{
AERROR
<<
"Obstacle ["
<<
id_
<<
"] has no type."
;
return
ErrorCode
::
PREDICTION_ERROR
;
}
return
ErrorCode
::
OK
;
}
void
Obstacle
::
SetTimestamp
(
const
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
Feature
*
feature
)
{
double
ts
=
timestamp
;
if
(
perception_obstacle
.
has_timestamp
()
&&
perception_obstacle
.
timestamp
()
>
0.0
)
{
ts
=
perception_obstacle
.
timestamp
();
}
feature
->
set_timestamp
(
ts
);
AINFO
<<
"Obstacle ["
<<
id_
<<
"] set timestamp ["
<<
std
::
fixed
<<
std
::
setprecision
(
6
)
<<
ts
<<
"]."
;
}
}
void
Obstacle
::
SetPosition
(
const
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
)
{
double
x
=
0.0
;
double
y
=
0.0
;
double
z
=
0.0
;
if
(
perception_obstacle
.
has_position
())
{
if
(
perception_obstacle
.
position
().
has_x
())
{
x
=
perception_obstacle
.
position
().
x
();
}
if
(
perception_obstacle
.
position
().
has_y
())
{
y
=
perception_obstacle
.
position
().
y
();
}
if
(
perception_obstacle
.
position
().
has_z
())
{
z
=
perception_obstacle
.
position
().
z
();
}
}
feature
->
mutable_position
()
->
set_x
(
x
);
feature
->
mutable_position
()
->
set_y
(
y
);
feature
->
mutable_position
()
->
set_z
(
z
);
AINFO
<<
"Obstacle ["
<<
id_
<<
"] set position ["
<<
std
::
fixed
<<
std
::
setprecision
(
6
)
<<
x
<<
", "
<<
std
::
fixed
<<
std
::
setprecision
(
6
)
<<
y
<<
", "
<<
std
::
fixed
<<
std
::
setprecision
(
6
)
<<
z
<<
"]."
;
}
}
// namespace prediction
}
// namespace prediction
}
// namespace apollo
}
// namespace apollo
modules/prediction/container/obstacles/obstacle.h
浏览文件 @
66157d14
...
@@ -30,6 +30,7 @@
...
@@ -30,6 +30,7 @@
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/prediction/proto/feature.pb.h"
#include "modules/prediction/proto/feature.pb.h"
#include "modules/common/proto/error_code.pb.h"
#include "modules/common/math/kalman_filter.h"
#include "modules/common/math/kalman_filter.h"
...
@@ -65,6 +66,23 @@ class Obstacle {
...
@@ -65,6 +66,23 @@ class Obstacle {
// TODO(author) void SetLaneGraphFeature(ObstacleClusters* p_cluster);
// TODO(author) void SetLaneGraphFeature(ObstacleClusters* p_cluster);
private:
apollo
::
common
::
ErrorCode
SetId
(
const
apollo
::
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
);
apollo
::
common
::
ErrorCode
SetType
(
const
apollo
::
perception
::
PerceptionObstacle
&
perception_obstacle
);
void
SetTimestamp
(
const
apollo
::
perception
::
PerceptionObstacle
&
perception_obstacle
,
const
double
timestamp
,
Feature
*
feature
);
void
SetPosition
(
const
apollo
::
perception
::
PerceptionObstacle
&
perception_obstacle
,
Feature
*
feature
);
private:
private:
int
id_
;
int
id_
;
apollo
::
perception
::
PerceptionObstacle
::
Type
type_
;
apollo
::
perception
::
PerceptionObstacle
::
Type
type_
;
...
@@ -74,7 +92,7 @@ class Obstacle {
...
@@ -74,7 +92,7 @@ class Obstacle {
std
::
unordered_map
<
std
::
string
,
std
::
unordered_map
<
std
::
string
,
apollo
::
common
::
math
::
KalmanFilter
<
double
,
4
,
2
,
0
>>
kf_lane_tracker_map_
;
apollo
::
common
::
math
::
KalmanFilter
<
double
,
4
,
2
,
0
>>
kf_lane_tracker_map_
;
// TODO(author) std::vector<const adu::hdmap::LaneInfo*> _current_lanes;
// TODO(author) std::vector<const adu::hdmap::LaneInfo*> _current_lanes;
static
std
::
mutex
_mutex
;
static
std
::
mutex
mutex_
;
};
};
}
// namespace prediction
}
// namespace prediction
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录