Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
5ddf4e37
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,体验更适合开发者的 AI 搜索 >>
未验证
提交
5ddf4e37
编写于
6月 23, 2020
作者:
J
Jinyun Zhou
提交者:
GitHub
6月 22, 2020
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: fix collision check in grid_search (#11524)
上级
52f7999f
变更
2
隐藏空白更改
内联
并排
Showing
2 changed file
with
27 addition
and
20 deletion
+27
-20
modules/planning/open_space/coarse_trajectory_generator/grid_search.cc
...ing/open_space/coarse_trajectory_generator/grid_search.cc
+21
-14
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
...ning/open_space/coarse_trajectory_generator/grid_search.h
+6
-6
未找到文件。
modules/planning/open_space/coarse_trajectory_generator/grid_search.cc
浏览文件 @
5ddf4e37
...
...
@@ -47,8 +47,7 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
for
(
const
auto
&
obstacle_linesegments
:
obstacles_linesegments_vec_
)
{
for
(
const
common
::
math
::
LineSegment2d
&
linesegment
:
obstacle_linesegments
)
{
if
(
linesegment
.
DistanceTo
({
node
->
GetGridX
(),
node
->
GetGridY
()})
<
node_radius_
)
{
if
(
linesegment
.
DistanceTo
({
node
->
GetX
(),
node
->
GetY
()})
<
node_radius_
)
{
return
false
;
}
}
...
...
@@ -58,34 +57,42 @@ bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
std
::
vector
<
std
::
shared_ptr
<
Node2d
>>
GridSearch
::
GenerateNextNodes
(
std
::
shared_ptr
<
Node2d
>
current_node
)
{
double
current_node_x
=
current_node
->
Get
Grid
X
();
double
current_node_y
=
current_node
->
Get
Grid
Y
();
double
current_node_x
=
current_node
->
GetX
();
double
current_node_y
=
current_node
->
GetY
();
double
current_node_path_cost
=
current_node
->
GetPathCost
();
double
diagonal_distance
=
std
::
sqrt
(
2.0
);
std
::
vector
<
std
::
shared_ptr
<
Node2d
>>
next_nodes
;
std
::
shared_ptr
<
Node2d
>
up
=
std
::
make_shared
<
Node2d
>
(
current_node_x
,
current_node_y
+
1.0
,
XYbounds_
);
std
::
shared_ptr
<
Node2d
>
up
=
std
::
make_shared
<
Node2d
>
(
current_node_x
,
current_node_y
+
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
up
->
SetPathCost
(
current_node_path_cost
+
1.0
);
std
::
shared_ptr
<
Node2d
>
up_right
=
std
::
make_shared
<
Node2d
>
(
current_node_x
+
1.0
,
current_node_y
+
1.0
,
XYbounds_
);
current_node_x
+
xy_grid_resolution_
,
current_node_y
+
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
up_right
->
SetPathCost
(
current_node_path_cost
+
diagonal_distance
);
std
::
shared_ptr
<
Node2d
>
right
=
std
::
make_shared
<
Node2d
>
(
current_node_x
+
1.0
,
current_node_y
,
XYbounds_
);
std
::
make_shared
<
Node2d
>
(
current_node_x
+
xy_grid_resolution_
,
current_node_y
,
xy_grid_resolution_
,
XYbounds_
);
right
->
SetPathCost
(
current_node_path_cost
+
1.0
);
std
::
shared_ptr
<
Node2d
>
down_right
=
std
::
make_shared
<
Node2d
>
(
current_node_x
+
1.0
,
current_node_y
-
1.0
,
XYbounds_
);
current_node_x
+
xy_grid_resolution_
,
current_node_y
-
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
down_right
->
SetPathCost
(
current_node_path_cost
+
diagonal_distance
);
std
::
shared_ptr
<
Node2d
>
down
=
std
::
make_shared
<
Node2d
>
(
current_node_x
,
current_node_y
-
1.0
,
XYbounds_
);
std
::
shared_ptr
<
Node2d
>
down
=
std
::
make_shared
<
Node2d
>
(
current_node_x
,
current_node_y
-
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
down
->
SetPathCost
(
current_node_path_cost
+
1.0
);
std
::
shared_ptr
<
Node2d
>
down_left
=
std
::
make_shared
<
Node2d
>
(
current_node_x
-
1.0
,
current_node_y
-
1.0
,
XYbounds_
);
current_node_x
-
xy_grid_resolution_
,
current_node_y
-
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
down_left
->
SetPathCost
(
current_node_path_cost
+
diagonal_distance
);
std
::
shared_ptr
<
Node2d
>
left
=
std
::
make_shared
<
Node2d
>
(
current_node_x
-
1.0
,
current_node_y
,
XYbounds_
);
std
::
make_shared
<
Node2d
>
(
current_node_x
-
xy_grid_resolution_
,
current_node_y
,
xy_grid_resolution_
,
XYbounds_
);
left
->
SetPathCost
(
current_node_path_cost
+
1.0
);
std
::
shared_ptr
<
Node2d
>
up_left
=
std
::
make_shared
<
Node2d
>
(
current_node_x
-
1.0
,
current_node_y
+
1.0
,
XYbounds_
);
current_node_x
-
xy_grid_resolution_
,
current_node_y
+
xy_grid_resolution_
,
xy_grid_resolution_
,
XYbounds_
);
up_left
->
SetPathCost
(
current_node_path_cost
+
diagonal_distance
);
next_nodes
.
emplace_back
(
up
);
...
...
modules/planning/open_space/coarse_trajectory_generator/grid_search.h
浏览文件 @
5ddf4e37
...
...
@@ -41,16 +41,12 @@ class Node2d {
Node2d
(
const
double
x
,
const
double
y
,
const
double
xy_resolution
,
const
std
::
vector
<
double
>&
XYbounds
)
{
// XYbounds with xmin, xmax, ymin, ymax
x_
=
x
;
y_
=
y
;
grid_x_
=
static_cast
<
int
>
((
x
-
XYbounds
[
0
])
/
xy_resolution
);
grid_y_
=
static_cast
<
int
>
((
y
-
XYbounds
[
2
])
/
xy_resolution
);
index_
=
ComputeStringIndex
(
grid_x_
,
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_
=
ComputeStringIndex
(
grid_x_
,
grid_y_
);
}
void
SetPathCost
(
const
double
path_cost
)
{
path_cost_
=
path_cost
;
cost_
=
path_cost_
+
heuristic_
;
...
...
@@ -61,6 +57,8 @@ class Node2d {
}
void
SetCost
(
const
double
cost
)
{
cost_
=
cost
;
}
void
SetPreNode
(
std
::
shared_ptr
<
Node2d
>
pre_node
)
{
pre_node_
=
pre_node
;
}
double
GetX
()
const
{
return
x_
;
}
double
GetY
()
const
{
return
y_
;
}
double
GetGridX
()
const
{
return
grid_x_
;
}
double
GetGridY
()
const
{
return
grid_y_
;
}
double
GetPathCost
()
const
{
return
path_cost_
;
}
...
...
@@ -86,6 +84,8 @@ class Node2d {
}
private:
double
x_
=
0.0
;
double
y_
=
0.0
;
int
grid_x_
=
0
;
int
grid_y_
=
0
;
double
path_cost_
=
0.0
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录