Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
9794e711
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,发现更多精彩内容 >>
提交
9794e711
编写于
8月 04, 2017
作者:
D
Dong Li
提交者:
lianglia-apollo
8月 04, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
change dp_road_graph to two steps
上级
7184e43d
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
42 addition
and
74 deletion
+42
-74
modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc
...planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc
+7
-2
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
+16
-26
modules/planning/optimizer/dp_poly_path/dp_road_graph.h
modules/planning/optimizer/dp_poly_path/dp_road_graph.h
+16
-15
modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc
...les/planning/optimizer/dp_poly_path/dp_road_graph_test.cc
+1
-2
modules/planning/optimizer/dp_poly_path/trajectory_cost.cc
modules/planning/optimizer/dp_poly_path/trajectory_cost.cc
+1
-24
modules/planning/optimizer/dp_poly_path/trajectory_cost.h
modules/planning/optimizer/dp_poly_path/trajectory_cost.h
+1
-5
未找到文件。
modules/planning/optimizer/dp_poly_path/dp_poly_path_optimizer.cc
浏览文件 @
9794e711
...
...
@@ -57,9 +57,14 @@ Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
CHECK_NOTNULL
(
decision_data
);
CHECK_NOTNULL
(
path_data
);
DPRoadGraph
dp_road_graph
(
config_
,
init_point
,
speed_data
);
if
(
!
dp_road_graph
.
FindPathTunnel
(
reference_line
,
decision_data
,
path_data
))
{
if
(
!
dp_road_graph
.
FindPathTunnel
(
reference_line
,
path_data
))
{
AERROR
<<
"Failed to find tunnel in road graph"
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"dp_road_graph failed"
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"dp_road_graph path generation"
);
}
if
(
!
dp_road_graph
.
ComputeObjectdecision
(
*
path_data
,
speed_data
,
reference_line
,
decision_data
))
{
AERROR
<<
"Failed to make decision based on tunnel"
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"dp_road_graph decision "
);
}
return
Status
::
OK
();
}
...
...
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
浏览文件 @
9794e711
...
...
@@ -51,16 +51,14 @@ DPRoadGraph::DPRoadGraph(const DpPolyPathConfig &config,
:
config_
(
config
),
init_point_
(
init_point
),
speed_data_
(
speed_data
)
{}
bool
DPRoadGraph
::
FindPathTunnel
(
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
,
PathData
*
const
path_data
)
{
CHECK_NOTNULL
(
decision_data
);
CHECK_NOTNULL
(
path_data
);
if
(
!
Init
(
reference_line
))
{
AERROR
<<
"Fail to init dp road graph!"
;
return
false
;
}
std
::
vector
<
DPRoadGraphNode
>
min_cost_path
;
if
(
!
Generate
(
reference_line
,
decision_data
,
&
min_cost_path
))
{
if
(
!
Generate
(
reference_line
,
&
min_cost_path
))
{
AERROR
<<
"Fail to generate graph!"
;
return
false
;
}
...
...
@@ -130,20 +128,13 @@ bool DPRoadGraph::FindPathTunnel(const ReferenceLine &reference_line,
}
path_data
->
set_discretized_path
(
path_points
);
// compute decision data
if
(
compute_object_decision_from_path
(
*
path_data
,
speed_data_
,
reference_line
,
decision_data
))
{
AINFO
<<
"Computing decision_data in dp path success"
;
}
else
{
AINFO
<<
"Computing decision_data in dp path fail"
;
}
return
true
;
}
bool
DPRoadGraph
::
Init
(
const
ReferenceLine
&
reference_line
)
{
if
(
!
reference_line
.
get_point_in_frenet_frame
({
init_point_
.
path_point
().
x
(),
init_point_
.
path_point
().
y
()},
&
init_sl_point_
))
{
if
(
!
reference_line
.
get_point_in_frenet_frame
(
{
init_point_
.
path_point
().
x
(),
init_point_
.
path_point
().
y
()},
&
init_sl_point_
))
{
AERROR
<<
"Fail to map init point from cartesian to sl coordinate!"
;
return
false
;
}
...
...
@@ -171,7 +162,6 @@ bool DPRoadGraph::Init(const ReferenceLine &reference_line) {
}
bool
DPRoadGraph
::
Generate
(
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
,
std
::
vector
<
DPRoadGraphNode
>
*
min_cost_path
)
{
if
(
!
min_cost_path
)
{
AERROR
<<
"the provided min_cost_path is null"
;
...
...
@@ -182,13 +172,13 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
AERROR
<<
"Fail to sampling point with path sampler!"
;
return
false
;
}
path_waypoints
.
insert
(
path_waypoints
.
begin
(),
std
::
vector
<
common
::
SLPoint
>
{
init_sl_point_
});
path_waypoints
.
insert
(
path_waypoints
.
begin
(),
std
::
vector
<
common
::
SLPoint
>
{
init_sl_point_
});
const
auto
&
vehicle_config_
=
common
::
VehicleConfigHelper
::
instance
()
->
GetConfig
();
TrajectoryCost
trajectory_cost
(
config_
,
reference_line
,
vehicle_config_
.
vehicle_param
(),
speed_data_
,
*
decision_data
);
vehicle_config_
.
vehicle_param
(),
speed_data_
);
std
::
vector
<
std
::
vector
<
DPRoadGraphNode
>>
graph_nodes
(
path_waypoints
.
size
());
graph_nodes
[
0
].
push_back
(
DPRoadGraphNode
(
init_sl_point_
,
nullptr
,
0.0
));
...
...
@@ -201,7 +191,7 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
for
(
std
::
size_t
i
=
0
;
i
<
level_points
.
size
();
++
i
)
{
const
auto
cur_sl_point
=
level_points
[
i
];
graph_nodes
[
level
].
push_back
(
DPRoadGraphNode
(
cur_sl_point
,
nullptr
));
auto
&
cur_node
=
graph_nodes
[
level
].
back
();
auto
&
cur_node
=
graph_nodes
[
level
].
back
();
for
(
std
::
size_t
j
=
0
;
j
<
prev_dp_nodes
.
size
();
++
j
)
{
const
auto
*
prev_dp_node
=
&
prev_dp_nodes
[
j
];
const
auto
prev_sl_point
=
prev_dp_node
->
sl_point
;
...
...
@@ -233,9 +223,10 @@ bool DPRoadGraph::Generate(const ReferenceLine &reference_line,
return
true
;
}
bool
DPRoadGraph
::
compute_object_decision_from_path
(
const
PathData
&
path_data
,
const
SpeedData
&
heuristic_speed_data
,
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
)
{
bool
DPRoadGraph
::
ComputeObjectdecision
(
const
PathData
&
path_data
,
const
SpeedData
&
heuristic_speed_data
,
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
)
{
CHECK_NOTNULL
(
decision_data
);
// Compute static obstacle decision
...
...
@@ -428,7 +419,6 @@ bool DPRoadGraph::SamplePathWaypoints(
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
init_point
,
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
*
const
points
)
{
CHECK
(
points
!=
nullptr
);
common
::
math
::
Vec2d
init_cartesian_point
(
init_point
.
path_point
().
x
(),
...
...
modules/planning/optimizer/dp_poly_path/dp_road_graph.h
浏览文件 @
9794e711
...
...
@@ -47,8 +47,13 @@ class DPRoadGraph {
~
DPRoadGraph
()
=
default
;
bool
FindPathTunnel
(
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
,
PathData
*
const
path_data
);
bool
ComputeObjectdecision
(
const
PathData
&
path_data
,
const
SpeedData
&
heuristic_speed_data
,
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
);
private:
/**
* an private inner struct for the dp algorithm
...
...
@@ -57,11 +62,12 @@ class DPRoadGraph {
public:
DPRoadGraphNode
()
=
default
;
DPRoadGraphNode
(
const
common
::
SLPoint
point_sl
,
const
DPRoadGraphNode
*
node_prev
)
DPRoadGraphNode
(
const
common
::
SLPoint
point_sl
,
const
DPRoadGraphNode
*
node_prev
)
:
sl_point
(
point_sl
),
min_cost_prev_node
(
node_prev
)
{}
DPRoadGraphNode
(
const
common
::
SLPoint
point_sl
,
const
DPRoadGraphNode
*
node_prev
,
const
double
cost
)
DPRoadGraphNode
(
const
common
::
SLPoint
point_sl
,
const
DPRoadGraphNode
*
node_prev
,
const
double
cost
)
:
sl_point
(
point_sl
),
min_cost_prev_node
(
node_prev
),
min_cost
(
cost
)
{}
void
UpdateCost
(
const
DPRoadGraphNode
*
node_prev
,
...
...
@@ -82,21 +88,16 @@ class DPRoadGraph {
bool
Init
(
const
ReferenceLine
&
reference_line
);
bool
Generate
(
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
,
std
::
vector
<
DPRoadGraphNode
>
*
min_cost_path
);
bool
compute_object_decision_from_path
(
const
PathData
&
path_data
,
const
SpeedData
&
heuristic_speed_data
,
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
);
bool
fill_ego_by_time
(
const
FrenetFramePath
&
frenet_frame_path
,
const
ReferenceLine
&
reference_line
,
const
SpeedData
&
heuristic_speed_data
,
int
evaluate_times
,
std
::
vector
<::
apollo
::
common
::
math
::
Box2d
>
*
ego_by_time
);
bool
SamplePathWaypoints
(
const
ReferenceLine
&
reference_line
,
bool
SamplePathWaypoints
(
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
init_point
,
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
*
const
points
);
...
...
modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc
浏览文件 @
9794e711
...
...
@@ -85,8 +85,7 @@ class DpRoadGraphTest : public PlanningTestBase {
TEST_F
(
DpRoadGraphTest
,
speed_road_graph
)
{
DPRoadGraph
road_graph
(
dp_poly_path_config_
,
init_point_
,
speed_data_
);
ASSERT_TRUE
(
reference_line_
!=
nullptr
);
bool
result
=
road_graph
.
FindPathTunnel
(
*
reference_line_
,
&
decision_data_
,
&
path_data_
);
bool
result
=
road_graph
.
FindPathTunnel
(
*
reference_line_
,
&
path_data_
);
EXPECT_TRUE
(
result
);
EXPECT_EQ
(
648
,
path_data_
.
discretized_path
().
num_of_points
());
EXPECT_EQ
(
648
,
path_data_
.
frenet_frame_path
().
number_of_points
());
...
...
modules/planning/optimizer/dp_poly_path/trajectory_cost.cc
浏览文件 @
9794e711
...
...
@@ -37,8 +37,7 @@ using TrajectoryPoint = ::apollo::common::TrajectoryPoint;
TrajectoryCost
::
TrajectoryCost
(
const
DpPolyPathConfig
&
config
,
const
ReferenceLine
&
reference_line
,
const
common
::
VehicleParam
&
vehicle_param
,
const
SpeedData
&
heuristic_speed_data
,
const
DecisionData
&
decision_data
)
const
SpeedData
&
heuristic_speed_data
)
:
config_
(
config
),
reference_line_
(
&
reference_line
),
vehicle_param_
(
vehicle_param
),
...
...
@@ -47,28 +46,6 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
std
::
min
(
heuristic_speed_data_
.
total_time
(),
FLAGS_prediction_total_time
);
evaluate_times_
=
static_cast
<
uint32_t
>
(
std
::
floor
(
total_time
/
config
.
eval_time_interval
()));
// Mapping Static obstacle
for
(
const
auto
ptr_static_obstacle
:
decision_data
.
StaticObstacles
())
{
static_obstacle_boxes_
.
push_back
(
ptr_static_obstacle
->
PerceptionBoundingBox
());
}
// Mapping dynamic obstacle
for
(
const
auto
ptr_dynamic_obstacle
:
decision_data
.
DynamicObstacles
())
{
const
auto
&
trajectory
=
ptr_dynamic_obstacle
->
Trajectory
();
std
::
vector
<
Box2d
>
obstacle_by_time
;
for
(
std
::
size_t
time
=
0
;
time
<
evaluate_times_
+
1
;
++
time
)
{
TrajectoryPoint
traj_point
=
ptr_dynamic_obstacle
->
GetPointAtTime
(
time
*
config
.
eval_time_interval
());
obstacle_by_time
.
push_back
(
ptr_dynamic_obstacle
->
GetBoundingBox
(
traj_point
));
}
dynamic_obstacle_trajectory_
.
push_back
(
std
::
move
(
obstacle_by_time
));
dynamic_obstacle_probability_
.
push_back
(
trajectory
.
probability
());
}
CHECK
(
dynamic_obstacle_trajectory_
.
size
()
==
dynamic_obstacle_probability_
.
size
());
}
double
TrajectoryCost
::
calculate
(
const
QuinticPolynomialCurve1d
&
curve
,
...
...
modules/planning/optimizer/dp_poly_path/trajectory_cost.h
浏览文件 @
9794e711
...
...
@@ -41,8 +41,7 @@ class TrajectoryCost {
explicit
TrajectoryCost
(
const
DpPolyPathConfig
&
config
,
const
ReferenceLine
&
reference_line
,
const
common
::
VehicleParam
&
vehicle_param
,
const
SpeedData
&
heuristic_speed_data
,
const
DecisionData
&
decision_data
);
const
SpeedData
&
heuristic_speed_data
);
double
calculate
(
const
QuinticPolynomialCurve1d
&
curve
,
const
double
start_s
,
const
double
end_s
)
const
;
...
...
@@ -51,9 +50,6 @@ class TrajectoryCost {
const
ReferenceLine
*
reference_line_
=
nullptr
;
const
common
::
VehicleParam
vehicle_param_
;
SpeedData
heuristic_speed_data_
;
std
::
vector
<::
apollo
::
common
::
math
::
Box2d
>
static_obstacle_boxes_
;
std
::
vector
<
std
::
vector
<::
apollo
::
common
::
math
::
Box2d
>>
dynamic_obstacle_trajectory_
;
std
::
vector
<
double
>
dynamic_obstacle_probability_
;
uint32_t
evaluate_times_
;
};
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录