Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
9f1b8658
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,发现更多精彩内容 >>
提交
9f1b8658
编写于
2月 08, 2019
作者:
Y
Yajia Zhang
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: renamed STBoundary; minor code refactor
上级
27606d24
变更
32
隐藏空白更改
内联
并排
Showing
32 changed file
with
204 addition
and
197 deletion
+204
-197
modules/planning/common/obstacle.cc
modules/planning/common/obstacle.cc
+11
-11
modules/planning/common/obstacle.h
modules/planning/common/obstacle.h
+9
-9
modules/planning/common/path_decision.cc
modules/planning/common/path_decision.cc
+1
-1
modules/planning/common/path_decision.h
modules/planning/common/path_decision.h
+1
-1
modules/planning/common/speed/BUILD
modules/planning/common/speed/BUILD
+1
-0
modules/planning/common/speed/st_boundary.cc
modules/planning/common/speed/st_boundary.cc
+41
-40
modules/planning/common/speed/st_boundary.h
modules/planning/common/speed/st_boundary.h
+22
-18
modules/planning/common/speed/st_boundary_test.cc
modules/planning/common/speed/st_boundary_test.cc
+6
-6
modules/planning/lattice/behavior/path_time_graph.cc
modules/planning/lattice/behavior/path_time_graph.cc
+17
-16
modules/planning/lattice/behavior/path_time_graph.h
modules/planning/lattice/behavior/path_time_graph.h
+4
-4
modules/planning/tasks/deciders/decider_creep.cc
modules/planning/tasks/deciders/decider_creep.cc
+2
-2
modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc
modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc
+1
-1
modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc
modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc
+3
-3
modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc
...planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc
+2
-2
modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc
...ing/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc
+2
-2
modules/planning/tasks/optimizers/path_decider/path_decider.cc
...es/planning/tasks/optimizers/path_decider/path_decider.cc
+1
-1
modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc
...tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc
+6
-6
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc
...ks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc
+10
-10
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h
...sks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h
+4
-4
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc
...tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc
+14
-14
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h
.../tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h
+5
-5
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
...mizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
+1
-1
modules/planning/tasks/optimizers/speed_decider/speed_decider.cc
.../planning/tasks/optimizers/speed_decider/speed_decider.cc
+10
-10
modules/planning/tasks/optimizers/speed_decider/speed_decider.h
...s/planning/tasks/optimizers/speed_decider/speed_decider.h
+3
-3
modules/planning/tasks/optimizers/speed_optimizer.cc
modules/planning/tasks/optimizers/speed_optimizer.cc
+6
-6
modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc
.../planning/tasks/optimizers/st_graph/st_boundary_mapper.cc
+9
-9
modules/planning/tasks/optimizers/st_graph/st_graph_data.cc
modules/planning/tasks/optimizers/st_graph/st_graph_data.cc
+2
-2
modules/planning/tasks/optimizers/st_graph/st_graph_data.h
modules/planning/tasks/optimizers/st_graph/st_graph_data.h
+3
-3
modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc
.../planning/tasks/optimizers/st_graph/st_graph_data_test.cc
+2
-2
modules/planning/traffic_rules/keep_clear.cc
modules/planning/traffic_rules/keep_clear.cc
+1
-1
modules/planning/tuning/autotuning_raw_feature_generator.cc
modules/planning/tuning/autotuning_raw_feature_generator.cc
+2
-2
modules/planning/tuning/autotuning_raw_feature_generator.h
modules/planning/tuning/autotuning_raw_feature_generator.h
+2
-2
未找到文件。
modules/planning/common/obstacle.cc
浏览文件 @
9f1b8658
...
...
@@ -330,7 +330,7 @@ void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
STPoint
(
end_s
-
adc_start_s
,
0.0
));
point_pairs
.
emplace_back
(
STPoint
(
start_s
-
adc_start_s
,
FLAGS_st_max_t
),
STPoint
(
end_s
-
adc_start_s
,
FLAGS_st_max_t
));
reference_line_st_boundary_
=
S
t
Boundary
(
point_pairs
);
reference_line_st_boundary_
=
S
T
Boundary
(
point_pairs
);
}
else
{
if
(
BuildTrajectoryStBoundary
(
reference_line
,
adc_start_s
,
&
reference_line_st_boundary_
))
{
...
...
@@ -347,7 +347,7 @@ void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
bool
Obstacle
::
BuildTrajectoryStBoundary
(
const
ReferenceLine
&
reference_line
,
const
double
adc_start_s
,
S
t
Boundary
*
const
st_boundary
)
{
S
T
Boundary
*
const
st_boundary
)
{
if
(
!
IsValidObstacle
(
perception_obstacle_
))
{
AERROR
<<
"Fail to build trajectory st boundary because object is not "
"valid. PerceptionObstacle: "
...
...
@@ -499,7 +499,7 @@ bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
});
polygon_points
.
erase
(
last
,
polygon_points
.
end
());
if
(
polygon_points
.
size
()
>
2
)
{
*
st_boundary
=
S
t
Boundary
(
polygon_points
);
*
st_boundary
=
S
T
Boundary
(
polygon_points
);
}
}
else
{
return
false
;
...
...
@@ -507,11 +507,11 @@ bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
return
true
;
}
const
S
t
Boundary
&
Obstacle
::
reference_line_st_boundary
()
const
{
const
S
T
Boundary
&
Obstacle
::
reference_line_st_boundary
()
const
{
return
reference_line_st_boundary_
;
}
const
S
t
Boundary
&
Obstacle
::
st_boundary
()
const
{
return
st_boundary_
;
}
const
S
T
Boundary
&
Obstacle
::
st_boundary
()
const
{
return
st_boundary_
;
}
const
std
::
vector
<
std
::
string
>&
Obstacle
::
decider_tags
()
const
{
return
decider_tags_
;
...
...
@@ -686,27 +686,27 @@ const SLBoundary& Obstacle::PerceptionSLBoundary() const {
return
sl_boundary_
;
}
void
Obstacle
::
SetStBoundary
(
const
S
t
Boundary
&
boundary
)
{
void
Obstacle
::
SetStBoundary
(
const
S
T
Boundary
&
boundary
)
{
st_boundary_
=
boundary
;
}
void
Obstacle
::
SetStBoundaryType
(
const
S
t
Boundary
::
BoundaryType
type
)
{
void
Obstacle
::
SetStBoundaryType
(
const
S
T
Boundary
::
BoundaryType
type
)
{
st_boundary_
.
SetBoundaryType
(
type
);
}
void
Obstacle
::
EraseStBoundary
()
{
st_boundary_
=
S
t
Boundary
();
}
void
Obstacle
::
EraseStBoundary
()
{
st_boundary_
=
S
T
Boundary
();
}
void
Obstacle
::
SetReferenceLineStBoundary
(
const
S
t
Boundary
&
boundary
)
{
void
Obstacle
::
SetReferenceLineStBoundary
(
const
S
T
Boundary
&
boundary
)
{
reference_line_st_boundary_
=
boundary
;
}
void
Obstacle
::
SetReferenceLineStBoundaryType
(
const
S
t
Boundary
::
BoundaryType
type
)
{
const
S
T
Boundary
::
BoundaryType
type
)
{
reference_line_st_boundary_
.
SetBoundaryType
(
type
);
}
void
Obstacle
::
EraseReferenceLineStBoundary
()
{
reference_line_st_boundary_
=
S
t
Boundary
();
reference_line_st_boundary_
=
S
T
Boundary
();
}
bool
Obstacle
::
IsValidObstacle
(
...
...
modules/planning/common/obstacle.h
浏览文件 @
9f1b8658
...
...
@@ -142,9 +142,9 @@ class Obstacle {
const
SLBoundary
&
PerceptionSLBoundary
()
const
;
const
S
t
Boundary
&
reference_line_st_boundary
()
const
;
const
S
T
Boundary
&
reference_line_st_boundary
()
const
;
const
S
t
Boundary
&
st_boundary
()
const
;
const
S
T
Boundary
&
st_boundary
()
const
;
const
std
::
vector
<
std
::
string
>&
decider_tags
()
const
;
...
...
@@ -157,15 +157,15 @@ class Obstacle {
const
ObjectDecisionType
&
decision
);
bool
HasLateralDecision
()
const
;
void
SetStBoundary
(
const
S
t
Boundary
&
boundary
);
void
SetStBoundary
(
const
S
T
Boundary
&
boundary
);
void
SetStBoundaryType
(
const
S
t
Boundary
::
BoundaryType
type
);
void
SetStBoundaryType
(
const
S
T
Boundary
::
BoundaryType
type
);
void
EraseStBoundary
();
void
SetReferenceLineStBoundary
(
const
S
t
Boundary
&
boundary
);
void
SetReferenceLineStBoundary
(
const
S
T
Boundary
&
boundary
);
void
SetReferenceLineStBoundaryType
(
const
S
t
Boundary
::
BoundaryType
type
);
void
SetReferenceLineStBoundaryType
(
const
S
T
Boundary
::
BoundaryType
type
);
void
EraseReferenceLineStBoundary
();
...
...
@@ -223,7 +223,7 @@ class Obstacle {
bool
BuildTrajectoryStBoundary
(
const
ReferenceLine
&
reference_line
,
const
double
adc_start_s
,
S
t
Boundary
*
const
st_boundary
);
S
T
Boundary
*
const
st_boundary
);
bool
IsValidObstacle
(
const
perception
::
PerceptionObstacle
&
perception_obstacle
);
...
...
@@ -243,8 +243,8 @@ class Obstacle {
std
::
vector
<
std
::
string
>
decider_tags_
;
SLBoundary
sl_boundary_
;
S
t
Boundary
reference_line_st_boundary_
;
S
t
Boundary
st_boundary_
;
S
T
Boundary
reference_line_st_boundary_
;
S
T
Boundary
st_boundary_
;
ObjectDecisionType
lateral_decision_
;
ObjectDecisionType
longitudinal_decision_
;
...
...
modules/planning/common/path_decision.cc
浏览文件 @
9f1b8658
...
...
@@ -41,7 +41,7 @@ const Obstacle *PathDecision::Find(const std::string &object_id) const {
}
void
PathDecision
::
SetStBoundary
(
const
std
::
string
&
id
,
const
S
t
Boundary
&
boundary
)
{
const
S
T
Boundary
&
boundary
)
{
auto
*
obstacle
=
obstacles_
.
Find
(
id
);
if
(
!
obstacle
)
{
...
...
modules/planning/common/path_decision.h
浏览文件 @
9f1b8658
...
...
@@ -54,7 +54,7 @@ class PathDecision {
Obstacle
*
Find
(
const
std
::
string
&
object_id
);
void
SetStBoundary
(
const
std
::
string
&
id
,
const
S
t
Boundary
&
boundary
);
void
SetStBoundary
(
const
std
::
string
&
id
,
const
S
T
Boundary
&
boundary
);
void
EraseStBoundaries
();
MainStop
main_stop
()
const
{
return
main_stop_
;
}
double
stop_reference_line_s
()
const
{
return
stop_reference_line_s_
;
}
...
...
modules/planning/common/speed/BUILD
浏览文件 @
9f1b8658
...
...
@@ -29,6 +29,7 @@ cc_library(
"//cyber/common:log"
,
"//modules/planning/proto:planning_proto"
,
"@gtest"
,
"//modules/planning/common:planning_gflags"
,
],
)
...
...
modules/planning/common/speed/st_boundary.cc
浏览文件 @
9f1b8658
...
...
@@ -22,6 +22,7 @@
#include "cyber/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/planning/common/planning_gflags.h"
namespace
apollo
{
namespace
planning
{
...
...
@@ -29,7 +30,7 @@ namespace planning {
using
common
::
math
::
LineSegment2d
;
using
common
::
math
::
Vec2d
;
S
tBoundary
::
St
Boundary
(
S
TBoundary
::
ST
Boundary
(
const
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>&
point_pairs
)
{
CHECK
(
IsValid
(
point_pairs
))
<<
"The input point_pairs are NOT valid"
;
...
...
@@ -62,12 +63,12 @@ StBoundary::StBoundary(
max_t_
=
lower_points_
.
back
().
t
();
}
bool
S
t
Boundary
::
IsPointNear
(
const
common
::
math
::
LineSegment2d
&
seg
,
bool
S
T
Boundary
::
IsPointNear
(
const
common
::
math
::
LineSegment2d
&
seg
,
const
Vec2d
&
point
,
const
double
max_dist
)
{
return
seg
.
DistanceSquareTo
(
point
)
<
max_dist
*
max_dist
;
}
std
::
string
S
t
Boundary
::
TypeName
(
BoundaryType
type
)
{
std
::
string
S
T
Boundary
::
TypeName
(
BoundaryType
type
)
{
if
(
type
==
BoundaryType
::
FOLLOW
)
{
return
"FOLLOW"
;
}
else
if
(
type
==
BoundaryType
::
KEEP_CLEAR
)
{
...
...
@@ -86,7 +87,7 @@ std::string StBoundary::TypeName(BoundaryType type) {
return
"UNKNOWN"
;
}
void
S
t
Boundary
::
RemoveRedundantPoints
(
void
S
T
Boundary
::
RemoveRedundantPoints
(
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>*
point_pairs
)
{
if
(
!
point_pairs
||
point_pairs
->
size
()
<=
2
)
{
return
;
...
...
@@ -114,7 +115,7 @@ void StBoundary::RemoveRedundantPoints(
point_pairs
->
resize
(
i
+
1
);
}
bool
S
t
Boundary
::
IsValid
(
bool
S
T
Boundary
::
IsValid
(
const
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>&
point_pairs
)
const
{
if
(
point_pairs
.
size
()
<
2
)
{
AERROR
<<
"point_pairs.size() must > 2. current point_pairs.size() = "
...
...
@@ -154,7 +155,7 @@ bool StBoundary::IsValid(
return
true
;
}
bool
S
t
Boundary
::
IsPointInBoundary
(
const
STPoint
&
st_point
)
const
{
bool
S
T
Boundary
::
IsPointInBoundary
(
const
STPoint
&
st_point
)
const
{
if
(
st_point
.
t
()
<=
min_t_
||
st_point
.
t
()
>=
max_t_
)
{
return
false
;
}
...
...
@@ -172,29 +173,29 @@ bool StBoundary::IsPointInBoundary(const STPoint& st_point) const {
return
(
check_upper
*
check_lower
<
0
);
}
STPoint
S
tBoundary
::
UpperLeftP
oint
()
const
{
STPoint
S
TBoundary
::
upper_left_p
oint
()
const
{
DCHECK
(
!
upper_points_
.
empty
())
<<
"StBoundary has zero points."
;
return
upper_points_
.
front
();
}
STPoint
S
tBoundary
::
UpperRightP
oint
()
const
{
STPoint
S
TBoundary
::
upper_right_p
oint
()
const
{
DCHECK
(
!
upper_points_
.
empty
())
<<
"StBoundary has zero points."
;
return
upper_points_
.
back
();
}
STPoint
S
tBoundary
::
BottomLeftP
oint
()
const
{
STPoint
S
TBoundary
::
bottom_left_p
oint
()
const
{
DCHECK
(
!
lower_points_
.
empty
())
<<
"StBoundary has zero points."
;
return
lower_points_
.
front
();
}
STPoint
S
tBoundary
::
BottomRightP
oint
()
const
{
STPoint
S
TBoundary
::
bottom_right_p
oint
()
const
{
DCHECK
(
!
lower_points_
.
empty
())
<<
"StBoundary has zero points."
;
return
lower_points_
.
back
();
}
S
tBoundary
St
Boundary
::
ExpandByS
(
const
double
s
)
const
{
S
TBoundary
ST
Boundary
::
ExpandByS
(
const
double
s
)
const
{
if
(
lower_points_
.
empty
())
{
return
S
t
Boundary
();
return
S
T
Boundary
();
}
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>
point_pairs
;
for
(
size_t
i
=
0
;
i
<
lower_points_
.
size
();
++
i
)
{
...
...
@@ -202,13 +203,13 @@ StBoundary StBoundary::ExpandByS(const double s) const {
STPoint
(
lower_points_
[
i
].
y
()
-
s
,
lower_points_
[
i
].
x
()),
STPoint
(
upper_points_
[
i
].
y
()
+
s
,
upper_points_
[
i
].
x
()));
}
return
S
t
Boundary
(
std
::
move
(
point_pairs
));
return
S
T
Boundary
(
std
::
move
(
point_pairs
));
}
S
tBoundary
St
Boundary
::
ExpandByT
(
const
double
t
)
const
{
S
TBoundary
ST
Boundary
::
ExpandByT
(
const
double
t
)
const
{
if
(
lower_points_
.
empty
())
{
AERROR
<<
"The current st_boundary has NO points."
;
return
S
t
Boundary
();
return
S
T
Boundary
();
}
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>
point_pairs
;
...
...
@@ -252,34 +253,34 @@ StBoundary StBoundary::ExpandByT(const double t) const {
std
::
fmax
(
point_pairs
.
back
().
second
.
s
(),
point_pairs
.
back
().
first
.
s
()
+
kMinSEpsilon
));
return
S
t
Boundary
(
std
::
move
(
point_pairs
));
return
S
T
Boundary
(
std
::
move
(
point_pairs
));
}
S
tBoundary
::
BoundaryType
St
Boundary
::
boundary_type
()
const
{
S
TBoundary
::
BoundaryType
ST
Boundary
::
boundary_type
()
const
{
return
boundary_type_
;
}
void
S
t
Boundary
::
SetBoundaryType
(
const
BoundaryType
&
boundary_type
)
{
void
S
T
Boundary
::
SetBoundaryType
(
const
BoundaryType
&
boundary_type
)
{
boundary_type_
=
boundary_type
;
}
const
std
::
string
&
S
t
Boundary
::
id
()
const
{
return
id_
;
}
const
std
::
string
&
S
T
Boundary
::
id
()
const
{
return
id_
;
}
void
S
t
Boundary
::
set_id
(
const
std
::
string
&
id
)
{
id_
=
id
;
}
void
S
T
Boundary
::
set_id
(
const
std
::
string
&
id
)
{
id_
=
id
;
}
double
S
t
Boundary
::
characteristic_length
()
const
{
double
S
T
Boundary
::
characteristic_length
()
const
{
return
characteristic_length_
;
}
void
S
t
Boundary
::
SetCharacteristicLength
(
const
double
characteristic_length
)
{
void
S
T
Boundary
::
SetCharacteristicLength
(
const
double
characteristic_length
)
{
characteristic_length_
=
characteristic_length
;
}
bool
S
t
Boundary
::
GetUnblockSRange
(
const
double
curr_time
,
double
*
s_upper
,
bool
S
T
Boundary
::
GetUnblockSRange
(
const
double
curr_time
,
double
*
s_upper
,
double
*
s_lower
)
const
{
CHECK_NOTNULL
(
s_upper
);
CHECK_NOTNULL
(
s_lower
);
*
s_upper
=
s_high_limit_
;
*
s_upper
=
FLAGS_decision_horizon
;
*
s_lower
=
0.0
;
if
(
curr_time
<
min_t_
||
curr_time
>
max_t_
)
{
return
true
;
...
...
@@ -315,7 +316,7 @@ bool StBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
return
true
;
}
bool
S
t
Boundary
::
GetBoundarySRange
(
const
double
curr_time
,
double
*
s_upper
,
bool
S
T
Boundary
::
GetBoundarySRange
(
const
double
curr_time
,
double
*
s_upper
,
double
*
s_lower
)
const
{
CHECK_NOTNULL
(
s_upper
);
CHECK_NOTNULL
(
s_lower
);
...
...
@@ -337,17 +338,17 @@ bool StBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
*
s_lower
=
lower_points_
[
left
].
s
()
+
r
*
(
lower_points_
[
right
].
s
()
-
lower_points_
[
left
].
s
());
*
s_upper
=
std
::
fmin
(
*
s_upper
,
s_high_limit_
);
*
s_upper
=
std
::
fmin
(
*
s_upper
,
FLAGS_decision_horizon
);
*
s_lower
=
std
::
fmax
(
*
s_lower
,
0.0
);
return
true
;
}
double
S
t
Boundary
::
min_s
()
const
{
return
min_s_
;
}
double
S
t
Boundary
::
min_t
()
const
{
return
min_t_
;
}
double
S
t
Boundary
::
max_s
()
const
{
return
max_s_
;
}
double
S
t
Boundary
::
max_t
()
const
{
return
max_t_
;
}
double
S
T
Boundary
::
min_s
()
const
{
return
min_s_
;
}
double
S
T
Boundary
::
min_t
()
const
{
return
min_t_
;
}
double
S
T
Boundary
::
max_s
()
const
{
return
max_s_
;
}
double
S
T
Boundary
::
max_t
()
const
{
return
max_t_
;
}
bool
S
t
Boundary
::
GetIndexRange
(
const
std
::
vector
<
STPoint
>&
points
,
bool
S
T
Boundary
::
GetIndexRange
(
const
std
::
vector
<
STPoint
>&
points
,
const
double
t
,
size_t
*
left
,
size_t
*
right
)
const
{
CHECK_NOTNULL
(
left
);
...
...
@@ -370,11 +371,11 @@ bool StBoundary::GetIndexRange(const std::vector<STPoint>& points,
return
true
;
}
S
tBoundary
StBoundary
::
GenerateStBoundary
(
S
TBoundary
STBoundary
::
CreateInstance
(
const
std
::
vector
<
STPoint
>&
lower_points
,
const
std
::
vector
<
STPoint
>&
upper_points
)
{
if
(
lower_points
.
size
()
!=
upper_points
.
size
()
||
lower_points
.
size
()
<
2
)
{
return
S
t
Boundary
();
return
S
T
Boundary
();
}
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>
point_pairs
;
...
...
@@ -383,10 +384,10 @@ StBoundary StBoundary::GenerateStBoundary(
STPoint
(
lower_points
.
at
(
i
).
s
(),
lower_points
.
at
(
i
).
t
()),
STPoint
(
upper_points
.
at
(
i
).
s
(),
upper_points
.
at
(
i
).
t
()));
}
return
S
t
Boundary
(
point_pairs
);
return
S
T
Boundary
(
point_pairs
);
}
S
tBoundary
St
Boundary
::
CutOffByT
(
const
double
t
)
const
{
S
TBoundary
ST
Boundary
::
CutOffByT
(
const
double
t
)
const
{
std
::
vector
<
STPoint
>
lower_points
;
std
::
vector
<
STPoint
>
upper_points
;
for
(
size_t
i
=
0
;
i
<
lower_points_
.
size
()
&&
i
<
upper_points_
.
size
();
...
...
@@ -397,22 +398,22 @@ StBoundary StBoundary::CutOffByT(const double t) const {
lower_points
.
push_back
(
lower_points_
[
i
]);
upper_points
.
push_back
(
upper_points_
[
i
]);
}
return
GenerateStBoundary
(
lower_points
,
upper_points
);
return
CreateInstance
(
lower_points
,
upper_points
);
}
void
S
t
Boundary
::
set_upper_left_point
(
STPoint
st_point
)
{
void
S
T
Boundary
::
set_upper_left_point
(
STPoint
st_point
)
{
upper_left_point_
=
std
::
move
(
st_point
);
}
void
S
t
Boundary
::
set_upper_right_point
(
STPoint
st_point
)
{
void
S
T
Boundary
::
set_upper_right_point
(
STPoint
st_point
)
{
upper_right_point_
=
std
::
move
(
st_point
);
}
void
S
t
Boundary
::
set_bottom_left_point
(
STPoint
st_point
)
{
void
S
T
Boundary
::
set_bottom_left_point
(
STPoint
st_point
)
{
bottom_left_point_
=
std
::
move
(
st_point
);
}
void
S
t
Boundary
::
set_bottom_right_point
(
STPoint
st_point
)
{
void
S
T
Boundary
::
set_bottom_right_point
(
STPoint
st_point
)
{
bottom_right_point_
=
std
::
move
(
st_point
);
}
...
...
modules/planning/common/speed/st_boundary.h
浏览文件 @
9f1b8658
...
...
@@ -21,6 +21,7 @@
#pragma once
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
...
...
@@ -37,34 +38,42 @@
namespace
apollo
{
namespace
planning
{
class
S
t
Boundary
:
public
common
::
math
::
Polygon2d
{
class
S
T
Boundary
:
public
common
::
math
::
Polygon2d
{
public:
S
t
Boundary
()
=
default
;
S
T
Boundary
()
=
default
;
explicit
S
t
Boundary
(
explicit
S
T
Boundary
(
const
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>&
point_pairs
);
explicit
StBoundary
(
const
common
::
math
::
Box2d
&
box
)
=
delete
;
explicit
StBoundary
(
std
::
vector
<
common
::
math
::
Vec2d
>
points
)
=
delete
;
explicit
STBoundary
(
const
common
::
math
::
Box2d
&
box
)
=
delete
;
~
StBoundary
()
=
default
;
explicit
STBoundary
(
std
::
vector
<
common
::
math
::
Vec2d
>
points
)
=
delete
;
static
STBoundary
CreateInstance
(
const
std
::
vector
<
STPoint
>&
lower_points
,
const
std
::
vector
<
STPoint
>&
upper_points
);
static
std
::
unique_ptr
<
STBoundary
>
CreateInstance
(
const
std
::
vector
<
std
::
pair
<
STPoint
,
STPoint
>>&
point_pairs
);
~
STBoundary
()
=
default
;
bool
IsEmpty
()
const
{
return
lower_points_
.
empty
();
}
bool
IsPointInBoundary
(
const
STPoint
&
st_point
)
const
;
STPoint
UpperLeftP
oint
()
const
;
STPoint
UpperRightP
oint
()
const
;
STPoint
upper_left_p
oint
()
const
;
STPoint
upper_right_p
oint
()
const
;
STPoint
BottomLeftP
oint
()
const
;
STPoint
BottomRightP
oint
()
const
;
STPoint
bottom_left_p
oint
()
const
;
STPoint
bottom_right_p
oint
()
const
;
void
set_upper_left_point
(
STPoint
st_point
);
void
set_upper_right_point
(
STPoint
st_point
);
void
set_bottom_left_point
(
STPoint
st_point
);
void
set_bottom_right_point
(
STPoint
st_point
);
S
t
Boundary
ExpandByS
(
const
double
s
)
const
;
S
t
Boundary
ExpandByT
(
const
double
t
)
const
;
S
T
Boundary
ExpandByS
(
const
double
s
)
const
;
S
T
Boundary
ExpandByT
(
const
double
t
)
const
;
// if you need to add boundary type, make sure you modify
// GetUnblockSRange accordingly.
...
...
@@ -103,11 +112,7 @@ class StBoundary : public common::math::Polygon2d {
std
::
vector
<
STPoint
>
upper_points
()
const
{
return
upper_points_
;
}
std
::
vector
<
STPoint
>
lower_points
()
const
{
return
lower_points_
;
}
static
StBoundary
GenerateStBoundary
(
const
std
::
vector
<
STPoint
>&
lower_points
,
const
std
::
vector
<
STPoint
>&
upper_points
);
StBoundary
CutOffByT
(
const
double
t
)
const
;
STBoundary
CutOffByT
(
const
double
t
)
const
;
private:
bool
IsValid
(
...
...
@@ -134,7 +139,6 @@ class StBoundary : public common::math::Polygon2d {
std
::
string
id_
;
double
characteristic_length_
=
1.0
;
double
s_high_limit_
=
200.0
;
double
min_s_
=
std
::
numeric_limits
<
double
>::
max
();
double
max_s_
=
std
::
numeric_limits
<
double
>::
lowest
();
double
min_t_
=
std
::
numeric_limits
<
double
>::
max
();
...
...
modules/planning/common/speed/st_boundary_test.cc
浏览文件 @
9f1b8658
...
...
@@ -38,10 +38,10 @@ TEST(StBoundaryTest, basic_test) {
point_pairs
.
emplace_back
(
lower_points
[
0
],
upper_points
[
0
]);
point_pairs
.
emplace_back
(
lower_points
[
1
],
upper_points
[
1
]);
S
t
Boundary
boundary
(
point_pairs
);
S
T
Boundary
boundary
(
point_pairs
);
EXPECT_EQ
(
boundary
.
id
(),
""
);
EXPECT_EQ
(
boundary
.
boundary_type
(),
S
t
Boundary
::
BoundaryType
::
UNKNOWN
);
EXPECT_EQ
(
boundary
.
boundary_type
(),
S
T
Boundary
::
BoundaryType
::
UNKNOWN
);
EXPECT_DOUBLE_EQ
(
0.0
,
boundary
.
min_s
());
EXPECT_DOUBLE_EQ
(
5.0
,
boundary
.
max_s
());
EXPECT_DOUBLE_EQ
(
0.0
,
boundary
.
min_t
());
...
...
@@ -62,9 +62,9 @@ TEST(StBoundaryTest, boundary_range) {
point_pairs
.
emplace_back
(
lower_points
[
0
],
upper_points
[
0
]);
point_pairs
.
emplace_back
(
lower_points
[
1
],
upper_points
[
1
]);
S
t
Boundary
boundary
(
point_pairs
);
S
T
Boundary
boundary
(
point_pairs
);
boundary
.
SetBoundaryType
(
S
t
Boundary
::
BoundaryType
::
YIELD
);
boundary
.
SetBoundaryType
(
S
T
Boundary
::
BoundaryType
::
YIELD
);
double
t
=
-
10.0
;
const
double
dt
=
0.01
;
while
(
t
<
10.0
)
{
...
...
@@ -103,7 +103,7 @@ TEST(StBoundaryTest, get_index_range) {
point_pairs
.
emplace_back
(
lower_points
[
0
],
upper_points
[
0
]);
point_pairs
.
emplace_back
(
lower_points
[
1
],
upper_points
[
1
]);
S
t
Boundary
boundary
(
point_pairs
);
S
T
Boundary
boundary
(
point_pairs
);
size_t
left
=
0
;
size_t
right
=
0
;
...
...
@@ -136,7 +136,7 @@ TEST(StBoundaryTest, remove_redundant_points) {
EXPECT_EQ
(
points
.
size
(),
5
);
S
t
Boundary
st_boundary
;
S
T
Boundary
st_boundary
;
st_boundary
.
RemoveRedundantPoints
(
&
points
);
EXPECT_EQ
(
points
.
size
(),
2
);
...
...
modules/planning/lattice/behavior/path_time_graph.cc
浏览文件 @
9f1b8658
...
...
@@ -203,13 +203,13 @@ STPoint PathTimeGraph::SetPathTimePoint(const std::string& obstacle_id,
return
path_time_point
;
}
const
std
::
vector
<
S
t
Boundary
>&
PathTimeGraph
::
GetPathTimeObstacles
()
const
std
::
vector
<
S
T
Boundary
>&
PathTimeGraph
::
GetPathTimeObstacles
()
const
{
return
path_time_obstacles_
;
}
bool
PathTimeGraph
::
GetPathTimeObstacle
(
const
std
::
string
&
obstacle_id
,
S
t
Boundary
*
path_time_obstacle
)
{
S
T
Boundary
*
path_time_obstacle
)
{
if
(
path_time_obstacle_map_
.
find
(
obstacle_id
)
==
path_time_obstacle_map_
.
end
())
{
return
false
;
...
...
@@ -226,13 +226,14 @@ std::vector<std::pair<double, double>> PathTimeGraph::GetPathBlockingIntervals(
if
(
t
>
pt_obstacle
.
max_t
()
||
t
<
pt_obstacle
.
min_t
())
{
continue
;
}
double
s_upper
=
lerp
(
pt_obstacle
.
UpperLeftP
oint
().
s
(),
pt_obstacle
.
UpperLeftPoint
().
t
(),
pt_obstacle
.
UpperRightP
oint
().
s
(),
pt_obstacle
.
UpperRightP
oint
().
t
(),
t
);
double
s_upper
=
lerp
(
pt_obstacle
.
upper_left_p
oint
().
s
(),
pt_obstacle
.
upper_left_point
().
t
(),
pt_obstacle
.
upper_right_p
oint
().
s
(),
pt_obstacle
.
upper_right_p
oint
().
t
(),
t
);
double
s_lower
=
lerp
(
pt_obstacle
.
BottomLeftPoint
().
s
(),
pt_obstacle
.
BottomLeftPoint
().
t
(),
pt_obstacle
.
BottomRightPoint
().
s
(),
pt_obstacle
.
BottomRightPoint
().
t
(),
t
);
double
s_lower
=
lerp
(
pt_obstacle
.
bottom_left_point
().
s
(),
pt_obstacle
.
bottom_left_point
().
t
(),
pt_obstacle
.
bottom_right_point
().
s
(),
pt_obstacle
.
bottom_right_point
().
t
(),
t
);
intervals
.
emplace_back
(
s_lower
,
s_upper
);
}
...
...
@@ -276,17 +277,17 @@ std::vector<STPoint> PathTimeGraph::GetObstacleSurroundingPoints(
double
t0
=
0.0
;
double
t1
=
0.0
;
if
(
s_dist
>
0.0
)
{
s0
=
pt_obstacle
.
UpperLeftP
oint
().
s
();
s1
=
pt_obstacle
.
UpperRightP
oint
().
s
();
s0
=
pt_obstacle
.
upper_left_p
oint
().
s
();
s1
=
pt_obstacle
.
upper_right_p
oint
().
s
();
t0
=
pt_obstacle
.
UpperLeftP
oint
().
t
();
t1
=
pt_obstacle
.
UpperRightP
oint
().
t
();
t0
=
pt_obstacle
.
upper_left_p
oint
().
t
();
t1
=
pt_obstacle
.
upper_right_p
oint
().
t
();
}
else
{
s0
=
pt_obstacle
.
BottomLeftP
oint
().
s
();
s1
=
pt_obstacle
.
BottomRightP
oint
().
s
();
s0
=
pt_obstacle
.
bottom_left_p
oint
().
s
();
s1
=
pt_obstacle
.
bottom_right_p
oint
().
s
();
t0
=
pt_obstacle
.
BottomLeftP
oint
().
t
();
t1
=
pt_obstacle
.
BottomRightP
oint
().
t
();
t0
=
pt_obstacle
.
bottom_left_p
oint
().
t
();
t1
=
pt_obstacle
.
bottom_right_p
oint
().
t
();
}
double
time_gap
=
t1
-
t0
;
...
...
modules/planning/lattice/behavior/path_time_graph.h
浏览文件 @
9f1b8658
...
...
@@ -47,10 +47,10 @@ class PathTimeGraph {
const
double
s_start
,
const
double
s_end
,
const
double
t_start
,
const
double
t_end
,
const
std
::
array
<
double
,
3
>&
init_d
);
const
std
::
vector
<
S
t
Boundary
>&
GetPathTimeObstacles
()
const
;
const
std
::
vector
<
S
T
Boundary
>&
GetPathTimeObstacles
()
const
;
bool
GetPathTimeObstacle
(
const
std
::
string
&
obstacle_id
,
S
t
Boundary
*
path_time_obstacle
);
S
T
Boundary
*
path_time_obstacle
);
std
::
vector
<
std
::
pair
<
double
,
double
>>
GetPathBlockingIntervals
(
const
double
t
)
const
;
...
...
@@ -102,8 +102,8 @@ class PathTimeGraph {
const
ReferenceLineInfo
*
ptr_reference_line_info_
;
std
::
array
<
double
,
3
>
init_d_
;
std
::
unordered_map
<
std
::
string
,
S
t
Boundary
>
path_time_obstacle_map_
;
std
::
vector
<
S
t
Boundary
>
path_time_obstacles_
;
std
::
unordered_map
<
std
::
string
,
S
T
Boundary
>
path_time_obstacle_map_
;
std
::
vector
<
S
T
Boundary
>
path_time_obstacles_
;
std
::
vector
<
SLBoundary
>
static_obs_sl_boundaries_
;
};
...
...
modules/planning/tasks/deciders/decider_creep.cc
浏览文件 @
9f1b8658
...
...
@@ -126,8 +126,8 @@ bool DeciderCreep::CheckCreepDone(const Frame& frame,
creep_config
.
min_boundary_t
())
{
const
double
kepsilon
=
1e-6
;
double
obstacle_traveled_s
=
obstacle
->
reference_line_st_boundary
().
BottomLeftP
oint
().
s
()
-
obstacle
->
reference_line_st_boundary
().
BottomRightP
oint
().
s
();
obstacle
->
reference_line_st_boundary
().
bottom_left_p
oint
().
s
()
-
obstacle
->
reference_line_st_boundary
().
bottom_right_p
oint
().
s
();
ADEBUG
<<
"obstacle["
<<
obstacle
->
Id
()
<<
"] obstacle_st_min_t["
<<
obstacle
->
reference_line_st_boundary
().
min_t
()
...
...
modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc
浏览文件 @
9f1b8658
...
...
@@ -59,7 +59,7 @@ void DpStCost::AddToKeepClearRange(
continue
;
}
if
(
obstacle
->
st_boundary
().
boundary_type
()
!=
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
continue
;
}
...
...
modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc
浏览文件 @
9f1b8658
...
...
@@ -44,11 +44,11 @@ namespace {
constexpr
double
kInf
=
std
::
numeric_limits
<
double
>::
infinity
();
bool
CheckOverlapOnDpStGraph
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
bool
CheckOverlapOnDpStGraph
(
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
StGraphPoint
&
p1
,
const
StGraphPoint
&
p2
)
{
const
common
::
math
::
LineSegment2d
seg
(
p1
.
point
(),
p2
.
point
());
for
(
const
auto
*
boundary
:
boundaries
)
{
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
continue
;
}
if
(
boundary
->
HasOverlap
(
seg
))
{
...
...
@@ -82,7 +82,7 @@ DpStGraph::DpStGraph(const StGraphData& st_graph_data,
Status
DpStGraph
::
Search
(
SpeedData
*
const
speed_data
)
{
constexpr
double
kBounadryEpsilon
=
1e-2
;
for
(
const
auto
&
boundary
:
st_graph_data_
.
st_boundaries
())
{
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
continue
;
}
if
(
boundary
->
IsPointInBoundary
({
0.0
,
0.0
})
||
...
...
modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc
浏览文件 @
9f1b8658
...
...
@@ -109,9 +109,9 @@ TEST_F(DpStGraphTest, simple) {
point_pairs
.
emplace_back
(
lower_points
[
0
],
upper_points
[
0
]);
point_pairs
.
emplace_back
(
lower_points
[
1
],
upper_points
[
1
]);
obstacle_list_
.
back
().
SetStBoundary
(
S
t
Boundary
(
point_pairs
));
obstacle_list_
.
back
().
SetStBoundary
(
S
T
Boundary
(
point_pairs
));
std
::
vector
<
const
S
t
Boundary
*>
boundaries
;
std
::
vector
<
const
S
T
Boundary
*>
boundaries
;
boundaries
.
push_back
(
&
(
obstacles_
.
back
()
->
st_boundary
()));
init_point_
.
mutable_path_point
()
->
set_x
(
0.0
);
...
...
modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @
9f1b8658
...
...
@@ -51,12 +51,12 @@ bool DpStSpeedOptimizer::SearchStGraph(
const
SpeedLimitDecider
&
speed_limit_decider
,
const
PathData
&
path_data
,
SpeedData
*
speed_data
,
PathDecision
*
path_decision
,
STGraphDebug
*
st_graph_debug
)
const
{
std
::
vector
<
const
S
t
Boundary
*>
boundaries
;
std
::
vector
<
const
S
T
Boundary
*>
boundaries
;
for
(
auto
*
obstacle
:
path_decision
->
obstacles
().
Items
())
{
auto
id
=
obstacle
->
Id
();
if
(
!
obstacle
->
st_boundary
().
IsEmpty
())
{
if
(
obstacle
->
st_boundary
().
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
path_decision
->
Find
(
id
)
->
SetBlockingObstacle
(
false
);
}
else
{
path_decision
->
Find
(
id
)
->
SetBlockingObstacle
(
true
);
...
...
modules/planning/tasks/optimizers/path_decider/path_decider.cc
浏览文件 @
9f1b8658
...
...
@@ -100,7 +100,7 @@ bool PathDecider::MakeStaticObstacleDecision(
continue
;
}
if
(
obstacle
->
reference_line_st_boundary
().
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
continue
;
}
...
...
modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc
浏览文件 @
9f1b8658
...
...
@@ -187,24 +187,24 @@ void PolyVTSpeedOptimizer::RecordSTGraphDebug(
auto
boundary_debug
=
st_graph_debug
->
add_boundary
();
boundary_debug
->
set_name
(
boundary
->
id
());
switch
(
boundary
->
boundary_type
())
{
case
S
t
Boundary
::
BoundaryType
::
FOLLOW
:
case
S
T
Boundary
::
BoundaryType
::
FOLLOW
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_FOLLOW
);
break
;
case
S
t
Boundary
::
BoundaryType
::
OVERTAKE
:
case
S
T
Boundary
::
BoundaryType
::
OVERTAKE
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_OVERTAKE
);
break
;
case
S
t
Boundary
::
BoundaryType
::
STOP
:
case
S
T
Boundary
::
BoundaryType
::
STOP
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_STOP
);
break
;
case
S
t
Boundary
::
BoundaryType
::
UNKNOWN
:
case
S
T
Boundary
::
BoundaryType
::
UNKNOWN
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_UNKNOWN
);
break
;
case
S
t
Boundary
::
BoundaryType
::
YIELD
:
case
S
T
Boundary
::
BoundaryType
::
YIELD
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_YIELD
);
break
;
case
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
:
case
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_KEEP_CLEAR
);
break
;
...
...
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc
浏览文件 @
9f1b8658
...
...
@@ -123,7 +123,7 @@ Status QpPiecewiseStGraph::Search(
Status
QpPiecewiseStGraph
::
AddConstraint
(
const
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
std
::
pair
<
double
,
double
>&
accel_bound
)
{
auto
*
constraint
=
generator_
->
mutable_constraint
();
// position, velocity, acceleration
...
...
@@ -212,7 +212,7 @@ Status QpPiecewiseStGraph::AddConstraint(
}
Status
QpPiecewiseStGraph
::
AddKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
SpeedLimit
&
speed_limit
)
{
auto
*
kernel
=
generator_
->
mutable_kernel
();
DCHECK_NOTNULL
(
kernel
);
...
...
@@ -284,7 +284,7 @@ Status QpPiecewiseStGraph::AddCruiseReferenceLineKernel(
}
Status
QpPiecewiseStGraph
::
AddFollowReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
)
{
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
)
{
auto
*
follow_kernel
=
generator_
->
mutable_kernel
();
std
::
vector
<
double
>
ref_s
;
std
::
vector
<
double
>
filtered_evaluate_t
;
...
...
@@ -294,7 +294,7 @@ Status QpPiecewiseStGraph::AddFollowReferenceLineKernel(
double
s_min
=
std
::
numeric_limits
<
double
>::
infinity
();
bool
success
=
false
;
for
(
const
auto
*
boundary
:
boundaries
)
{
if
(
boundary
->
boundary_type
()
!=
S
t
Boundary
::
BoundaryType
::
FOLLOW
)
{
if
(
boundary
->
boundary_type
()
!=
S
T
Boundary
::
BoundaryType
::
FOLLOW
)
{
continue
;
}
if
(
curr_t
<
boundary
->
min_t
()
||
curr_t
>
boundary
->
max_t
())
{
...
...
@@ -336,12 +336,12 @@ Status QpPiecewiseStGraph::AddFollowReferenceLineKernel(
}
Status
QpPiecewiseStGraph
::
GetSConstraintByTime
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
time
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
time
,
const
double
total_path_s
,
double
*
const
s_upper_bound
,
double
*
const
s_lower_bound
)
const
{
*
s_upper_bound
=
total_path_s
;
for
(
const
S
t
Boundary
*
boundary
:
boundaries
)
{
for
(
const
S
T
Boundary
*
boundary
:
boundaries
)
{
double
s_upper
=
0.0
;
double
s_lower
=
0.0
;
...
...
@@ -349,12 +349,12 @@ Status QpPiecewiseStGraph::GetSConstraintByTime(
continue
;
}
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
STOP
||
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
FOLLOW
||
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
YIELD
)
{
if
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
STOP
||
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
FOLLOW
||
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
YIELD
)
{
*
s_upper_bound
=
std
::
fmin
(
*
s_upper_bound
,
s_upper
);
}
else
{
DCHECK
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
OVERTAKE
);
DCHECK
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
OVERTAKE
);
*
s_lower_bound
=
std
::
fmax
(
*
s_lower_bound
,
s_lower
);
}
}
...
...
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h
浏览文件 @
9f1b8658
...
...
@@ -56,11 +56,11 @@ class QpPiecewiseStGraph {
// Add st graph constraint
common
::
Status
AddConstraint
(
const
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
std
::
pair
<
double
,
double
>&
accel_bound
);
// Add objective function
common
::
Status
AddKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
common
::
Status
AddKernel
(
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
SpeedLimit
&
speed_limit
);
// solve
...
...
@@ -68,7 +68,7 @@ class QpPiecewiseStGraph {
// extract upper lower bound for constraint;
common
::
Status
GetSConstraintByTime
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
time
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
time
,
const
double
total_path_s
,
double
*
const
s_upper_bound
,
double
*
const
s_lower_bound
)
const
;
...
...
@@ -78,7 +78,7 @@ class QpPiecewiseStGraph {
const
double
weight
);
common
::
Status
AddFollowReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
);
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
);
common
::
Status
EstimateSpeedUpperBound
(
const
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
...
...
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc
浏览文件 @
9f1b8658
...
...
@@ -156,7 +156,7 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
Status
QpSplineStGraph
::
AddConstraint
(
const
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
std
::
pair
<
double
,
double
>&
accel_bound
)
{
Spline1dConstraint
*
constraint
=
spline_solver_
->
mutable_spline_constraint
();
...
...
@@ -253,7 +253,7 @@ Status QpSplineStGraph::AddConstraint(
bool
has_follow
=
false
;
double
delta_s
=
1.0
;
for
(
const
auto
*
boundary
:
boundaries
)
{
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
FOLLOW
)
{
if
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
FOLLOW
)
{
has_follow
=
true
;
delta_s
=
std
::
fmin
(
delta_s
,
boundary
->
min_s
()
-
fabs
(
boundary
->
characteristic_length
()));
...
...
@@ -280,7 +280,7 @@ Status QpSplineStGraph::AddConstraint(
}
Status
QpSplineStGraph
::
AddKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
SpeedLimit
&
speed_limit
)
{
Spline1dKernel
*
spline_kernel
=
spline_solver_
->
mutable_spline_kernel
();
...
...
@@ -369,7 +369,7 @@ Status QpSplineStGraph::AddCruiseReferenceLineKernel(const double weight) {
}
Status
QpSplineStGraph
::
AddFollowReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
)
{
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
)
{
auto
*
spline_kernel
=
spline_solver_
->
mutable_spline_kernel
();
std
::
vector
<
double
>
ref_s
;
std
::
vector
<
double
>
filtered_evaluate_t
;
...
...
@@ -378,7 +378,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
double
s_min
=
std
::
numeric_limits
<
double
>::
infinity
();
bool
success
=
false
;
for
(
const
auto
*
boundary
:
boundaries
)
{
if
(
boundary
->
boundary_type
()
!=
S
t
Boundary
::
BoundaryType
::
FOLLOW
)
{
if
(
boundary
->
boundary_type
()
!=
S
T
Boundary
::
BoundaryType
::
FOLLOW
)
{
continue
;
}
if
(
curr_t
<
boundary
->
min_t
()
||
curr_t
>
boundary
->
max_t
())
{
...
...
@@ -421,7 +421,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
}
Status
QpSplineStGraph
::
AddYieldReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
)
{
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
)
{
auto
*
spline_kernel
=
spline_solver_
->
mutable_spline_kernel
();
std
::
vector
<
double
>
ref_s
;
std
::
vector
<
double
>
filtered_evaluate_t
;
...
...
@@ -430,7 +430,7 @@ Status QpSplineStGraph::AddYieldReferenceLineKernel(
double
s_min
=
std
::
numeric_limits
<
double
>::
infinity
();
bool
success
=
false
;
for
(
const
auto
*
boundary
:
boundaries
)
{
if
(
boundary
->
boundary_type
()
!=
S
t
Boundary
::
BoundaryType
::
YIELD
)
{
if
(
boundary
->
boundary_type
()
!=
S
T
Boundary
::
BoundaryType
::
YIELD
)
{
continue
;
}
if
(
curr_t
<
boundary
->
min_t
()
||
curr_t
>
boundary
->
max_t
())
{
...
...
@@ -484,12 +484,12 @@ bool QpSplineStGraph::AddDpStReferenceKernel(const double weight) const {
}
Status
QpSplineStGraph
::
GetSConstraintByTime
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
time
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
time
,
const
double
total_path_s
,
double
*
const
s_upper_bound
,
double
*
const
s_lower_bound
)
const
{
*
s_upper_bound
=
total_path_s
;
for
(
const
S
t
Boundary
*
boundary
:
boundaries
)
{
for
(
const
S
T
Boundary
*
boundary
:
boundaries
)
{
double
s_upper
=
0.0
;
double
s_lower
=
0.0
;
...
...
@@ -497,16 +497,16 @@ Status QpSplineStGraph::GetSConstraintByTime(
continue
;
}
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
STOP
||
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
FOLLOW
||
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
YIELD
)
{
if
(
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
STOP
||
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
FOLLOW
||
boundary
->
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
YIELD
)
{
*
s_upper_bound
=
std
::
fmin
(
*
s_upper_bound
,
s_upper
);
}
else
if
(
boundary
->
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
OVERTAKE
)
{
S
T
Boundary
::
BoundaryType
::
OVERTAKE
)
{
*
s_lower_bound
=
std
::
fmax
(
*
s_lower_bound
,
s_lower
);
}
else
{
AWARN
<<
"Unhandled boundary type: "
<<
S
t
Boundary
::
TypeName
(
boundary
->
boundary_type
());
<<
S
T
Boundary
::
TypeName
(
boundary
->
boundary_type
());
}
}
...
...
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h
浏览文件 @
9f1b8658
...
...
@@ -59,11 +59,11 @@ class QpSplineStGraph {
// Add st graph constraint
common
::
Status
AddConstraint
(
const
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
std
::
pair
<
double
,
double
>&
accel_bound
);
// Add objective function
common
::
Status
AddKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
common
::
Status
AddKernel
(
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
SpeedLimit
&
speed_limit
);
// solve
...
...
@@ -71,7 +71,7 @@ class QpSplineStGraph {
// extract upper lower bound for constraint;
common
::
Status
GetSConstraintByTime
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
time
,
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
time
,
const
double
total_path_s
,
double
*
const
s_upper_bound
,
double
*
const
s_lower_bound
)
const
;
...
...
@@ -80,11 +80,11 @@ class QpSplineStGraph {
// follow line kernel
common
::
Status
AddFollowReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
);
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
);
// yield line kernel
common
::
Status
AddYieldReferenceLineKernel
(
const
std
::
vector
<
const
S
t
Boundary
*>&
boundaries
,
const
double
weight
);
const
std
::
vector
<
const
S
T
Boundary
*>&
boundaries
,
const
double
weight
);
const
SpeedData
GetHistorySpeed
()
const
;
common
::
Status
EstimateSpeedUpperBound
(
...
...
modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @
9f1b8658
...
...
@@ -89,7 +89,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
"Mapping obstacle for qp st speed optimizer failed!"
);
}
std
::
vector
<
const
S
t
Boundary
*>
boundaries
;
std
::
vector
<
const
S
T
Boundary
*>
boundaries
;
for
(
auto
*
obstacle
:
path_decision
->
obstacles
().
Items
())
{
auto
id
=
obstacle
->
Id
();
if
(
!
obstacle
->
st_boundary
().
IsEmpty
())
{
...
...
modules/planning/tasks/optimizers/speed_decider/speed_decider.cc
浏览文件 @
9f1b8658
...
...
@@ -62,7 +62,7 @@ common::Status SpeedDecider::Execute(Frame* frame,
SpeedDecider
::
StPosition
SpeedDecider
::
GetStPosition
(
const
PathDecision
*
const
path_decision
,
const
SpeedData
&
speed_profile
,
const
S
t
Boundary
&
st_boundary
)
const
{
const
S
T
Boundary
&
st_boundary
)
const
{
StPosition
st_position
=
BELOW
;
if
(
st_boundary
.
IsEmpty
())
{
return
st_position
;
...
...
@@ -86,7 +86,7 @@ SpeedDecider::StPosition SpeedDecider::GetStPosition(
ADEBUG
<<
"speed profile cross st_boundaries."
;
st_position
=
CROSS
;
if
(
st_boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
st_boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
!
CheckKeepClearCrossable
(
path_decision
,
speed_profile
,
st_boundary
))
{
st_position
=
BELOW
;
...
...
@@ -111,7 +111,7 @@ SpeedDecider::StPosition SpeedDecider::GetStPosition(
bool
SpeedDecider
::
CheckKeepClearCrossable
(
const
PathDecision
*
const
path_decision
,
const
SpeedData
&
speed_profile
,
const
S
t
Boundary
&
keep_clear_st_boundary
)
const
{
const
S
T
Boundary
&
keep_clear_st_boundary
)
const
{
bool
keep_clear_crossable
=
true
;
const
auto
&
last_speed_point
=
speed_profile
.
back
();
...
...
@@ -205,7 +205,7 @@ Status SpeedDecider::MakeObjectDecision(
}
auto
position
=
GetStPosition
(
path_decision
,
speed_profile
,
boundary
);
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
CheckKeepClearBlocked
(
path_decision
,
*
obstacle
))
{
position
=
BELOW
;
}
...
...
@@ -218,7 +218,7 @@ Status SpeedDecider::MakeObjectDecision(
switch
(
position
)
{
case
BELOW
:
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
ObjectDecisionType
stop_decision
;
if
(
CreateStopDecision
(
*
mutable_obstacle
,
&
stop_decision
,
0.0
))
{
mutable_obstacle
->
AddLongitudinalDecision
(
"dp_st_graph/keep_clear"
,
...
...
@@ -254,7 +254,7 @@ Status SpeedDecider::MakeObjectDecision(
}
break
;
case
ABOVE
:
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
ObjectDecisionType
ignore
;
ignore
.
mutable_ignore
();
mutable_obstacle
->
AddLongitudinalDecision
(
"dp_st_graph"
,
ignore
);
...
...
@@ -308,7 +308,7 @@ bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle,
const
auto
&
boundary
=
obstacle
.
st_boundary
();
double
fence_s
=
adc_sl_boundary_
.
end_s
()
+
boundary
.
min_s
()
+
stop_distance
;
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
fence_s
=
obstacle
.
PerceptionSLBoundary
().
start_s
();
}
const
double
main_stop_s
=
...
...
@@ -329,7 +329,7 @@ bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle,
stop_point
->
set_z
(
0.0
);
stop
->
set_stop_heading
(
fence_point
.
heading
());
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
)
{
stop
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_CLEAR_ZONE
);
}
...
...
@@ -465,8 +465,8 @@ bool SpeedDecider::CreateOvertakeDecision(
return
true
;
}
bool
SpeedDecider
::
CheckIsFollowByT
(
const
S
t
Boundary
&
boundary
)
const
{
if
(
boundary
.
BottomLeftPoint
().
s
()
>
boundary
.
BottomRightP
oint
().
s
())
{
bool
SpeedDecider
::
CheckIsFollowByT
(
const
S
T
Boundary
&
boundary
)
const
{
if
(
boundary
.
bottom_left_point
().
s
()
>
boundary
.
bottom_right_p
oint
().
s
())
{
return
false
;
}
constexpr
double
kFollowTimeEpsilon
=
1e-3
;
...
...
modules/planning/tasks/optimizers/speed_decider/speed_decider.h
浏览文件 @
9f1b8658
...
...
@@ -44,12 +44,12 @@ class SpeedDecider : public Task {
StPosition
GetStPosition
(
const
PathDecision
*
const
path_decision
,
const
SpeedData
&
speed_profile
,
const
S
t
Boundary
&
st_boundary
)
const
;
const
S
T
Boundary
&
st_boundary
)
const
;
bool
CheckKeepClearCrossable
(
const
PathDecision
*
const
path_decision
,
const
SpeedData
&
speed_profile
,
const
S
t
Boundary
&
keep_clear_st_boundary
)
const
;
const
S
T
Boundary
&
keep_clear_st_boundary
)
const
;
bool
CheckKeepClearBlocked
(
const
PathDecision
*
const
path_decision
,
...
...
@@ -62,7 +62,7 @@ class SpeedDecider : public Task {
* @return true if the ADC believe it should follow the obstacle, and
* false otherwise.
**/
bool
CheckIsFollowByT
(
const
S
t
Boundary
&
boundary
)
const
;
bool
CheckIsFollowByT
(
const
S
T
Boundary
&
boundary
)
const
;
bool
CreateStopDecision
(
const
Obstacle
&
obstacle
,
ObjectDecisionType
*
const
stop_decision
,
...
...
modules/planning/tasks/optimizers/speed_optimizer.cc
浏览文件 @
9f1b8658
...
...
@@ -66,24 +66,24 @@ void SpeedOptimizer::RecordSTGraphDebug(const StGraphData& st_graph_data,
auto
boundary_debug
=
st_graph_debug
->
add_boundary
();
boundary_debug
->
set_name
(
boundary
->
id
());
switch
(
boundary
->
boundary_type
())
{
case
S
t
Boundary
::
BoundaryType
::
FOLLOW
:
case
S
T
Boundary
::
BoundaryType
::
FOLLOW
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_FOLLOW
);
break
;
case
S
t
Boundary
::
BoundaryType
::
OVERTAKE
:
case
S
T
Boundary
::
BoundaryType
::
OVERTAKE
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_OVERTAKE
);
break
;
case
S
t
Boundary
::
BoundaryType
::
STOP
:
case
S
T
Boundary
::
BoundaryType
::
STOP
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_STOP
);
break
;
case
S
t
Boundary
::
BoundaryType
::
UNKNOWN
:
case
S
T
Boundary
::
BoundaryType
::
UNKNOWN
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_UNKNOWN
);
break
;
case
S
t
Boundary
::
BoundaryType
::
YIELD
:
case
S
T
Boundary
::
BoundaryType
::
YIELD
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_YIELD
);
break
;
case
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
:
case
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
:
boundary_debug
->
set_type
(
StGraphBoundaryDebug
::
ST_BOUNDARY_TYPE_KEEP_CLEAR
);
break
;
...
...
modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc
浏览文件 @
9f1b8658
...
...
@@ -300,8 +300,8 @@ bool StBoundaryMapper::MapStopDecision(
point_pairs
.
emplace_back
(
STPoint
(
s_min
,
planning_time_
),
STPoint
(
s_max
+
st_boundary_config_
.
boundary_buffer
(),
planning_time_
));
auto
boundary
=
S
t
Boundary
(
point_pairs
);
boundary
.
SetBoundaryType
(
S
t
Boundary
::
BoundaryType
::
STOP
);
auto
boundary
=
S
T
Boundary
(
point_pairs
);
boundary
.
SetBoundaryType
(
S
T
Boundary
::
BoundaryType
::
STOP
);
boundary
.
SetCharacteristicLength
(
st_boundary_config_
.
boundary_buffer
());
boundary
.
set_id
(
stop_obstacle
->
Id
());
stop_obstacle
->
SetStBoundary
(
boundary
);
...
...
@@ -317,7 +317,7 @@ Status StBoundaryMapper::MapWithoutDecision(Obstacle* obstacle) const {
return
Status
::
OK
();
}
auto
boundary
=
S
tBoundary
::
GenerateStBoundary
(
lower_points
,
upper_points
)
auto
boundary
=
S
TBoundary
::
CreateInstance
(
lower_points
,
upper_points
)
.
ExpandByS
(
boundary_s_buffer
)
.
ExpandByT
(
boundary_t_buffer
);
boundary
.
set_id
(
obstacle
->
Id
());
...
...
@@ -496,24 +496,24 @@ Status StBoundaryMapper::MapWithDecision(
lower_points
.
emplace_back
(
extend_lower_s
,
planning_time_
);
}
auto
boundary
=
S
tBoundary
::
GenerateStBoundary
(
lower_points
,
upper_points
)
auto
boundary
=
S
TBoundary
::
CreateInstance
(
lower_points
,
upper_points
)
.
ExpandByS
(
boundary_s_buffer
)
.
ExpandByT
(
boundary_t_buffer
);
// get characteristic_length and boundary_type.
S
tBoundary
::
BoundaryType
b_type
=
St
Boundary
::
BoundaryType
::
UNKNOWN
;
S
TBoundary
::
BoundaryType
b_type
=
ST
Boundary
::
BoundaryType
::
UNKNOWN
;
double
characteristic_length
=
0.0
;
if
(
decision
.
has_follow
())
{
characteristic_length
=
std
::
fabs
(
decision
.
follow
().
distance_s
());
b_type
=
S
t
Boundary
::
BoundaryType
::
FOLLOW
;
b_type
=
S
T
Boundary
::
BoundaryType
::
FOLLOW
;
}
else
if
(
decision
.
has_yield
())
{
characteristic_length
=
std
::
fabs
(
decision
.
yield
().
distance_s
());
boundary
=
S
tBoundary
::
GenerateStBoundary
(
lower_points
,
upper_points
)
boundary
=
S
TBoundary
::
CreateInstance
(
lower_points
,
upper_points
)
.
ExpandByS
(
characteristic_length
);
b_type
=
S
t
Boundary
::
BoundaryType
::
YIELD
;
b_type
=
S
T
Boundary
::
BoundaryType
::
YIELD
;
}
else
if
(
decision
.
has_overtake
())
{
characteristic_length
=
std
::
fabs
(
decision
.
overtake
().
distance_s
());
b_type
=
S
t
Boundary
::
BoundaryType
::
OVERTAKE
;
b_type
=
S
T
Boundary
::
BoundaryType
::
OVERTAKE
;
}
else
{
DCHECK
(
false
)
<<
"Obj decision should be either yield or overtake: "
<<
decision
.
DebugString
();
...
...
modules/planning/tasks/optimizers/st_graph/st_graph_data.cc
浏览文件 @
9f1b8658
...
...
@@ -25,7 +25,7 @@ namespace planning {
using
apollo
::
common
::
TrajectoryPoint
;
StGraphData
::
StGraphData
(
const
std
::
vector
<
const
S
t
Boundary
*>&
st_boundaries
,
StGraphData
::
StGraphData
(
const
std
::
vector
<
const
S
T
Boundary
*>&
st_boundaries
,
const
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
double
path_data_length
)
...
...
@@ -34,7 +34,7 @@ StGraphData::StGraphData(const std::vector<const StBoundary*>& st_boundaries,
speed_limit_
(
speed_limit
),
path_data_length_
(
path_data_length
)
{}
const
std
::
vector
<
const
S
t
Boundary
*>&
StGraphData
::
st_boundaries
()
const
{
const
std
::
vector
<
const
S
T
Boundary
*>&
StGraphData
::
st_boundaries
()
const
{
return
st_boundaries_
;
}
...
...
modules/planning/tasks/optimizers/st_graph/st_graph_data.h
浏览文件 @
9f1b8658
...
...
@@ -33,12 +33,12 @@ namespace planning {
class
StGraphData
{
public:
StGraphData
(
const
std
::
vector
<
const
S
t
Boundary
*>&
st_boundaries
,
StGraphData
(
const
std
::
vector
<
const
S
T
Boundary
*>&
st_boundaries
,
const
apollo
::
common
::
TrajectoryPoint
&
init_point
,
const
SpeedLimit
&
speed_limit
,
const
double
path_data_length
);
StGraphData
()
=
default
;
const
std
::
vector
<
const
S
t
Boundary
*>&
st_boundaries
()
const
;
const
std
::
vector
<
const
S
T
Boundary
*>&
st_boundaries
()
const
;
const
apollo
::
common
::
TrajectoryPoint
&
init_point
()
const
;
...
...
@@ -47,7 +47,7 @@ class StGraphData {
double
path_data_length
()
const
;
private:
std
::
vector
<
const
S
t
Boundary
*>
st_boundaries_
;
std
::
vector
<
const
S
T
Boundary
*>
st_boundaries_
;
apollo
::
common
::
TrajectoryPoint
init_point_
;
SpeedLimit
speed_limit_
;
...
...
modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc
浏览文件 @
9f1b8658
...
...
@@ -25,8 +25,8 @@ namespace apollo {
namespace
planning
{
TEST
(
StGraphDataTest
,
basic_test
)
{
std
::
vector
<
const
S
t
Boundary
*>
boundary_vec
;
auto
boundary
=
S
t
Boundary
();
std
::
vector
<
const
S
T
Boundary
*>
boundary_vec
;
auto
boundary
=
S
T
Boundary
();
boundary_vec
.
push_back
(
&
boundary
);
apollo
::
common
::
TrajectoryPoint
traj_point
;
traj_point
.
mutable_path_point
()
->
set_x
(
1.1
);
...
...
modules/planning/traffic_rules/keep_clear.cc
浏览文件 @
9f1b8658
...
...
@@ -176,7 +176,7 @@ bool KeepClear::BuildKeepClearObstacle(
return
false
;
}
path_obstacle
->
SetReferenceLineStBoundaryType
(
S
t
Boundary
::
BoundaryType
::
KEEP_CLEAR
);
S
T
Boundary
::
BoundaryType
::
KEEP_CLEAR
);
return
true
;
}
...
...
modules/planning/tuning/autotuning_raw_feature_generator.cc
浏览文件 @
9f1b8658
...
...
@@ -158,13 +158,13 @@ void AutotuningRawFeatureGenerator::GenerateSTBoundaries(
}
void
AutotuningRawFeatureGenerator
::
ConvertToDiscretizedBoundaries
(
const
S
t
Boundary
&
boundary
,
const
double
speed
)
{
const
S
T
Boundary
&
boundary
,
const
double
speed
)
{
for
(
size_t
i
=
0
;
i
<
eval_time_
.
size
();
++
i
)
{
double
upper
=
0.0
;
double
lower
=
0.0
;
bool
suc
=
boundary
.
GetBoundarySRange
(
eval_time_
[
i
],
&
upper
,
&
lower
);
if
(
suc
)
{
if
(
boundary
.
boundary_type
()
==
S
t
Boundary
::
BoundaryType
::
STOP
)
{
if
(
boundary
.
boundary_type
()
==
S
T
Boundary
::
BoundaryType
::
STOP
)
{
stop_boundaries_
[
i
].
push_back
({{
lower
,
upper
,
speed
}});
}
else
{
obs_boundaries_
[
i
].
push_back
({{
lower
,
upper
,
speed
}});
...
...
modules/planning/tuning/autotuning_raw_feature_generator.h
浏览文件 @
9f1b8658
...
...
@@ -75,7 +75,7 @@ class AutotuningRawFeatureGenerator {
/**
* Convert st boundaries to discretized boundaries
*/
void
ConvertToDiscretizedBoundaries
(
const
S
t
Boundary
&
boundary
,
void
ConvertToDiscretizedBoundaries
(
const
S
T
Boundary
&
boundary
,
const
double
speed
);
common
::
Status
EvaluateSpeedPoint
(
const
common
::
SpeedPoint
&
speed_point
,
...
...
@@ -88,7 +88,7 @@ class AutotuningRawFeatureGenerator {
const
ReferenceLineInfo
&
reference_line_info_
;
const
Frame
&
frame_
;
const
SpeedLimit
&
speed_limit_
;
std
::
vector
<
const
S
t
Boundary
*>
boundaries_
;
std
::
vector
<
const
S
T
Boundary
*>
boundaries_
;
// covers the boundary info lower s upper s as well as the speed of obs
std
::
vector
<
std
::
vector
<
std
::
array
<
double
,
3
>>>
obs_boundaries_
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录