Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
8b19a995
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,发现更多精彩内容 >>
提交
8b19a995
编写于
3月 20, 2019
作者:
P
panjiacheng
提交者:
Jiaming Tao
3月 21, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: refactored path_bounds_decider.
上级
9a5b0b18
变更
2
显示空白变更内容
内联
并排
Showing
2 changed file
with
124 addition
and
102 deletion
+124
-102
modules/planning/tasks/deciders/path_bounds_decider.cc
modules/planning/tasks/deciders/path_bounds_decider.cc
+64
-54
modules/planning/tasks/deciders/path_bounds_decider.h
modules/planning/tasks/deciders/path_bounds_decider.h
+60
-48
未找到文件。
modules/planning/tasks/deciders/path_bounds_decider.cc
浏览文件 @
8b19a995
...
...
@@ -61,6 +61,9 @@ Status PathBoundsDecider::Process(
CHECK_NOTNULL
(
frame
);
CHECK_NOTNULL
(
reference_line_info
);
// Initialize.
InitPathBoundsDecider
(
*
frame
,
*
reference_line_info
);
// The decided path bounds should be in the format of: (s, l_min, l_max).
PathBoundary
fallback_path_boundaries
;
PathBoundary
path_boundaries
;
...
...
@@ -92,8 +95,9 @@ Status PathBoundsDecider::Process(
}
// Generate path boundaries.
std
::
string
path_bounds_msg
=
GenerateRegularPathBoundary
(
frame
,
reference_line_info
,
&
path_boundaries
);
std
::
string
path_bounds_msg
=
GenerateRegularPathBoundary
(
*
frame
,
*
reference_line_info
,
LaneBorrowInfo
::
NO_BORROW
,
&
path_boundaries
);
if
(
path_bounds_msg
!=
""
)
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
path_bounds_msg
);
}
...
...
@@ -122,44 +126,73 @@ Status PathBoundsDecider::Process(
return
Status
::
OK
();
}
std
::
string
PathBoundsDecider
::
GenerateRegularPathBoundary
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
)
{
// Sanity checks.
CHECK_NOTNULL
(
frame
);
CHECK_NOTNULL
(
reference_line_info
);
void
PathBoundsDecider
::
InitPathBoundsDecider
(
const
Frame
&
frame
,
const
ReferenceLineInfo
&
reference_line_info
)
{
const
ReferenceLine
&
reference_line
=
reference_line_info
.
reference_line
();
const
common
::
TrajectoryPoint
&
planning_start_point
=
frame
.
PlanningStartPoint
();
// Reset variables.
blocking_obstacle_id_
=
""
;
adc_frenet_s_
=
0.0
;
adc_frenet_l_
=
0.0
;
adc_lane_width_
=
0.0
;
// Initialize some private variables.
// ADC s/l info.
auto
adc_frenet_position
=
reference_line
.
GetFrenetPoint
(
planning_start_point
.
path_point
());
adc_frenet_s_
=
adc_frenet_position
.
s
();
adc_frenet_l_
=
adc_frenet_position
.
l
();
auto
adc_sl_info
=
reference_line
.
ToFrenetFrame
(
planning_start_point
);
adc_frenet_sd_
=
adc_sl_info
.
first
[
1
];
adc_frenet_ld_
=
adc_sl_info
.
second
[
1
]
*
adc_frenet_sd_
;
// ADC's lane width.
double
lane_left_width
=
0.0
;
double
lane_right_width
=
0.0
;
if
(
!
reference_line
.
GetLaneWidth
(
adc_frenet_s_
,
&
lane_left_width
,
&
lane_right_width
))
{
AWARN
<<
"Failed to get lane width at planning start point."
;
adc_lane_width_
=
kDefaultLaneWidth
;
}
else
{
adc_lane_width_
=
lane_left_width
+
lane_right_width
;
}
}
std
::
string
PathBoundsDecider
::
GenerateRegularPathBoundary
(
const
Frame
&
frame
,
const
ReferenceLineInfo
&
reference_line_info
,
const
LaneBorrowInfo
lane_borrow_info
,
PathBoundary
*
const
path_boundary
)
{
// 1. Initialize the path boundaries to be an indefinitely large area.
if
(
!
InitPathBoundary
(
reference_line_info
->
reference_line
(),
frame
->
PlanningStartPoint
(),
path_boundaries
))
{
if
(
!
InitPathBoundary
(
reference_line_info
.
reference_line
(),
path_boundary
))
{
const
std
::
string
msg
=
"Failed to initialize path boundaries."
;
AERROR
<<
msg
;
return
msg
;
}
// PathBoundsDebugString(*path_boundar
ies
);
// PathBoundsDebugString(*path_boundar
y
);
// 2. Decide a rough boundary based on road info and ADC's position
if
(
!
GetBoundaryFromLanesAndADC
(
reference_line_info
->
reference_line
(),
0
,
0.1
,
path_boundaries
))
{
if
(
!
GetBoundaryFromLanesAndADC
(
reference_line_info
.
reference_line
()
,
0
,
0.1
,
path_boundary
))
{
const
std
::
string
msg
=
"Failed to decide a rough boundary based on "
"road information."
;
AERROR
<<
msg
;
return
msg
;
}
// PathBoundsDebugString(*path_boundar
ies
);
// PathBoundsDebugString(*path_boundar
y
);
// 3. Fine-tune the boundary based on static obstacles
// TODO(all): in the future, add side-pass functionality.
if
(
!
GetBoundaryFromStaticObstacles
(
reference_line_info
->
path_decision
(),
path_boundaries
))
{
if
(
!
GetBoundaryFromStaticObstacles
(
reference_line_info
.
path_decision
(),
path_boundary
))
{
const
std
::
string
msg
=
"Failed to decide fine tune the boundaries after "
"taking into consideration all static obstacles."
;
AERROR
<<
msg
;
return
msg
;
}
// PathBoundsDebugString(*path_boundar
ies
);
// PathBoundsDebugString(*path_boundar
y
);
// 4. Adjust the boundary considering dynamic obstacles
// TODO(all): may need to implement this in the future.
...
...
@@ -170,14 +203,14 @@ std::string PathBoundsDecider::GenerateRegularPathBoundary(
std
::
string
PathBoundsDecider
::
GenerateFallbackPathBoundary
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>
*
const
path_boundaries
)
{
PathBoundary
*
const
path_boundaries
)
{
// Sanity checks.
CHECK_NOTNULL
(
frame
);
CHECK_NOTNULL
(
reference_line_info
);
// 1. Initialize the path boundaries to be an indefinitely large area.
if
(
!
InitPathBoundary
(
reference_line_info
->
reference_line
(),
frame
->
PlanningStartPoint
(),
path_boundaries
))
{
path_boundaries
))
{
const
std
::
string
msg
=
"Failed to initialize fallback path boundaries."
;
AERROR
<<
msg
;
return
msg
;
...
...
@@ -200,38 +233,10 @@ std::string PathBoundsDecider::GenerateFallbackPathBoundary(
}
bool
PathBoundsDecider
::
InitPathBoundary
(
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
planning_start_point
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
)
{
const
ReferenceLine
&
reference_line
,
PathBoundary
*
const
path_boundary
)
{
// Sanity checks.
CHECK_NOTNULL
(
path_boundaries
);
path_boundaries
->
clear
();
// Reset variables.
blocking_obstacle_id_
=
""
;
adc_frenet_s_
=
0.0
;
adc_frenet_l_
=
0.0
;
adc_lane_width_
=
0.0
;
// Initialize some private variables.
// ADC s/l info.
auto
adc_frenet_position
=
reference_line
.
GetFrenetPoint
(
planning_start_point
.
path_point
());
adc_frenet_s_
=
adc_frenet_position
.
s
();
adc_frenet_l_
=
adc_frenet_position
.
l
();
auto
adc_sl_info
=
reference_line
.
ToFrenetFrame
(
planning_start_point
);
adc_frenet_sd_
=
adc_sl_info
.
first
[
1
];
adc_frenet_ld_
=
adc_sl_info
.
second
[
1
]
*
adc_frenet_sd_
;
// ADC's lane width.
double
lane_left_width
=
0.0
;
double
lane_right_width
=
0.0
;
if
(
!
reference_line
.
GetLaneWidth
(
adc_frenet_s_
,
&
lane_left_width
,
&
lane_right_width
))
{
AWARN
<<
"Failed to get lane width at planning start point."
;
adc_lane_width_
=
kDefaultLaneWidth
;
}
else
{
adc_lane_width_
=
lane_left_width
+
lane_right_width
;
}
CHECK_NOTNULL
(
path_boundary
);
path_boundary
->
clear
();
// Starting from ADC's current position, increment until the horizon, and
// set lateral bounds to be infinite at every spot.
...
...
@@ -239,10 +244,15 @@ bool PathBoundsDecider::InitPathBoundary(
curr_s
<
std
::
min
(
adc_frenet_s_
+
kPathBoundsDeciderHorizon
,
reference_line
.
Length
());
curr_s
+=
kPathBoundsDeciderResolution
)
{
path_boundar
ies
->
emplace_back
(
curr_s
,
std
::
numeric_limits
<
double
>::
lowest
(),
path_boundar
y
->
emplace_back
(
curr_s
,
std
::
numeric_limits
<
double
>::
lowest
(),
std
::
numeric_limits
<
double
>::
max
());
}
// return.
if
(
path_boundary
->
empty
())
{
ADEBUG
<<
"Empty path boundary in InitPathBoundary"
;
return
false
;
}
return
true
;
}
...
...
@@ -374,10 +384,10 @@ bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
// obstacles whose headings differ from road-headings a lot.
// TODO(all): (future work) this can be improved in the future.
bool
PathBoundsDecider
::
GetBoundaryFromStaticObstacles
(
PathDecision
*
const
path_decision
,
const
PathDecision
&
path_decision
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
)
{
// Preprocessing.
auto
indexed_obstacles
=
path_decision
->
obstacles
();
auto
indexed_obstacles
=
path_decision
.
obstacles
();
auto
sorted_obstacles
=
SortObstaclesForSweepLine
(
indexed_obstacles
);
double
center_line
=
adc_frenet_l_
;
size_t
obs_idx
=
0
;
...
...
modules/planning/tasks/deciders/path_bounds_decider.h
浏览文件 @
8b19a995
...
...
@@ -36,14 +36,21 @@ namespace planning {
class
PathBoundsDecider
:
public
Decider
{
public:
enum
class
LaneBorrowInfo
{
LEFT_BORROW
,
NO_BORROW
,
RIGHT_BORROW
,
};
explicit
PathBoundsDecider
(
const
TaskConfig
&
config
);
private:
common
::
Status
Process
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
)
override
;
/**
* @brief: The regular path boundary generation considers the ADC itself
void
InitPathBoundsDecider
(
const
Frame
&
frame
,
const
ReferenceLineInfo
&
reference_line_info
);
/** @brief: The regular path boundary generation considers the ADC itself
* and other static environments:
* - ADC's position (lane-changing considerations)
* - lane info
...
...
@@ -52,15 +59,16 @@ class PathBoundsDecider : public Decider {
* care of by the path planning.
* @param: frame
* @param: reference_line_info
* @param: lane_borrow_info: which lane to borrow.
* @param: The generated regular path_boundary, if there is one.
* @return: A failure message. If succeeded, return "" (empty string).
*/
std
::
string
GenerateRegularPathBoundary
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
);
const
Frame
&
frame
,
const
ReferenceLineInfo
&
reference_line_info
,
const
LaneBorrowInfo
lane_borrow_info
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundary
);
/**
* @brief: The fallback path only considers:
/** @brief: The fallback path only considers:
* - ADC's position (so that boundary must contain ADC's position)
* - lane info
* It is supposed to be the last resort in case regular path generation
...
...
@@ -80,18 +88,23 @@ class PathBoundsDecider : public Decider {
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
);
bool
InitPathBoundary
(
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
planning_start_point
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundar
ies
);
/** @brief: Initializes an empty path boundary.
*/
bool
InitPathBoundary
(
const
ReferenceLine
&
reference_line
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundar
y
);
/** @brief: Refine the boundary based on lane-info and ADC's location.
* It will comply to the lane-boundary. However, if the ADC itself
* is out of the given lane(s), it will adjust the boundary
* accordingly to include ADC's current position.
*/
bool
GetBoundaryFromLanesAndADC
(
const
ReferenceLine
&
reference_line
,
int
lane_borrowing
,
double
ADC_buffer
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
);
bool
GetBoundaryFromStaticObstacles
(
PathDecision
*
const
path_decision
,
const
PathDecision
&
path_decision
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
>>*
const
path_boundaries
);
bool
GetLaneInfoFromPoint
(
double
point_x
,
double
point_y
,
double
point_z
,
...
...
@@ -118,8 +131,7 @@ class PathBoundsDecider : public Decider {
const
std
::
vector
<
std
::
tuple
<
int
,
double
,
double
,
double
,
std
::
string
>>&
new_entering_obstacles
);
/**
* @brief Update the path_boundary at "idx", as well as the new center-line.
/** @brief Update the path_boundary at "idx", as well as the new center-line.
* It also checks if ADC is blocked (lmax < lmin).
* @param The current index of the path_bounds
* @param The minimum left boundary (l_max)
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录