Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
c10d88ce
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,发现更多精彩内容 >>
提交
c10d88ce
编写于
9月 14, 2018
作者:
J
JasonZhou404
提交者:
Qi Luo
9月 26, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: Create Node3d for Hybrid Astar
上级
cd05bd11
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
151 addition
and
73 deletion
+151
-73
modules/planning/open_space/node3d.cc
modules/planning/open_space/node3d.cc
+57
-0
modules/planning/open_space/node3d.h
modules/planning/open_space/node3d.h
+70
-0
modules/planning/planner/open_space/hybrid_a_star.cc
modules/planning/planner/open_space/hybrid_a_star.cc
+4
-33
modules/planning/planner/open_space/hybrid_a_star.h
modules/planning/planner/open_space/hybrid_a_star.h
+20
-40
未找到文件。
modules/planning/open_space/node3d.cc
0 → 100644
浏览文件 @
c10d88ce
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/*
* node3d.cc
*/
#include "modules/planning/open_space/node3d.h"
namespace
apollo
{
namespace
planning
{
Node3d
::
Node3d
(
double
x
,
double
y
,
double
phi
,
double
x_grid
,
double
y_grid
,
double
phi_grid
)
{
x_
=
x
;
y_
=
y
;
phi_
=
phi
;
x_grid_
=
x_grid
;
y_grid_
=
y_grid
;
phi_grid_
=
phi_grid
;
}
Box2d
Node3d
::
GetBoundingBox
(
const
common
::
VehicleParam
&
vehicle_param_
)
{
double
ego_length
=
vehicle_config_
.
vehicle_param
().
length
();
double
ego_width
=
vehicle_config_
.
vehicle_param
().
width
();
Box2d
ego_box
({
x_
,
y_
},
phi_
,
ego_length
,
ego_width
);
double
shift_distance
=
ego_length
/
2.0
-
vehicle_config
.
vehicle_param
().
back_edge_to_center
();
Vec2d
shift_vec
{
shift_distance
*
std
::
cos
(
phi_
),
shift_distance
*
std
::
sin
(
phi_
)};
ego_box
.
Shift
(
shift_vec
);
return
ego_box
;
}
bool
Node3d
::
operator
==
(
const
SmartPtr
<
Node3d
>
right
)
const
{
return
x_grid_
==
right
->
GetGridX
()
&&
y_grid_
==
right
->
GetGridY
()
&&
phi_grid_
==
right
->
GetGridPhi
();
}
int
Node3d
::
GetIndex
()
{
int
index
=
}
}
}
\ No newline at end of file
modules/planning/open_space/node3d.h
0 → 100644
浏览文件 @
c10d88ce
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/*
* node3d.h
*/
#ifndef MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
#define MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
#include <memory>
#include <vector>
#include "modules/common/math/box2d.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/constraint_checker/collision_checker.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
common
::
math
::
Box2d
;
class
Node3d
{
public:
explicit
Node3d
(
double
x
,
double
y
,
double
phi
,
double
x_grid
,
double
y_grid
,
double
phi_grid
);
virtual
~
Node3d
()
=
default
;
Box2d
GetBoundingBox
(
const
common
::
VehicleParam
&
vehicle_param_
);
double
GetCost
()
{
return
current_cost_
+
heuristic_cost_
;
};
std
::
size_t
GetGridX
()
{
return
x_grid_
;
};
std
::
size_t
GetGridY
()
{
return
y_grid_
;
};
std
::
size_t
GetGridPhi
()
{
return
phi_grid_
;
};
double
GetX
()
{
return
x_
;
};
double
GetY
()
{
return
y_
;
};
double
GetPhi
()
{
return
phi_
;
};
bool
operator
==
(
const
SmartPtr
<
Node3d
>
right
)
const
;
int
GetIndex
();
private:
double
x_
=
0.0
;
double
y_
=
0.0
;
double
phi_
=
0.0
;
std
::
size_t
x_grid_
=
0
;
std
::
size_t
y_grid_
=
0
;
std
::
size_t
phi_grid_
=
0
;
double
current_cost_
=
0.0
;
double
heuristic_cost_
=
0.0
;
SmartPtr
<
Node3d
>
pre_node
=
nullptr
;
double
steering_
=
0.0
;
// true for moving forward and false for moving backward
bool
direction_
=
true
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_OPEN_SPACE_NODE3D_H_
\ No newline at end of file
modules/planning/planner/open_space/hybrid_a_star.cc
浏览文件 @
c10d88ce
...
...
@@ -23,52 +23,23 @@
namespace
apollo
{
namespace
planning
{
Node
::
Node
(
double
x
,
double
y
,
double
phi
,
double
x_grid
,
double
y_grid
,
double
phi_grid
)
{
x_
=
x
;
y_
=
y
;
phi_
=
phi
;
x_grid_
=
x_grid
;
y_grid_
=
y_grid
;
phi_grid_
=
phi_grid
;
}
Box2d
Node
::
GetBoundingBox
(
const
common
::
VehicleParam
&
vehicle_param_
)
{
double
ego_length
=
vehicle_config_
.
vehicle_param
().
length
();
double
ego_width
=
vehicle_config_
.
vehicle_param
().
width
();
Box2d
ego_box
({
x_
,
y_
},
phi_
,
ego_length
,
ego_width
);
double
shift_distance
=
ego_length
/
2.0
-
vehicle_config
.
vehicle_param
().
back_edge_to_center
();
Vec2d
shift_vec
{
shift_distance
*
std
::
cos
(
phi_
),
shift_distance
*
std
::
sin
(
phi_
)};
ego_box
.
Shift
(
shift_vec
);
return
ego_box
;
}
bool
Node
::
operator
==
(
const
Node
*
right
)
const
{
return
x_grid_
==
right
->
GetGridX
()
&&
y_grid_
==
right
->
GetGridY
()
&&
phi_grid_
==
right
->
GetGridPhi
();
}
HybridAStar
::
HybridAStar
(
double
sx
,
double
sy
,
double
sphi
,
double
ex
,
double
ey
,
double
ephi
,
std
::
vector
<
const
Obstacle
*>
obstacles
,
double
xy_grid_resolution
,
double
phi_grid_resolution
)
{
start_node_
.
reset
(
new
Node
(
sx
,
sy
,
sphi
,
sx
/
xy_grid_resolution
,
start_node_
.
reset
(
new
Node
3d
(
sx
,
sy
,
sphi
,
sx
/
xy_grid_resolution
,
sy
/
xy_grid_resolution
,
sphi
/
phi_grid_resolution
));
end_node_
.
reset
(
new
Node
(
ex
,
ey
,
ephi
,
ex
/
xy_grid_resolution
,
end_node_
.
reset
(
new
Node
3d
(
ex
,
ey
,
ephi
,
ex
/
xy_grid_resolution
,
ey
/
xy_grid_resolution
,
ephi
/
phi_grid_resolution
));
obstacles_
=
obstacles
;
xy_grid_resolution_
=
xy_grid_resolution
;
phi_grid_resolution_
=
phi_grid_resolution
;
};
Status
HybridAStar
::
Plan
(){
}
};
Status
HybridAStar
::
Plan
()
{}
}
// namespace planning
}
// namespace apollo
\ No newline at end of file
modules/planning/planner/open_space/hybrid_a_star.h
浏览文件 @
c10d88ce
...
...
@@ -24,52 +24,32 @@
#include <map>
#include <queue>
#include <vector>
#include <memory>
#include "modules/common/log.h"
#include "modules/common/math/box2d.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/constraint_checker/collision_checker.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
math
::
Box2d
;
class
Node
{
public:
explicit
Node
(
double
x
,
double
y
,
double
phi
,
double
x_grid
,
double
y_grid
,
double
phi_grid
);
virtual
~
Node
()
=
default
;
Box2d
GetBoundingBox
(
const
common
::
VehicleParam
&
vehicle_param_
);
double
GetCost
()
{
return
current_cost_
+
heuristic_cost_
;
};
std
::
size_t
GetGridX
()
{
return
x_grid_
;
};
std
::
size_t
GetGridY
()
{
return
y_grid_
;
};
std
::
size_t
GetGridPhi
()
{
return
phi_grid_
;
};
double
GetX
()
{
return
x_
;
};
double
GetY
()
{
return
y_
;
};
double
GetPhi
()
{
return
phi_
;
};
bool
operator
==
(
const
Node
*
right
)
const
;
private:
double
x_
=
0.0
;
double
y_
=
0.0
;
double
phi_
=
0.0
;
std
::
size_t
x_grid_
=
0
;
std
::
size_t
y_grid_
=
0
;
std
::
size_t
phi_grid_
=
0
;
double
current_cost_
=
0.0
;
double
heuristic_cost_
=
0.0
;
Node
*
pre_node
=
nullptr
;
double
steering_
=
0.0
;
// true for moving forward and false for moving backward
bool
direction_
=
true
;
};
struct
OpenSpaceConf
{
// for Hybrid A Star Warm Start
double
xy_grid_resolution
;
double
phi_grid_resolution
;
double
obstacle_grid_resolution
;
double
max_x
;
double
max_y
;
double
max_phi
;
double
min_x
;
double
min_y
;
double
min_phi
;
}
class
HybridAStar
{
public:
explicit
HybridAStar
(
Node
start_node
,
Node
end_node
,
explicit
HybridAStar
(
Node
3d
start_node
,
Node3d
end_node
,
std
::
vector
<
const
Obstacle
*>
obstacles
,
double
xy_grid_resolution
,
double
phi_grid_resolution
,
double
obstacle_grid_resolution
);
...
...
@@ -91,14 +71,14 @@ class HybridAStar {
double
xy_grid_resolution_
;
double
phi_grid_resolution_
;
std
::
vector
<
const
Obstacle
*>
obstacles_
;
std
::
unique_ptr
<
Node
>
start_node_
;
std
::
unique_ptr
<
Node
>
end_node_
;
auto
cmp
=
[](
Node
*
left
,
Node
*
right
)
{
std
::
unique_ptr
<
Node
3d
>
start_node_
;
std
::
unique_ptr
<
Node
3d
>
end_node_
;
auto
cmp
=
[](
SmartPtr
<
Node3d
>
left
,
SmartPtr
<
Node3d
>
right
)
{
return
left
->
GetCost
()
<=
right
->
GetCost
();
};
std
::
priority_queue
<
Node
*
,
std
::
vector
<
Node
*
>
,
decltype
(
cmp
)
>
open_pq_
(
cmp
);
std
::
map
<
double
,
Node
*
>
open_set_
;
std
::
map
<
double
,
Node
*
>
close_set_
;
std
::
priority_queue
<
SmartPtr
<
Node3d
>
,
std
::
vector
<
SmartPtr
<
Node3d
>
>
,
decltype
(
cmp
)
>
open_pq_
(
cmp
);
std
::
map
<
double
,
SmartPtr
<
Node3d
>
>
open_set_
;
std
::
map
<
double
,
SmartPtr
<
Node3d
>
>
close_set_
;
};
}
// namespace planning
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录