Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
62070328
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,发现更多精彩内容 >>
提交
62070328
编写于
9月 23, 2018
作者:
K
kechxu
提交者:
Liangliang Zhang
9月 23, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Prediction: implement prioritizing obstacles
上级
a7af45b1
变更
13
隐藏空白更改
内联
并排
Showing
13 changed file
with
148 addition
and
39 deletion
+148
-39
modules/prediction/common/BUILD
modules/prediction/common/BUILD
+1
-0
modules/prediction/common/environment_features.cc
modules/prediction/common/environment_features.cc
+14
-4
modules/prediction/common/environment_features.h
modules/prediction/common/environment_features.h
+11
-3
modules/prediction/container/obstacles/obstacles_container.cc
...les/prediction/container/obstacles/obstacles_container.cc
+5
-16
modules/prediction/container/obstacles/obstacles_container.h
modules/prediction/container/obstacles/obstacles_container.h
+6
-8
modules/prediction/scenario/BUILD
modules/prediction/scenario/BUILD
+2
-0
modules/prediction/scenario/analyzer/scenario_analyzer.cc
modules/prediction/scenario/analyzer/scenario_analyzer.cc
+2
-2
modules/prediction/scenario/analyzer/scenario_analyzer.h
modules/prediction/scenario/analyzer/scenario_analyzer.h
+1
-1
modules/prediction/scenario/feature_extractor/feature_extractor.cc
...rediction/scenario/feature_extractor/feature_extractor.cc
+1
-1
modules/prediction/scenario/scenario_features/scenario_features.h
...prediction/scenario/scenario_features/scenario_features.h
+1
-1
modules/prediction/scenario/scenario_manager.cc
modules/prediction/scenario/scenario_manager.cc
+90
-3
modules/prediction/scenario/scenario_manager.h
modules/prediction/scenario/scenario_manager.h
+13
-0
modules/prediction/scenario/scenario_manager_test.cc
modules/prediction/scenario/scenario_manager_test.cc
+1
-0
未找到文件。
modules/prediction/common/BUILD
浏览文件 @
62070328
...
...
@@ -158,6 +158,7 @@ cc_library(
srcs
=
[
"environment_features.cc"
],
hdrs
=
[
"environment_features.h"
],
deps
=
[
"//modules/common/proto:geometry_proto"
,
"//framework:cybertron"
,
],
)
...
...
modules/prediction/common/environment_features.cc
浏览文件 @
62070328
...
...
@@ -20,12 +20,22 @@
namespace
apollo
{
namespace
prediction
{
void
EnvironmentFeatures
::
set_ego_velocity
(
const
double
ego_velocity
)
{
ego_velocity_
=
ego_velocity
;
void
EnvironmentFeatures
::
set_ego_position
(
const
double
x
,
const
double
y
)
{
ego_position_
.
set_x
(
x
);
ego_position_
.
set_y
(
y
);
ego_position_
.
set_z
(
0.0
);
}
double
EnvironmentFeatures
::
get_ego_velocity
()
const
{
return
ego_velocity_
;
const
apollo
::
common
::
Point3D
&
EnvironmentFeatures
::
get_ego_position
()
const
{
return
ego_position_
;
}
void
EnvironmentFeatures
::
set_ego_speed
(
const
double
ego_speed
)
{
ego_speed_
=
ego_speed
;
}
double
EnvironmentFeatures
::
get_ego_speed
()
const
{
return
ego_speed_
;
}
void
EnvironmentFeatures
::
set_ego_heading
(
const
double
ego_heading
)
{
...
...
modules/prediction/common/environment_features.h
浏览文件 @
62070328
...
...
@@ -22,6 +22,8 @@
#include <utility>
#include <vector>
#include "modules/common/proto/geometry.pb.h"
namespace
apollo
{
namespace
prediction
{
...
...
@@ -31,9 +33,13 @@ class EnvironmentFeatures {
virtual
~
EnvironmentFeatures
()
=
default
;
void
set_ego_velocity
(
const
double
ego_velocity
);
void
set_ego_position
(
const
double
x
,
const
double
y
);
const
apollo
::
common
::
Point3D
&
get_ego_position
()
const
;
double
get_ego_velocity
()
const
;
void
set_ego_speed
(
const
double
ego_speed
);
double
get_ego_speed
()
const
;
void
set_ego_acceleration
(
const
double
ego_acceleration
);
...
...
@@ -80,7 +86,9 @@ class EnvironmentFeatures {
const
std
::
vector
<
int
>&
get_obstacle_ids
()
const
;
private:
double
ego_velocity_
=
0.0
;
apollo
::
common
::
Point3D
ego_position_
;
double
ego_speed_
=
0.0
;
double
ego_acceleration_
=
0.0
;
...
...
modules/prediction/container/obstacles/obstacles_container.cc
浏览文件 @
62070328
...
...
@@ -76,6 +76,11 @@ Obstacle* ObstaclesContainer::GetObstacle(const int id) {
return
obstacles_
.
GetSilently
(
id
);
}
const
std
::
vector
<
int
>&
ObstaclesContainer
::
GetCurrentFramePredictableObstacleIds
()
const
{
return
curr_frame_predictable_obstacle_ids_
;
}
void
ObstaclesContainer
::
Clear
()
{
obstacles_
.
Clear
();
timestamp_
=
-
1.0
;
...
...
@@ -118,18 +123,6 @@ void ObstaclesContainer::BuildLaneGraph() {
}
}
void
ObstaclesContainer
::
PrioritizeObstacles
(
const
Scenario
&
scenario
,
const
EnvironmentFeatures
&
environment_features
)
{
// TODO(all) implement
// According to scenario and filtered lanes and junctions, etc
// set priorities for obstacles
if
(
scenario
.
type
()
==
Scenario
::
CRUISE
||
scenario
.
type
()
==
Scenario
::
CRUISE_URBAN
||
scenario
.
type
()
==
Scenario
::
CRUISE_HIGHWAY
)
{
PrioritizeObstaclesInCruise
();
}
}
bool
ObstaclesContainer
::
IsPredictable
(
const
PerceptionObstacle
&
perception_obstacle
)
{
if
(
!
perception_obstacle
.
has_type
()
||
...
...
@@ -139,9 +132,5 @@ bool ObstaclesContainer::IsPredictable(
return
true
;
}
void
ObstaclesContainer
::
PrioritizeObstaclesInCruise
()
{
// TODO(kechxu) implement
}
}
// namespace prediction
}
// namespace apollo
modules/prediction/container/obstacles/obstacles_container.h
浏览文件 @
62070328
...
...
@@ -68,12 +68,6 @@ class ObstaclesContainer : public Container {
*/
void
BuildLaneGraph
();
/**
* @brief Set obstacles' priority to predict
*/
void
PrioritizeObstacles
(
const
Scenario
&
scenario
,
const
EnvironmentFeatures
&
environment_features
);
/**
* @brief Get obstacle pointer
* @param Obstacle ID
...
...
@@ -81,6 +75,12 @@ class ObstaclesContainer : public Container {
*/
Obstacle
*
GetObstacle
(
const
int
id
);
/**
* @brief Get predictable obstacle IDs in the current frame
* @return Predictable obstacle IDs in the current frame
*/
const
std
::
vector
<
int
>&
GetCurrentFramePredictableObstacleIds
()
const
;
/**
* @brief Clear obstacle container
*/
...
...
@@ -94,8 +94,6 @@ class ObstaclesContainer : public Container {
*/
bool
IsPredictable
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
);
void
PrioritizeObstaclesInCruise
();
private:
double
timestamp_
=
-
1.0
;
common
::
util
::
LRUCache
<
int
,
Obstacle
>
obstacles_
;
...
...
modules/prediction/scenario/BUILD
浏览文件 @
62070328
...
...
@@ -9,10 +9,12 @@ cc_library(
deps
=
[
"//framework:cybertron"
,
"//modules/common/adapters:adapter_gflags"
,
"//modules/common/math:geometry"
,
"//modules/prediction/proto:prediction_conf_proto"
,
"//modules/prediction/container:container_manager"
,
"//modules/prediction/scenario/analyzer:scenario_analyzer"
,
"//modules/prediction/scenario/feature_extractor:feature_extractor"
,
"//modules/prediction/scenario/scenario_features:scenario_features"
,
],
)
...
...
modules/prediction/scenario/analyzer/scenario_analyzer.cc
浏览文件 @
62070328
...
...
@@ -35,8 +35,8 @@ const Scenario& ScenarioAnalyzer::scenario() const {
return
scenario_
;
}
std
::
shared_ptr
<
const
ScenarioFeatures
>
ScenarioAnalyzer
::
GetScenarioFeatures
()
const
{
std
::
shared_ptr
<
ScenarioFeatures
>
ScenarioAnalyzer
::
GetScenarioFeatures
()
{
return
nullptr
;
}
...
...
modules/prediction/scenario/analyzer/scenario_analyzer.h
浏览文件 @
62070328
...
...
@@ -36,7 +36,7 @@ class ScenarioAnalyzer {
const
Scenario
&
scenario
()
const
;
std
::
shared_ptr
<
const
ScenarioFeatures
>
GetScenarioFeatures
()
const
;
std
::
shared_ptr
<
ScenarioFeatures
>
GetScenarioFeatures
()
;
private:
Scenario
scenario_
;
};
...
...
modules/prediction/scenario/feature_extractor/feature_extractor.cc
浏览文件 @
62070328
...
...
@@ -77,7 +77,7 @@ const EnvironmentFeatures& FeatureExtractor::GetEnvironmentFeatures() const {
void
FeatureExtractor
::
ExtractEgoVehicleFeatures
()
{
// TODO(all): change this to ego_speed and ego_heading
environment_features_
.
set_ego_
velocity
(
pose_container_
->
GetSpeed
());
environment_features_
.
set_ego_
speed
(
pose_container_
->
GetSpeed
());
environment_features_
.
set_ego_heading
(
pose_container_
->
GetTheta
());
// TODO(all): add acceleration if needed
}
...
...
modules/prediction/scenario/scenario_features/scenario_features.h
浏览文件 @
62070328
...
...
@@ -28,7 +28,7 @@ class ScenarioFeatures {
public:
ScenarioFeatures
()
=
default
;
~
ScenarioFeatures
()
=
default
;
virtual
~
ScenarioFeatures
()
=
default
;
};
}
// namespace prediction
...
...
modules/prediction/scenario/scenario_manager.cc
浏览文件 @
62070328
...
...
@@ -15,7 +15,12 @@
*****************************************************************************/
#include "modules/prediction/scenario/scenario_manager.h"
#include <cmath>
#include <algorithm>
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/math/vec2d.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/obstacles/obstacles_container.h"
...
...
@@ -23,6 +28,7 @@ namespace apollo {
namespace
prediction
{
using
apollo
::
common
::
adapter
::
AdapterConfig
;
using
apollo
::
common
::
math
::
Vec2d
;
ScenarioManager
::
ScenarioManager
()
{}
...
...
@@ -41,9 +47,90 @@ void ScenarioManager::PrioritizeObstacles() {
ObstaclesContainer
*
obstacles_container
=
dynamic_cast
<
ObstaclesContainer
*>
(
ContainerManager
::
Instance
()
->
GetContainer
(
AdapterConfig
::
PERCEPTION_OBSTACLES
));
obstacles_container
->
PrioritizeObstacles
(
scenario_analyzer_
.
scenario
(),
feature_extractor_
.
GetEnvironmentFeatures
());
if
(
obstacles_container
==
nullptr
)
{
AERROR
<<
"Null obstacles container found."
;
return
;
}
const
std
::
vector
<
int
>&
obstacle_ids
=
obstacles_container
->
GetCurrentFramePredictableObstacleIds
();
for
(
const
int
obstacle_id
:
obstacle_ids
)
{
Obstacle
*
obstacle_ptr
=
obstacles_container
->
GetObstacle
(
obstacle_id
);
PrioritizeObstacle
(
feature_extractor_
.
GetEnvironmentFeatures
(),
scenario_analyzer_
.
GetScenarioFeatures
(),
obstacle_ptr
);
}
}
void
ScenarioManager
::
PrioritizeObstacle
(
const
EnvironmentFeatures
&
environment_features
,
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
,
Obstacle
*
obstacle_ptr
)
{
const
auto
&
scenario_type
=
scenario_analyzer_
.
scenario
().
type
();
if
(
scenario_type
==
Scenario
::
CRUISE
||
scenario_type
==
Scenario
::
CRUISE_URBAN
||
scenario_type
==
Scenario
::
CRUISE_HIGHWAY
)
{
PrioritizeObstacleInCruise
(
environment_features
,
std
::
dynamic_pointer_cast
<
CruiseScenarioFeatures
>
(
scenario_features
),
obstacle_ptr
);
}
else
if
(
scenario_type
==
Scenario
::
JUNCTION
||
scenario_type
==
Scenario
::
JUNCTION_TRAFFIC_LIGHT
||
scenario_type
==
Scenario
::
JUNCTION_STOP_SIGN
)
{
// TODO(kechxu) PrioritizeObstaclesInJunction
}
}
void
ScenarioManager
::
PrioritizeObstacleInCruise
(
const
EnvironmentFeatures
&
environment_features
,
std
::
shared_ptr
<
CruiseScenarioFeatures
>
cruise_scenario_features
,
Obstacle
*
obstacle_ptr
)
{
if
(
obstacle_ptr
->
history_size
()
==
0
)
{
AERROR
<<
"Obstacle ["
<<
obstacle_ptr
->
id
()
<<
"] has no feature."
;
return
;
}
Feature
*
latest_feature_ptr
=
obstacle_ptr
->
mutable_latest_feature
();
bool
has_lane_of_interest
=
false
;
if
(
obstacle_ptr
->
IsOnLane
())
{
for
(
const
auto
&
curr_lane
:
latest_feature_ptr
->
lane
().
current_lane_feature
())
{
const
std
::
string
&
curr_lane_id
=
curr_lane
.
lane_id
();
if
(
cruise_scenario_features
->
IsLaneOfInterest
(
curr_lane_id
))
{
has_lane_of_interest
=
true
;
break
;
}
}
if
(
!
has_lane_of_interest
)
{
for
(
const
auto
&
nearby_lane
:
latest_feature_ptr
->
lane
().
nearby_lane_feature
())
{
const
std
::
string
&
nearby_lane_id
=
nearby_lane
.
lane_id
();
if
(
cruise_scenario_features
->
IsLaneOfInterest
(
nearby_lane_id
))
{
has_lane_of_interest
=
true
;
break
;
}
}
}
if
(
!
has_lane_of_interest
)
{
latest_feature_ptr
->
set_priority
(
Feature
::
IGNORE
);
}
}
else
{
// Obstacle has neither lane nor neighbor lanes
// Filter obstacle by its relative position to ego vehicle
double
ego_x
=
environment_features
.
get_ego_position
().
x
();
double
ego_y
=
environment_features
.
get_ego_position
().
y
();
double
ego_heading
=
environment_features
.
get_ego_heading
();
double
ego_speed
=
environment_features
.
get_ego_speed
();
double
obstacle_x
=
latest_feature_ptr
->
position
().
x
();
double
obstacle_y
=
latest_feature_ptr
->
position
().
y
();
Vec2d
ego_to_obstacle_vec
(
obstacle_x
-
ego_x
,
obstacle_y
-
ego_y
);
Vec2d
ego_vec
=
Vec2d
::
CreateUnitVec2d
(
ego_heading
);
double
l
=
ego_vec
.
CrossProd
(
ego_to_obstacle_vec
);
double
s
=
ego_to_obstacle_vec
.
InnerProd
(
ego_vec
);
if
(
std
::
abs
(
l
)
>
10.0
||
s
>
std
::
max
(
20.0
,
ego_speed
*
5.0
)
||
s
<
0.0
)
{
latest_feature_ptr
->
set_priority
(
Feature
::
IGNORE
);
}
}
}
}
// namespace prediction
...
...
modules/prediction/scenario/scenario_manager.h
浏览文件 @
62070328
...
...
@@ -23,12 +23,15 @@
#include <vector>
#include <string>
#include <memory>
#include "cybertron/common/macros.h"
#include "modules/prediction/proto/scenario.pb.h"
#include "modules/prediction/proto/prediction_conf.pb.h"
#include "modules/prediction/scenario/feature_extractor/feature_extractor.h"
#include "modules/prediction/scenario/analyzer/scenario_analyzer.h"
#include "modules/prediction/container/obstacles/obstacle.h"
#include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
namespace
apollo
{
namespace
prediction
{
...
...
@@ -48,6 +51,16 @@ class ScenarioManager {
private:
void
PrioritizeObstacles
();
void
PrioritizeObstacle
(
const
EnvironmentFeatures
&
environment_features
,
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
,
Obstacle
*
obstacle_ptr
);
void
PrioritizeObstacleInCruise
(
const
EnvironmentFeatures
&
environment_features
,
std
::
shared_ptr
<
CruiseScenarioFeatures
>
scenario_features
,
Obstacle
*
obstacle_ptr
);
private:
FeatureExtractor
feature_extractor_
;
ScenarioAnalyzer
scenario_analyzer_
;
...
...
modules/prediction/scenario/scenario_manager_test.cc
浏览文件 @
62070328
...
...
@@ -38,6 +38,7 @@ class ScenarioManagerTest : public ::testing::Test {
};
TEST_F
(
ScenarioManagerTest
,
run
)
{
// TODO(kechxu) add unit tests with concrete contents
ContainerManager
::
Instance
()
->
RegisterContainers
();
std
::
unique_ptr
<
Container
>
adc_traj_container
=
ContainerManager
::
Instance
()
->
CreateContainer
(
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录