Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f37d7a5d
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,发现更多精彩内容 >>
提交
f37d7a5d
编写于
2月 07, 2019
作者:
K
kechxu
提交者:
Kecheng Xu
2月 07, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Prediction: move prioritize obstacles code to the class ObstaclesPrioritizer
上级
40684f13
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
253 addition
and
154 deletion
+253
-154
modules/prediction/scenario/BUILD
modules/prediction/scenario/BUILD
+1
-0
modules/prediction/scenario/prioritization/BUILD
modules/prediction/scenario/prioritization/BUILD
+19
-0
modules/prediction/scenario/prioritization/obstacles_prioritizer.cc
...ediction/scenario/prioritization/obstacles_prioritizer.cc
+184
-0
modules/prediction/scenario/prioritization/obstacles_prioritizer.h
...rediction/scenario/prioritization/obstacles_prioritizer.h
+46
-0
modules/prediction/scenario/scenario_manager.cc
modules/prediction/scenario/scenario_manager.cc
+3
-144
modules/prediction/scenario/scenario_manager.h
modules/prediction/scenario/scenario_manager.h
+0
-10
未找到文件。
modules/prediction/scenario/BUILD
浏览文件 @
f37d7a5d
...
...
@@ -9,6 +9,7 @@ cc_library(
deps
=
[
"//modules/prediction/scenario/analyzer:scenario_analyzer"
,
"//modules/prediction/scenario/feature_extractor"
,
"//modules/prediction/scenario/prioritization:obstacles_prioritizer"
,
],
)
...
...
modules/prediction/scenario/prioritization/BUILD
0 → 100644
浏览文件 @
f37d7a5d
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"obstacles_prioritizer"
,
srcs
=
[
"obstacles_prioritizer.cc"
],
hdrs
=
[
"obstacles_prioritizer.h"
],
deps
=
[
"//modules/prediction/common:environment_features"
,
"//modules/prediction/container:container_manager"
,
"//modules/prediction/container/obstacles:obstacles_container"
,
"//modules/prediction/container/pose:pose_container"
,
"//modules/prediction/scenario/scenario_features:scenario_features"
,
"//modules/prediction/scenario/scenario_features:cruise_scenario_features"
,
],
)
cpplint
()
modules/prediction/scenario/prioritization/obstacles_prioritizer.cc
0 → 100644
浏览文件 @
f37d7a5d
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/prediction/scenario/prioritization/obstacles_prioritizer.h"
#include <algorithm>
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/common/prediction_map.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/pose/pose_container.h"
namespace
apollo
{
namespace
prediction
{
using
common
::
adapter
::
AdapterConfig
;
using
common
::
math
::
Vec2d
;
using
common
::
math
::
Box2d
;
using
apollo
::
perception
::
PerceptionObstacle
;
void
ObstaclesPrioritizer
::
PrioritizeObstacles
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
)
{
AssignIgnoreLevel
(
environment_features
,
scenario_features
);
}
void
ObstaclesPrioritizer
::
AssignIgnoreLevel
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
ptr_scenario_features
)
{
auto
obstacles_container
=
ContainerManager
::
Instance
()
->
GetContainer
<
ObstaclesContainer
>
(
AdapterConfig
::
PERCEPTION_OBSTACLES
);
if
(
obstacles_container
==
nullptr
)
{
AERROR
<<
"Obstacles container pointer is a null pointer."
;
return
;
}
auto
pose_container
=
ContainerManager
::
Instance
()
->
GetContainer
<
PoseContainer
>
(
AdapterConfig
::
LOCALIZATION
);
if
(
pose_container
==
nullptr
)
{
AERROR
<<
"Pose container pointer is a null pointer."
;
return
;
}
const
PerceptionObstacle
*
pose_obstacle_ptr
=
pose_container
->
ToPerceptionObstacle
();
if
(
pose_obstacle_ptr
==
nullptr
)
{
AERROR
<<
"Pose obstacle pointer is a null pointer."
;
return
;
}
double
pose_theta
=
pose_obstacle_ptr
->
theta
();
double
pose_x
=
pose_obstacle_ptr
->
position
().
x
();
double
pose_y
=
pose_obstacle_ptr
->
position
().
y
();
ADEBUG
<<
"Get pose ("
<<
pose_x
<<
", "
<<
pose_y
<<
", "
<<
pose_theta
<<
")"
;
// Build rectangular scan_area
Box2d
scan_box
({
pose_x
+
FLAGS_scan_length
/
2.0
*
std
::
cos
(
pose_theta
),
pose_y
+
FLAGS_scan_length
/
2.0
*
std
::
sin
(
pose_theta
)},
pose_theta
,
FLAGS_scan_length
,
FLAGS_scan_width
);
const
auto
&
obstacle_ids
=
obstacles_container
->
curr_frame_predictable_obstacle_ids
();
for
(
const
int
&
obstacle_id
:
obstacle_ids
)
{
Obstacle
*
obstacle_ptr
=
obstacles_container
->
GetObstacle
(
obstacle_id
);
if
(
obstacle_ptr
->
history_size
()
==
0
)
{
AERROR
<<
"Obstacle ["
<<
obstacle_ptr
->
id
()
<<
"] has no feature."
;
continue
;
}
Feature
*
latest_feature_ptr
=
obstacle_ptr
->
mutable_latest_feature
();
double
obstacle_x
=
latest_feature_ptr
->
position
().
x
();
double
obstacle_y
=
latest_feature_ptr
->
position
().
y
();
Vec2d
ego_to_obstacle_vec
(
obstacle_x
-
pose_x
,
obstacle_y
-
pose_y
);
Vec2d
ego_vec
=
Vec2d
::
CreateUnitVec2d
(
pose_theta
);
double
s
=
ego_to_obstacle_vec
.
InnerProd
(
ego_vec
);
double
pedestrian_like_nearby_lane_radius
=
FLAGS_pedestrian_nearby_lane_search_radius
;
bool
is_near_lane
=
PredictionMap
::
HasNearbyLane
(
obstacle_x
,
obstacle_y
,
pedestrian_like_nearby_lane_radius
);
// Decide if we need consider this obstacle
bool
is_in_scan_area
=
scan_box
.
IsPointIn
({
obstacle_x
,
obstacle_y
});
bool
is_on_lane
=
obstacle_ptr
->
IsOnLane
();
bool
is_pedestrian_like_in_front_near_lanes
=
s
>
FLAGS_back_dist_ignore_ped
&&
(
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
PEDESTRIAN
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
BICYCLE
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
UNKNOWN
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
UNKNOWN_MOVABLE
)
&&
is_near_lane
;
bool
is_near_junction
=
obstacle_ptr
->
IsNearJunction
();
bool
need_consider
=
is_in_scan_area
||
is_on_lane
||
is_near_junction
||
is_pedestrian_like_in_front_near_lanes
;
if
(
!
need_consider
)
{
latest_feature_ptr
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
IGNORE
);
}
else
{
latest_feature_ptr
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
NORMAL
);
}
}
}
void
ObstaclesPrioritizer
::
AssignIgnoreLevelForCruiseScenario
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
CruiseScenarioFeatures
>
cruise_scenario_features
,
ObstaclesContainer
*
ptr_obstacle_contrainer
)
{
const
auto
&
obstacle_ids
=
ptr_obstacle_contrainer
->
curr_frame_predictable_obstacle_ids
();
for
(
const
int
&
obstacle_id
:
obstacle_ids
)
{
Obstacle
*
obstacle_ptr
=
ptr_obstacle_contrainer
->
GetObstacle
(
obstacle_id
);
if
(
obstacle_ptr
->
history_size
()
==
0
)
{
AERROR
<<
"Obstacle ["
<<
obstacle_ptr
->
id
()
<<
"] has no feature."
;
continue
;
}
Feature
*
latest_feature_ptr
=
obstacle_ptr
->
mutable_latest_feature
();
if
(
obstacle_ptr
->
IsOnLane
())
{
bool
has_lane_of_interest
=
false
;
for
(
const
auto
&
curr_lane
:
latest_feature_ptr
->
lane
().
current_lane_feature
())
{
const
auto
&
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
auto
&
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
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
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
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
IGNORE
);
}
}
}
}
}
// namespace prediction
}
// namespace apollo
modules/prediction/scenario/prioritization/obstacles_prioritizer.h
0 → 100644
浏览文件 @
f37d7a5d
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include <memory>
#include "modules/prediction/container/obstacles/obstacles_container.h"
#include "modules/prediction/scenario/scenario_features/scenario_features.h"
#include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
namespace
apollo
{
namespace
prediction
{
class
ObstaclesPrioritizer
{
public:
ObstaclesPrioritizer
()
=
delete
;
static
void
PrioritizeObstacles
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
);
private:
static
void
AssignIgnoreLevel
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
);
static
void
AssignIgnoreLevelForCruiseScenario
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
CruiseScenarioFeatures
>
scenario_features
,
ObstaclesContainer
*
ptr_obstacle_contrainer
);
};
}
// namespace prediction
}
// namespace apollo
modules/prediction/scenario/scenario_manager.cc
浏览文件 @
f37d7a5d
...
...
@@ -19,6 +19,7 @@
#include <algorithm>
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/scenario/prioritization/obstacles_prioritizer.h"
namespace
apollo
{
namespace
prediction
{
...
...
@@ -38,155 +39,13 @@ void ScenarioManager::Run() {
current_scenario_
=
ptr_scenario_features
->
scenario
();
if
(
FLAGS_enable_prioritize_obstacles
)
{
PrioritizeObstacles
(
environment_features
,
ptr_scenario_features
);
ObstaclesPrioritizer
::
PrioritizeObstacles
(
environment_features
,
ptr_scenario_features
);
}
// TODO(all) other functionalities including lane, junction filters
}
const
Scenario
&
ScenarioManager
::
scenario
()
const
{
return
current_scenario_
;
}
void
ScenarioManager
::
PrioritizeObstacles
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
ptr_scenario_features
)
{
auto
obstacles_container
=
ContainerManager
::
Instance
()
->
GetContainer
<
ObstaclesContainer
>
(
AdapterConfig
::
PERCEPTION_OBSTACLES
);
if
(
obstacles_container
==
nullptr
)
{
AERROR
<<
"Obstacles container pointer is a null pointer."
;
return
;
}
auto
pose_container
=
ContainerManager
::
Instance
()
->
GetContainer
<
PoseContainer
>
(
AdapterConfig
::
LOCALIZATION
);
if
(
pose_container
==
nullptr
)
{
AERROR
<<
"Pose container pointer is a null pointer."
;
return
;
}
const
PerceptionObstacle
*
pose_obstacle_ptr
=
pose_container
->
ToPerceptionObstacle
();
if
(
pose_obstacle_ptr
==
nullptr
)
{
AERROR
<<
"Pose obstacle pointer is a null pointer."
;
return
;
}
double
pose_theta
=
pose_obstacle_ptr
->
theta
();
double
pose_x
=
pose_obstacle_ptr
->
position
().
x
();
double
pose_y
=
pose_obstacle_ptr
->
position
().
y
();
ADEBUG
<<
"Get pose ("
<<
pose_x
<<
", "
<<
pose_y
<<
", "
<<
pose_theta
<<
")"
;
// Build rectangular scan_area
Box2d
scan_box
({
pose_x
+
FLAGS_scan_length
/
2.0
*
std
::
cos
(
pose_theta
),
pose_y
+
FLAGS_scan_length
/
2.0
*
std
::
sin
(
pose_theta
)},
pose_theta
,
FLAGS_scan_length
,
FLAGS_scan_width
);
const
auto
&
obstacle_ids
=
obstacles_container
->
curr_frame_predictable_obstacle_ids
();
for
(
const
int
&
obstacle_id
:
obstacle_ids
)
{
Obstacle
*
obstacle_ptr
=
obstacles_container
->
GetObstacle
(
obstacle_id
);
if
(
obstacle_ptr
->
history_size
()
==
0
)
{
AERROR
<<
"Obstacle ["
<<
obstacle_ptr
->
id
()
<<
"] has no feature."
;
continue
;
}
Feature
*
latest_feature_ptr
=
obstacle_ptr
->
mutable_latest_feature
();
double
obstacle_x
=
latest_feature_ptr
->
position
().
x
();
double
obstacle_y
=
latest_feature_ptr
->
position
().
y
();
Vec2d
ego_to_obstacle_vec
(
obstacle_x
-
pose_x
,
obstacle_y
-
pose_y
);
Vec2d
ego_vec
=
Vec2d
::
CreateUnitVec2d
(
pose_theta
);
double
s
=
ego_to_obstacle_vec
.
InnerProd
(
ego_vec
);
double
pedestrian_like_nearby_lane_radius
=
FLAGS_pedestrian_nearby_lane_search_radius
;
bool
is_near_lane
=
PredictionMap
::
HasNearbyLane
(
obstacle_x
,
obstacle_y
,
pedestrian_like_nearby_lane_radius
);
// Decide if we need consider this obstacle
bool
is_in_scan_area
=
scan_box
.
IsPointIn
({
obstacle_x
,
obstacle_y
});
bool
is_on_lane
=
obstacle_ptr
->
IsOnLane
();
bool
is_pedestrian_like_in_front_near_lanes
=
s
>
FLAGS_back_dist_ignore_ped
&&
(
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
PEDESTRIAN
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
BICYCLE
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
UNKNOWN
||
latest_feature_ptr
->
type
()
==
PerceptionObstacle
::
UNKNOWN_MOVABLE
)
&&
is_near_lane
;
bool
is_near_junction
=
obstacle_ptr
->
IsNearJunction
();
bool
need_consider
=
is_in_scan_area
||
is_on_lane
||
is_near_junction
||
is_pedestrian_like_in_front_near_lanes
;
if
(
!
need_consider
)
{
latest_feature_ptr
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
IGNORE
);
}
else
{
latest_feature_ptr
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
NORMAL
);
}
}
}
void
ScenarioManager
::
PrioritizeObstaclesForCruiseScenario
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
CruiseScenarioFeatures
>
cruise_scenario_features
,
ObstaclesContainer
*
ptr_obstacle_contrainer
)
{
const
auto
&
obstacle_ids
=
ptr_obstacle_contrainer
->
curr_frame_predictable_obstacle_ids
();
for
(
const
int
&
obstacle_id
:
obstacle_ids
)
{
Obstacle
*
obstacle_ptr
=
ptr_obstacle_contrainer
->
GetObstacle
(
obstacle_id
);
if
(
obstacle_ptr
->
history_size
()
==
0
)
{
AERROR
<<
"Obstacle ["
<<
obstacle_ptr
->
id
()
<<
"] has no feature."
;
continue
;
}
Feature
*
latest_feature_ptr
=
obstacle_ptr
->
mutable_latest_feature
();
if
(
obstacle_ptr
->
IsOnLane
())
{
bool
has_lane_of_interest
=
false
;
for
(
const
auto
&
curr_lane
:
latest_feature_ptr
->
lane
().
current_lane_feature
())
{
const
auto
&
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
auto
&
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
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
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
->
mutable_priority
()
->
set_priority
(
ObstaclePriority
::
IGNORE
);
}
}
}
}
}
// namespace prediction
}
// namespace apollo
modules/prediction/scenario/scenario_manager.h
浏览文件 @
f37d7a5d
...
...
@@ -42,16 +42,6 @@ class ScenarioManager {
*/
const
Scenario
&
scenario
()
const
;
private:
void
PrioritizeObstacles
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
ScenarioFeatures
>
scenario_features
);
void
PrioritizeObstaclesForCruiseScenario
(
const
EnvironmentFeatures
&
environment_features
,
const
std
::
shared_ptr
<
CruiseScenarioFeatures
>
scenario_features
,
ObstaclesContainer
*
ptr_obstacle_contrainer
);
private:
Scenario
current_scenario_
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录