Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
7c7ba8bf
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,发现更多精彩内容 >>
提交
7c7ba8bf
编写于
1月 16, 2019
作者:
J
JasonZhou404
提交者:
Kecheng Xu
1月 18, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: OpenSpace: integrate dp heu into hybrid a*
上级
b5d5a40f
变更
5
隐藏空白更改
内联
并排
Showing
5 changed file
with
81 addition
and
26 deletion
+81
-26
modules/planning/conf/planner_open_space_config.pb.txt
modules/planning/conf/planner_open_space_config.pb.txt
+1
-1
modules/planning/open_space/coarse_trajectory_generator/grid_search.cc
...ing/open_space/coarse_trajectory_generator/grid_search.cc
+33
-3
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
...ning/open_space/coarse_trajectory_generator/grid_search.h
+22
-8
modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
...g/open_space/coarse_trajectory_generator/hybrid_a_star.cc
+12
-8
modules/planning/testdata/conf/open_space_standard_parking_lot.pb.txt
...ning/testdata/conf/open_space_standard_parking_lot.pb.txt
+13
-6
未找到文件。
modules/planning/conf/planner_open_space_config.pb.txt
浏览文件 @
7c7ba8bf
...
...
@@ -12,7 +12,7 @@ roi_config : {
warm_start_config : {
xy_grid_resolution : 0.3
phi_grid_resolution : 0.1
next_node_num :
2
0
next_node_num :
1
0
step_size : 0.5
traj_forward_penalty : 1.0
traj_back_penalty : 1.0
...
...
modules/planning/open_space/coarse_trajectory_generator/grid_search.cc
浏览文件 @
7c7ba8bf
...
...
@@ -37,6 +37,12 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
if
(
obstacles_linesegments_vec_
.
size
()
==
0
)
{
return
true
;
}
double
node_grid_x
=
node
->
GetGridX
();
double
node_grid_y
=
node
->
GetGridY
();
if
(
node_grid_x
>
max_grid_x_
||
node_grid_x
<
0
||
node_grid_y
>
max_grid_y_
||
node_grid_y
<
0
)
{
return
false
;
}
for
(
const
auto
&
obstacle_linesegments
:
obstacles_linesegments_vec_
)
{
for
(
const
common
::
math
::
LineSegment2d
&
linesegment
:
obstacle_linesegments
)
{
...
...
@@ -97,7 +103,7 @@ bool GridSearch::GenerateAStarPath(
const
std
::
vector
<
double
>&
XYbounds
,
const
std
::
vector
<
std
::
vector
<
common
::
math
::
LineSegment2d
>>&
obstacles_linesegments_vec
,
double
*
optimal_path_cos
t
)
{
GridAStartResult
*
resul
t
)
{
std
::
priority_queue
<
std
::
pair
<
double
,
double
>
,
std
::
vector
<
std
::
pair
<
double
,
double
>>
,
cmp
>
open_pq
;
...
...
@@ -151,7 +157,7 @@ bool GridSearch::GenerateAStarPath(
AERROR
<<
"Grid A searching return null ptr(open_set ran out)"
;
return
false
;
}
*
optimal_path_cost
=
final_node_
->
GetPathCost
()
*
xy_grid_resolution_
;
LoadGridAStarResult
(
result
)
;
ADEBUG
<<
"explored node num is "
<<
explored_node_num
;
return
true
;
}
...
...
@@ -166,6 +172,9 @@ bool GridSearch::GenerateDpMap(
std
::
unordered_map
<
double
,
std
::
shared_ptr
<
Node2d
>>
open_set
;
dp_map_
=
decltype
(
dp_map_
)();
XYbounds_
=
XYbounds
;
// XYbounds with xmin, xmax, ymin, ymax
max_grid_y_
=
std
::
round
((
XYbounds_
[
3
]
-
XYbounds_
[
2
])
/
xy_grid_resolution_
);
max_grid_x_
=
std
::
round
((
XYbounds_
[
1
]
-
XYbounds_
[
0
])
/
xy_grid_resolution_
);
std
::
shared_ptr
<
Node2d
>
end_node
=
std
::
make_shared
<
Node2d
>
(
ex
,
ey
,
xy_grid_resolution_
,
XYbounds_
);
obstacles_linesegments_vec_
=
obstacles_linesegments_vec
;
...
...
@@ -208,8 +217,29 @@ bool GridSearch::GenerateDpMap(
double
GridSearch
::
CheckDpMap
(
const
double
&
sx
,
const
double
&
sy
)
{
double
index
=
Node2d
::
CalcIndex
(
sx
,
sy
,
xy_grid_resolution_
,
XYbounds_
);
return
dp_map_
[
index
]
->
GetCost
();
if
(
dp_map_
.
find
(
index
)
!=
dp_map_
.
end
())
{
return
dp_map_
[
index
]
->
GetCost
()
*
xy_grid_resolution_
;
}
else
{
return
std
::
numeric_limits
<
double
>::
infinity
();
}
}
void
GridSearch
::
LoadGridAStarResult
(
GridAStartResult
*
result
)
{
(
*
result
).
path_cost
=
final_node_
->
GetPathCost
()
*
xy_grid_resolution_
;
std
::
shared_ptr
<
Node2d
>
current_node
=
final_node_
;
std
::
vector
<
double
>
grid_a_x
;
std
::
vector
<
double
>
grid_a_y
;
while
(
current_node
->
GetPreNode
()
!=
nullptr
)
{
grid_a_x
.
push_back
(
current_node
->
GetGridX
()
*
xy_grid_resolution_
+
XYbounds_
[
0
]);
grid_a_y
.
push_back
(
current_node
->
GetGridY
()
*
xy_grid_resolution_
+
XYbounds_
[
2
]);
current_node
=
current_node
->
GetPreNode
();
}
std
::
reverse
(
grid_a_x
.
begin
(),
grid_a_x
.
end
());
std
::
reverse
(
grid_a_y
.
begin
(),
grid_a_y
.
end
());
(
*
result
).
x
=
std
::
move
(
grid_a_x
);
(
*
result
).
y
=
std
::
move
(
grid_a_y
);
}
}
// namespace planning
}
// namespace apollo
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
浏览文件 @
7c7ba8bf
...
...
@@ -25,6 +25,8 @@
#include <unordered_map>
#include <utility>
#include <vector>
#include <algorithm>
#include <limits>
#include "cyber/common/log.h"
#include "modules/common/math/line_segment2d.h"
...
...
@@ -48,18 +50,21 @@ class Node2d {
grid_y_
=
grid_y
;
index_
=
grid_x_
*
(
XYbounds
[
3
]
-
XYbounds
[
2
])
+
grid_y_
;
}
void
SetPathCost
(
const
double
&
path_cost
)
{
path_cost_
=
path_cost
;
}
void
SetHeuristic
(
const
double
&
heuristic
)
{
heuristic_
=
heuristic
;
}
void
SetPathCost
(
const
double
&
path_cost
)
{
path_cost_
=
path_cost
;
cost_
=
path_cost_
+
heuristic_
;
}
void
SetHeuristic
(
const
double
&
heuristic
)
{
heuristic_
=
heuristic
;
cost_
=
path_cost_
+
heuristic_
;
}
void
SetCost
(
const
double
&
cost
)
{
cost_
=
cost
;
}
void
SetPreNode
(
std
::
shared_ptr
<
Node2d
>
pre_node
)
{
pre_node_
=
pre_node
;
}
double
GetGridX
()
const
{
return
grid_x_
;
}
double
GetGridY
()
const
{
return
grid_y_
;
}
double
GetPathCost
()
const
{
return
path_cost_
;
}
double
GetHeuCost
()
const
{
return
heuristic_
;
}
double
GetCost
()
{
cost_
=
path_cost_
+
heuristic_
;
return
cost_
;
}
double
GetCost
()
{
return
cost_
;
}
double
GetIndex
()
const
{
return
index_
;
}
std
::
shared_ptr
<
Node2d
>
GetPreNode
()
{
return
pre_node_
;
}
static
double
CalcIndex
(
const
double
&
x
,
const
double
&
y
,
...
...
@@ -80,10 +85,16 @@ class Node2d {
double
path_cost_
=
0.0
;
double
heuristic_
=
0.0
;
double
cost_
=
0.0
;
double
index_
=
0
;
double
index_
=
0
.0
;
std
::
shared_ptr
<
Node2d
>
pre_node_
=
nullptr
;
};
struct
GridAStartResult
{
std
::
vector
<
double
>
x
;
std
::
vector
<
double
>
y
;
double
path_cost
=
0.0
;
};
class
GridSearch
{
public:
explicit
GridSearch
(
const
PlannerOpenSpaceConfig
&
open_space_conf
);
...
...
@@ -93,7 +104,7 @@ class GridSearch {
const
std
::
vector
<
double
>&
XYbounds
,
const
std
::
vector
<
std
::
vector
<
common
::
math
::
LineSegment2d
>>&
obstacles_linesegments_vec
,
double
*
optimal_path_cos
t
);
GridAStartResult
*
resul
t
);
bool
GenerateDpMap
(
const
double
&
ex
,
const
double
&
ey
,
const
std
::
vector
<
double
>&
XYbounds
,
const
std
::
vector
<
std
::
vector
<
common
::
math
::
LineSegment2d
>>&
...
...
@@ -106,11 +117,14 @@ class GridSearch {
std
::
vector
<
std
::
shared_ptr
<
Node2d
>>
GenerateNextNodes
(
std
::
shared_ptr
<
Node2d
>
node
);
bool
CheckConstraints
(
std
::
shared_ptr
<
Node2d
>
node
);
void
LoadGridAStarResult
(
GridAStartResult
*
result
);
private:
double
xy_grid_resolution_
=
0.0
;
double
node_radius_
=
0.0
;
std
::
vector
<
double
>
XYbounds_
;
double
max_grid_x_
=
0.0
;
double
max_grid_y_
=
0.0
;
std
::
shared_ptr
<
Node2d
>
start_node_
;
std
::
shared_ptr
<
Node2d
>
end_node_
;
std
::
shared_ptr
<
Node2d
>
final_node_
;
...
...
modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
浏览文件 @
7c7ba8bf
...
...
@@ -102,7 +102,7 @@ bool HybridAStar::ValidityCheck(std::shared_ptr<Node3d> node) {
}
}
return
true
;
}
// namespace planning
}
std
::
shared_ptr
<
Node3d
>
HybridAStar
::
LoadRSPinCS
(
const
std
::
shared_ptr
<
ReedSheppPath
>
reeds_shepp_to_end
,
...
...
@@ -210,13 +210,8 @@ double HybridAStar::TrajCost(std::shared_ptr<Node3d> current_node,
bool
HybridAStar
::
HoloObstacleHeuristic
(
std
::
shared_ptr
<
Node3d
>
next_node
,
double
*
optimal_path_cost
)
{
if
(
!
grid_a_star_heuristic_generator_
->
GenerateAStarPath
(
next_node
->
GetX
(),
next_node
->
GetY
(),
end_node_
->
GetX
(),
end_node_
->
GetY
(),
XYbounds_
,
obstacles_linesegments_vec_
,
optimal_path_cost
))
{
AERROR
<<
"grid_a_star_heuristic failed"
;
return
false
;
}
*
optimal_path_cost
=
grid_a_star_heuristic_generator_
->
CheckDpMap
(
next_node
->
GetX
(),
next_node
->
GetY
());
return
true
;
}
...
...
@@ -345,6 +340,10 @@ bool HybridAStar::Plan(
AERROR
<<
"end_node in collision with obstacles"
;
return
false
;
}
double
map_time
=
Clock
::
NowInSeconds
();
grid_a_star_heuristic_generator_
->
GenerateDpMap
(
ex
,
ey
,
XYbounds_
,
obstacles_linesegments_vec_
);
AINFO
<<
"map time "
<<
Clock
::
NowInSeconds
()
-
map_time
;
// load open set, pq
open_set_
.
insert
(
std
::
make_pair
(
start_node_
->
GetIndex
(),
start_node_
));
open_pq_
.
push
(
...
...
@@ -353,6 +352,7 @@ bool HybridAStar::Plan(
// Hybrid A* begins
size_t
explored_node_num
=
0
;
double
heuristic_time
=
0.0
;
double
rs_time
=
0.0
;
double
start_time
=
0.0
;
double
end_time
=
0.0
;
while
(
!
open_pq_
.
empty
())
{
...
...
@@ -362,9 +362,12 @@ bool HybridAStar::Plan(
std
::
shared_ptr
<
Node3d
>
current_node
=
open_set_
[
current_id
];
// check if a analystic curve could be connected from current configuration
// to the end configuration without collision. if so, search ends.
start_time
=
Clock
::
NowInSeconds
();
if
(
AnalyticExpansion
(
current_node
))
{
break
;
}
end_time
=
Clock
::
NowInSeconds
();
rs_time
+=
end_time
-
start_time
;
close_set_
.
insert
(
std
::
make_pair
(
current_node
->
GetIndex
(),
current_node
));
for
(
size_t
i
=
0
;
i
<
next_node_num_
;
++
i
)
{
std
::
shared_ptr
<
Node3d
>
next_node
=
Next_node_generator
(
current_node
,
i
);
...
...
@@ -406,6 +409,7 @@ bool HybridAStar::Plan(
}
AINFO
<<
"explored node num is "
<<
explored_node_num
;
AINFO
<<
"heuristic time is "
<<
heuristic_time
;
AINFO
<<
"rs time is "
<<
rs_time
;
return
true
;
}
}
// namespace planning
...
...
modules/planning/testdata/conf/open_space_standard_parking_lot.pb.txt
浏览文件 @
7c7ba8bf
...
...
@@ -6,23 +6,21 @@ is_near_destination_threshold : 0.05
roi_config : {
roi_longitudinal_range : 15.0
parking_start_range : 12.0
parking_inwards : false
}
warm_start_config : {
xy_grid_resolution : 0.3
phi_grid_resolution : 0.1
next_node_num :
2
0
next_node_num :
1
0
step_size : 0.5
traj_forward_penalty : 1.0
traj_back_penalty : 1.0
traj_gear_switch_penalty : 10.0
traj_steer_penalty : 0.0
traj_steer_change_penalty : 0.0
heu_rs_forward_penalty : 1.0
heu_rs_back_penalty : 1.0
heu_rs_gear_switch_penalty : 10.0
heu_rs_steer_penalty : 1.0
heu_rs_steer_change_penalty : 1.0
grid_a_star_xy_resolution : 1.0
node_radius : 0.5
}
dual_variable_warm_start_config : {
...
...
@@ -76,4 +74,13 @@ distance_approach_config : {
ipopt_alpha_for_y : "min"
ipopt_recalc_y : "yes"
}
}
trajectory_partition_config : {
interpolated_pieces_num : 50
initial_gear_check_horizon : 3
heading_searching_range : 0.3
gear_shift_period_duration : 2.0
gear_shift_max_t : 3.0
gear_shift_unit_t : 0.02
}
\ No newline at end of file
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录