Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
75c19907
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,发现更多精彩内容 >>
提交
75c19907
编写于
10月 26, 2017
作者:
D
Dong Li
提交者:
Liangliang Zhang
10月 26, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
pnc_map: add position history in pnc map
which can be used to define change lane behavior
上级
e3f3988c
变更
5
隐藏空白更改
内联
并排
Showing
5 changed file
with
96 addition
and
24 deletion
+96
-24
modules/map/pnc_map/pnc_map.cc
modules/map/pnc_map/pnc_map.cc
+35
-16
modules/map/pnc_map/pnc_map.h
modules/map/pnc_map/pnc_map.h
+6
-3
modules/map/pnc_map/pnc_map_test.cc
modules/map/pnc_map/pnc_map_test.cc
+42
-1
modules/planning/common/frame.cc
modules/planning/common/frame.cc
+6
-1
modules/planning/reference_line/reference_line_provider.cc
modules/planning/reference_line/reference_line_provider.cc
+7
-3
未找到文件。
modules/map/pnc_map/pnc_map.cc
浏览文件 @
75c19907
...
@@ -248,6 +248,27 @@ PncMap::PncMap(const HDMap *hdmap) : hdmap_(hdmap) {}
...
@@ -248,6 +248,27 @@ PncMap::PncMap(const HDMap *hdmap) : hdmap_(hdmap) {}
const
hdmap
::
HDMap
*
PncMap
::
hdmap
()
const
{
return
hdmap_
;
}
const
hdmap
::
HDMap
*
PncMap
::
hdmap
()
const
{
return
hdmap_
;
}
bool
PncMap
::
UpdatePosition
(
const
common
::
PointENU
&
point
)
{
if
(
!
GetNearestPointFromRouting
(
point
,
&
current_waypoint_
))
{
AERROR
<<
"Failed to get waypoint from routing"
;
return
false
;
}
auto
current_route_index
=
GetWaypointIndex
(
current_waypoint_
);
if
(
current_route_index
.
size
()
!=
3
||
current_route_index
[
0
]
<
0
)
{
AERROR
<<
"Failed to get routing index from waypoint"
;
return
false
;
}
// on the same passage as last time, don't update
if
(
route_index_
.
size
()
!=
3
||
route_index_
[
0
]
!=
current_route_index
[
0
]
||
route_index_
[
1
]
!=
current_route_index
[
1
])
{
passage_start_point_
=
point
;
}
current_point_
=
point
;
route_index_
=
current_route_index
;
return
true
;
}
bool
PncMap
::
UpdateRoutingResponse
(
const
routing
::
RoutingResponse
&
routing
)
{
bool
PncMap
::
UpdateRoutingResponse
(
const
routing
::
RoutingResponse
&
routing
)
{
if
(
routing_
.
has_header
()
&&
routing
.
has_header
()
&&
if
(
routing_
.
has_header
()
&&
routing
.
has_header
()
&&
routing_
.
header
().
sequence_num
()
==
routing
.
header
().
sequence_num
()
&&
routing_
.
header
().
sequence_num
()
==
routing
.
header
().
sequence_num
()
&&
...
@@ -268,7 +289,10 @@ bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) {
...
@@ -268,7 +289,10 @@ bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) {
}
}
}
}
}
}
last_waypoint_
.
reset
(
nullptr
);
current_waypoint_
.
lane
=
nullptr
;
route_index_
.
clear
();
current_point_
.
Clear
();
passage_start_point_
.
Clear
();
routing_
=
routing
;
routing_
=
routing
;
return
true
;
return
true
;
}
}
...
@@ -376,21 +400,15 @@ std::vector<int> PncMap::GetNeighborPassages(const routing::RoadSegment &road,
...
@@ -376,21 +400,15 @@ std::vector<int> PncMap::GetNeighborPassages(const routing::RoadSegment &road,
}
}
bool
PncMap
::
GetRouteSegments
(
bool
PncMap
::
GetRouteSegments
(
const
common
::
PointENU
&
point
,
const
double
backward_length
,
const
double
backward_length
,
const
double
forward_length
,
const
double
forward_length
,
std
::
vector
<
RouteSegments
>
*
const
route_segments
)
const
{
std
::
vector
<
RouteSegments
>
*
const
route_segments
)
const
{
LaneWaypoint
waypoint
;
if
(
!
current_waypoint_
.
lane
||
route_index_
.
size
()
!=
3
||
if
(
!
GetNearestPointFromRouting
(
point
,
&
waypoint
))
{
route_index_
[
0
]
<
0
)
{
AERROR
<<
"Failed to get waypoint from routing"
;
AERROR
<<
"Invalid position, use UpdatePosition() function first"
;
return
false
;
}
auto
index
=
GetWaypointIndex
(
waypoint
);
if
(
index
.
size
()
!=
3
||
index
[
0
]
<
0
)
{
AERROR
<<
"Failed to get routing index from waypoint"
;
return
false
;
return
false
;
}
}
int
road_index
=
index
[
0
];
const
int
road_index
=
route_index_
[
0
];
int
passage_index
=
index
[
1
];
const
int
passage_index
=
route_index_
[
1
];
const
auto
&
road
=
routing_
.
road
(
road_index
);
const
auto
&
road
=
routing_
.
road
(
road_index
);
// raw filter to find all neighboring passages
// raw filter to find all neighboring passages
auto
drive_passages
=
GetNeighborPassages
(
road
,
passage_index
);
auto
drive_passages
=
GetNeighborPassages
(
road
,
passage_index
);
...
@@ -401,9 +419,10 @@ bool PncMap::GetRouteSegments(
...
@@ -401,9 +419,10 @@ bool PncMap::GetRouteSegments(
ADEBUG
<<
"Failed to convert passage to lane segments."
;
ADEBUG
<<
"Failed to convert passage to lane segments."
;
continue
;
continue
;
}
}
auto
nearest_point
=
point
;
auto
nearest_point
=
current_point_
;
if
(
index
==
passage_index
)
{
if
(
index
==
passage_index
)
{
nearest_point
=
waypoint
.
lane
->
GetSmoothPoint
(
waypoint
.
s
);
nearest_point
=
current_waypoint_
.
lane
->
GetSmoothPoint
(
current_waypoint_
.
s
);
}
}
double
s
=
0.0
;
double
s
=
0.0
;
double
l
=
0.0
;
double
l
=
0.0
;
...
@@ -414,7 +433,7 @@ bool PncMap::GetRouteSegments(
...
@@ -414,7 +433,7 @@ bool PncMap::GetRouteSegments(
continue
;
continue
;
}
}
// check if possible to drive from current waypoint to the passage.
// check if possible to drive from current waypoint to the passage.
if
(
index
!=
passage_index
&&
!
segments
.
CanDriveFrom
(
waypoint
))
{
if
(
index
!=
passage_index
&&
!
segments
.
CanDriveFrom
(
current_waypoint_
))
{
ADEBUG
<<
"You cannot drive from current waypoint to passage: "
<<
index
;
ADEBUG
<<
"You cannot drive from current waypoint to passage: "
<<
index
;
continue
;
continue
;
}
}
...
...
modules/map/pnc_map/pnc_map.h
浏览文件 @
75c19907
...
@@ -139,14 +139,14 @@ class PncMap {
...
@@ -139,14 +139,14 @@ class PncMap {
const
hdmap
::
HDMap
*
hdmap
()
const
;
const
hdmap
::
HDMap
*
hdmap
()
const
;
bool
UpdateRoutingResponse
(
const
routing
::
RoutingResponse
&
routing_response
);
bool
UpdateRoutingResponse
(
const
routing
::
RoutingResponse
&
routing_response
);
bool
UpdatePosition
(
const
common
::
PointENU
&
point
);
const
routing
::
RoutingResponse
&
routing_response
()
const
;
const
routing
::
RoutingResponse
&
routing_response
()
const
;
static
bool
CreatePathFromLaneSegments
(
const
RouteSegments
&
segments
,
static
bool
CreatePathFromLaneSegments
(
const
RouteSegments
&
segments
,
Path
*
const
path
);
Path
*
const
path
);
bool
GetRouteSegments
(
const
common
::
PointENU
&
point
,
bool
GetRouteSegments
(
const
double
backward_length
,
const
double
backward_length
,
const
double
forward_length
,
const
double
forward_length
,
std
::
vector
<
RouteSegments
>
*
const
route_segments
)
const
;
std
::
vector
<
RouteSegments
>
*
const
route_segments
)
const
;
...
@@ -195,7 +195,10 @@ class PncMap {
...
@@ -195,7 +195,10 @@ class PncMap {
private:
private:
routing
::
RoutingResponse
routing_
;
routing
::
RoutingResponse
routing_
;
std
::
unordered_set
<
std
::
string
>
routing_lane_ids_
;
std
::
unordered_set
<
std
::
string
>
routing_lane_ids_
;
std
::
unique_ptr
<
LaneWaypoint
>
last_waypoint_
;
LaneWaypoint
current_waypoint_
;
common
::
PointENU
current_point_
;
std
::
vector
<
int
>
route_index_
;
common
::
PointENU
passage_start_point_
;
const
hdmap
::
HDMap
*
hdmap_
=
nullptr
;
const
hdmap
::
HDMap
*
hdmap_
=
nullptr
;
};
};
...
...
modules/map/pnc_map/pnc_map_test.cc
浏览文件 @
75c19907
...
@@ -136,7 +136,10 @@ TEST_F(PncMapTest, GetRouteSegments) {
...
@@ -136,7 +136,10 @@ TEST_F(PncMapTest, GetRouteSegments) {
ASSERT_TRUE
(
lane
);
ASSERT_TRUE
(
lane
);
auto
point
=
lane
->
GetSmoothPoint
(
0
);
auto
point
=
lane
->
GetSmoothPoint
(
0
);
std
::
vector
<
RouteSegments
>
segments
;
std
::
vector
<
RouteSegments
>
segments
;
bool
result
=
pnc_map_
->
GetRouteSegments
(
point
,
10
,
30
,
&
segments
);
pnc_map_
->
route_index_
.
clear
();
EXPECT_FALSE
(
pnc_map_
->
GetRouteSegments
(
10
,
30
,
&
segments
));
EXPECT_TRUE
(
pnc_map_
->
UpdatePosition
(
point
));
bool
result
=
pnc_map_
->
GetRouteSegments
(
10
,
30
,
&
segments
);
ASSERT_TRUE
(
result
);
ASSERT_TRUE
(
result
);
ASSERT_EQ
(
2
,
segments
.
size
());
ASSERT_EQ
(
2
,
segments
.
size
());
EXPECT_NEAR
(
40
,
RouteLength
(
segments
[
0
]),
1e-4
);
EXPECT_NEAR
(
40
,
RouteLength
(
segments
[
0
]),
1e-4
);
...
@@ -147,6 +150,44 @@ TEST_F(PncMapTest, GetRouteSegments) {
...
@@ -147,6 +150,44 @@ TEST_F(PncMapTest, GetRouteSegments) {
EXPECT_FALSE
(
segments
[
1
].
IsOnSegment
());
EXPECT_FALSE
(
segments
[
1
].
IsOnSegment
());
}
}
TEST_F
(
PncMapTest
,
UpdatePosition
)
{
auto
lane1
=
hdmap_
.
GetLaneById
(
hdmap
::
MakeMapId
(
"9_1_-1"
));
auto
first_point
=
lane1
->
GetSmoothPoint
(
5
);
pnc_map_
->
route_index_
.
clear
();
pnc_map_
->
UpdatePosition
(
first_point
);
EXPECT_EQ
(
3
,
pnc_map_
->
route_index_
.
size
());
EXPECT_EQ
(
0
,
pnc_map_
->
route_index_
[
0
]);
EXPECT_EQ
(
2
,
pnc_map_
->
route_index_
[
1
]);
EXPECT_EQ
(
0
,
pnc_map_
->
route_index_
[
2
]);
EXPECT_EQ
(
lane1
,
pnc_map_
->
current_waypoint_
.
lane
);
EXPECT_NEAR
(
5
,
pnc_map_
->
current_waypoint_
.
s
,
1e-4
);
EXPECT_EQ
(
first_point
.
x
(),
pnc_map_
->
passage_start_point_
.
x
());
EXPECT_EQ
(
first_point
.
y
(),
pnc_map_
->
passage_start_point_
.
y
());
EXPECT_EQ
(
first_point
.
z
(),
pnc_map_
->
passage_start_point_
.
z
());
EXPECT_EQ
(
first_point
.
x
(),
pnc_map_
->
current_point_
.
x
());
EXPECT_EQ
(
first_point
.
y
(),
pnc_map_
->
current_point_
.
y
());
EXPECT_EQ
(
first_point
.
z
(),
pnc_map_
->
current_point_
.
z
());
auto
second_point
=
lane1
->
GetSmoothPoint
(
6
);
pnc_map_
->
UpdatePosition
(
second_point
);
EXPECT_EQ
(
3
,
pnc_map_
->
route_index_
.
size
());
EXPECT_EQ
(
0
,
pnc_map_
->
route_index_
[
0
]);
EXPECT_EQ
(
2
,
pnc_map_
->
route_index_
[
1
]);
EXPECT_EQ
(
0
,
pnc_map_
->
route_index_
[
2
]);
EXPECT_EQ
(
lane1
,
pnc_map_
->
current_waypoint_
.
lane
);
EXPECT_NEAR
(
6
,
pnc_map_
->
current_waypoint_
.
s
,
1e-4
);
EXPECT_EQ
(
first_point
.
x
(),
pnc_map_
->
passage_start_point_
.
x
());
EXPECT_EQ
(
first_point
.
y
(),
pnc_map_
->
passage_start_point_
.
y
());
EXPECT_EQ
(
first_point
.
z
(),
pnc_map_
->
passage_start_point_
.
z
());
EXPECT_EQ
(
second_point
.
x
(),
pnc_map_
->
current_point_
.
x
());
EXPECT_EQ
(
second_point
.
y
(),
pnc_map_
->
current_point_
.
y
());
EXPECT_EQ
(
second_point
.
z
(),
pnc_map_
->
current_point_
.
z
());
pnc_map_
->
route_index_
.
clear
();
}
TEST_F
(
PncMapTest
,
GetNeighborPassages
)
{
TEST_F
(
PncMapTest
,
GetNeighborPassages
)
{
const
auto
&
road0
=
routing_
.
road
(
0
);
const
auto
&
road0
=
routing_
.
road
(
0
);
{
{
...
...
modules/planning/common/frame.cc
浏览文件 @
75c19907
...
@@ -284,7 +284,12 @@ bool Frame::CreateReferenceLineFromRouting(
...
@@ -284,7 +284,12 @@ bool Frame::CreateReferenceLineFromRouting(
?
FLAGS_look_forward_distance
?
FLAGS_look_forward_distance
:
FLAGS_look_forward_min_distance
;
:
FLAGS_look_forward_min_distance
;
if
(
!
pnc_map_
->
GetRouteSegments
(
position
,
FLAGS_look_backward_distance
,
if
(
!
pnc_map_
->
UpdatePosition
(
position
))
{
AERROR
<<
"Failed to update position: "
<<
position
.
ShortDebugString
()
<<
" in pnc map."
;
return
false
;
}
if
(
!
pnc_map_
->
GetRouteSegments
(
FLAGS_look_backward_distance
,
look_forward_distance
,
&
route_segments
))
{
look_forward_distance
,
&
route_segments
))
{
AERROR
<<
"Failed to extract segments from routing"
;
AERROR
<<
"Failed to extract segments from routing"
;
return
false
;
return
false
;
...
...
modules/planning/reference_line/reference_line_provider.cc
浏览文件 @
75c19907
...
@@ -138,7 +138,12 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
...
@@ -138,7 +138,12 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
:
FLAGS_look_forward_min_distance
;
:
FLAGS_look_forward_min_distance
;
{
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
pnc_map_mutex_
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
pnc_map_mutex_
);
if
(
!
pnc_map_
->
GetRouteSegments
(
position
,
FLAGS_look_backward_distance
,
if
(
!
pnc_map_
->
UpdatePosition
(
position
))
{
AERROR
<<
"Failed to update pnc_map position: "
<<
position
.
ShortDebugString
();
return
false
;
}
if
(
!
pnc_map_
->
GetRouteSegments
(
FLAGS_look_backward_distance
,
look_forward_distance
,
&
route_segments
))
{
look_forward_distance
,
&
route_segments
))
{
AERROR
<<
"Failed to extract segments from routing"
;
AERROR
<<
"Failed to extract segments from routing"
;
return
false
;
return
false
;
...
@@ -161,8 +166,7 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
...
@@ -161,8 +166,7 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
ReferenceLine
raw_reference_line
(
hdmap_path
);
ReferenceLine
raw_reference_line
(
hdmap_path
);
ReferenceLine
reference_line
;
ReferenceLine
reference_line
;
if
(
FLAGS_enable_spiral_reference_line
)
{
if
(
FLAGS_enable_spiral_reference_line
)
{
if
(
!
spiral_smoother
.
Smooth
(
raw_reference_line
,
if
(
!
spiral_smoother
.
Smooth
(
raw_reference_line
,
&
reference_line
))
{
&
reference_line
))
{
AERROR
<<
"Failed to smooth reference_line with spiral smoother"
;
AERROR
<<
"Failed to smooth reference_line with spiral smoother"
;
}
}
}
else
{
}
else
{
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录