Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
946d1bd8
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,发现更多精彩内容 >>
提交
946d1bd8
编写于
2月 22, 2018
作者:
K
kechxu
提交者:
Yajia Zhang
2月 22, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: add sampling for neighbor points
上级
af9c589c
变更
9
显示空白变更内容
内联
并排
Showing
9 changed file
with
122 addition
and
37 deletion
+122
-37
modules/planning/lattice/behavior_decider/behavior_decider.cc
...les/planning/lattice/behavior_decider/behavior_decider.cc
+52
-26
modules/planning/lattice/behavior_decider/behavior_decider.h
modules/planning/lattice/behavior_decider/behavior_decider.h
+14
-0
modules/planning/lattice/behavior_decider/condition_filter.cc
...les/planning/lattice/behavior_decider/condition_filter.cc
+3
-3
modules/planning/lattice/behavior_decider/condition_filter.h
modules/planning/lattice/behavior_decider/condition_filter.h
+3
-3
modules/planning/lattice/trajectory_generator/end_condition_sampler.cc
...ing/lattice/trajectory_generator/end_condition_sampler.cc
+14
-0
modules/planning/lattice/trajectory_generator/end_condition_sampler.h
...ning/lattice/trajectory_generator/end_condition_sampler.h
+4
-0
modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc
...ng/lattice/trajectory_generator/trajectory1d_generator.cc
+25
-3
modules/planning/lattice/trajectory_generator/trajectory1d_generator.h
...ing/lattice/trajectory_generator/trajectory1d_generator.h
+4
-0
modules/planning/proto/lattice_structure.proto
modules/planning/proto/lattice_structure.proto
+3
-2
未找到文件。
modules/planning/lattice/behavior_decider/behavior_decider.cc
浏览文件 @
946d1bd8
...
...
@@ -47,14 +47,14 @@ PlanningTarget BehaviorDecider::Analyze(
CHECK
(
frame
!=
nullptr
);
CHECK_GT
(
discretized_reference_line
.
size
(),
0
);
PlanningTarget
r
et
;
PlanningTarget
planning_targ
et
;
if
(
ScenarioManager
::
instance
()
->
ComputeWorldDecision
(
frame
,
reference_line_info
,
&
r
et
)
!=
0
)
{
frame
,
reference_line_info
,
&
planning_targ
et
)
!=
0
)
{
AERROR
<<
"ComputeWorldDecision error!"
;
}
for
(
const
auto
&
reference_point
:
discretized_reference_line
)
{
r
et
.
mutable_discretized_reference_line
()
planning_targ
et
.
mutable_discretized_reference_line
()
->
add_discretized_reference_line_point
()
->
CopyFrom
(
reference_point
);
}
...
...
@@ -64,14 +64,56 @@ PlanningTarget BehaviorDecider::Analyze(
FLAGS_planning_upper_speed_limit
,
path_time_neighborhood_
);
// AddSampleBounds(condition_filter, &planning_target);
AddNeighborPoints
(
condition_filter
,
&
planning_target
);
return
planning_target
;
}
void
BehaviorDecider
::
AddSampleBounds
(
const
ConditionFilter
&
condition_filter
,
PlanningTarget
*
const
planning_target
)
{
std
::
vector
<
SampleBound
>
sample_bounds
=
condition_filter
.
QuerySampleBounds
();
for
(
const
auto
&
sample_bound
:
sample_bounds
)
{
planning_target
->
add_sample_bound
()
->
CopyFrom
(
sample_bound
);
}
if
(
sample_bounds
.
empty
())
{
ADEBUG
<<
"Sample_bounds empty"
;
}
else
{
for
(
const
SampleBound
&
sample_bound
:
sample_bounds
)
{
ADEBUG
<<
"Sample_bound: "
<<
sample_bound
.
ShortDebugString
();
}
}
}
void
BehaviorDecider
::
AddNeighborPoints
(
const
ConditionFilter
&
condition_filter
,
PlanningTarget
*
const
planning_target
)
{
std
::
vector
<
SamplePoint
>
neighbor_points
=
condition_filter
.
QueryNeighborPoints
();
for
(
const
auto
&
neighbor_point
:
neighbor_points
)
{
planning_target
->
add_neighbor_point
()
->
CopyFrom
(
neighbor_point
);
}
if
(
neighbor_points
.
empty
())
{
ADEBUG
<<
"Sample_bounds empty"
;
}
else
{
for
(
const
SamplePoint
&
neighbor_point
:
neighbor_points
)
{
ADEBUG
<<
"Neighbor point: "
<<
neighbor_point
.
ShortDebugString
();
}
}
}
static
int
decision_cycles
=
0
;
if
(
FLAGS_enable_lattice_st_image_dump
)
{
void
BehaviorDecider
::
DumpLatticeImage
(
const
int
index
,
const
common
::
TrajectoryPoint
&
init_planning_point
,
const
ConditionFilter
&
condition_filter
,
ReferenceLineInfo
*
const
reference_line_info
)
{
apollo
::
planning_internal
::
LatticeStTraining
st_data
;
double
timestamp
=
init_planning_point
.
relative_time
();
std
::
string
st_img_name
=
"DecisionCycle_"
+
std
::
to_string
(
decision_cycles
)
+
"_"
+
std
::
to_string
(
index
)
+
"_"
+
std
::
to_string
(
timestamp
);
if
(
condition_filter
.
GenerateLatticeStPixels
(
&
st_data
,
timestamp
,
st_img_name
))
{
...
...
@@ -83,22 +125,6 @@ PlanningTarget BehaviorDecider::Analyze(
ptr_debug
->
mutable_planning_data
()
->
mutable_lattice_st_image
()
->
CopyFrom
(
st_data
);
}
}
decision_cycles
+=
1
;
// Debug SampleBound
if
(
sample_bounds
.
empty
())
{
ADEBUG
<<
"Sample_bounds empty"
;
}
else
{
for
(
const
SampleBound
&
sample_bound
:
sample_bounds
)
{
ADEBUG
<<
"Sample_bound: "
<<
sample_bound
.
ShortDebugString
();
}
}
for
(
const
auto
&
sample_bound
:
sample_bounds
)
{
ret
.
add_sample_bound
()
->
CopyFrom
(
sample_bound
);
}
return
ret
;
}
}
// namespace planning
...
...
modules/planning/lattice/behavior_decider/behavior_decider.h
浏览文件 @
946d1bd8
...
...
@@ -29,6 +29,7 @@
#include "modules/planning/common/frame.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/lattice/behavior_decider/condition_filter.h"
#include "modules/planning/lattice/behavior_decider/path_time_graph.h"
#include "modules/planning/proto/lattice_structure.pb.h"
#include "modules/planning/proto/planning.pb.h"
...
...
@@ -51,6 +52,19 @@ class BehaviorDecider {
const
std
::
vector
<
common
::
PathPoint
>&
discretized_reference_line
);
private:
void
AddSampleBounds
(
const
ConditionFilter
&
condition_filter
,
PlanningTarget
*
const
plannint_target
);
void
AddNeighborPoints
(
const
ConditionFilter
&
condition_filter
,
PlanningTarget
*
const
plannint_target
);
void
DumpLatticeImage
(
const
int
index
,
const
common
::
TrajectoryPoint
&
init_planning_point
,
const
ConditionFilter
&
condition_filter
,
ReferenceLineInfo
*
const
reference_line_info
);
std
::
shared_ptr
<
PathTimeGraph
>
path_time_neighborhood_
;
};
...
...
modules/planning/lattice/behavior_decider/condition_filter.cc
浏览文件 @
946d1bd8
...
...
@@ -159,7 +159,7 @@ PathTimePointPair ConditionFilter::QueryPathTimeObstacleIntervals(
return
block_interval
;
}
std
::
vector
<
SamplePoint
>
ConditionFilter
::
Query
Sample
Points
()
const
{
std
::
vector
<
SamplePoint
>
ConditionFilter
::
Query
Neighbor
Points
()
const
{
std
::
vector
<
SamplePoint
>
sample_points
;
for
(
const
auto
&
path_time_obstacle
:
ptr_path_time_neighborhood_
->
GetPathTimeObstacles
())
{
...
...
@@ -250,7 +250,7 @@ std::vector<double> ConditionFilter::UniformTimeStamps(
// Compute pixel img for lattice st
bool
ConditionFilter
::
GenerateLatticeStPixels
(
apollo
::
planning_internal
::
LatticeStTraining
*
st_data
,
double
timestamp
,
std
::
string
st_img_name
)
{
std
::
string
st_img_name
)
const
{
int
num_rows
=
250
;
int
num_cols
=
160
;
double
s_step
=
100.0
/
static_cast
<
double
>
(
num_rows
);
...
...
@@ -297,7 +297,7 @@ bool ConditionFilter::GenerateLatticeStPixels(
return
true
;
}
bool
ConditionFilter
::
WithinObstacleSt
(
double
s
,
double
t
)
{
bool
ConditionFilter
::
WithinObstacleSt
(
double
s
,
double
t
)
const
{
const
auto
&
path_time_obstacles
=
ptr_path_time_neighborhood_
->
GetPathTimeObstacles
();
...
...
modules/planning/lattice/behavior_decider/condition_filter.h
浏览文件 @
946d1bd8
...
...
@@ -45,16 +45,16 @@ class ConditionFilter {
std
::
vector
<
SampleBound
>
QuerySampleBounds
()
const
;
std
::
vector
<
SamplePoint
>
Query
Sample
Points
()
const
;
std
::
vector
<
SamplePoint
>
Query
Neighbor
Points
()
const
;
std
::
vector
<
std
::
pair
<
PathTimePoint
,
PathTimePoint
>>
QueryPathTimeObstacleIntervals
(
const
double
t
)
const
;
bool
GenerateLatticeStPixels
(
apollo
::
planning_internal
::
LatticeStTraining
*
st_data
,
double
timestamp
,
std
::
string
st_img_name
);
std
::
string
st_img_name
)
const
;
bool
WithinObstacleSt
(
double
s
,
double
t
);
bool
WithinObstacleSt
(
double
s
,
double
t
)
const
;
private:
// Return true only if t is within the range of time slot,
...
...
modules/planning/lattice/trajectory_generator/end_condition_sampler.cc
浏览文件 @
946d1bd8
...
...
@@ -147,5 +147,19 @@ EndConditionSampler::SampleLonEndConditionsForPathTimeBounds(
return
end_s_conditions
;
}
std
::
vector
<
std
::
pair
<
std
::
array
<
double
,
3
>
,
double
>>
EndConditionSampler
::
SampleLonEndConditionsForNeighborPoints
(
const
PlanningTarget
&
planning_target
)
const
{
std
::
vector
<
std
::
pair
<
std
::
array
<
double
,
3
>
,
double
>>
end_s_conditions
;
for
(
const
SamplePoint
&
neighbor_point
:
planning_target
.
neighbor_point
())
{
double
s
=
neighbor_point
.
path_time_point
().
s
();
double
v
=
neighbor_point
.
v
();
std
::
array
<
double
,
3
>
end_state
=
{
s
,
v
,
0.0
};
end_s_conditions
.
push_back
({
end_state
,
neighbor_point
.
path_time_point
().
t
()});
}
return
end_s_conditions
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/lattice/trajectory_generator/end_condition_sampler.h
浏览文件 @
946d1bd8
...
...
@@ -54,6 +54,10 @@ class EndConditionSampler {
SampleLonEndConditionsForPathTimeBounds
(
const
PlanningTarget
&
planning_target
)
const
;
std
::
vector
<
std
::
pair
<
std
::
array
<
double
,
3
>
,
double
>>
SampleLonEndConditionsForNeighborPoints
(
const
PlanningTarget
&
planning_target
)
const
;
private:
std
::
array
<
double
,
3
>
init_s_
;
...
...
modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc
浏览文件 @
946d1bd8
...
...
@@ -123,6 +123,26 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForPathTimeBound(
}
}
void
Trajectory1dGenerator
::
GenerateSpeedProfilesForNeighborPoints
(
const
PlanningTarget
&
planning_target
,
std
::
vector
<
std
::
shared_ptr
<
Curve1d
>>*
ptr_lon_trajectory_bundle
)
const
{
std
::
vector
<
std
::
pair
<
std
::
array
<
double
,
3
>
,
double
>>
end_conditions
=
end_condition_sampler_
->
SampleLonEndConditionsForNeighborPoints
(
planning_target
);
for
(
const
auto
&
end_condition
:
end_conditions
)
{
std
::
shared_ptr
<
LatticeTrajectory1d
>
lattice_traj_ptr
(
new
LatticeTrajectory1d
(
std
::
shared_ptr
<
Curve1d
>
(
new
QuinticPolynomialCurve1d
(
init_lon_state_
,
end_condition
.
first
,
end_condition
.
second
))));
lattice_traj_ptr
->
set_target_position
(
end_condition
.
first
[
0
]);
lattice_traj_ptr
->
set_target_velocity
(
end_condition
.
first
[
1
]);
lattice_traj_ptr
->
set_target_time
(
end_condition
.
second
);
ptr_lon_trajectory_bundle
->
push_back
(
lattice_traj_ptr
);
}
}
void
Trajectory1dGenerator
::
GenerateLongitudinalTrajectoryBundle
(
const
PlanningTarget
&
planning_target
,
std
::
vector
<
std
::
shared_ptr
<
Curve1d
>>*
ptr_lon_trajectory_bundle
)
const
{
...
...
@@ -130,8 +150,10 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
GenerateSpeedProfilesForCruising
(
planning_target
.
cruise_speed
(),
ptr_lon_trajectory_bundle
);
//
GenerateSpeedProfilesForPathTimeBound
(
planning_target
,
// GenerateSpeedProfilesForPathTimeBound(planning_target,
// ptr_lon_trajectory_bundle);
GenerateSpeedProfilesForNeighborPoints
(
planning_target
,
ptr_lon_trajectory_bundle
);
if
(
planning_target
.
has_stop_point
())
{
...
...
modules/planning/lattice/trajectory_generator/trajectory1d_generator.h
浏览文件 @
946d1bd8
...
...
@@ -59,6 +59,10 @@ class Trajectory1dGenerator {
const
PlanningTarget
&
planning_target
,
std
::
vector
<
std
::
shared_ptr
<
Curve1d
>>*
ptr_lon_trajectory_bundle
)
const
;
void
GenerateSpeedProfilesForNeighborPoints
(
const
PlanningTarget
&
planning_target
,
std
::
vector
<
std
::
shared_ptr
<
Curve1d
>>*
ptr_lon_trajectory_bundle
)
const
;
void
GenerateLongitudinalTrajectoryBundle
(
const
PlanningTarget
&
planning_target
,
std
::
vector
<
std
::
shared_ptr
<
Curve1d
>>*
ptr_lon_trajectory_bundle
)
const
;
...
...
modules/planning/proto/lattice_structure.proto
浏览文件 @
946d1bd8
...
...
@@ -46,6 +46,7 @@ message SampleBound {
message
PlanningTarget
{
optional
DiscretizedReferenceLine
discretized_reference_line
=
1
;
repeated
SampleBound
sample_bound
=
2
;
optional
double
stop_point
=
3
;
optional
double
cruise_speed
=
4
;
repeated
SamplePoint
neighbor_point
=
3
;
optional
double
stop_point
=
4
;
optional
double
cruise_speed
=
5
;
}
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录