Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
d537d178
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,发现更多精彩内容 >>
提交
d537d178
编写于
9月 19, 2018
作者:
J
Jiangtao Hu
提交者:
Kecheng Xu
9月 19, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
perception: fix build breakage for new perception pb change.
上级
718ba03e
变更
38
隐藏空白更改
内联
并排
Showing
38 changed file
with
166 addition
and
164 deletion
+166
-164
modules/common/util/BUILD
modules/common/util/BUILD
+0
-1
modules/common/util/util.cc
modules/common/util/util.cc
+3
-3
modules/common/util/util.h
modules/common/util/util.h
+2
-3
modules/dreamview/backend/simulation_world/simulation_world_service.cc
...view/backend/simulation_world/simulation_world_service.cc
+8
-7
modules/dreamview/backend/simulation_world/simulation_world_service_test.cc
...backend/simulation_world/simulation_world_service_test.cc
+7
-7
modules/perception/obstacle/base/object.cc
modules/perception/obstacle/base/object.cc
+1
-1
modules/perception/obstacle/base/object_test.cc
modules/perception/obstacle/base/object_test.cc
+1
-1
modules/perception/obstacle/onboard/lidar_process_test.cc
modules/perception/obstacle/onboard/lidar_process_test.cc
+2
-2
modules/planning/common/lag_prediction.cc
modules/planning/common/lag_prediction.cc
+2
-2
modules/planning/common/obstacle.cc
modules/planning/common/obstacle.cc
+2
-2
modules/planning/common/obstacle_test.cc
modules/planning/common/obstacle_test.cc
+6
-6
modules/planning/toolkits/deciders/crosswalk.cc
modules/planning/toolkits/deciders/crosswalk.cc
+6
-6
modules/planning/toolkits/deciders/front_vehicle.cc
modules/planning/toolkits/deciders/front_vehicle.cc
+7
-7
modules/planning/toolkits/deciders/pull_over.cc
modules/planning/toolkits/deciders/pull_over.cc
+2
-2
modules/planning/toolkits/deciders/stop_sign.cc
modules/planning/toolkits/deciders/stop_sign.cc
+12
-12
modules/planning/toolkits/optimizers/path_decider/path_decider.cc
...planning/toolkits/optimizers/path_decider/path_decider.cc
+2
-2
modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc
...lanning/toolkits/optimizers/road_graph/trajectory_cost.cc
+2
-2
modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc
...anning/toolkits/optimizers/speed_decider/speed_decider.cc
+10
-10
modules/prediction/container/obstacles/obstacle.cc
modules/prediction/container/obstacles/obstacle.cc
+9
-9
modules/prediction/container/obstacles/obstacle.h
modules/prediction/container/obstacles/obstacle.h
+3
-3
modules/prediction/container/obstacles/obstacle_test.cc
modules/prediction/container/obstacles/obstacle_test.cc
+2
-2
modules/prediction/container/obstacles/obstacles_container.cc
...les/prediction/container/obstacles/obstacles_container.cc
+1
-1
modules/prediction/container/obstacles/obstacles_container_test.cc
...rediction/container/obstacles/obstacles_container_test.cc
+6
-6
modules/prediction/container/pose/pose_container.cc
modules/prediction/container/pose/pose_container.cc
+4
-4
modules/prediction/container/pose/pose_container.h
modules/prediction/container/pose/pose_container.h
+3
-3
modules/prediction/evaluator/evaluator_manager.cc
modules/prediction/evaluator/evaluator_manager.cc
+7
-7
modules/prediction/predictor/free_move/free_move_predictor.cc
...les/prediction/predictor/free_move/free_move_predictor.cc
+1
-1
modules/prediction/predictor/free_move/free_move_predictor_test.cc
...rediction/predictor/free_move/free_move_predictor_test.cc
+1
-1
modules/prediction/predictor/predictor_manager.cc
modules/prediction/predictor/predictor_manager.cc
+8
-8
modules/prediction/proto/feature.proto
modules/prediction/proto/feature.proto
+1
-1
modules/prediction/proto/prediction_conf.proto
modules/prediction/proto/prediction_conf.proto
+1
-1
modules/third_party_perception/common/third_party_perception_util.cc
...rd_party_perception/common/third_party_perception_util.cc
+8
-8
modules/third_party_perception/common/third_party_perception_util.h
...ird_party_perception/common/third_party_perception_util.h
+8
-8
modules/third_party_perception/conversion.cc
modules/third_party_perception/conversion.cc
+19
-17
modules/third_party_perception/fusion.cc
modules/third_party_perception/fusion.cc
+2
-1
modules/third_party_perception/proto/BUILD
modules/third_party_perception/proto/BUILD
+0
-1
modules/third_party_perception/proto/radar_obstacle.proto
modules/third_party_perception/proto/radar_obstacle.proto
+6
-5
modules/third_party_perception/third_party_perception.cc
modules/third_party_perception/third_party_perception.cc
+1
-1
未找到文件。
modules/common/util/BUILD
浏览文件 @
d537d178
...
...
@@ -26,7 +26,6 @@ cc_library(
"//modules/common/math"
,
"//modules/common/proto:pnc_point_proto"
,
"//modules/common/time"
,
"//modules/perception/proto:perception_proto"
,
],
)
...
...
modules/common/util/util.cc
浏览文件 @
d537d178
...
...
@@ -54,9 +54,9 @@ PointENU MakePointENU(const math::Vec2d& xy) {
return
point_enu
;
}
apollo
::
perception
::
Point
MakePerceptionPoint
(
const
double
x
,
const
double
y
,
const
double
z
)
{
perception
::
Point
point3d
;
apollo
::
common
::
Point3D
MakePoint3D
(
const
double
x
,
const
double
y
,
const
double
z
)
{
common
::
Point3D
point3d
;
point3d
.
set_x
(
x
);
point3d
.
set_y
(
y
);
point3d
.
set_z
(
z
);
...
...
modules/common/util/util.h
浏览文件 @
d537d178
...
...
@@ -34,7 +34,6 @@
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/common/math/vec2d.h"
...
...
@@ -127,8 +126,8 @@ PointENU operator+(const PointENU enu, const math::Vec2d& xy);
PointENU
MakePointENU
(
const
math
::
Vec2d
&
xy
);
apollo
::
perception
::
Point
MakePerceptionPoint
(
const
double
x
,
const
double
y
,
const
double
z
);
apollo
::
common
::
Point3D
MakePoint3D
(
const
double
x
,
const
double
y
,
const
double
z
);
SpeedPoint
MakeSpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
const
double
da
);
...
...
modules/dreamview/backend/simulation_world/simulation_world_service.cc
浏览文件 @
d537d178
...
...
@@ -125,22 +125,22 @@ void SetObstacleType(const PerceptionObstacle &obstacle, Object *world_object) {
}
switch
(
obstacle
.
type
())
{
case
PerceptionObstacle
::
UNKNOWN
:
case
perception
::
UNKNOWN
:
world_object
->
set_type
(
Object_Type_UNKNOWN
);
break
;
case
PerceptionObstacle
::
UNKNOWN_MOVABLE
:
case
perception
::
UNKNOWN_MOVABLE
:
world_object
->
set_type
(
Object_Type_UNKNOWN_MOVABLE
);
break
;
case
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
:
case
perception
::
UNKNOWN_UNMOVABLE
:
world_object
->
set_type
(
Object_Type_UNKNOWN_UNMOVABLE
);
break
;
case
PerceptionObstacle
::
PEDESTRIAN
:
case
perception
::
PEDESTRIAN
:
world_object
->
set_type
(
Object_Type_PEDESTRIAN
);
break
;
case
PerceptionObstacle
::
BICYCLE
:
case
perception
::
BICYCLE
:
world_object
->
set_type
(
Object_Type_BICYCLE
);
break
;
case
PerceptionObstacle
::
VEHICLE
:
case
perception
::
VEHICLE
:
world_object
->
set_type
(
Object_Type_VEHICLE
);
break
;
default:
...
...
@@ -505,7 +505,8 @@ void SimulationWorldService::SetObstacleInfo(const PerceptionObstacle &obstacle,
world_object
->
set_speed_heading
(
std
::
atan2
(
obstacle
.
velocity
().
y
(),
obstacle
.
velocity
().
x
()));
world_object
->
set_timestamp_sec
(
obstacle
.
timestamp
());
world_object
->
set_confidence
(
obstacle
.
confidence
());
// FIXME(all): adjust to new perception pb
// world_object->set_confidence(obstacle.confidence());
}
void
SimulationWorldService
::
SetObstaclePolygon
(
...
...
modules/dreamview/backend/simulation_world/simulation_world_service_test.cc
浏览文件 @
d537d178
...
...
@@ -208,23 +208,23 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
PerceptionObstacles
obstacles
;
PerceptionObstacle
*
obstacle1
=
obstacles
.
add_perception_obstacle
();
obstacle1
->
set_id
(
1
);
apollo
::
perception
::
Point
*
point1
=
obstacle1
->
add_polygon_point
();
apollo
::
common
::
Point3D
*
point1
=
obstacle1
->
add_polygon_point
();
point1
->
set_x
(
0.0
);
point1
->
set_y
(
0.0
);
apollo
::
perception
::
Point
*
point2
=
obstacle1
->
add_polygon_point
();
apollo
::
common
::
Point3D
*
point2
=
obstacle1
->
add_polygon_point
();
point2
->
set_x
(
0.0
);
point2
->
set_y
(
1.0
);
apollo
::
perception
::
Point
*
point3
=
obstacle1
->
add_polygon_point
();
apollo
::
common
::
Point3D
*
point3
=
obstacle1
->
add_polygon_point
();
point3
->
set_x
(
-
1.0
);
point3
->
set_y
(
0.0
);
apollo
::
perception
::
Point
*
point4
=
obstacle1
->
add_polygon_point
();
apollo
::
common
::
Point3D
*
point4
=
obstacle1
->
add_polygon_point
();
point4
->
set_x
(
-
1.0
);
point4
->
set_y
(
0.0
);
obstacle1
->
set_timestamp
(
1489794020.123
);
obstacle1
->
set_type
(
apollo
::
perception
::
PerceptionObstacle_Type_
UNKNOWN
);
obstacle1
->
set_type
(
apollo
::
perception
::
UNKNOWN
);
PerceptionObstacle
*
obstacle2
=
obstacles
.
add_perception_obstacle
();
obstacle2
->
set_id
(
2
);
apollo
::
perception
::
Point
*
point
=
obstacle2
->
mutable_position
();
apollo
::
common
::
Point3D
*
point
=
obstacle2
->
mutable_position
();
point
->
set_x
(
1.0
);
point
->
set_y
(
2.0
);
obstacle2
->
set_theta
(
3.0
);
...
...
@@ -233,7 +233,7 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
obstacle2
->
set_height
(
6.0
);
obstacle2
->
mutable_velocity
()
->
set_x
(
3.0
);
obstacle2
->
mutable_velocity
()
->
set_y
(
4.0
);
obstacle2
->
set_type
(
apollo
::
perception
::
PerceptionObstacle_Type_
VEHICLE
);
obstacle2
->
set_type
(
apollo
::
perception
::
VEHICLE
);
sim_world_service_
->
UpdateSimulationWorld
(
obstacles
);
sim_world_service_
->
world_
.
clear_object
();
...
...
modules/perception/obstacle/base/object.cc
浏览文件 @
d537d178
...
...
@@ -146,7 +146,7 @@ void Object::Serialize(PerceptionObstacle* pb_obj) const {
pb_obj
->
set_confidence
(
score
);
pb_obj
->
set_confidence_type
(
score_type
);
pb_obj
->
set_tracking_time
(
tracking_time
);
pb_obj
->
set_type
(
static_cast
<
PerceptionObstacle
::
Type
>
(
type
));
pb_obj
->
set_type
(
static_cast
<
perception
::
Type
>
(
type
));
pb_obj
->
set_timestamp
(
latest_tracked_time
);
// in seconds.
}
...
...
modules/perception/obstacle/base/object_test.cc
浏览文件 @
d537d178
...
...
@@ -160,7 +160,7 @@ TEST(ObjectTest, test_Deserialize) {
pb_obj
.
set_width
(
1.0
);
pb_obj
.
set_height
(
1.5
);
pb_obj
.
set_tracking_time
(
20.1
);
pb_obj
.
set_type
(
PerceptionObstacle
::
BICYCLE
);
pb_obj
.
set_type
(
perception
::
BICYCLE
);
pb_obj
.
set_timestamp
(
1147012345.678
);
PolygonDType
polygon
;
...
...
modules/perception/obstacle/onboard/lidar_process_test.cc
浏览文件 @
d537d178
...
...
@@ -114,9 +114,9 @@ TEST_F(LidarProcessTest, test_GeneratePbMsg) {
lidar_process_
.
GeneratePbMsg
(
&
obstacles
);
EXPECT_EQ
(
obstacles
.
perception_obstacle_size
(),
2
);
EXPECT_EQ
(
obstacles
.
perception_obstacle
(
0
).
type
(),
PerceptionObstacle
::
VEHICLE
);
perception
::
VEHICLE
);
EXPECT_EQ
(
obstacles
.
perception_obstacle
(
1
).
type
(),
PerceptionObstacle
::
PEDESTRIAN
);
perception
::
PEDESTRIAN
);
}
}
// namespace perception
...
...
modules/planning/common/lag_prediction.cc
浏览文件 @
d537d178
...
...
@@ -65,7 +65,7 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
for
(
const
auto
&
obstacle
:
latest_prediction
->
prediction_obstacle
())
{
const
auto
&
perception
=
obstacle
.
perception_obstacle
();
if
(
perception
.
confidence
()
<
FLAGS_perception_confidence_threshold
&&
perception
.
type
()
!=
PerceptionObstacle
::
VEHICLE
)
{
perception
.
type
()
!=
perception
::
VEHICLE
)
{
continue
;
}
double
distance
=
...
...
@@ -84,7 +84,7 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
const
auto
&
perception
=
obstacle
.
perception_obstacle
();
auto
id
=
perception
.
id
();
if
(
perception
.
confidence
()
<
FLAGS_perception_confidence_threshold
&&
perception
.
type
()
!=
PerceptionObstacle
::
VEHICLE
)
{
perception
.
type
()
!=
perception
::
VEHICLE
)
{
continue
;
}
if
(
protected_obstacles
.
count
(
id
)
>
0
)
{
...
...
modules/planning/common/obstacle.cc
浏览文件 @
d537d178
...
...
@@ -110,7 +110,7 @@ common::TrajectoryPoint* Obstacle::AddTrajectoryPoint() {
}
bool
Obstacle
::
IsStaticObstacle
(
const
PerceptionObstacle
&
perception_obstacle
)
{
if
(
perception_obstacle
.
type
()
==
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
)
{
if
(
perception_obstacle
.
type
()
==
perception
::
UNKNOWN_UNMOVABLE
)
{
return
true
;
}
auto
moving_speed
=
std
::
hypot
(
perception_obstacle
.
velocity
().
x
(),
...
...
@@ -249,7 +249,7 @@ std::unique_ptr<Obstacle> Obstacle::CreateStaticVirtualObstacles(
perception_obstacle
.
set_width
(
obstacle_box
.
width
());
perception_obstacle
.
set_height
(
FLAGS_virtual_stop_wall_height
);
perception_obstacle
.
set_type
(
perception
::
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
);
perception
::
UNKNOWN_UNMOVABLE
);
perception_obstacle
.
set_tracking_time
(
1.0
);
std
::
vector
<
common
::
math
::
Vec2d
>
corner_points
;
...
...
modules/planning/common/obstacle_test.cc
浏览文件 @
d537d178
...
...
@@ -45,22 +45,22 @@ TEST(Obstacle, IsStaticObstacle) {
perception_obstacle
.
mutable_velocity
()
->
set_y
(
0.5
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
UNKNOWN
);
perception_obstacle
.
set_type
(
perception
::
UNKNOWN
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
);
perception_obstacle
.
set_type
(
perception
::
UNKNOWN_UNMOVABLE
);
EXPECT_TRUE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
UNKNOWN_MOVABLE
);
perception_obstacle
.
set_type
(
perception
::
UNKNOWN_MOVABLE
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
PEDESTRIAN
);
perception_obstacle
.
set_type
(
perception
::
PEDESTRIAN
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
BICYCLE
);
perception_obstacle
.
set_type
(
perception
::
BICYCLE
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
set_type
(
PerceptionObstacle
::
VEHICLE
);
perception_obstacle
.
set_type
(
perception
::
VEHICLE
);
EXPECT_FALSE
(
Obstacle
::
IsStaticObstacle
(
perception_obstacle
));
perception_obstacle
.
mutable_velocity
()
->
set_x
(
0.5
);
...
...
modules/planning/toolkits/deciders/crosswalk.cc
浏览文件 @
d537d178
...
...
@@ -125,15 +125,15 @@ void Crosswalk::MakeDecisions(Frame* const frame,
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
->
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
path_obstacle
->
Id
();
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type_Name
(
obstacle_type
);
// check type
if
(
obstacle_type
!=
PerceptionObstacle
::
PEDESTRIAN
&&
obstacle_type
!=
PerceptionObstacle
::
BICYCLE
&&
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN
)
{
if
(
obstacle_type
!=
perception
::
PEDESTRIAN
&&
obstacle_type
!=
perception
::
BICYCLE
&&
obstacle_type
!=
perception
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
perception
::
UNKNOWN
)
{
ADEBUG
<<
"obstacle_id["
<<
obstacle_id
<<
"] type["
<<
obstacle_type_name
<<
"]. skip"
;
continue
;
...
...
modules/planning/toolkits/deciders/front_vehicle.cc
浏览文件 @
d537d178
...
...
@@ -235,9 +235,9 @@ std::string FrontVehicle::FindPassableObstacle(
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
->
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
std
::
to_string
(
perception_obstacle
.
id
());
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type_Name
(
obstacle_type
);
if
(
path_obstacle
->
obstacle
()
->
IsVirtual
()
||
!
path_obstacle
->
obstacle
()
->
IsStatic
())
{
...
...
@@ -313,9 +313,9 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) {
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
->
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
std
::
to_string
(
perception_obstacle
.
id
());
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type_Name
(
obstacle_type
);
if
(
path_obstacle
->
obstacle
()
->
IsVirtual
()
||
!
path_obstacle
->
obstacle
()
->
IsStatic
())
{
...
...
@@ -388,9 +388,9 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) {
ObjectDecisionType
stop
;
auto
stop_decision
=
stop
.
mutable_stop
();
if
(
obstacle_type
==
PerceptionObstacle
::
UNKNOWN_MOVABLE
||
obstacle_type
==
PerceptionObstacle
::
BICYCLE
||
obstacle_type
==
PerceptionObstacle
::
VEHICLE
)
{
if
(
obstacle_type
==
perception
::
UNKNOWN_MOVABLE
||
obstacle_type
==
perception
::
BICYCLE
||
obstacle_type
==
perception
::
VEHICLE
)
{
stop_decision
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_HEAD_VEHICLE
);
}
else
{
...
...
modules/planning/toolkits/deciders/pull_over.cc
浏览文件 @
d537d178
...
...
@@ -148,9 +148,9 @@ PullOver::ValidateStopPointCode PullOver::IsValidStop(
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
->
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
std
::
to_string
(
perception_obstacle
.
id
());
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type_Name
(
obstacle_type
);
if
(
path_obstacle
->
obstacle
()
->
IsVirtual
()
||
!
path_obstacle
->
obstacle
()
->
IsStatic
())
{
...
...
modules/planning/toolkits/deciders/stop_sign.cc
浏览文件 @
d537d178
...
...
@@ -474,14 +474,14 @@ int StopSign::AddWatchVehicle(const PathObstacle& path_obstacle,
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
.
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
std
::
to_string
(
perception_obstacle
.
id
());
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
perception
::
Type_Name
(
obstacle_type
);
// check type
if
(
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN
&&
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
PerceptionObstacle
::
BICYCLE
&&
obstacle_type
!=
PerceptionObstacle
::
VEHICLE
)
{
if
(
obstacle_type
!=
perception
::
UNKNOWN
&&
obstacle_type
!=
perception
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
perception
::
BICYCLE
&&
obstacle_type
!=
perception
::
VEHICLE
)
{
ADEBUG
<<
"obstacle_id["
<<
obstacle_id
<<
"] type["
<<
obstacle_type_name
<<
"]. skip"
;
return
0
;
...
...
@@ -572,8 +572,8 @@ int StopSign::RemoveWatchVehicle(
const
PerceptionObstacle
&
perception_obstacle
=
path_obstacle
.
obstacle
()
->
Perception
();
const
std
::
string
&
obstacle_id
=
std
::
to_string
(
perception_obstacle
.
id
());
PerceptionObstacle
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
PerceptionObstacle_
Type_Name
(
obstacle_type
);
perception
::
Type
obstacle_type
=
perception_obstacle
.
type
();
std
::
string
obstacle_type_name
=
perception
::
Type_Name
(
obstacle_type
);
// check if being watched
if
(
std
::
find
(
watch_vehicle_ids
.
begin
(),
watch_vehicle_ids
.
end
(),
...
...
@@ -584,10 +584,10 @@ int StopSign::RemoveWatchVehicle(
}
// check type
if
(
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN
&&
obstacle_type
!=
PerceptionObstacle
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
PerceptionObstacle
::
BICYCLE
&&
obstacle_type
!=
PerceptionObstacle
::
VEHICLE
)
{
if
(
obstacle_type
!=
perception
::
UNKNOWN
&&
obstacle_type
!=
perception
::
UNKNOWN_MOVABLE
&&
obstacle_type
!=
perception
::
BICYCLE
&&
obstacle_type
!=
perception
::
VEHICLE
)
{
ADEBUG
<<
"obstacle_id["
<<
obstacle_id
<<
"] type["
<<
obstacle_type_name
<<
"]. skip"
;
return
0
;
...
...
modules/planning/toolkits/optimizers/path_decider/path_decider.cc
浏览文件 @
d537d178
...
...
@@ -90,9 +90,9 @@ bool PathDecider::MakeStaticObstacleDecision(
const
auto
&
obstacle
=
*
path_obstacle
->
obstacle
();
bool
is_bycycle_or_pedestrain
=
(
obstacle
.
Perception
().
type
()
==
perception
::
PerceptionObstacle
::
BICYCLE
||
perception
::
BICYCLE
||
obstacle
.
Perception
().
type
()
==
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
perception
::
PEDESTRIAN
);
if
(
!
is_bycycle_or_pedestrain
&&
!
obstacle
.
IsStatic
())
{
continue
;
...
...
modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc
浏览文件 @
d537d178
...
...
@@ -79,9 +79,9 @@ TrajectoryCost::TrajectoryCost(
const
auto
*
ptr_obstacle
=
ptr_path_obstacle
->
obstacle
();
bool
is_bycycle_or_pedestrian
=
(
ptr_obstacle
->
Perception
().
type
()
==
perception
::
PerceptionObstacle
::
BICYCLE
||
perception
::
BICYCLE
||
ptr_obstacle
->
Perception
().
type
()
==
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
perception
::
PEDESTRIAN
);
if
(
Obstacle
::
IsVirtualObstacle
(
ptr_obstacle
->
Perception
()))
{
// Virtual obstacle
...
...
modules/planning/toolkits/optimizers/speed_decider/speed_decider.cc
浏览文件 @
d537d178
...
...
@@ -290,10 +290,10 @@ bool SpeedDecider::CreateStopDecision(const PathObstacle& path_obstacle,
stop
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_CLEAR_ZONE
);
}
PerceptionObstacle
::
Type
obstacle_type
=
perception
::
Type
obstacle_type
=
path_obstacle
.
obstacle
()
->
Perception
().
type
();
ADEBUG
<<
"STOP: obstacle_id["
<<
path_obstacle
.
obstacle
()
->
Id
()
<<
"] obstacle_type["
<<
PerceptionObstacle_
Type_Name
(
obstacle_type
)
<<
"] obstacle_type["
<<
perception
::
Type_Name
(
obstacle_type
)
<<
"]"
;
return
true
;
...
...
@@ -329,10 +329,10 @@ bool SpeedDecider::CreateFollowDecision(
fence_point
->
set_z
(
0.0
);
follow
->
set_fence_heading
(
ref_point
.
heading
());
PerceptionObstacle
::
Type
obstacle_type
=
perception
::
Type
obstacle_type
=
path_obstacle
.
obstacle
()
->
Perception
().
type
();
ADEBUG
<<
"FOLLOW: obstacle_id["
<<
path_obstacle
.
obstacle
()
->
Id
()
<<
"] obstacle_type["
<<
PerceptionObstacle_
Type_Name
(
obstacle_type
)
<<
"] obstacle_type["
<<
perception
::
Type_Name
(
obstacle_type
)
<<
"]"
;
return
true
;
...
...
@@ -343,12 +343,12 @@ bool SpeedDecider::CreateYieldDecision(
ObjectDecisionType
*
const
yield_decision
)
const
{
DCHECK_NOTNULL
(
yield_decision
);
PerceptionObstacle
::
Type
obstacle_type
=
perception
::
Type
obstacle_type
=
path_obstacle
.
obstacle
()
->
Perception
().
type
();
double
yield_distance
=
FLAGS_yield_distance
;
switch
(
obstacle_type
)
{
case
PerceptionObstacle
::
PEDESTRIAN
:
case
PerceptionObstacle
::
BICYCLE
:
case
perception
::
PEDESTRIAN
:
case
perception
::
BICYCLE
:
yield_distance
=
FLAGS_yield_distance_pedestrian_bycicle
;
break
;
default:
...
...
@@ -380,7 +380,7 @@ bool SpeedDecider::CreateYieldDecision(
yield
->
set_fence_heading
(
ref_point
.
heading
());
ADEBUG
<<
"YIELD: obstacle_id["
<<
path_obstacle
.
obstacle
()
->
Id
()
<<
"] obstacle_type["
<<
PerceptionObstacle_
Type_Name
(
obstacle_type
)
<<
"] obstacle_type["
<<
perception
::
Type_Name
(
obstacle_type
)
<<
"]"
;
return
true
;
...
...
@@ -423,10 +423,10 @@ bool SpeedDecider::CreateOvertakeDecision(
overtake
->
mutable_fence_point
()
->
set_z
(
0.0
);
overtake
->
set_fence_heading
(
ref_point
.
heading
());
PerceptionObstacle
::
Type
obstacle_type
=
perception
::
Type
obstacle_type
=
path_obstacle
.
obstacle
()
->
Perception
().
type
();
ADEBUG
<<
"OVERTAKE: obstacle_id["
<<
path_obstacle
.
obstacle
()
->
Id
()
<<
"] obstacle_type["
<<
PerceptionObstacle_
Type_Name
(
obstacle_type
)
<<
"] obstacle_type["
<<
perception
::
Type_Name
(
obstacle_type
)
<<
"]"
;
return
true
;
...
...
modules/prediction/container/obstacles/obstacle.cc
浏览文件 @
d537d178
...
...
@@ -61,7 +61,7 @@ Obstacle::Obstacle() {
{
heading_filter_param
}};
}
PerceptionObstacle
::
Type
Obstacle
::
type
()
const
{
return
type_
;
}
perception
::
Type
Obstacle
::
type
()
const
{
return
type_
;
}
int
Obstacle
::
id
()
const
{
return
id_
;
}
...
...
@@ -161,7 +161,7 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
InitKFMotionTracker
(
feature
);
}
UpdateKFMotionTracker
(
feature
);
if
(
type_
==
PerceptionObstacle
::
PEDESTRIAN
)
{
if
(
type_
==
perception
::
PEDESTRIAN
)
{
if
(
!
kf_pedestrian_tracker_
.
IsInitialized
())
{
InitKFPedestrianTracker
(
feature
);
}
...
...
@@ -179,7 +179,7 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
SetNearbyLanes
(
&
feature
);
if
(
FLAGS_adjust_vehicle_heading_by_lane
&&
type_
==
PerceptionObstacle
::
VEHICLE
)
{
type_
==
perception
::
VEHICLE
)
{
AdjustHeadingByLane
(
&
feature
);
}
...
...
@@ -262,7 +262,7 @@ void Obstacle::UpdateStatus(Feature* feature) {
<<
"]."
;
// Update pedestrian motion belief
if
(
type_
==
PerceptionObstacle
::
PEDESTRIAN
)
{
if
(
type_
==
perception
::
PEDESTRIAN
)
{
if
(
!
kf_pedestrian_tracker_
.
IsInitialized
())
{
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has not initialized pedestrian tracker."
;
...
...
@@ -408,8 +408,8 @@ void Obstacle::SetVelocity(const PerceptionObstacle& perception_obstacle,
}
}
double
filtered_heading
=
heading_filter_
.
Filter
(
velocity_heading
);
if
(
type_
==
PerceptionObstacle
::
BICYCLE
||
type_
==
PerceptionObstacle
::
PEDESTRIAN
)
{
if
(
type_
==
perception
::
BICYCLE
||
type_
==
perception
::
PEDESTRIAN
)
{
velocity_heading
=
filtered_heading
;
}
velocity_x
=
speed
*
std
::
cos
(
velocity_heading
);
...
...
@@ -795,7 +795,7 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
}
// Ignore bike and sidewalk lanes for vehicles
if
(
type_
==
PerceptionObstacle
::
VEHICLE
&&
if
(
type_
==
perception
::
VEHICLE
&&
nearby_lane
->
lane
().
has_type
()
&&
(
nearby_lane
->
lane
().
type
()
==
::
apollo
::
hdmap
::
Lane
::
BIKING
||
nearby_lane
->
lane
().
type
()
==
::
apollo
::
hdmap
::
Lane
::
SIDEWALK
))
{
...
...
@@ -1038,8 +1038,8 @@ void Obstacle::SetMotionStatus() {
double
std
=
FLAGS_still_obstacle_position_std
;
double
speed_threshold
=
FLAGS_still_obstacle_speed_threshold
;
if
(
type_
==
PerceptionObstacle
::
PEDESTRIAN
||
type_
==
PerceptionObstacle
::
BICYCLE
)
{
if
(
type_
==
perception
::
PEDESTRIAN
||
type_
==
perception
::
BICYCLE
)
{
speed_threshold
=
FLAGS_still_pedestrian_speed_threshold
;
std
=
FLAGS_still_pedestrian_position_std
;
}
...
...
modules/prediction/container/obstacles/obstacle.h
浏览文件 @
d537d178
...
...
@@ -73,7 +73,7 @@ class Obstacle {
* @brief Get the type of perception obstacle's type.
* @return The type pf perception obstacle.
*/
perception
::
PerceptionObstacle
::
Type
type
()
const
;
perception
::
Type
type
()
const
;
/**
* @brief Get the obstacle's ID.
...
...
@@ -244,8 +244,8 @@ class Obstacle {
private:
int
id_
=
-
1
;
perception
::
PerceptionObstacle
::
Type
type_
=
perception
::
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
;
perception
::
Type
type_
=
perception
::
UNKNOWN_UNMOVABLE
;
std
::
deque
<
Feature
>
feature_history_
;
common
::
math
::
KalmanFilter
<
double
,
6
,
2
,
0
>
kf_motion_tracker_
;
common
::
math
::
KalmanFilter
<
double
,
2
,
2
,
4
>
kf_pedestrian_tracker_
;
...
...
modules/prediction/container/obstacles/obstacle_test.cc
浏览文件 @
d537d178
...
...
@@ -64,7 +64,7 @@ TEST_F(ObstacleTest, VehicleBasic) {
Obstacle
*
obstacle_ptr
=
container_
.
GetObstacle
(
1
);
EXPECT_TRUE
(
obstacle_ptr
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr
->
id
(),
1
);
EXPECT_EQ
(
obstacle_ptr
->
type
(),
perception
::
PerceptionObstacle
::
VEHICLE
);
EXPECT_EQ
(
obstacle_ptr
->
type
(),
perception
::
VEHICLE
);
EXPECT_TRUE
(
obstacle_ptr
->
IsOnLane
());
EXPECT_EQ
(
obstacle_ptr
->
history_size
(),
3
);
EXPECT_DOUBLE_EQ
(
obstacle_ptr
->
timestamp
(),
0.2
);
...
...
@@ -143,7 +143,7 @@ TEST_F(ObstacleTest, PedestrianBasic) {
Obstacle
*
obstacle_ptr
=
container_
.
GetObstacle
(
101
);
EXPECT_TRUE
(
obstacle_ptr
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr
->
id
(),
101
);
EXPECT_EQ
(
obstacle_ptr
->
type
(),
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
EXPECT_EQ
(
obstacle_ptr
->
type
(),
perception
::
PEDESTRIAN
);
EXPECT_EQ
(
obstacle_ptr
->
history_size
(),
3
);
EXPECT_DOUBLE_EQ
(
obstacle_ptr
->
timestamp
(),
0.2
);
}
...
...
modules/prediction/container/obstacles/obstacles_container.cc
浏览文件 @
d537d178
...
...
@@ -123,7 +123,7 @@ void ObstaclesContainer::PrioritizeObstacles() {
bool
ObstaclesContainer
::
IsPredictable
(
const
PerceptionObstacle
&
perception_obstacle
)
{
if
(
!
perception_obstacle
.
has_type
()
||
perception_obstacle
.
type
()
==
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
)
{
perception_obstacle
.
type
()
==
perception
::
UNKNOWN_UNMOVABLE
)
{
return
false
;
}
return
true
;
...
...
modules/prediction/container/obstacles/obstacles_container_test.cc
浏览文件 @
d537d178
...
...
@@ -48,19 +48,19 @@ TEST_F(ObstaclesContainerTest, Vehicles) {
Obstacle
*
obstacle_ptr0
=
container_
.
GetObstacle
(
0
);
EXPECT_TRUE
(
obstacle_ptr0
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr0
->
id
(),
0
);
EXPECT_EQ
(
obstacle_ptr0
->
type
(),
perception
::
PerceptionObstacle
::
VEHICLE
);
EXPECT_EQ
(
obstacle_ptr0
->
type
(),
perception
::
VEHICLE
);
Obstacle
*
obstacle_ptr1
=
container_
.
GetObstacle
(
1
);
EXPECT_TRUE
(
obstacle_ptr1
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr1
->
id
(),
1
);
EXPECT_EQ
(
obstacle_ptr1
->
type
(),
perception
::
PerceptionObstacle
::
VEHICLE
);
EXPECT_EQ
(
obstacle_ptr1
->
type
(),
perception
::
VEHICLE
);
Obstacle
*
obstacle_ptr2
=
container_
.
GetObstacle
(
2
);
EXPECT_TRUE
(
obstacle_ptr2
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr2
->
id
(),
2
);
EXPECT_EQ
(
obstacle_ptr2
->
type
(),
perception
::
PerceptionObstacle
::
VEHICLE
);
EXPECT_EQ
(
obstacle_ptr2
->
type
(),
perception
::
VEHICLE
);
Obstacle
*
obstacle_ptr3
=
container_
.
GetObstacle
(
3
);
EXPECT_TRUE
(
obstacle_ptr3
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr3
->
id
(),
3
);
EXPECT_EQ
(
obstacle_ptr3
->
type
(),
perception
::
PerceptionObstacle
::
VEHICLE
);
EXPECT_EQ
(
obstacle_ptr3
->
type
(),
perception
::
VEHICLE
);
Obstacle
*
obstacle_ptr4
=
container_
.
GetObstacle
(
4
);
EXPECT_TRUE
(
obstacle_ptr4
==
nullptr
);
}
...
...
@@ -70,12 +70,12 @@ TEST_F(ObstaclesContainerTest, Pedestrian) {
EXPECT_TRUE
(
obstacle_ptr101
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr101
->
id
(),
101
);
EXPECT_EQ
(
obstacle_ptr101
->
type
(),
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
perception
::
PEDESTRIAN
);
Obstacle
*
obstacle_ptr102
=
container_
.
GetObstacle
(
102
);
EXPECT_TRUE
(
obstacle_ptr102
!=
nullptr
);
EXPECT_EQ
(
obstacle_ptr102
->
id
(),
102
);
EXPECT_EQ
(
obstacle_ptr102
->
type
(),
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
perception
::
PEDESTRIAN
);
Obstacle
*
obstacle_ptr103
=
container_
.
GetObstacle
(
103
);
EXPECT_TRUE
(
obstacle_ptr103
==
nullptr
);
}
...
...
modules/prediction/container/pose/pose_container.cc
浏览文件 @
d537d178
...
...
@@ -24,7 +24,7 @@ namespace prediction {
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
Point
;
using
apollo
::
common
::
Point3D
;
void
PoseContainer
::
Insert
(
const
::
google
::
protobuf
::
Message
&
message
)
{
localization
::
LocalizationEstimate
localization
;
...
...
@@ -56,7 +56,7 @@ void PoseContainer::Update(
obstacle_ptr_
->
Clear
();
obstacle_ptr_
->
set_id
(
ID
);
Point
position
;
Point
3D
position
;
position
.
set_x
(
localization
.
pose
().
position
().
x
());
position
.
set_y
(
localization
.
pose
().
position
().
y
());
position
.
set_z
(
localization
.
pose
().
position
().
z
());
...
...
@@ -76,7 +76,7 @@ void PoseContainer::Update(
}
obstacle_ptr_
->
set_theta
(
theta
);
Point
velocity
;
Point
3D
velocity
;
velocity
.
set_x
(
localization
.
pose
().
linear_velocity
().
x
());
velocity
.
set_y
(
localization
.
pose
().
linear_velocity
().
y
());
velocity
.
set_z
(
localization
.
pose
().
linear_velocity
().
z
());
...
...
@@ -110,7 +110,7 @@ double PoseContainer::GetTheta() const {
return
obstacle_ptr_
->
theta
();
}
Point
PoseContainer
::
GetPosition
()
const
{
common
::
Point3D
PoseContainer
::
GetPosition
()
const
{
return
obstacle_ptr_
->
position
();
}
...
...
modules/prediction/container/pose/pose_container.h
浏览文件 @
d537d178
...
...
@@ -77,7 +77,7 @@ class PoseContainer : public Container {
* @brief Get adc position
* @return adc position
*/
apollo
::
perception
::
Point
GetPosition
()
const
;
apollo
::
common
::
Point3D
GetPosition
()
const
;
private:
/**
...
...
@@ -88,8 +88,8 @@ class PoseContainer : public Container {
public:
static
const
int
ID
=
-
1
;
static
const
perception
::
PerceptionObstacle
::
Type
type_
=
perception
::
PerceptionObstacle
::
VEHICLE
;
static
const
perception
::
Type
type_
=
perception
::
VEHICLE
;
private:
std
::
unique_ptr
<
perception
::
PerceptionObstacle
>
obstacle_ptr_
;
...
...
modules/prediction/evaluator/evaluator_manager.cc
浏览文件 @
d537d178
...
...
@@ -55,18 +55,18 @@ void EvaluatorManager::Init(const PredictionConf& config) {
if
(
obstacle_conf
.
has_obstacle_status
()
&&
obstacle_conf
.
obstacle_status
()
==
ObstacleConf
::
ON_LANE
)
{
switch
(
obstacle_conf
.
obstacle_type
())
{
case
PerceptionObstacle
::
VEHICLE
:
{
case
perception
::
VEHICLE
:
{
vehicle_on_lane_evaluator_
=
obstacle_conf
.
evaluator_type
();
break
;
}
case
PerceptionObstacle
::
BICYCLE
:
{
case
perception
::
BICYCLE
:
{
cyclist_on_lane_evaluator_
=
obstacle_conf
.
evaluator_type
();
break
;
}
case
PerceptionObstacle
::
PEDESTRIAN
:
{
case
perception
::
PEDESTRIAN
:
{
break
;
}
case
PerceptionObstacle
::
UNKNOWN
:
{
case
perception
::
UNKNOWN
:
{
default_on_lane_evaluator_
=
obstacle_conf
.
evaluator_type
();
break
;
}
...
...
@@ -116,21 +116,21 @@ void EvaluatorManager::Run(
}
switch
(
perception_obstacle
.
type
())
{
case
PerceptionObstacle
::
VEHICLE
:
{
case
perception
::
VEHICLE
:
{
if
(
obstacle
->
IsOnLane
())
{
evaluator
=
GetEvaluator
(
vehicle_on_lane_evaluator_
);
CHECK_NOTNULL
(
evaluator
);
}
break
;
}
case
PerceptionObstacle
::
BICYCLE
:
{
case
perception
::
BICYCLE
:
{
if
(
obstacle
->
IsOnLane
())
{
evaluator
=
GetEvaluator
(
cyclist_on_lane_evaluator_
);
CHECK_NOTNULL
(
evaluator
);
}
break
;
}
case
PerceptionObstacle
::
PEDESTRIAN
:
{
case
perception
::
PEDESTRIAN
:
{
break
;
}
default:
{
...
...
modules/prediction/predictor/free_move/free_move_predictor.cc
浏览文件 @
d537d178
...
...
@@ -57,7 +57,7 @@ void FreeMovePredictor::Predict(Obstacle* obstacle) {
std
::
vector
<
TrajectoryPoint
>
points
(
0
);
double
prediction_total_time
=
FLAGS_prediction_duration
;
if
(
obstacle
->
type
()
==
PerceptionObstacle
::
PEDESTRIAN
)
{
if
(
obstacle
->
type
()
==
perception
::
PEDESTRIAN
)
{
prediction_total_time
=
FLAGS_prediction_pedestrian_total_time
;
}
DrawFreeMoveTrajectoryPoints
(
position
,
velocity
,
acc
,
theta
,
...
...
modules/prediction/predictor/free_move/free_move_predictor_test.cc
浏览文件 @
d537d178
...
...
@@ -71,7 +71,7 @@ TEST_F(FreeMovePredictorTest, General) {
TEST_F
(
FreeMovePredictorTest
,
Pedestrian
)
{
perception_obstacles_
.
mutable_perception_obstacle
(
0
)
->
set_type
(
::
apollo
::
perception
::
P
erceptionObstacle
::
P
EDESTRIAN
);
::
apollo
::
perception
::
PEDESTRIAN
);
apollo
::
perception
::
PerceptionObstacle
perception_obstacle
=
perception_obstacles_
.
perception_obstacle
(
0
);
ObstaclesContainer
container
;
...
...
modules/prediction/predictor/predictor_manager.cc
浏览文件 @
d537d178
...
...
@@ -63,7 +63,7 @@ void PredictorManager::Init(const PredictionConf& config) {
}
switch
(
obstacle_conf
.
obstacle_type
())
{
case
PerceptionObstacle
::
VEHICLE
:
{
case
perception
::
VEHICLE
:
{
if
(
obstacle_conf
.
has_obstacle_status
())
{
if
(
obstacle_conf
.
obstacle_status
()
==
ObstacleConf
::
ON_LANE
)
{
vehicle_on_lane_predictor_
=
obstacle_conf
.
predictor_type
();
...
...
@@ -74,7 +74,7 @@ void PredictorManager::Init(const PredictionConf& config) {
}
break
;
}
case
PerceptionObstacle
::
BICYCLE
:
{
case
perception
::
BICYCLE
:
{
if
(
obstacle_conf
.
has_obstacle_status
())
{
if
(
obstacle_conf
.
obstacle_status
()
==
ObstacleConf
::
ON_LANE
)
{
cyclist_on_lane_predictor_
=
obstacle_conf
.
predictor_type
();
...
...
@@ -85,11 +85,11 @@ void PredictorManager::Init(const PredictionConf& config) {
}
break
;
}
case
PerceptionObstacle
::
PEDESTRIAN
:
{
case
perception
::
PEDESTRIAN
:
{
pedestrian_predictor_
=
obstacle_conf
.
predictor_type
();
break
;
}
case
PerceptionObstacle
::
UNKNOWN
:
{
case
perception
::
UNKNOWN
:
{
if
(
obstacle_conf
.
has_obstacle_status
())
{
if
(
obstacle_conf
.
obstacle_status
()
==
ObstacleConf
::
ON_LANE
)
{
default_on_lane_predictor_
=
obstacle_conf
.
predictor_type
();
...
...
@@ -160,7 +160,7 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
predictor
=
GetPredictor
(
ObstacleConf
::
EMPTY_PREDICTOR
);
}
else
{
switch
(
perception_obstacle
.
type
())
{
case
PerceptionObstacle
::
VEHICLE
:
{
case
perception
::
VEHICLE
:
{
if
(
obstacle
->
IsOnLane
())
{
predictor
=
GetPredictor
(
vehicle_on_lane_predictor_
);
}
else
{
...
...
@@ -168,11 +168,11 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
}
break
;
}
case
PerceptionObstacle
::
PEDESTRIAN
:
{
case
perception
::
PEDESTRIAN
:
{
predictor
=
GetPredictor
(
pedestrian_predictor_
);
break
;
}
case
PerceptionObstacle
::
BICYCLE
:
{
case
perception
::
BICYCLE
:
{
if
(
obstacle
->
IsOnLane
()
&&
!
obstacle
->
IsNearJunction
())
{
predictor
=
GetPredictor
(
cyclist_on_lane_predictor_
);
}
else
{
...
...
@@ -194,7 +194,7 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
if
(
predictor
!=
nullptr
)
{
predictor
->
Predict
(
obstacle
);
if
(
FLAGS_enable_trim_prediction_trajectory
&&
obstacle
->
type
()
==
PerceptionObstacle
::
VEHICLE
)
{
obstacle
->
type
()
==
perception
::
VEHICLE
)
{
CHECK_NOTNULL
(
adc_trajectory_container
);
predictor
->
TrimTrajectories
(
obstacle
,
adc_trajectory_container
);
}
...
...
modules/prediction/proto/feature.proto
浏览文件 @
d537d178
...
...
@@ -64,7 +64,7 @@ message Feature {
optional
double
t_acc
=
21
[
deprecated
=
true
];
optional
bool
is_still
=
22
[
default
=
false
];
optional
apollo.perception.
PerceptionObstacle.
Type
type
=
23
;
optional
apollo.perception.Type
type
=
23
;
optional
double
label_update_time_delta
=
24
;
enum
Priority
{
...
...
modules/prediction/proto/prediction_conf.proto
浏览文件 @
d537d178
...
...
@@ -27,7 +27,7 @@ message ObstacleConf {
SINGLE_LANE_PREDICTOR
=
5
;
}
optional
apollo.perception.
PerceptionObstacle.
Type
obstacle_type
=
1
;
optional
apollo.perception.Type
obstacle_type
=
1
;
optional
ObstacleStatus
obstacle_status
=
2
;
optional
EvaluatorType
evaluator_type
=
3
;
optional
PredictorType
predictor_type
=
4
;
...
...
modules/third_party_perception/common/third_party_perception_util.cc
浏览文件 @
d537d178
...
...
@@ -38,7 +38,7 @@ using apollo::common::PointENU;
using
apollo
::
common
::
Quaternion
;
using
apollo
::
hdmap
::
HDMapUtil
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
Point
;
using
apollo
::
common
::
Point3D
;
double
GetAngleFromQuaternion
(
const
Quaternion
quaternion
)
{
double
theta
=
std
::
atan2
(
2.0
*
quaternion
.
qw
()
*
quaternion
.
qz
()
+
...
...
@@ -123,18 +123,18 @@ double GetDefaultObjectWidth(const int object_type) {
return
default_object_width
;
}
Point
SLtoXY
(
const
Point
&
point
,
const
double
theta
)
{
Point
3D
SLtoXY
(
const
Point3D
&
point
,
const
double
theta
)
{
return
SLtoXY
(
point
.
x
(),
point
.
y
(),
theta
);
}
Point
SLtoXY
(
const
double
x
,
const
double
y
,
const
double
theta
)
{
Point
converted_point
;
Point
3D
SLtoXY
(
const
double
x
,
const
double
y
,
const
double
theta
)
{
Point
3D
converted_point
;
converted_point
.
set_x
(
x
*
std
::
cos
(
theta
)
+
y
*
std
::
sin
(
theta
));
converted_point
.
set_y
(
x
*
std
::
sin
(
theta
)
-
y
*
std
::
cos
(
theta
));
return
converted_point
;
}
double
Distance
(
const
Point
&
point1
,
const
Point
&
point2
)
{
double
Distance
(
const
Point
3D
&
point1
,
const
Point3D
&
point2
)
{
double
distance
=
std
::
sqrt
((
point1
.
x
()
-
point2
.
x
())
*
(
point1
.
x
()
-
point2
.
x
())
+
(
point1
.
y
()
-
point2
.
y
())
*
(
point1
.
y
()
-
point2
.
y
()));
...
...
@@ -166,7 +166,7 @@ double GetNearestLaneHeading(const PointENU& point_enu) {
return
lane_heading
;
}
double
GetNearestLaneHeading
(
const
Point
&
point
)
{
double
GetNearestLaneHeading
(
const
Point
3D
&
point
)
{
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
hdmap
==
nullptr
)
{
AERROR
<<
"Failed to get nearest lane for point "
<<
point
.
DebugString
();
...
...
@@ -197,7 +197,7 @@ double GetNearestLaneHeading(const double x, const double y, const double z) {
return
GetNearestLaneHeading
(
point_enu
);
}
double
GetLateralDistanceToNearestLane
(
const
Point
&
point
)
{
double
GetLateralDistanceToNearestLane
(
const
Point
3D
&
point
)
{
auto
*
hdmap
=
HDMapUtil
::
BaseMapPtr
();
if
(
hdmap
==
nullptr
)
{
AERROR
<<
"Failed to get nearest lane for point "
<<
point
.
DebugString
();
...
...
@@ -224,7 +224,7 @@ double GetLateralDistanceToNearestLane(const Point& point) {
return
nearest_l
;
}
double
Speed
(
const
Point
&
point
)
{
double
Speed
(
const
Point
3D
&
point
)
{
return
std
::
sqrt
(
point
.
x
()
*
point
.
x
()
+
point
.
y
()
+
point
.
y
());
}
...
...
modules/third_party_perception/common/third_party_perception_util.h
浏览文件 @
d537d178
...
...
@@ -42,7 +42,7 @@ void FillPerceptionPolygon(
const
double
length
,
const
double
width
,
const
double
height
,
const
double
heading
);
// TODO(lizh): change it to
PerceptionObstacle
::VEHICLE or so
// TODO(lizh): change it to
perception
::VEHICLE or so
// when perception obstacle type is extended.
// object type | int
// car | 0
...
...
@@ -54,26 +54,26 @@ double GetDefaultObjectLength(const int object_type);
double
GetDefaultObjectWidth
(
const
int
object_type
);
apollo
::
perception
::
Point
SLtoXY
(
const
double
x
,
const
double
y
,
apollo
::
common
::
Point3D
SLtoXY
(
const
double
x
,
const
double
y
,
const
double
theta
);
apollo
::
perception
::
Point
SLtoXY
(
const
apollo
::
perception
::
Point
&
point
,
apollo
::
common
::
Point3D
SLtoXY
(
const
apollo
::
common
::
Point3D
&
point
,
const
double
theta
);
double
Distance
(
const
apollo
::
perception
::
Point
&
point1
,
const
apollo
::
perception
::
Point
&
point2
);
double
Distance
(
const
apollo
::
common
::
Point3D
&
point1
,
const
apollo
::
common
::
Point3D
&
point2
);
double
Speed
(
const
apollo
::
perception
::
Point
&
point
);
double
Speed
(
const
apollo
::
common
::
Point3D
&
point
);
double
Speed
(
const
double
vx
,
const
double
vy
);
double
GetNearestLaneHeading
(
const
apollo
::
common
::
PointENU
&
point_enu
);
double
GetNearestLaneHeading
(
const
apollo
::
perception
::
Point
&
point
);
double
GetNearestLaneHeading
(
const
apollo
::
common
::
Point3D
&
point
);
double
GetNearestLaneHeading
(
const
double
x
,
const
double
y
,
const
double
z
);
double
GetLateralDistanceToNearestLane
(
const
apollo
::
perception
::
Point
&
point
);
double
GetLateralDistanceToNearestLane
(
const
apollo
::
common
::
Point3D
&
point
);
double
HeadingDifference
(
const
double
theta1
,
const
double
theta2
);
...
...
modules/third_party_perception/conversion.cc
浏览文件 @
d537d178
...
...
@@ -44,7 +44,7 @@ using apollo::drivers::Mobileye;
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacles
;
using
apollo
::
perception
::
Point
;
using
apollo
::
common
::
Point3D
;
std
::
map
<
std
::
int32_t
,
::
apollo
::
hdmap
::
LaneBoundaryType_Type
>
lane_conversion_map
=
{{
0
,
apollo
::
hdmap
::
LaneBoundaryType
::
DOTTED_YELLOW
},
...
...
@@ -157,7 +157,7 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
mob_x
+=
FLAGS_mobileye_pos_adjust
;
// offset: imu <-> mobileye
mob_x
+=
mob_l
/
2.0
;
// make x the middle point of the vehicle.
Point
xy_point
=
SLtoXY
(
mob_x
,
mob_y
,
adc_theta
);
Point
3D
xy_point
=
SLtoXY
(
mob_x
,
mob_y
,
adc_theta
);
// TODO(QiL) : Clean this up after data collection and validation
double
converted_x
=
0.0
;
...
...
@@ -225,20 +225,20 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
switch
(
mob_type
)
{
case
0
:
case
1
:
{
mob
->
set_type
(
PerceptionObstacle
::
VEHICLE
);
// VEHICLE
mob
->
set_type
(
perception
::
VEHICLE
);
// VEHICLE
break
;
}
case
2
:
case
4
:
{
mob
->
set_type
(
PerceptionObstacle
::
BICYCLE
);
// BIKE
mob
->
set_type
(
perception
::
BICYCLE
);
// BIKE
break
;
}
case
3
:
{
mob
->
set_type
(
PerceptionObstacle
::
PEDESTRIAN
);
// PED
mob
->
set_type
(
perception
::
PEDESTRIAN
);
// PED
break
;
}
default:
{
mob
->
set_type
(
PerceptionObstacle
::
UNKNOWN
);
// UNKNOWN
mob
->
set_type
(
perception
::
UNKNOWN
);
// UNKNOWN
break
;
}
}
...
...
@@ -256,7 +256,8 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
mob
->
position
().
z
(),
mob
->
length
(),
mob
->
width
(),
mob
->
height
(),
mob
->
theta
());
mob
->
set_confidence
(
0.5
);
// FIXME(all): adjust based on new perception pb
// mob->set_confidence(0.5);
}
return
obstacles
;
...
...
@@ -287,15 +288,15 @@ RadarObstacles ContiToRadarObstacles(
rob
.
set_width
(
GetDefaultObjectWidth
(
4
));
rob
.
set_height
(
3.0
);
Point
relative_pos_sl
;
Point
3D
relative_pos_sl
;
// TODO(QiL): load the radar configs here
relative_pos_sl
.
set_x
(
contiobs
.
longitude_dist
());
relative_pos_sl
.
set_y
(
contiobs
.
lateral_dist
());
rob
.
mutable_relative_position
()
->
CopyFrom
(
relative_pos_sl
);
Point
relative_pos_xy
=
SLtoXY
(
relative_pos_sl
,
adc_theta
);
Point
absolute_pos
;
Point
3D
relative_pos_xy
=
SLtoXY
(
relative_pos_sl
,
adc_theta
);
Point
3D
absolute_pos
;
absolute_pos
.
set_x
(
adc_pos
.
x
()
+
relative_pos_xy
.
x
());
absolute_pos
.
set_y
(
adc_pos
.
y
()
+
relative_pos_xy
.
y
());
absolute_pos
.
set_z
(
adc_pos
.
z
());
...
...
@@ -305,7 +306,7 @@ RadarObstacles ContiToRadarObstacles(
rob
.
mutable_relative_velocity
()
->
set_y
(
contiobs
.
lateral_vel
());
const
auto
iter
=
last_radar_obstacles
.
radar_obstacle
().
find
(
index
);
Point
absolute_vel
;
Point
3D
absolute_vel
;
if
(
iter
==
last_radar_obstacles
.
radar_obstacle
().
end
())
{
rob
.
set_count
(
0
);
rob
.
set_movable
(
false
);
...
...
@@ -422,7 +423,7 @@ RadarObstacles DelphiToRadarObstacles(
const
double
range
=
data_500
.
can_tx_track_range
();
const
double
angle
=
data_500
.
can_tx_track_angle
()
*
M_PI
/
180.0
;
Point
relative_pos_sl
;
Point
3D
relative_pos_sl
;
relative_pos_sl
.
set_x
(
range
*
std
::
cos
(
angle
)
+
FLAGS_radar_pos_adjust
+
// offset: imu <-> mobileye
rob
.
length
()
/
...
...
@@ -430,8 +431,8 @@ RadarObstacles DelphiToRadarObstacles(
relative_pos_sl
.
set_y
(
range
*
std
::
sin
(
angle
));
rob
.
mutable_relative_position
()
->
CopyFrom
(
relative_pos_sl
);
Point
relative_pos_xy
=
SLtoXY
(
relative_pos_sl
,
adc_theta
);
Point
absolute_pos
;
Point
3D
relative_pos_xy
=
SLtoXY
(
relative_pos_sl
,
adc_theta
);
Point
3D
absolute_pos
;
absolute_pos
.
set_x
(
adc_pos
.
x
()
+
relative_pos_xy
.
x
());
absolute_pos
.
set_y
(
adc_pos
.
y
()
+
relative_pos_xy
.
y
());
absolute_pos
.
set_z
(
adc_pos
.
z
());
...
...
@@ -448,7 +449,7 @@ RadarObstacles DelphiToRadarObstacles(
lateral_vel
*
std
::
cos
(
angle
));
const
auto
iter
=
last_radar_obstacles
.
radar_obstacle
().
find
(
index
);
Point
absolute_vel
;
Point
3D
absolute_vel
;
if
(
iter
==
last_radar_obstacles
.
radar_obstacle
().
end
())
{
rob
.
set_count
(
0
);
rob
.
set_movable
(
false
);
...
...
@@ -499,7 +500,7 @@ PerceptionObstacles RadarObstaclesToPerceptionObstacles(
pob
->
set_id
(
radar_obstacle
.
id
()
+
FLAGS_radar_id_offset
);
pob
->
set_type
(
PerceptionObstacle
::
VEHICLE
);
pob
->
set_type
(
perception
::
VEHICLE
);
pob
->
set_length
(
radar_obstacle
.
length
());
pob
->
set_width
(
radar_obstacle
.
width
());
pob
->
set_height
(
radar_obstacle
.
height
());
...
...
@@ -518,7 +519,8 @@ PerceptionObstacles RadarObstaclesToPerceptionObstacles(
FillPerceptionPolygon
(
pob
,
pob
->
position
().
x
(),
pob
->
position
().
y
(),
pob
->
position
().
z
(),
pob
->
length
(),
pob
->
width
(),
pob
->
height
(),
pob
->
theta
());
pob
->
set_confidence
(
0.01
);
// FIXME(all): adjust based on new perception pb
// pob->set_confidence(0.01);
}
obstacles
.
mutable_header
()
->
CopyFrom
(
radar_obstacles
.
header
());
...
...
modules/third_party_perception/fusion.cc
浏览文件 @
d537d178
...
...
@@ -78,7 +78,8 @@ PerceptionObstacles MobileyeRadarFusion(
for
(
auto
&
radar_obstacle
:
*
(
radar_obstacles_fusion
.
mutable_perception_obstacle
()))
{
if
(
HasOverlap
(
mobileye_obstacle
,
radar_obstacle
))
{
mobileye_obstacle
.
set_confidence
(
0.99
);
// FIXME(all): adjust based on new perception pb.
// mobileye_obstacle.set_confidence(0.99);
mobileye_obstacle
.
mutable_velocity
()
->
CopyFrom
(
radar_obstacle
.
velocity
());
}
...
...
modules/third_party_perception/proto/BUILD
浏览文件 @
d537d178
...
...
@@ -16,6 +16,5 @@ proto_library(
"//modules/common/proto:geometry_proto_lib"
,
"//modules/common/proto:error_code_proto_lib"
,
"//modules/common/proto:header_proto_lib"
,
"//modules/perception/proto:perception_proto_lib"
,
],
)
modules/third_party_perception/proto/radar_obstacle.proto
浏览文件 @
d537d178
...
...
@@ -3,14 +3,15 @@ syntax = "proto2";
package
apollo
.
third_party_perception
;
import
"modules/common/proto/error_code.proto"
;
import
"modules/common/proto/geometry.proto"
;
import
"modules/common/proto/header.proto"
;
import
"modules/perception/proto/perception_obstacle.proto"
;
// next ID: 14
message
RadarObstacle
{
optional
int32
id
=
1
;
// obstacle ID.
optional
apollo.
perception.Point
relative_position
=
optional
apollo.
common.Point3D
relative_position
=
2
;
// obstacle position in the sl coordinate system.
optional
apollo.
perception.Point
relative_velocity
=
optional
apollo.
common.Point3D
relative_velocity
=
3
;
// obstacle relative velocity.
optional
double
rcs
=
4
;
// radar signal intensity.
optional
bool
movable
=
5
;
// whether this obstacle is able to move.
...
...
@@ -18,8 +19,8 @@ message RadarObstacle {
optional
double
length
=
7
;
optional
double
height
=
8
;
optional
double
theta
=
9
;
optional
apollo.
perception.Point
absolute_position
=
10
;
optional
apollo.
perception.Point
absolute_velocity
=
11
;
optional
apollo.
common.Point3D
absolute_position
=
10
;
optional
apollo.
common.Point3D
absolute_velocity
=
11
;
optional
int32
count
=
12
;
optional
int32
moving_frames_count
=
13
;
}
...
...
modules/third_party_perception/third_party_perception.cc
浏览文件 @
d537d178
...
...
@@ -39,7 +39,7 @@ using apollo::drivers::Mobileye;
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacles
;
using
apollo
::
perception
::
Point
;
using
apollo
::
common
::
Point3D
;
std
::
string
ThirdPartyPerception
::
Name
()
const
{
return
FLAGS_third_party_perception_node_name
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录