Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
6d7f0871
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,发现更多精彩内容 >>
提交
6d7f0871
编写于
2月 14, 2018
作者:
L
Liangliang Zhang
提交者:
Jiangtao Hu
2月 14, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Relative Map: added processing for navigation line.
上级
8e44662c
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
89 addition
and
3 deletion
+89
-3
modules/map/relative_map/navigation_lane.cc
modules/map/relative_map/navigation_lane.cc
+62
-3
modules/map/relative_map/navigation_lane.h
modules/map/relative_map/navigation_lane.h
+14
-0
modules/map/relative_map/relative_map.cc
modules/map/relative_map/relative_map.cc
+6
-0
modules/map/relative_map/relative_map.h
modules/map/relative_map/relative_map.h
+7
-0
未找到文件。
modules/map/relative_map/navigation_lane.cc
浏览文件 @
6d7f0871
...
...
@@ -22,7 +22,13 @@
namespace
apollo
{
namespace
relative_map
{
namespace
{
double
GetDistance
(
const
common
::
VehicleState
&
adc_state
,
const
common
::
PathPoint
&
p
)
{
return
std
::
hypot
(
adc_state
.
x
()
-
p
.
x
(),
adc_state
.
y
()
-
p
.
y
());
}
}
using
apollo
::
perception
::
PerceptionObstacles
;
using
apollo
::
common
::
VehicleStateProvider
;
...
...
@@ -37,11 +43,19 @@ bool NavigationLane::Update(const PerceptionObstacles& perception_obstacles) {
// update adc_state_ from VehicleStateProvider
adc_state_
=
VehicleStateProvider
::
instance
()
->
vehicle_state
();
// TODO(All): consider lane_marker qualities when converting lane_marker to
// navigation path
navigation_path_
.
Clear
();
auto
*
path
=
navigation_path_
.
mutable_path
();
ConvertLaneMarkerToPath
(
perception_obstacles_
.
lane_marker
(),
path
);
const
auto
&
lane_marker
=
perception_obstacles_
.
lane_marker
();
const
double
kQualityThreshold
=
0.5
;
if
(
navigation_info_
.
navigation_path_size
()
>
0
&&
std
::
fmax
(
lane_marker
.
left_lane_marker
().
quality
(),
lane_marker
.
right_lane_marker
().
quality
())
<
kQualityThreshold
)
{
ConvertNavigationLineToPath
();
}
else
{
ConvertLaneMarkerToPath
(
perception_obstacles_
.
lane_marker
(),
path
);
}
return
true
;
}
...
...
@@ -51,6 +65,51 @@ double NavigationLane::EvaluateCubicPolynomial(const double c0, const double c1,
return
c3
*
std
::
pow
(
z
,
3
)
+
c2
*
std
::
pow
(
z
,
2
)
+
c1
*
z
+
c0
;
}
void
NavigationLane
::
ConvertNavigationLineToPath
()
{
if
(
navigation_info_
.
navigation_path_size
()
==
0
||
!
navigation_info_
.
navigation_path
(
0
).
has_path
()
||
navigation_info_
.
navigation_path
(
0
).
path
().
path_point_size
()
==
0
)
{
// path is empty
return
;
}
navigation_path_
.
Clear
();
auto
*
curr_path
=
navigation_path_
.
mutable_path
();
UpdateProjectionIndex
();
// TODO(All): support multiple navigation path
// only support 1 navigation path
const
auto
&
path
=
navigation_info_
.
navigation_path
(
0
).
path
();
int
curr_project_index
=
last_project_index_
;
for
(
int
i
=
curr_project_index
;
i
<
path
.
path_point_size
();
++
i
)
{
auto
*
point
=
curr_path
->
add_path_point
();
point
->
CopyFrom
(
path
.
path_point
(
i
));
const
double
accumulated_s
=
path
.
path_point
(
i
).
s
()
-
path
.
path_point
(
curr_project_index
).
s
();
point
->
set_s
(
accumulated_s
);
}
}
// project adc_state_ onto path
void
NavigationLane
::
UpdateProjectionIndex
()
{
// TODO(All): support multiple navigation path
// only support 1 navigation path
const
auto
&
path
=
navigation_info_
.
navigation_path
(
0
).
path
();
int
index
=
0
;
double
min_d
=
std
::
numeric_limits
<
double
>::
max
();
for
(
int
i
=
last_project_index_
;
i
+
1
<
path
.
path_point_size
();
++
i
)
{
const
double
d
=
GetDistance
(
adc_state_
,
path
.
path_point
(
i
));
if
(
d
<
min_d
)
{
min_d
=
d
;
index
=
i
;
}
const
double
kMaxDistance
=
50.0
;
if
(
d
>
kMaxDistance
)
{
break
;
}
}
last_project_index_
=
index
;
}
void
NavigationLane
::
ConvertLaneMarkerToPath
(
const
perception
::
LaneMarkers
&
lane_marker
,
common
::
Path
*
path
)
{
const
auto
&
left_lane
=
lane_marker
.
left_lane_marker
();
...
...
modules/map/relative_map/navigation_lane.h
浏览文件 @
6d7f0871
...
...
@@ -30,6 +30,9 @@ class NavigationLane {
~
NavigationLane
()
=
default
;
bool
Update
(
const
perception
::
PerceptionObstacles
&
perception_obstacles
);
void
UpdateNavigationInfo
(
const
NavigationInfo
&
navigation_info
)
{
navigation_info_
=
navigation_info
;
}
const
NavigationPath
&
Path
()
{
return
navigation_path_
;
}
...
...
@@ -40,10 +43,21 @@ class NavigationLane {
void
ConvertLaneMarkerToPath
(
const
perception
::
LaneMarkers
&
lane_marker
,
common
::
Path
*
path
);
void
ConvertNavigationLineToPath
();
void
UpdateProjectionIndex
();
// received from topic: /apollo/perception_obstacles
perception
::
PerceptionObstacles
perception_obstacles_
;
// received from topic: /apollo/navigation
NavigationInfo
navigation_info_
;
// navigation_path_ is the combined results from perception and navigation
NavigationPath
navigation_path_
;
int
last_project_index_
=
0
;
common
::
VehicleState
adc_state_
;
};
...
...
modules/map/relative_map/relative_map.cc
浏览文件 @
6d7f0871
...
...
@@ -91,6 +91,7 @@ apollo::common::Status RelativeMap::Start() {
buffer
.
INFO
(
"RelativeMap started"
);
AdapterManager
::
AddPerceptionObstaclesCallback
(
&
RelativeMap
::
RunOnce
,
this
);
AdapterManager
::
AddNavigationCallback
(
&
RelativeMap
::
RunOnce
,
this
);
if
(
AdapterManager
::
GetPerceptionObstacles
()
->
Empty
())
{
AWARN
<<
"Perception is not ready."
;
...
...
@@ -107,6 +108,11 @@ void RelativeMap::RunOnce(const PerceptionObstacles& perception_obstacles) {
Publish
(
&
map_msg
);
}
void
RelativeMap
::
RunOnce
(
const
NavigationInfo
&
navigation_info
)
{
AdapterManager
::
Observe
();
navigation_lane_
.
UpdateNavigationInfo
(
navigation_info
);
}
void
RelativeMap
::
CreateMapFromPerception
(
const
PerceptionObstacles
&
perception_obstacles
,
MapMsg
*
map_msg
)
{
// update vehicle state from localization and chassis
...
...
modules/map/relative_map/relative_map.h
浏览文件 @
6d7f0871
...
...
@@ -70,6 +70,13 @@ class RelativeMap : public RelativeMapInterface {
void
RunOnce
(
const
perception
::
PerceptionObstacles
&
perception_obstacles
)
override
;
/**
* @brief Data callback upon receiving a navigation message.
* This function is used to update the navigation path.
* @param navigation_info received message.
*/
void
RunOnce
(
const
NavigationInfo
&
navigation_info
);
private:
void
CreateMapFromPerception
(
const
apollo
::
perception
::
PerceptionObstacles
&
perception_obstacles
,
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录