Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
eae0bdee
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,发现更多精彩内容 >>
提交
eae0bdee
编写于
8月 30, 2017
作者:
S
siyangy
提交者:
unacao
8月 30, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Dreamview: Add a dummy start point for SimControl (#1364)
上级
f2776a8f
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
71 addition
and
42 deletion
+71
-42
modules/dreamview/backend/map/map_service.cc
modules/dreamview/backend/map/map_service.cc
+30
-14
modules/dreamview/backend/map/map_service.h
modules/dreamview/backend/map/map_service.h
+7
-1
modules/dreamview/backend/map/map_service_test.cc
modules/dreamview/backend/map/map_service_test.cc
+8
-0
modules/dreamview/backend/sim_control/sim_control.cc
modules/dreamview/backend/sim_control/sim_control.cc
+21
-17
modules/dreamview/backend/sim_control/sim_control.h
modules/dreamview/backend/sim_control/sim_control.h
+4
-3
modules/dreamview/backend/sim_control/sim_control_test.cc
modules/dreamview/backend/sim_control/sim_control_test.cc
+1
-7
未找到文件。
modules/dreamview/backend/map/map_service.cc
浏览文件 @
eae0bdee
...
@@ -233,6 +233,19 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const {
...
@@ -233,6 +233,19 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const {
return
result
;
return
result
;
}
}
bool
MapService
::
GetNearestLane
(
const
double
x
,
const
double
y
,
LaneInfoConstPtr
*
nearest_lane
,
double
*
nearest_s
,
double
*
nearest_l
)
const
{
PointENU
point
;
point
.
set_x
(
x
);
point
.
set_y
(
y
);
if
(
BaseMap
().
GetNearestLane
(
point
,
nearest_lane
,
nearest_s
,
nearest_l
)
<
0
)
{
AERROR
<<
"Failed to get nearest lane!"
;
return
false
;
}
return
true
;
}
bool
MapService
::
GetPointsFromRouting
(
const
RoutingResponse
&
routing
,
bool
MapService
::
GetPointsFromRouting
(
const
RoutingResponse
&
routing
,
std
::
vector
<
MapPathPoint
>
*
points
)
const
{
std
::
vector
<
MapPathPoint
>
*
points
)
const
{
Path
path
;
Path
path
;
...
@@ -253,13 +266,9 @@ bool MapService::GetPointsFromRouting(const RoutingResponse &routing,
...
@@ -253,13 +266,9 @@ bool MapService::GetPointsFromRouting(const RoutingResponse &routing,
bool
MapService
::
GetPoseWithRegardToLane
(
const
double
x
,
const
double
y
,
bool
MapService
::
GetPoseWithRegardToLane
(
const
double
x
,
const
double
y
,
double
*
theta
,
double
*
s
)
const
{
double
*
theta
,
double
*
s
)
const
{
apollo
::
common
::
PointENU
point
;
point
.
set_x
(
x
);
point
.
set_y
(
y
);
double
l
;
double
l
;
LaneInfoConstPtr
nearest_lane
;
LaneInfoConstPtr
nearest_lane
;
if
(
BaseMap
().
GetNearestLane
(
point
,
&
nearest_lane
,
s
,
&
l
)
<
0
)
{
if
(
!
GetNearestLane
(
x
,
y
,
&
nearest_lane
,
s
,
&
l
))
{
AERROR
<<
"Failed to get nearest lane!"
;
return
false
;
return
false
;
}
}
...
@@ -269,26 +278,33 @@ bool MapService::GetPoseWithRegardToLane(const double x, const double y,
...
@@ -269,26 +278,33 @@ bool MapService::GetPoseWithRegardToLane(const double x, const double y,
bool
MapService
::
ConstructLaneWayPoint
(
bool
MapService
::
ConstructLaneWayPoint
(
const
double
x
,
const
double
y
,
const
double
x
,
const
double
y
,
RoutingRequest
::
LaneWaypoint
*
laneWayPoint
)
const
{
RoutingRequest
::
LaneWaypoint
*
laneWayPoint
)
const
{
apollo
::
common
::
PointENU
point
;
point
.
set_x
(
x
);
point
.
set_y
(
y
);
double
s
,
l
;
double
s
,
l
;
LaneInfoConstPtr
lane
;
LaneInfoConstPtr
lane
;
if
(
BaseMap
().
GetNearestLane
(
point
,
&
lane
,
&
s
,
&
l
)
<
0
)
{
if
(
!
GetNearestLane
(
x
,
y
,
&
lane
,
&
s
,
&
l
))
{
AERROR
<<
"Failed to get nearest lane!"
;
return
false
;
return
false
;
}
}
laneWayPoint
->
set_id
(
lane
->
id
().
id
());
laneWayPoint
->
set_id
(
lane
->
id
().
id
());
laneWayPoint
->
set_s
(
s
);
laneWayPoint
->
set_s
(
s
);
auto
*
pose
=
laneWayPoint
->
mutable_pose
();
auto
*
pose
=
laneWayPoint
->
mutable_pose
();
pose
->
set_x
(
x
);
pose
->
set_x
(
x
);
pose
->
set_y
(
y
);
pose
->
set_y
(
y
);
return
true
;
return
true
;
}
}
bool
MapService
::
GetStartPoint
(
apollo
::
common
::
PointENU
*
start_point
)
const
{
// Start from origin to find a lane from the map.
double
s
,
l
;
LaneInfoConstPtr
lane
;
if
(
!
GetNearestLane
(
0.0
,
0.0
,
&
lane
,
&
s
,
&
l
))
{
return
false
;
}
*
start_point
=
lane
->
GetSmoothPoint
(
0.0
);
return
true
;
}
}
// namespace dreamview
}
// namespace dreamview
}
// namespace apollo
}
// namespace apollo
modules/dreamview/backend/map/map_service.h
浏览文件 @
eae0bdee
...
@@ -80,6 +80,8 @@ class MapService {
...
@@ -80,6 +80,8 @@ class MapService {
bool
GetPoseWithRegardToLane
(
const
double
x
,
const
double
y
,
double
*
theta
,
bool
GetPoseWithRegardToLane
(
const
double
x
,
const
double
y
,
double
*
theta
,
double
*
s
)
const
;
double
*
s
)
const
;
// Get a point on the map to serve as dummy start point of SimControl
bool
GetStartPoint
(
apollo
::
common
::
PointENU
*
start_point
)
const
;
/**
/**
* @brief The function fills out proper routing lane waypoint
* @brief The function fills out proper routing lane waypoint
...
@@ -91,13 +93,17 @@ class MapService {
...
@@ -91,13 +93,17 @@ class MapService {
*/
*/
bool
ConstructLaneWayPoint
(
bool
ConstructLaneWayPoint
(
const
double
x
,
const
double
y
,
const
double
x
,
const
double
y
,
::
apollo
::
routing
::
RoutingRequest
::
LaneWaypoint
*
laneWayPoint
)
const
;
::
apollo
::
routing
::
RoutingRequest
::
LaneWaypoint
*
laneWayPoint
)
const
;
private:
private:
const
::
apollo
::
hdmap
::
HDMap
&
BaseMap
()
const
{
const
::
apollo
::
hdmap
::
HDMap
&
BaseMap
()
const
{
return
pnc_map_
.
HDMap
();
return
pnc_map_
.
HDMap
();
}
}
bool
GetNearestLane
(
const
double
x
,
const
double
y
,
apollo
::
hdmap
::
LaneInfoConstPtr
*
nearest_lane
,
double
*
nearest_s
,
double
*
nearest_l
)
const
;
// A wrapper around HDMap to provide some convenient utils dreamview needs.
// A wrapper around HDMap to provide some convenient utils dreamview needs.
const
::
apollo
::
hdmap
::
PncMap
pnc_map_
;
const
::
apollo
::
hdmap
::
PncMap
pnc_map_
;
// A downsampled map for dreamview frontend display.
// A downsampled map for dreamview frontend display.
...
...
modules/dreamview/backend/map/map_service_test.cc
浏览文件 @
eae0bdee
...
@@ -89,5 +89,13 @@ TEST_F(MapServiceTest, RetrieveMapElements) {
...
@@ -89,5 +89,13 @@ TEST_F(MapServiceTest, RetrieveMapElements) {
EXPECT_EQ
(
"l1"
,
map
.
lane
(
0
).
id
().
id
());
EXPECT_EQ
(
"l1"
,
map
.
lane
(
0
).
id
().
id
());
}
}
TEST_F
(
MapServiceTest
,
GetStartPoint
)
{
PointENU
start_point
;
EXPECT_TRUE
(
map_service
.
GetStartPoint
(
&
start_point
));
EXPECT_DOUBLE_EQ
(
-
1826.4050789145094
,
start_point
.
x
());
EXPECT_DOUBLE_EQ
(
-
3027.5187874953263
,
start_point
.
y
());
EXPECT_DOUBLE_EQ
(
0.0
,
start_point
.
z
());
}
}
// namespace dreamview
}
// namespace dreamview
}
// namespace apollo
}
// namespace apollo
modules/dreamview/backend/sim_control/sim_control.cc
浏览文件 @
eae0bdee
...
@@ -61,34 +61,33 @@ SimControl::SimControl(const MapService* map_service)
...
@@ -61,34 +61,33 @@ SimControl::SimControl(const MapService* map_service)
initial_start_
(
true
),
initial_start_
(
true
),
enabled_
(
FLAGS_enable_sim_control
)
{
enabled_
(
FLAGS_enable_sim_control
)
{
if
(
enabled_
)
{
if
(
enabled_
)
{
RoutingResponse
routing
;
apollo
::
common
::
PointENU
start_point
;
if
(
!
GetProtoFromFile
(
FLAGS_routing_response_file
,
&
routing
))
{
if
(
!
map_service_
->
GetStartPoint
(
&
start_point
))
{
AWARN
<<
"Unable to read start point from file: "
AWARN
<<
"Failed to get a dummy start point from map!"
;
<<
FLAGS_routing_response_file
;
return
;
return
;
}
}
SetStartPoint
(
routing
);
SetStartPoint
(
start_point
.
x
(),
start_point
.
y
()
);
}
}
}
}
void
SimControl
::
SetStartPoint
(
const
RoutingResponse
&
routing
)
{
void
SimControl
::
SetStartPoint
(
const
double
x
,
const
double
y
)
{
next_point_
.
set_v
(
0.0
);
next_point_
.
set_v
(
0.0
);
next_point_
.
set_a
(
0.0
);
next_point_
.
set_a
(
0.0
);
auto
*
p
=
next_point_
.
mutable_path_point
();
auto
*
next_point
=
next_point_
.
mutable_path_point
();
next_point
->
set_x
(
x
);
p
->
set_x
(
routing
.
routing_request
().
start
().
pose
().
x
());
next_point
->
set_y
(
y
);
p
->
set_y
(
routing
.
routing_request
().
start
().
pose
().
y
());
next_point
->
set_z
(
0.0
);
p
->
set_z
(
0.0
);
double
theta
=
0.0
;
double
theta
=
0.0
;
double
s
=
0.0
;
double
s
=
0.0
;
if
(
!
map_service_
->
GetPoseWithRegardToLane
(
p
->
x
(),
p
->
y
(),
&
theta
,
&
s
))
{
if
(
!
map_service_
->
GetPoseWithRegardToLane
(
next_point
->
x
(),
next_point
->
y
(),
AERROR
<<
"Failed to get heading!"
;
&
theta
,
&
s
))
{
AERROR
<<
"Failed to get heading from map! Treat theta and s as 0.0!"
;
}
}
p
->
set_theta
(
theta
);
next_point
->
set_theta
(
theta
);
p
->
set_s
(
s
);
next_point
->
set_s
(
s
);
p
->
set_kappa
(
0.0
);
next_point
->
set_kappa
(
0.0
);
prev_point_index_
=
next_point_index_
=
0
;
prev_point_index_
=
next_point_index_
=
0
;
received_planning_
=
false
;
received_planning_
=
false
;
...
@@ -98,11 +97,16 @@ void SimControl::SetStartPoint(const RoutingResponse& routing) {
...
@@ -98,11 +97,16 @@ void SimControl::SetStartPoint(const RoutingResponse& routing) {
}
}
}
}
void
SimControl
::
OnRoutingResponse
(
const
RoutingResponse
&
routing
)
{
const
auto
&
start_pose
=
routing
.
routing_request
().
start
().
pose
();
SetStartPoint
(
start_pose
.
x
(),
start_pose
.
y
());
}
void
SimControl
::
Start
()
{
void
SimControl
::
Start
()
{
if
(
initial_start_
)
{
if
(
initial_start_
)
{
// Setup planning and routing result data callback.
// Setup planning and routing result data callback.
AdapterManager
::
AddPlanningCallback
(
&
SimControl
::
OnPlanning
,
this
);
AdapterManager
::
AddPlanningCallback
(
&
SimControl
::
OnPlanning
,
this
);
AdapterManager
::
AddRoutingResponseCallback
(
&
SimControl
::
SetStartPoint
,
AdapterManager
::
AddRoutingResponseCallback
(
&
SimControl
::
OnRoutingResponse
,
this
);
this
);
// Start timer to publish localiztion and chassis messages.
// Start timer to publish localiztion and chassis messages.
...
...
modules/dreamview/backend/sim_control/sim_control.h
浏览文件 @
eae0bdee
...
@@ -62,10 +62,11 @@ class SimControl {
...
@@ -62,10 +62,11 @@ class SimControl {
private:
private:
void
OnPlanning
(
const
apollo
::
planning
::
ADCTrajectory
&
trajectory
);
void
OnPlanning
(
const
apollo
::
planning
::
ADCTrajectory
&
trajectory
);
void
OnRoutingResponse
(
const
apollo
::
routing
::
RoutingResponse
&
routing
);
// Reset the start point
according to the RoutingResponse, which can be rea
d
// Reset the start point
, which can be a dummy point on the map or receive
d
// from
file or received from a publisher
.
// from
the routing module
.
void
SetStartPoint
(
const
apollo
::
routing
::
RoutingResponse
&
routing
);
void
SetStartPoint
(
const
double
x
,
const
double
y
);
void
Freeze
();
void
Freeze
();
...
...
modules/dreamview/backend/sim_control/sim_control_test.cc
浏览文件 @
eae0bdee
...
@@ -116,13 +116,7 @@ TEST_F(SimControlTest, Test) {
...
@@ -116,13 +116,7 @@ TEST_F(SimControlTest, Test) {
const
double
timestamp
=
100.0
;
const
double
timestamp
=
100.0
;
adc_trajectory
.
mutable_header
()
->
set_timestamp_sec
(
timestamp
);
adc_trajectory
.
mutable_header
()
->
set_timestamp_sec
(
timestamp
);
RoutingResponse
routing
;
sim_control_
.
SetStartPoint
(
1.0
,
1.0
);
routing
.
mutable_routing_request
()
->
mutable_start
()
->
mutable_pose
()
->
set_x
(
1.0
);
routing
.
mutable_routing_request
()
->
mutable_start
()
->
mutable_pose
()
->
set_y
(
1.0
);
sim_control_
.
SetStartPoint
(
routing
);
AdapterManager
::
AddPlanningCallback
(
&
SimControl
::
OnPlanning
,
&
sim_control_
);
AdapterManager
::
AddPlanningCallback
(
&
SimControl
::
OnPlanning
,
&
sim_control_
);
AdapterManager
::
GetPlanning
()
->
OnReceive
(
adc_trajectory
);
AdapterManager
::
GetPlanning
()
->
OnReceive
(
adc_trajectory
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录