Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e9b7759a
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,发现更多精彩内容 >>
提交
e9b7759a
编写于
3月 20, 2019
作者:
J
JasonZhou404
提交者:
Calvin Miao
3月 20, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: OpenSpace: grid search map key bugfix
上级
cb08ad29
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
39 addition
and
38 deletion
+39
-38
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
+10
-10
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
...ning/open_space/coarse_trajectory_generator/grid_search.h
+25
-24
modules/tools/open_space_visualization/auto_param_tuning.py
modules/tools/open_space_visualization/auto_param_tuning.py
+3
-3
未找到文件。
modules/planning/conf/planner_open_space_config.pb.txt
浏览文件 @
e9b7759a
...
...
@@ -13,7 +13,7 @@ warm_start_config {
traj_gear_switch_penalty: 10.0
traj_steer_penalty: 0.0
traj_steer_change_penalty: 0.0
grid_a_star_xy_resolution:
1.0
grid_a_star_xy_resolution:
0.5
node_radius: 0.5
}
dual_variable_warm_start_config {
...
...
modules/planning/open_space/coarse_trajectory_generator/grid_search.cc
浏览文件 @
e9b7759a
...
...
@@ -105,11 +105,11 @@ bool GridSearch::GenerateAStarPath(
const
std
::
vector
<
std
::
vector
<
common
::
math
::
LineSegment2d
>>&
obstacles_linesegments_vec
,
GridAStartResult
*
result
)
{
std
::
priority_queue
<
std
::
pair
<
double
,
double
>
,
std
::
vector
<
std
::
pair
<
double
,
double
>>
,
cmp
>
std
::
priority_queue
<
std
::
pair
<
std
::
string
,
double
>
,
std
::
vector
<
std
::
pair
<
std
::
string
,
double
>>
,
cmp
>
open_pq
;
std
::
unordered_map
<
double
,
std
::
shared_ptr
<
Node2d
>>
open_set
;
std
::
unordered_map
<
double
,
std
::
shared_ptr
<
Node2d
>>
close_set
;
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
Node2d
>>
open_set
;
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
Node2d
>>
close_set
;
XYbounds_
=
XYbounds
;
std
::
shared_ptr
<
Node2d
>
start_node
=
std
::
make_shared
<
Node2d
>
(
sx
,
sy
,
xy_grid_resolution_
,
XYbounds_
);
...
...
@@ -123,7 +123,7 @@ bool GridSearch::GenerateAStarPath(
// Grid a star begins
size_t
explored_node_num
=
0
;
while
(
!
open_pq
.
empty
())
{
double
current_id
=
open_pq
.
top
().
first
;
std
::
string
current_id
=
open_pq
.
top
().
first
;
open_pq
.
pop
();
std
::
shared_ptr
<
Node2d
>
current_node
=
open_set
[
current_id
];
// Check destination
...
...
@@ -167,10 +167,10 @@ bool GridSearch::GenerateDpMap(
const
double
&
ex
,
const
double
&
ey
,
const
std
::
vector
<
double
>&
XYbounds
,
const
std
::
vector
<
std
::
vector
<
common
::
math
::
LineSegment2d
>>&
obstacles_linesegments_vec
)
{
std
::
priority_queue
<
std
::
pair
<
double
,
double
>
,
std
::
vector
<
std
::
pair
<
double
,
double
>>
,
cmp
>
std
::
priority_queue
<
std
::
pair
<
std
::
string
,
double
>
,
std
::
vector
<
std
::
pair
<
std
::
string
,
double
>>
,
cmp
>
open_pq
;
std
::
unordered_map
<
double
,
std
::
shared_ptr
<
Node2d
>>
open_set
;
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
Node2d
>>
open_set
;
dp_map_
=
decltype
(
dp_map_
)();
XYbounds_
=
XYbounds
;
// XYbounds with xmin, xmax, ymin, ymax
...
...
@@ -185,7 +185,7 @@ bool GridSearch::GenerateDpMap(
// Grid a star begins
size_t
explored_node_num
=
0
;
while
(
!
open_pq
.
empty
())
{
double
current_id
=
open_pq
.
top
().
first
;
std
::
string
current_id
=
open_pq
.
top
().
first
;
open_pq
.
pop
();
std
::
shared_ptr
<
Node2d
>
current_node
=
open_set
[
current_id
];
dp_map_
.
insert
(
std
::
make_pair
(
current_node
->
GetIndex
(),
current_node
));
...
...
@@ -217,7 +217,7 @@ bool GridSearch::GenerateDpMap(
}
double
GridSearch
::
CheckDpMap
(
const
double
&
sx
,
const
double
&
sy
)
{
double
index
=
Node2d
::
CalcIndex
(
sx
,
sy
,
xy_grid_resolution_
,
XYbounds_
);
std
::
string
index
=
Node2d
::
CalcIndex
(
sx
,
sy
,
xy_grid_resolution_
,
XYbounds_
);
if
(
dp_map_
.
find
(
index
)
!=
dp_map_
.
end
())
{
return
dp_map_
[
index
]
->
GetCost
()
*
xy_grid_resolution_
;
}
else
{
...
...
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
浏览文件 @
e9b7759a
...
...
@@ -24,6 +24,7 @@
#include <limits>
#include <memory>
#include <queue>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
...
...
@@ -37,55 +38,55 @@ namespace planning {
class
Node2d
{
public:
Node2d
(
const
double
&
x
,
const
double
&
y
,
const
double
&
xy_resolution
,
Node2d
(
const
double
x
,
const
double
y
,
const
double
xy_resolution
,
const
std
::
vector
<
double
>&
XYbounds
)
{
// XYbounds with xmin, xmax, ymin, ymax
grid_x_
=
st
d
::
round
((
x
-
XYbounds
[
0
])
/
xy_resolution
);
grid_y_
=
st
d
::
round
((
y
-
XYbounds
[
2
])
/
xy_resolution
);
index_
=
grid_x_
*
(
XYbounds
[
3
]
-
XYbounds
[
2
])
+
grid_y_
;
grid_x_
=
st
atic_cast
<
int
>
((
x
-
XYbounds
[
0
])
/
xy_resolution
);
grid_y_
=
st
atic_cast
<
int
>
((
y
-
XYbounds
[
2
])
/
xy_resolution
);
index_
=
std
::
to_string
(
grid_x_
)
+
"_"
+
std
::
to_string
(
grid_y_
)
;
}
Node2d
(
const
double
&
grid_x
,
const
double
&
grid_y
,
Node2d
(
const
int
grid_x
,
const
int
grid_y
,
const
std
::
vector
<
double
>&
XYbounds
)
{
grid_x_
=
grid_x
;
grid_y_
=
grid_y
;
index_
=
grid_x_
*
(
XYbounds
[
3
]
-
XYbounds
[
2
])
+
grid_y_
;
index_
=
std
::
to_string
(
grid_x_
)
+
"_"
+
std
::
to_string
(
grid_y_
)
;
}
void
SetPathCost
(
const
double
&
path_cost
)
{
void
SetPathCost
(
const
double
path_cost
)
{
path_cost_
=
path_cost
;
cost_
=
path_cost_
+
heuristic_
;
}
void
SetHeuristic
(
const
double
&
heuristic
)
{
void
SetHeuristic
(
const
double
heuristic
)
{
heuristic_
=
heuristic
;
cost_
=
path_cost_
+
heuristic_
;
}
void
SetCost
(
const
double
&
cost
)
{
cost_
=
cost
;
}
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
()
{
return
cost_
;
}
double
GetIndex
()
const
{
return
index_
;
}
std
::
shared_ptr
<
Node2d
>
GetPreNode
()
{
return
pre_node_
;
}
static
double
CalcIndex
(
const
double
&
x
,
const
double
&
y
,
const
double
&
xy_resolution
,
const
std
::
vector
<
double
>&
XYbounds
)
{
double
GetCost
()
const
{
return
cost_
;
}
std
::
string
GetIndex
()
const
{
return
index_
;
}
std
::
shared_ptr
<
Node2d
>
GetPreNode
()
const
{
return
pre_node_
;
}
static
std
::
string
CalcIndex
(
const
double
x
,
const
double
y
,
const
double
xy_resolution
,
const
std
::
vector
<
double
>&
XYbounds
)
{
// XYbounds with xmin, xmax, ymin, ymax
double
grid_x
=
std
::
round
((
x
-
XYbounds
[
0
])
/
xy_resolution
);
double
grid_y
=
std
::
round
((
y
-
XYbounds
[
2
])
/
xy_resolution
);
return
grid_x
*
(
XYbounds
[
3
]
-
XYbounds
[
2
])
+
grid_y
;
int
grid_x
=
static_cast
<
int
>
((
x
-
XYbounds
[
0
])
/
xy_resolution
);
int
grid_y
=
static_cast
<
int
>
((
y
-
XYbounds
[
2
])
/
xy_resolution
);
return
std
::
to_string
(
grid_x
)
+
"_"
+
std
::
to_string
(
grid_y
)
;
}
bool
operator
==
(
const
Node2d
&
right
)
const
{
return
right
.
GetIndex
()
==
index_
;
}
private:
double
grid_x_
=
0.
0
;
double
grid_y_
=
0.
0
;
int
grid_x_
=
0
;
int
grid_y_
=
0
;
double
path_cost_
=
0.0
;
double
heuristic_
=
0.0
;
double
cost_
=
0.0
;
double
index_
=
0.0
;
std
::
string
index_
;
std
::
shared_ptr
<
Node2d
>
pre_node_
=
nullptr
;
};
...
...
@@ -132,12 +133,12 @@ class GridSearch {
obstacles_linesegments_vec_
;
struct
cmp
{
bool
operator
()(
const
std
::
pair
<
double
,
double
>&
left
,
const
std
::
pair
<
double
,
double
>&
right
)
const
{
bool
operator
()(
const
std
::
pair
<
std
::
string
,
double
>&
left
,
const
std
::
pair
<
std
::
string
,
double
>&
right
)
const
{
return
left
.
second
>=
right
.
second
;
}
};
std
::
unordered_map
<
double
,
std
::
shared_ptr
<
Node2d
>>
dp_map_
;
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
Node2d
>>
dp_map_
;
};
}
// namespace planning
}
// namespace apollo
modules/tools/open_space_visualization/auto_param_tuning.py
浏览文件 @
e9b7759a
...
...
@@ -27,11 +27,11 @@ from modules.planning.proto import planner_open_space_config_pb2
random
.
seed
(
99999
)
rand_num
=
100
rand_num
=
100
0
original_file_path
=
"/apollo/modules/planning/conf/planner_open_space_config.pb.txt"
optimal_file_path
=
"/apollo/modules/planning/conf/optimal_planner_open_space_config.pb.txt"
#
tunning_object = "coarse_trajectory"
tunning_object
=
"smooth_trajectory"
tunning_object
=
"coarse_trajectory"
#
tunning_object = "smooth_trajectory"
def
load_open_space_protobuf
(
filename
):
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录