Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
c86e9897
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,发现更多精彩内容 >>
提交
c86e9897
编写于
2月 08, 2018
作者:
Y
YajiaZhang
提交者:
Jiangtao Hu
2月 08, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: code clean up for lattice planner
上级
7e10cbde
变更
8
隐藏空白更改
内联
并排
Showing
8 changed file
with
32 addition
and
39 deletion
+32
-39
modules/planning/common/planning_gflags.cc
modules/planning/common/planning_gflags.cc
+1
-5
modules/planning/common/planning_gflags.h
modules/planning/common/planning_gflags.h
+0
-2
modules/planning/constraint_checker/collision_checker.cc
modules/planning/constraint_checker/collision_checker.cc
+3
-6
modules/planning/lattice/behavior_decider/path_time_neighborhood.cc
...anning/lattice/behavior_decider/path_time_neighborhood.cc
+4
-2
modules/planning/lattice/trajectory_generator/end_condition_sampler.cc
...ing/lattice/trajectory_generator/end_condition_sampler.cc
+1
-1
modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc
...ning/lattice/trajectory_generator/trajectory_evaluator.cc
+19
-21
modules/planning/lattice/trajectory_generator/trajectory_evaluator.h
...nning/lattice/trajectory_generator/trajectory_evaluator.h
+3
-1
modules/planning/planner/lattice/lattice_planner.cc
modules/planning/planner/lattice/lattice_planner.cc
+1
-1
未找到文件。
modules/planning/common/planning_gflags.cc
浏览文件 @
c86e9897
...
...
@@ -381,11 +381,10 @@ DEFINE_int32(num_lattice_traj_to_plot, 5,
DEFINE_double
(
default_cruise_speed
,
5.0
,
"default cruise speed"
);
DEFINE_double
(
spiral_downsample_curvature_thred
,
0.02
,
"curvature threshold for down sampling reference line points"
);
DEFINE_bool
(
enable_sample_bound_planning
,
true
,
"enable SampleBound based motion planning"
);
DEFINE_bool
(
enable_lattice_st_image_dump
,
false
,
"enable sending the lattice st image"
);
DEFINE_bool
(
enable_auto_tuning
,
false
,
"enable auto tuning data emission"
);
DEFINE_double
(
trajectory_time_resolution
,
0.1
,
"Trajectory time resolution in planning"
);
DEFINE_double
(
trajectory_space_resolution
,
1.0
,
...
...
@@ -395,9 +394,6 @@ DEFINE_double(collision_buffer_expansion_ratio, 0.2,
"to expand in collision checking"
);
DEFINE_double
(
decision_horizon
,
200.0
,
"Longitudinal horizon for decision making"
);
DEFINE_double
(
lateral_obstacle_ignore_thred
,
2.0
,
"Lateral threshold for "
"obstacles to be considered in path-time graph"
);
DEFINE_bool
(
enable_backup_trajectory
,
false
,
"If generate backup trajectory when planning fail"
);
DEFINE_double
(
backup_trajectory_cost
,
1000.0
,
...
...
modules/planning/common/planning_gflags.h
浏览文件 @
c86e9897
...
...
@@ -198,7 +198,6 @@ DECLARE_double(lattice_epsilon);
DECLARE_int32
(
num_lattice_traj_to_plot
);
DECLARE_double
(
default_cruise_speed
);
DECLARE_double
(
spiral_downsample_curvature_thred
);
DECLARE_bool
(
enable_sample_bound_planning
);
DECLARE_bool
(
enable_lattice_st_image_dump
);
DECLARE_bool
(
enable_auto_tuning
);
DECLARE_double
(
trajectory_time_resolution
);
...
...
@@ -206,7 +205,6 @@ DECLARE_double(trajectory_space_resolution);
DECLARE_double
(
lateral_acceleration_bound
);
DECLARE_double
(
collision_buffer_expansion_ratio
);
DECLARE_double
(
decision_horizon
);
DECLARE_double
(
lateral_obstacle_ignore_thred
);
DECLARE_bool
(
enable_backup_trajectory
);
DECLARE_double
(
backup_trajectory_cost
);
...
...
modules/planning/constraint_checker/collision_checker.cc
浏览文件 @
c86e9897
...
...
@@ -106,23 +106,20 @@ void CollisionChecker::BuildPredictedEnv(
bool
CollisionChecker
::
IgnoreObstaclesBehind
(
const
std
::
array
<
double
,
3
>&
adc_init_d
)
{
bool
ignore
=
false
;
if
(
std
::
abs
(
adc_init_d
[
0
])
<
FLAGS_lateral_obstacle_ignore_thred
)
{
ignore
=
true
;
}
return
ignore
;
return
std
::
abs
(
adc_init_d
[
0
])
<
FLAGS_default_reference_line_width
*
0.5
;
}
bool
CollisionChecker
::
IsBehind
(
const
Obstacle
*
obstacle
,
const
std
::
array
<
double
,
3
>&
adc_init_s
,
const
std
::
vector
<
PathPoint
>&
discretized_reference_line
)
{
double
half_lane_width
=
FLAGS_default_reference_line_width
*
0.5
;
TrajectoryPoint
point
=
obstacle
->
GetPointAtTime
(
0.0
);
std
::
pair
<
double
,
double
>
sl_point
=
ReferenceLineFrameConverter
::
CartesianToFrenet
(
discretized_reference_line
,
point
.
path_point
().
x
(),
point
.
path_point
().
y
());
if
(
sl_point
.
first
<
adc_init_s
[
0
]
&&
std
::
abs
(
sl_point
.
second
)
<
FLAGS_lateral_obstacle_ignore_thred
)
{
std
::
abs
(
sl_point
.
second
)
<
half_lane_width
)
{
ADEBUG
<<
"Ignore obstacle ["
<<
obstacle
->
Id
()
<<
"]"
;
return
true
;
}
...
...
modules/planning/lattice/behavior_decider/path_time_neighborhood.cc
浏览文件 @
c86e9897
...
...
@@ -112,6 +112,8 @@ SLBoundary PathTimeNeighborhood::ComputeObstacleBoundary(
void
PathTimeNeighborhood
::
SetupObstacles
(
const
std
::
vector
<
const
Obstacle
*>&
obstacles
,
const
std
::
vector
<
PathPoint
>&
discretized_ref_points
)
{
double
half_lane_width
=
FLAGS_default_reference_line_width
*
0.5
;
for
(
const
Obstacle
*
obstacle
:
obstacles
)
{
if
(
prediction_traj_map_
.
find
(
obstacle
->
Id
())
==
prediction_traj_map_
.
end
())
{
...
...
@@ -135,8 +137,8 @@ void PathTimeNeighborhood::SetupObstacles(
// the obstacle is not shown on the region to be considered.
if
(
sl_boundary
.
end_s
()
<
path_range_
.
first
||
sl_boundary
.
start_s
()
>
path_range_
.
second
||
(
sl_boundary
.
start_l
()
>
FLAGS_lateral_obstacle_ignore_thred
&&
sl_boundary
.
end_l
()
<
-
FLAGS_lateral_obstacle_ignore_thred
))
{
(
sl_boundary
.
start_l
()
>
half_lane_width
&&
sl_boundary
.
end_l
()
<
-
half_lane_width
))
{
if
(
path_time_obstacle_map_
.
find
(
obstacle
->
Id
())
!=
path_time_obstacle_map_
.
end
())
{
break
;
...
...
modules/planning/lattice/trajectory_generator/end_condition_sampler.cc
浏览文件 @
c86e9897
...
...
@@ -42,7 +42,7 @@ std::vector<std::pair<std::array<double, 3>, double>>
EndConditionSampler
::
SampleLatEndConditions
()
const
{
std
::
vector
<
std
::
pair
<
std
::
array
<
double
,
3
>
,
double
>>
end_d_conditions
;
std
::
array
<
double
,
5
>
end_d_candidates
=
{
0.0
,
-
0.25
,
0.25
,
-
0.5
,
0.5
};
std
::
array
<
double
,
4
>
end_s_candidates
=
{
5.0
,
10.0
,
20.0
,
3
0.0
};
std
::
array
<
double
,
4
>
end_s_candidates
=
{
10.0
,
20.0
,
30.0
,
4
0.0
};
for
(
const
auto
&
s
:
end_s_candidates
)
{
for
(
const
auto
&
d
:
end_d_candidates
)
{
...
...
modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc
浏览文件 @
c86e9897
...
...
@@ -42,6 +42,12 @@ TrajectoryEvaluator::TrajectoryEvaluator(
std
::
shared_ptr
<
PathTimeNeighborhood
>
pathtime_neighborhood
)
:
is_auto_tuning_
(
is_auto_tuning
),
pathtime_neighborhood_
(
pathtime_neighborhood
)
{
double
start_time
=
0.0
;
double
end_time
=
FLAGS_trajectory_time_length
;
path_time_intervals_
=
pathtime_neighborhood_
->
GetPathBlockingIntervals
(
start_time
,
end_time
,
FLAGS_trajectory_time_resolution
);
for
(
const
auto
lon_trajectory
:
lon_trajectories
)
{
if
(
!
ConstraintChecker1d
::
IsValidLongitudinalTrajectory
(
*
lon_trajectory
))
{
continue
;
...
...
@@ -53,7 +59,7 @@ TrajectoryEvaluator::TrajectoryEvaluator(
}
if
(
!
is_auto_tuning_
)
{
double
cost
=
Evaluate
(
planning_target
,
lon_trajectory
,
lat_trajectory
,
nullptr
);
Evaluate
(
planning_target
,
lon_trajectory
,
lat_trajectory
);
cost_queue_
.
push
(
PairCost
({
lon_trajectory
,
lat_trajectory
},
cost
));
}
else
{
std
::
vector
<
double
>
cost_components
;
...
...
@@ -143,7 +149,7 @@ double TrajectoryEvaluator::Evaluate(
double
lat_offset_cost
=
LatOffsetCost
(
lat_trajectory
,
s_values
);
double
lat_comfort_cost
=
LatComfortCost
(
lon_trajectory
,
lat_trajectory
);
if
(
cost_components
)
{
if
(
cost_components
!=
nullptr
)
{
cost_components
->
push_back
(
lon_travel_cost
);
cost_components
->
push_back
(
lon_jerk_cost
);
cost_components
->
push_back
(
lon_collision_cost
);
...
...
@@ -236,35 +242,27 @@ double TrajectoryEvaluator::LonObjectiveCost(
// while constructing trajectory evaluator
double
TrajectoryEvaluator
::
LonCollisionCost
(
const
std
::
shared_ptr
<
Trajectory1d
>&
lon_trajectory
)
const
{
double
start_time
=
0.0
;
double
end_time
=
FLAGS_trajectory_time_length
;
const
auto
&
pt_intervals
=
pathtime_neighborhood_
->
GetPathBlockingIntervals
(
start_time
,
end_time
,
FLAGS_trajectory_time_resolution
);
double
cost_sqr_sum
=
0.0
;
double
cost_abs_sum
=
0.0
;
for
(
std
::
size_t
i
=
0
;
i
<
p
t_intervals
.
size
();
++
i
)
{
const
auto
&
pt_interval
=
p
t_intervals
[
i
];
for
(
std
::
size_t
i
=
0
;
i
<
p
ath_time_intervals_
.
size
();
++
i
)
{
const
auto
&
pt_interval
=
p
ath_time_intervals_
[
i
];
if
(
pt_interval
.
empty
())
{
continue
;
}
double
t
=
start_time
+
i
*
FLAGS_trajectory_time_resolution
;
double
t
=
i
*
FLAGS_trajectory_time_resolution
;
double
traj_s
=
lon_trajectory
->
Evaluate
(
0
,
t
);
double
sigma
=
FLAGS_lon_collision_cost_std
;
for
(
const
auto
&
m
:
pt_interval
)
{
double
cost
=
0.0
;
if
(
traj_s
>
m
.
first
-
FLAGS_lon_collision_yield_buffer
&&
traj_s
<
m
.
second
+
FLAGS_lon_collision_overtake_buffer
)
{
cost
=
1.0
;
}
else
if
(
traj_s
<
m
.
first
)
{
double
dist
=
(
m
.
first
-
FLAGS_lon_collision_yield_buffer
)
-
traj_s
;
cost
=
std
::
exp
(
-
dist
*
dist
/
(
2.0
*
sigma
*
sigma
));
}
else
if
(
traj_s
>
m
.
second
)
{
double
dist
=
traj_s
-
(
m
.
second
+
FLAGS_lon_collision_overtake_buffer
);
cost
=
std
::
exp
(
-
dist
*
dist
/
(
2.0
*
sigma
*
sigma
));
double
dist
=
0.0
;
if
(
traj_s
<
m
.
first
-
FLAGS_lon_collision_yield_buffer
)
{
dist
=
m
.
first
-
FLAGS_lon_collision_yield_buffer
-
traj_s
;
}
else
if
(
traj_s
>
m
.
second
+
FLAGS_lon_collision_overtake_buffer
)
{
dist
=
traj_s
-
m
.
second
-
FLAGS_lon_collision_overtake_buffer
;
}
double
cost
=
std
::
exp
(
-
dist
*
dist
/
(
2.0
*
sigma
*
sigma
));
cost_sqr_sum
+=
cost
*
cost
;
cost_abs_sum
+=
std
::
abs
(
cost
)
;
cost_abs_sum
+=
cost
;
}
}
return
cost_sqr_sum
/
(
cost_abs_sum
+
FLAGS_lattice_epsilon
);
...
...
modules/planning/lattice/trajectory_generator/trajectory_evaluator.h
浏览文件 @
c86e9897
...
...
@@ -77,7 +77,7 @@ class TrajectoryEvaluator {
double
Evaluate
(
const
PlanningTarget
&
planning_target
,
const
std
::
shared_ptr
<
Curve1d
>&
lon_trajectory
,
const
std
::
shared_ptr
<
Curve1d
>&
lat_trajectory
,
std
::
vector
<
double
>*
cost_components
)
const
;
std
::
vector
<
double
>*
cost_components
=
nullptr
)
const
;
double
LatOffsetCost
(
const
std
::
shared_ptr
<
Curve1d
>&
lat_trajectory
,
const
std
::
vector
<
double
>&
s_values
)
const
;
...
...
@@ -119,6 +119,8 @@ class TrajectoryEvaluator {
bool
is_auto_tuning_
=
false
;
std
::
shared_ptr
<
PathTimeNeighborhood
>
pathtime_neighborhood_
;
std
::
vector
<
std
::
vector
<
std
::
pair
<
double
,
double
>>>
path_time_intervals_
;
};
}
// namespace planning
...
...
modules/planning/planner/lattice/lattice_planner.cc
浏览文件 @
c86e9897
...
...
@@ -26,7 +26,6 @@
#include <utility>
#include <vector>
#include "modules/planning/lattice/trajectory_generator/backup_trajectory_generator.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/macro.h"
...
...
@@ -35,6 +34,7 @@
#include "modules/planning/constraint_checker/collision_checker.h"
#include "modules/planning/constraint_checker/constraint_checker.h"
#include "modules/planning/lattice/behavior_decider/path_time_neighborhood.h"
#include "modules/planning/lattice/trajectory_generator/backup_trajectory_generator.h"
#include "modules/planning/lattice/trajectory_generator/trajectory1d_generator.h"
#include "modules/planning/lattice/trajectory_generator/trajectory_combiner.h"
#include "modules/planning/lattice/trajectory_generator/trajectory_evaluator.h"
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录