Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
26b2dbd1
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,发现更多精彩内容 >>
提交
26b2dbd1
编写于
7月 26, 2017
作者:
U
unacao
提交者:
siyangy
7月 26, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
parse decision result in dreamview backend (#44)
上级
5faa7ad3
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
425 addition
and
90 deletion
+425
-90
modules/dreamview/backend/simulation_world/simulation_world_service.cc
...view/backend/simulation_world/simulation_world_service.cc
+235
-29
modules/dreamview/backend/simulation_world/simulation_world_service.h
...mview/backend/simulation_world/simulation_world_service.h
+11
-0
modules/dreamview/backend/simulation_world/simulation_world_service_test.cc
...backend/simulation_world/simulation_world_service_test.cc
+179
-61
未找到文件。
modules/dreamview/backend/simulation_world/simulation_world_service.cc
浏览文件 @
26b2dbd1
...
...
@@ -33,17 +33,14 @@
using
apollo
::
common
::
Point3D
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
adapter
::
MonitorAdapter
;
using
apollo
::
common
::
adapter
::
LocalizationAdapter
;
using
apollo
::
common
::
adapter
::
ChassisAdapter
;
using
apollo
::
common
::
adapter
::
PerceptionObstaclesAdapter
;
using
apollo
::
common
::
adapter
::
PlanningAdapter
;
using
apollo
::
common
::
config
::
VehicleConfigHelper
;
using
apollo
::
common
::
monitor
::
MonitorMessage
;
using
apollo
::
common
::
monitor
::
MonitorMessageItem
;
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
planning
::
ADCTrajectory
;
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
planning
::
DecisionResult
;
using
apollo
::
planning
::
StopReasonCode
;
using
apollo
::
canbus
::
Chassis
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacles
;
...
...
@@ -146,6 +143,99 @@ void SetObstacleType(const PerceptionObstacle &obstacle, Object *world_object) {
}
}
void
SetStopReason
(
const
StopReasonCode
&
reason_code
,
Decision
*
decision
)
{
switch
(
reason_code
)
{
case
StopReasonCode
::
STOP_REASON_HEAD_VEHICLE
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_HEAD_VEHICLE
);
break
;
case
StopReasonCode
::
STOP_REASON_DESTINATION
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_DESTINATION
);
break
;
case
StopReasonCode
::
STOP_REASON_PEDESTRIAN
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_PEDESTRIAN
);
break
;
case
StopReasonCode
::
STOP_REASON_OBSTACLE
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_OBSTACLE
);
break
;
case
StopReasonCode
::
STOP_REASON_SIGNAL
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_SIGNAL
);
break
;
case
StopReasonCode
::
STOP_REASON_STOP_SIGN
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_STOP_SIGN
);
break
;
case
StopReasonCode
::
STOP_REASON_YIELD_SIGN
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_YIELD_SIGN
);
break
;
case
StopReasonCode
::
STOP_REASON_CLEAR_ZONE
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_CLEAR_ZONE
);
break
;
case
StopReasonCode
::
STOP_REASON_CROSSWALK
:
decision
->
set_stopreason
(
Decision
::
STOP_REASON_CROSSWALK
);
break
;
default:
AWARN
<<
"Unrecognizable stop reason code:"
<<
reason_code
;
}
}
void
UpdateTurnSignal
(
const
apollo
::
common
::
VehicleSignal
&
signal
,
Object
*
auto_driving_car
)
{
if
(
signal
.
turn_signal
()
==
apollo
::
common
::
VehicleSignal
::
TURN_LEFT
)
{
auto_driving_car
->
set_current_signal
(
"LEFT"
);
}
else
if
(
signal
.
turn_signal
()
==
apollo
::
common
::
VehicleSignal
::
TURN_RIGHT
)
{
auto_driving_car
->
set_current_signal
(
"RIGHT"
);
}
else
if
(
signal
.
emergency_light
())
{
auto_driving_car
->
set_current_signal
(
"EMERGENCY"
);
}
else
{
auto_driving_car
->
set_current_signal
(
""
);
}
}
bool
LocateMarker
(
const
apollo
::
planning
::
ObjectDecisionType
&
decision
,
double
heading
,
Decision
*
world_decision
)
{
apollo
::
common
::
PointENU
fence_point
;
if
(
decision
.
has_stop
()
&&
decision
.
stop
().
has_stop_point
())
{
world_decision
->
set_type
(
Decision_Type_STOP
);
fence_point
=
decision
.
stop
().
stop_point
();
}
else
if
(
decision
.
has_follow
()
&&
decision
.
follow
().
has_follow_point
())
{
world_decision
->
set_type
(
Decision_Type_FOLLOW
);
fence_point
=
decision
.
follow
().
follow_point
();
}
else
if
(
decision
.
has_yield
()
&&
decision
.
yield
().
has_yield_point
())
{
world_decision
->
set_type
(
Decision_Type_YIELD
);
fence_point
=
decision
.
yield
().
yield_point
();
}
else
if
(
decision
.
has_overtake
()
&&
decision
.
overtake
().
has_overtake_point
())
{
world_decision
->
set_type
(
Decision_Type_OVERTAKE
);
fence_point
=
decision
.
overtake
().
overtake_point
();
}
else
{
return
false
;
}
world_decision
->
set_position_x
(
fence_point
.
x
());
world_decision
->
set_position_y
(
fence_point
.
y
());
world_decision
->
set_heading
(
heading
);
return
true
;
}
void
FindNudgeRegion
(
const
apollo
::
planning
::
ObjectDecisionType
&
decision
,
const
Object
&
world_obj
,
Decision
*
world_decision
)
{
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
for
(
auto
&
polygon_pt
:
world_obj
.
polygon_point
())
{
points
.
emplace_back
(
polygon_pt
.
x
(),
polygon_pt
.
y
());
}
const
apollo
::
common
::
math
::
Polygon2d
obj_polygon
(
points
);
const
apollo
::
common
::
math
::
Polygon2d
&
nudge_polygon
=
obj_polygon
.
ExpandByDistance
(
fabs
(
decision
.
nudge
().
distance_l
()));
const
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
&
nudge_points
=
nudge_polygon
.
points
();
for
(
auto
&
nudge_pt
:
nudge_points
)
{
PolygonPoint
*
poly_pt
=
world_decision
->
add_polygon_point
();
poly_pt
->
set_x
(
nudge_pt
.
x
());
poly_pt
->
set_y
(
nudge_pt
.
y
());
}
world_decision
->
set_type
(
Decision_Type_NUDGE
);
}
}
// namespace
constexpr
int
SimulationWorldService
::
kMaxMonitorItems
;
...
...
@@ -161,9 +251,14 @@ const SimulationWorld &SimulationWorldService::Update() {
AdapterManager
::
Observe
();
UpdateWithLatestObserved
(
"Chassis"
,
AdapterManager
::
GetChassis
());
UpdateWithLatestObserved
(
"Localization"
,
AdapterManager
::
GetLocalization
());
obj_map_
.
clear
();
UpdateWithLatestObserved
(
"PerceptionObstacles"
,
AdapterManager
::
GetPerceptionObstacles
());
UpdateWithLatestObserved
(
"Planning"
,
AdapterManager
::
GetPlanning
());
world_
.
clear_object
();
for
(
auto
&
kv
:
obj_map_
)
{
*
world_
.
add_object
()
=
kv
.
second
;
}
return
world_
;
}
...
...
@@ -260,17 +355,7 @@ void SimulationWorldService::UpdateSimulationWorld(const Chassis &chassis) {
}
auto_driving_car
->
set_steering_angle
(
angle_percentage
);
if
(
chassis
.
signal
().
turn_signal
()
==
::
apollo
::
common
::
VehicleSignal
::
TURN_LEFT
)
{
auto_driving_car
->
set_current_signal
(
"LEFT"
);
}
else
if
(
chassis
.
signal
().
turn_signal
()
==
::
apollo
::
common
::
VehicleSignal
::
TURN_RIGHT
)
{
auto_driving_car
->
set_current_signal
(
"RIGHT"
);
}
else
if
(
chassis
.
signal
().
emergency_light
())
{
auto_driving_car
->
set_current_signal
(
"EMERGENCY"
);
}
else
{
auto_driving_car
->
set_current_signal
(
""
);
}
UpdateTurnSignal
(
chassis
.
signal
(),
auto_driving_car
);
auto_driving_car
->
set_disengage_type
(
DeduceDisengageType
(
chassis
));
...
...
@@ -284,16 +369,38 @@ void SimulationWorldService::UpdateSimulationWorld(const Chassis &chassis) {
std
::
max
(
world_
.
timestamp_sec
(),
chassis
.
header
().
timestamp_sec
()));
}
Object
&
SimulationWorldService
::
CreateWorldObject
(
const
PerceptionObstacle
&
obstacle
)
{
const
std
::
string
id
=
std
::
to_string
(
obstacle
.
id
());
// Create a new world object if the id does not exists in the map yet.
if
(
obj_map_
.
find
(
id
)
==
obj_map_
.
end
())
{
Object
&
world_obj
=
obj_map_
[
id
];
SetObstacleInfo
(
obstacle
,
&
world_obj
);
SetObstaclePolygon
(
obstacle
,
&
world_obj
);
SetObstacleType
(
obstacle
,
&
world_obj
);
}
return
obj_map_
[
id
];
}
template
<
>
void
SimulationWorldService
::
UpdateSimulationWorld
(
const
PerceptionObstacles
&
obstacles
)
{
for
(
const
auto
&
obstacle
:
obstacles
.
perception_obstacle
())
{
CreateWorldObject
(
obstacle
);
}
world_
.
set_timestamp_sec
(
std
::
max
(
world_
.
timestamp_sec
(),
obstacles
.
header
().
timestamp_sec
()));
}
void
SimulationWorldService
::
UpdatePlanningTrajectory
(
const
ADCTrajectory
&
trajectory
)
{
const
double
cutoff_time
=
world_
.
auto_driving_car
().
timestamp_sec
();
const
double
header_time
=
trajectory
.
header
().
timestamp_sec
();
const
size_t
trajectory_length
=
trajectory
.
trajectory_point_size
();
util
::
TrajectoryPointCollector
collector
(
&
world_
);
size_t
i
=
0
;
const
size_t
trajectory_length
=
trajectory
.
trajectory_point_size
();
bool
collecting_started
=
false
;
while
(
i
<
trajectory_length
)
{
const
TrajectoryPoint
&
point
=
trajectory
.
trajectory_point
(
i
);
...
...
@@ -302,8 +409,8 @@ void SimulationWorldService::UpdateSimulationWorld(
// localization/chassis message) will be dropped.
//
// Note that the last two points are always included.
if
(
collecting_started
||
point
.
relative_time
()
+
header_time
>=
cutoff_time
)
{
if
(
collecting_started
||
point
.
relative_time
()
+
header_time
>=
cutoff_time
)
{
collecting_started
=
true
;
collector
.
Collect
(
point
);
if
(
i
==
trajectory_length
-
1
)
{
...
...
@@ -327,21 +434,120 @@ void SimulationWorldService::UpdateSimulationWorld(
++
i
;
}
}
}
void
SimulationWorldService
::
UpdateMainDecision
(
const
apollo
::
planning
::
MainDecision
&
main_decision
,
double
update_timestamp_sec
,
Object
*
world_main_stop
)
{
apollo
::
common
::
math
::
Vec2d
stop_pt
;
double
stop_heading
=
0.0
;
if
(
main_decision
.
has_not_ready
())
{
// The car is not ready!
// Use the current ADC pose since it is better not to self-drive.
stop_pt
.
set_x
(
world_
.
auto_driving_car
().
position_x
());
stop_pt
.
set_y
(
world_
.
auto_driving_car
().
position_y
());
stop_heading
=
world_
.
auto_driving_car
().
heading
();
world_main_stop
->
add_decision
()
->
set_stopreason
(
Decision
::
STOP_REASON_NOT_READY
);
}
else
if
(
main_decision
.
has_estop
())
{
// Emergency stop.
// Use the current ADC pose since it is better to stop immediately.
stop_pt
.
set_x
(
world_
.
auto_driving_car
().
position_x
());
stop_pt
.
set_y
(
world_
.
auto_driving_car
().
position_y
());
stop_heading
=
world_
.
auto_driving_car
().
heading
();
world_main_stop
->
add_decision
()
->
set_stopreason
(
Decision
::
STOP_REASON_EMERGENCY
);
world_
.
mutable_auto_driving_car
()
->
set_current_signal
(
"EMERGENCY"
);
}
else
{
// Normal stop.
const
apollo
::
planning
::
MainStop
&
stop
=
main_decision
.
stop
();
stop_pt
.
set_x
(
stop
.
stop_point
().
x
());
stop_pt
.
set_y
(
stop
.
stop_point
().
y
());
stop_heading
=
stop
.
stop_heading
();
if
(
stop
.
has_reason_code
())
{
SetStopReason
(
stop
.
reason_code
(),
world_main_stop
->
add_decision
());
}
}
world_main_stop
->
set_position_x
(
stop_pt
.
x
());
world_main_stop
->
set_position_y
(
stop_pt
.
y
());
world_main_stop
->
set_heading
(
stop_heading
);
world_main_stop
->
set_timestamp_sec
(
update_timestamp_sec
);
}
void
SimulationWorldService
::
UpdateDecision
(
const
DecisionResult
&
decision_res
,
double
header_time
)
{
// Update turn signal.
UpdateTurnSignal
(
decision_res
.
vehicle_signal
(),
world_
.
mutable_auto_driving_car
());
apollo
::
planning
::
MainDecision
main_decision
=
decision_res
.
main_decision
();
// Update speed limit.
if
(
main_decision
.
target_lane_size
()
>
0
)
{
world_
.
set_speed_limit
(
main_decision
.
target_lane
(
0
).
speed_limit
());
}
world_
.
set_timestamp_sec
(
std
::
max
(
world_
.
timestamp_sec
(),
header_time
));
// Update relevant main stop with reason.
world_
.
clear_main_stop
();
if
(
main_decision
.
has_not_ready
()
||
main_decision
.
has_estop
()
||
main_decision
.
has_stop
())
{
Object
*
world_main_stop
=
world_
.
mutable_main_stop
();
UpdateMainDecision
(
main_decision
,
header_time
,
world_main_stop
);
}
// Update obstacle decision.
for
(
const
auto
&
obj_decision
:
decision_res
.
object_decision
().
decision
())
{
if
(
obj_decision
.
has_prediction
())
{
const
auto
&
p_obj
=
obj_decision
.
prediction
().
perception_obstacle
();
const
std
::
string
id
=
std
::
to_string
(
p_obj
.
id
());
// If the object does not exist in the map yet, it could be extra virtual
// objects created by prediction/decision modules.
if
(
obj_map_
.
find
(
id
)
==
obj_map_
.
end
())
{
CreateWorldObject
(
p_obj
);
}
Object
&
world_obj
=
obj_map_
[
id
];
if
(
obj_decision
.
type
()
==
apollo
::
planning
::
ObjectDecision_ObjectType_VIRTUAL
)
{
world_obj
.
set_type
(
Object_Type_VIRTUAL
);
}
for
(
const
auto
&
decision
:
obj_decision
.
object_decision
())
{
Decision
*
world_decision
=
world_obj
.
add_decision
();
world_decision
->
set_type
(
Decision_Type_IGNORE
);
if
(
decision
.
has_stop
()
||
decision
.
has_follow
()
||
decision
.
has_yield
()
||
decision
.
has_overtake
())
{
if
(
!
LocateMarker
(
decision
,
world_
.
auto_driving_car
().
heading
(),
world_decision
))
{
AWARN
<<
"No decision marker position found for object id="
<<
world_obj
.
id
();
continue
;
}
}
else
if
(
decision
.
has_nudge
())
{
if
(
world_obj
.
polygon_point_size
()
==
0
)
{
AWARN
<<
"No polygon points found for object id="
<<
world_obj
.
id
();
continue
;
}
FindNudgeRegion
(
decision
,
world_obj
,
world_decision
);
}
else
if
(
decision
.
has_sidepass
())
{
world_decision
->
set_type
(
Decision_Type_SIDEPASS
);
}
}
world_obj
.
set_timestamp_sec
(
std
::
max
(
world_obj
.
timestamp_sec
(),
header_time
));
}
}
}
template
<
>
void
SimulationWorldService
::
UpdateSimulationWorld
(
const
PerceptionObstacles
&
obstacles
)
{
for
(
const
auto
&
obstacle
:
obstacles
.
perception_obstacle
())
{
Object
*
world_obj
=
world_
.
add_object
();
SetObstacleInfo
(
obstacle
,
world_obj
);
SetObstaclePolygon
(
obstacle
,
world_obj
);
SetObstacleType
(
obstacle
,
world_obj
);
}
world_
.
set_timestamp_sec
(
std
::
max
(
world_
.
timestamp_sec
(),
obstacles
.
header
().
timestamp_sec
()));
const
ADCTrajectory
&
trajectory
)
{
const
double
header_time
=
trajectory
.
header
().
timestamp_sec
();
UpdatePlanningTrajectory
(
trajectory
);
UpdateDecision
(
trajectory
.
decision
(),
header_time
);
world_
.
set_timestamp_sec
(
std
::
max
(
world_
.
timestamp_sec
(),
header_time
));
}
void
SimulationWorldService
::
RegisterMonitorCallback
()
{
...
...
modules/dreamview/backend/simulation_world/simulation_world_service.h
浏览文件 @
26b2dbd1
...
...
@@ -106,6 +106,15 @@ class SimulationWorldService {
template
<
typename
DataType
>
void
UpdateSimulationWorld
(
const
DataType
&
data
);
Object
&
CreateWorldObject
(
const
apollo
::
perception
::
PerceptionObstacle
&
obstacle
);
void
UpdatePlanningTrajectory
(
const
apollo
::
planning
::
ADCTrajectory
&
trajectory
);
void
UpdateDecision
(
const
apollo
::
planning
::
DecisionResult
&
decision_res
,
double
header_time
);
void
UpdateMainDecision
(
const
apollo
::
planning
::
MainDecision
&
main_decision
,
double
update_timestamp_sec
,
Object
*
world_main_stop
);
/**
* @brief Check whether a particular adapter has been initialized correctly.
*/
...
...
@@ -152,6 +161,7 @@ class SimulationWorldService {
// The handle of MapService, not owned by SimulationWorldService.
MapService
*
map_service_
;
// The map holding obstacle string id to the actual object
std
::
unordered_map
<
std
::
string
,
Object
>
obj_map_
;
FRIEND_TEST
(
SimulationWorldServiceTest
,
UpdateMonitorSuccess
);
...
...
@@ -161,6 +171,7 @@ class SimulationWorldService {
FRIEND_TEST
(
SimulationWorldServiceTest
,
UpdateLocalization
);
FRIEND_TEST
(
SimulationWorldServiceTest
,
UpdatePerceptionObstacles
);
FRIEND_TEST
(
SimulationWorldServiceTest
,
UpdatePlanningTrajectory
);
FRIEND_TEST
(
SimulationWorldServiceTest
,
UpdateDecision
);
};
}
// namespace dreamview
...
...
modules/dreamview/backend/simulation_world/simulation_world_service_test.cc
浏览文件 @
26b2dbd1
...
...
@@ -28,6 +28,7 @@ using apollo::common::monitor::MonitorMessage;
using
apollo
::
localization
::
LocalizationEstimate
;
using
apollo
::
planning
::
ADCTrajectory
;
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
planning
::
DecisionResult
;
using
apollo
::
perception
::
PerceptionObstacle
;
using
apollo
::
perception
::
PerceptionObstacles
;
...
...
@@ -63,11 +64,11 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorSuccess) {
"I am the previous message."
);
sim_world_service_
->
UpdateSimulationWorld
(
monitor
);
EXPECT_EQ
(
2
,
sim_world_service_
->
world_
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am the latest message."
,
sim_world_service_
->
world_
.
monitor
().
item
(
0
).
msg
());
EXPECT_EQ
(
"I am the
previous message."
,
sim_world_service_
->
world_
.
monitor
().
item
(
1
).
msg
());
const
SimulationWorld
&
world
=
sim_world_service_
->
world
();
EXPECT_EQ
(
2
,
world
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am the
latest message."
,
world
.
monitor
().
item
(
0
).
msg
());
EXPECT_EQ
(
"I am the previous message."
,
world
.
monitor
().
item
(
1
).
msg
());
}
TEST_F
(
SimulationWorldServiceTest
,
UpdateMonitorRemove
)
{
...
...
@@ -88,14 +89,14 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorRemove) {
sim_world_service_
->
world_
.
monitor
().
item
(
last
).
msg
());
sim_world_service_
->
UpdateSimulationWorld
(
monitor
);
const
SimulationWorld
&
world
=
sim_world_service_
->
world
();
EXPECT_EQ
(
SimulationWorldService
::
kMaxMonitorItems
,
sim_world_service_
->
world_
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am message -2"
,
sim_world_service_
->
world_
.
monitor
().
item
(
0
).
msg
());
EXPECT_EQ
(
"I am message -1"
,
sim_world_service_
->
world_
.
monitor
().
item
(
1
).
msg
());
world
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am message -2"
,
world
.
monitor
().
item
(
0
).
msg
());
EXPECT_EQ
(
"I am message -1"
,
world
.
monitor
().
item
(
1
).
msg
());
EXPECT_EQ
(
"I am message "
+
std
::
to_string
(
last
-
monitor
.
item_size
()),
sim_world_service_
->
world_
.
monitor
().
item
(
last
).
msg
());
world
.
monitor
().
item
(
last
).
msg
());
}
TEST_F
(
SimulationWorldServiceTest
,
UpdateMonitorTruncate
)
{
...
...
@@ -113,19 +114,20 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorTruncate) {
->
set_timestamp_sec
(
1990
);
sim_world_service_
->
UpdateSimulationWorld
(
monitor
);
const
SimulationWorld
&
world
=
sim_world_service_
->
world
();
int
last
=
SimulationWorldService
::
kMaxMonitorItems
-
1
;
EXPECT_EQ
(
SimulationWorldService
::
kMaxMonitorItems
,
sim_world_service_
->
world_
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am message 0"
,
sim_world_service_
->
world_
.
monitor
().
item
(
0
).
msg
());
world
.
monitor
().
item_size
());
EXPECT_EQ
(
"I am message 0"
,
world
.
monitor
().
item
(
0
).
msg
());
EXPECT_EQ
(
"I am message "
+
std
::
to_string
(
last
),
sim_world_service_
->
world_
.
monitor
().
item
(
last
).
msg
());
world
.
monitor
().
item
(
last
).
msg
());
}
TEST_F
(
SimulationWorldServiceTest
,
UpdateChassisInfo
)
{
// Prepare the chassis message that will be used to update the
// SimulationWorld object.
::
apollo
::
canbus
::
Chassis
chassis
;
Chassis
chassis
;
chassis
.
set_speed_mps
(
25
);
chassis
.
set_throttle_percentage
(
50
);
chassis
.
set_brake_percentage
(
10
);
...
...
@@ -151,7 +153,7 @@ TEST_F(SimulationWorldServiceTest, UpdateChassisInfo) {
TEST_F
(
SimulationWorldServiceTest
,
UpdateLocalization
)
{
// Prepare the localization message that will be used to update the
// SimulationWorld object.
::
apollo
::
localization
::
LocalizationEstimate
localization
;
LocalizationEstimate
localization
;
localization
.
mutable_pose
()
->
mutable_position
()
->
set_x
(
1.0
);
localization
.
mutable_pose
()
->
mutable_position
()
->
set_y
(
1.5
);
localization
.
mutable_pose
()
->
mutable_orientation
()
->
set_qx
(
0.0
);
...
...
@@ -177,41 +179,6 @@ TEST_F(SimulationWorldServiceTest, UpdateLocalization) {
car
.
heading
());
}
TEST_F
(
SimulationWorldServiceTest
,
UpdatePlanningTrajectory
)
{
// Prepare the trajectory message that will be used to update the
// SimulationWorld object.
ADCTrajectory
planning_trajectory
;
for
(
int
i
=
0
;
i
<
30
;
++
i
)
{
TrajectoryPoint
*
point
=
planning_trajectory
.
add_trajectory_point
();
point
->
mutable_path_point
()
->
set_x
(
i
*
10
);
point
->
mutable_path_point
()
->
set_y
(
i
*
10
+
10
);
}
// Commit the update.
sim_world_service_
->
UpdateSimulationWorld
(
planning_trajectory
);
// Check the update result.
EXPECT_EQ
(
sim_world_service_
->
world_
.
planning_trajectory_size
(),
4
);
// Check first point.
{
const
Object
point
=
sim_world_service_
->
world_
.
planning_trajectory
(
0
);
EXPECT_DOUBLE_EQ
(
0.0
,
point
.
position_x
());
EXPECT_DOUBLE_EQ
(
10.0
,
point
.
position_y
());
EXPECT_DOUBLE_EQ
(
atan2
(
100.0
,
100.0
),
point
.
heading
());
EXPECT_EQ
(
point
.
polygon_point_size
(),
4
);
}
// Check last point.
{
const
Object
point
=
sim_world_service_
->
world_
.
planning_trajectory
(
3
);
EXPECT_DOUBLE_EQ
(
280.0
,
point
.
position_x
());
EXPECT_DOUBLE_EQ
(
290.0
,
point
.
position_y
());
EXPECT_DOUBLE_EQ
(
atan2
(
100.0
,
100.0
),
point
.
heading
());
EXPECT_EQ
(
point
.
polygon_point_size
(),
4
);
}
}
TEST_F
(
SimulationWorldServiceTest
,
UpdatePerceptionObstacles
)
{
PerceptionObstacles
obstacles
;
PerceptionObstacle
*
obstacle1
=
obstacles
.
add_perception_obstacle
();
...
...
@@ -227,7 +194,7 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
point3
->
set_y
(
0.0
);
obstacle1
->
set_timestamp
(
1489794020.123
);
obstacle1
->
set_type
(
apollo
::
perception
::
PerceptionObstacle_Type_UNKNOWN
);
apollo
::
perception
::
PerceptionObstacle
*
obstacle2
=
PerceptionObstacle
*
obstacle2
=
obstacles
.
add_perception_obstacle
();
obstacle2
->
set_id
(
2
);
apollo
::
perception
::
Point
*
point
=
obstacle2
->
mutable_position
();
...
...
@@ -240,20 +207,24 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
obstacle2
->
set_type
(
apollo
::
perception
::
PerceptionObstacle_Type_VEHICLE
);
sim_world_service_
->
UpdateSimulationWorld
(
obstacles
);
sim_world_service_
->
world_
.
clear_object
();
for
(
auto
&
kv
:
sim_world_service_
->
obj_map_
)
{
*
sim_world_service_
->
world_
.
add_object
()
=
kv
.
second
;
}
EXPECT_EQ
(
2
,
sim_world_service_
->
world_
.
object_size
());
for
(
const
auto
&
object
:
sim_world_service_
->
world_
.
object
())
{
for
(
const
auto
&
object
:
sim_world_service_
->
world_
.
object
())
{
if
(
object
.
id
()
==
"1"
)
{
EXPECT_
NEAR
(
1489794020.123
,
object
.
timestamp_sec
(),
kEpsilon
);
EXPECT_
DOUBLE_EQ
(
1489794020.123
,
object
.
timestamp_sec
()
);
EXPECT_EQ
(
3
,
object
.
polygon_point_size
());
EXPECT_EQ
(
Object_Type_UNKNOWN
,
object
.
type
());
}
else
if
(
object
.
id
()
==
"2"
)
{
EXPECT_
NEAR
(
1.0
,
object
.
position_x
(),
kEpsilon
);
EXPECT_
NEAR
(
2.0
,
object
.
position_y
(),
kEpsilon
);
EXPECT_
NEAR
(
3.0
,
object
.
heading
(),
kEpsilon
);
EXPECT_
NEAR
(
4.0
,
object
.
length
(),
kEpsilon
);
EXPECT_
NEAR
(
5.0
,
object
.
width
(),
kEpsilon
);
EXPECT_
NEAR
(
6.0
,
object
.
height
(),
kEpsilon
);
EXPECT_
DOUBLE_EQ
(
1.0
,
object
.
position_x
()
);
EXPECT_
DOUBLE_EQ
(
2.0
,
object
.
position_y
()
);
EXPECT_
DOUBLE_EQ
(
3.0
,
object
.
heading
()
);
EXPECT_
DOUBLE_EQ
(
4.0
,
object
.
length
()
);
EXPECT_
DOUBLE_EQ
(
5.0
,
object
.
width
()
);
EXPECT_
DOUBLE_EQ
(
6.0
,
object
.
height
()
);
EXPECT_EQ
(
0
,
object
.
polygon_point_size
());
EXPECT_EQ
(
Object_Type_VEHICLE
,
object
.
type
());
}
else
{
...
...
@@ -262,5 +233,152 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
}
}
TEST_F
(
SimulationWorldServiceTest
,
UpdatePlanningTrajectory
)
{
// Prepare the trajectory message that will be used to update the
// SimulationWorld object.
ADCTrajectory
planning_trajectory
;
for
(
int
i
=
0
;
i
<
30
;
++
i
)
{
TrajectoryPoint
*
point
=
planning_trajectory
.
add_trajectory_point
();
point
->
mutable_path_point
()
->
set_x
(
i
*
10
);
point
->
mutable_path_point
()
->
set_y
(
i
*
10
+
10
);
}
// Commit the update.
sim_world_service_
->
UpdatePlanningTrajectory
(
planning_trajectory
);
// Check the update result.
const
SimulationWorld
&
world
=
sim_world_service_
->
world
();
EXPECT_EQ
(
4
,
world
.
planning_trajectory_size
());
// Check first point.
{
const
Object
point
=
world
.
planning_trajectory
(
0
);
EXPECT_DOUBLE_EQ
(
0.0
,
point
.
position_x
());
EXPECT_DOUBLE_EQ
(
10.0
,
point
.
position_y
());
EXPECT_DOUBLE_EQ
(
atan2
(
100.0
,
100.0
),
point
.
heading
());
EXPECT_EQ
(
4
,
point
.
polygon_point_size
());
}
// Check last point.
{
const
Object
point
=
world
.
planning_trajectory
(
3
);
EXPECT_DOUBLE_EQ
(
280.0
,
point
.
position_x
());
EXPECT_DOUBLE_EQ
(
290.0
,
point
.
position_y
());
EXPECT_DOUBLE_EQ
(
atan2
(
100.0
,
100.0
),
point
.
heading
());
EXPECT_EQ
(
4
,
point
.
polygon_point_size
());
}
}
TEST_F
(
SimulationWorldServiceTest
,
UpdateDecision
)
{
DecisionResult
decision_res
;
decision_res
.
mutable_vehicle_signal
()
->
set_turn_signal
(
apollo
::
common
::
VehicleSignal
::
TURN_RIGHT
);
apollo
::
planning
::
MainDecision
*
main_decision
=
decision_res
.
mutable_main_decision
();
main_decision
->
add_target_lane
()
->
set_speed_limit
(
35
);
apollo
::
planning
::
MainStop
*
main_stop
=
main_decision
->
mutable_stop
();
main_stop
->
mutable_stop_point
()
->
set_x
(
45678.9
);
main_stop
->
mutable_stop_point
()
->
set_y
(
1234567.8
);
main_stop
->
set_stop_heading
(
1.234
);
main_stop
->
set_reason_code
(
apollo
::
planning
::
StopReasonCode
::
STOP_REASON_CROSSWALK
);
apollo
::
planning
::
ObjectDecisions
*
obj_decisions
=
decision_res
.
mutable_object_decision
();
// The 1st obstacle has two decisions: nudge or sidepass.
apollo
::
planning
::
ObjectDecision
*
obj_decision1
=
obj_decisions
->
add_decision
();
obj_decision1
->
set_type
(
apollo
::
planning
::
ObjectDecision_ObjectType_VIRTUAL
);
PerceptionObstacle
*
perception1
=
obj_decision1
->
mutable_prediction
()
->
mutable_perception_obstacle
();
perception1
->
set_id
(
1
);
perception1
->
mutable_position
()
->
set_x
(
1.0
);
perception1
->
mutable_position
()
->
set_y
(
2.0
);
perception1
->
set_theta
(
3.0
);
perception1
->
set_length
(
4.0
);
perception1
->
set_width
(
5.0
);
perception1
->
set_height
(
6.0
);
for
(
int
i
=
0
;
i
<
3
;
++
i
)
{
apollo
::
perception
::
Point
*
perception_pt
=
perception1
->
add_polygon_point
();
if
(
i
<
2
)
{
perception_pt
->
set_x
(
10.0
*
(
i
+
1
));
perception_pt
->
set_y
(
20.0
*
(
i
+
1
));
}
else
{
perception_pt
->
set_y
(
10.0
*
(
i
+
1
));
perception_pt
->
set_x
(
20.0
*
(
i
+
1
));
}
}
obj_decision1
->
add_object_decision
()
->
mutable_nudge
()
->
set_distance_l
(
1.8
);
obj_decision1
->
add_object_decision
()
->
mutable_sidepass
();
// The 2nd obstacle has one decision: follow.
apollo
::
planning
::
ObjectDecision
*
obj_decision2
=
obj_decisions
->
add_decision
();
obj_decision2
->
set_type
(
apollo
::
planning
::
ObjectDecision_ObjectType_PERCEPTION
);
PerceptionObstacle
*
perception2
=
obj_decision2
->
mutable_prediction
()
->
mutable_perception_obstacle
();
perception2
->
set_id
(
2
);
perception2
->
mutable_position
()
->
set_x
(
-
1860.48981629632
);
perception2
->
mutable_position
()
->
set_y
(
-
3001.9591838696506
);
apollo
::
planning
::
ObjectFollow
*
follow
=
obj_decision2
->
add_object_decision
()
->
mutable_follow
();
follow
->
set_distance_s
(
2.0
);
apollo
::
common
::
PointENU
*
follow_point
=
follow
->
mutable_follow_point
();
follow_point
->
set_x
(
-
1859.98
);
follow_point
->
set_y
(
-
3000.03
);
sim_world_service_
->
UpdateDecision
(
decision_res
,
1501095053
);
const
SimulationWorld
&
world
=
sim_world_service_
->
world
();
EXPECT_EQ
(
"RIGHT"
,
world
.
auto_driving_car
().
current_signal
());
EXPECT_EQ
(
35
,
world
.
speed_limit
());
const
Object
&
world_main_stop
=
world
.
main_stop
();
EXPECT_EQ
(
world_main_stop
.
decision
(
0
).
stopreason
(),
Decision
::
STOP_REASON_CROSSWALK
);
EXPECT_DOUBLE_EQ
(
45678.9
,
world_main_stop
.
position_x
());
EXPECT_DOUBLE_EQ
(
1234567.8
,
world_main_stop
.
position_y
());
EXPECT_DOUBLE_EQ
(
1.234
,
world_main_stop
.
heading
());
sim_world_service_
->
world_
.
clear_object
();
for
(
auto
&
kv
:
sim_world_service_
->
obj_map_
)
{
*
sim_world_service_
->
world_
.
add_object
()
=
kv
.
second
;
}
EXPECT_EQ
(
world
.
object_size
(),
2
);
for
(
int
i
=
0
;
i
<
2
;
++
i
)
{
const
Object
&
obj_dec
=
world
.
object
(
i
);
if
(
obj_dec
.
id
()
==
"1"
)
{
EXPECT_EQ
(
Object_Type_VIRTUAL
,
obj_dec
.
type
());
EXPECT_DOUBLE_EQ
(
1.0
,
obj_dec
.
position_x
());
EXPECT_DOUBLE_EQ
(
2.0
,
obj_dec
.
position_y
());
EXPECT_DOUBLE_EQ
(
3.0
,
obj_dec
.
heading
());
EXPECT_DOUBLE_EQ
(
4.0
,
obj_dec
.
length
());
EXPECT_DOUBLE_EQ
(
5.0
,
obj_dec
.
width
());
EXPECT_DOUBLE_EQ
(
6.0
,
obj_dec
.
height
());
EXPECT_EQ
(
obj_dec
.
decision_size
(),
2
);
for
(
int
j
=
0
;
j
<
obj_dec
.
decision_size
();
++
j
)
{
const
Decision
&
decision
=
obj_dec
.
decision
(
j
);
if
(
j
==
0
)
{
EXPECT_EQ
(
decision
.
type
(),
Decision_Type_NUDGE
);
EXPECT_EQ
(
decision
.
polygon_point_size
(),
67
);
}
else
{
EXPECT_EQ
(
decision
.
type
(),
Decision_Type_SIDEPASS
);
}
}
}
else
{
EXPECT_NE
(
Object_Type_VIRTUAL
,
obj_dec
.
type
());
EXPECT_EQ
(
1
,
obj_dec
.
decision_size
());
const
Decision
&
decision
=
obj_dec
.
decision
(
0
);
EXPECT_EQ
(
Decision_Type_FOLLOW
,
decision
.
type
());
EXPECT_DOUBLE_EQ
(
-
1859.98
,
decision
.
position_x
());
EXPECT_DOUBLE_EQ
(
-
3000.03
,
decision
.
position_y
());
EXPECT_DOUBLE_EQ
(
0.0
,
decision
.
heading
());
}
}
}
}
// namespace dreamview
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录