Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f5b34cc1
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,发现更多精彩内容 >>
提交
f5b34cc1
编写于
11月 16, 2018
作者:
L
Liangliang Zhang
提交者:
Jiangtao Hu
12月 13, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: fixed heading issues in side pass.
上级
197f0a92
变更
12
隐藏空白更改
内联
并排
Showing
12 changed file
with
105 addition
and
29 deletion
+105
-29
modules/planning/common/BUILD
modules/planning/common/BUILD
+48
-0
modules/planning/common/path/BUILD
modules/planning/common/path/BUILD
+10
-1
modules/planning/common/path/path_data.cc
modules/planning/common/path/path_data.cc
+17
-8
modules/planning/reference_line/reference_line.cc
modules/planning/reference_line/reference_line.cc
+8
-4
modules/planning/reference_line/reference_line.h
modules/planning/reference_line/reference_line.h
+1
-1
modules/planning/scenarios/side_pass/side_pass_stage.cc
modules/planning/scenarios/side_pass/side_pass_stage.cc
+7
-8
modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc
...nning/scenarios/side_pass/side_pass_stop_on_wait_point.cc
+1
-1
modules/planning/std_planning.cc
modules/planning/std_planning.cc
+3
-0
modules/planning/toolkits/deciders/side_pass_path_decider.cc
modules/planning/toolkits/deciders/side_pass_path_decider.cc
+5
-3
modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc
...p_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc
+2
-1
modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc
...its/optimizers/qp_spline_path/qp_spline_path_generator.cc
+1
-1
modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc
.../planning/toolkits/optimizers/road_graph/dp_road_graph.cc
+2
-1
未找到文件。
modules/planning/common/BUILD
浏览文件 @
f5b34cc1
...
@@ -7,6 +7,9 @@ cc_library(
...
@@ -7,6 +7,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"indexed_list.h"
,
"indexed_list.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
linkopts
=
[
linkopts
=
[
"-lboost_thread"
,
"-lboost_thread"
,
],
],
...
@@ -33,6 +36,9 @@ cc_library(
...
@@ -33,6 +36,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"indexed_queue.h"
,
"indexed_queue.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/common/util:map_util"
,
"//modules/common/util:map_util"
,
],
],
...
@@ -66,6 +72,9 @@ cc_library(
...
@@ -66,6 +72,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"obstacle.h"
,
"obstacle.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":indexed_list"
,
":indexed_list"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/configs:vehicle_config_helper"
,
...
@@ -101,6 +110,9 @@ cc_library(
...
@@ -101,6 +110,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"planning_context.h"
,
"planning_context.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":planning_gflags"
,
":planning_gflags"
,
"//cyber/common:log"
,
"//cyber/common:log"
,
...
@@ -120,6 +132,9 @@ cc_library(
...
@@ -120,6 +132,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"path_decision.h"
,
"path_decision.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":obstacle"
,
":obstacle"
,
"//modules/planning/reference_line"
,
"//modules/planning/reference_line"
,
...
@@ -134,6 +149,9 @@ cc_library(
...
@@ -134,6 +149,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"planning_gflags.h"
,
"planning_gflags.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//external:gflags"
,
"//external:gflags"
,
],
],
...
@@ -147,6 +165,9 @@ cc_library(
...
@@ -147,6 +165,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"reference_line_info.h"
,
"reference_line_info.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":ego_info"
,
":ego_info"
,
":path_decision"
,
":path_decision"
,
...
@@ -186,6 +207,9 @@ cc_library(
...
@@ -186,6 +207,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"speed_profile_generator.h"
,
"speed_profile_generator.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":frame"
,
":frame"
,
":reference_line_info"
,
":reference_line_info"
,
...
@@ -234,6 +258,9 @@ cc_library(
...
@@ -234,6 +258,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"change_lane_decider.h"
,
"change_lane_decider.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":planning_gflags"
,
":planning_gflags"
,
":reference_line_info"
,
":reference_line_info"
,
...
@@ -247,6 +274,9 @@ cc_library(
...
@@ -247,6 +274,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"local_view.h"
,
"local_view.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/localization/proto:localization_proto"
,
"//modules/localization/proto:localization_proto"
,
"//modules/map/relative_map/proto:navigation_proto"
,
"//modules/map/relative_map/proto:navigation_proto"
,
...
@@ -266,6 +296,9 @@ cc_library(
...
@@ -266,6 +296,9 @@ cc_library(
"frame.h"
,
"frame.h"
,
"frame_manager.h"
,
"frame_manager.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":local_view"
,
":local_view"
,
":change_lane_decider"
,
":change_lane_decider"
,
...
@@ -315,6 +348,9 @@ cc_library(
...
@@ -315,6 +348,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"speed_limit.h"
,
"speed_limit.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/common/math"
,
"//modules/common/math"
,
"//modules/planning/proto:planning_proto"
,
"//modules/planning/proto:planning_proto"
,
...
@@ -341,6 +377,9 @@ cc_library(
...
@@ -341,6 +377,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"ego_info.h"
,
"ego_info.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":obstacle"
,
":obstacle"
,
"//cyber/common:log"
,
"//cyber/common:log"
,
...
@@ -361,6 +400,9 @@ cc_library(
...
@@ -361,6 +400,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"threshold.h"
,
"threshold.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":frame"
,
":frame"
,
":speed_limit"
,
":speed_limit"
,
...
@@ -398,6 +440,9 @@ cc_test(
...
@@ -398,6 +440,9 @@ cc_test(
cc_library
(
cc_library
(
name
=
"planning_common"
,
name
=
"planning_common"
,
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"threshold"
,
"threshold"
,
":ego_info"
,
":ego_info"
,
...
@@ -416,6 +461,9 @@ cc_library(
...
@@ -416,6 +461,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"trajectory_info.h"
,
"trajectory_info.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/planning/common/path:path_data"
,
"//modules/planning/common/path:path_data"
,
"//modules/planning/common/speed:speed_data"
,
"//modules/planning/common/speed:speed_data"
,
...
...
modules/planning/common/path/BUILD
浏览文件 @
f5b34cc1
...
@@ -10,10 +10,13 @@ cc_library(
...
@@ -10,10 +10,13 @@ cc_library(
hdrs
=
[
hdrs
=
[
"discretized_path.h"
,
"discretized_path.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/common/proto:pnc_point_proto"
,
"//modules/common"
,
"//modules/common"
,
"//modules/common/math:linear_interpolation"
,
"//modules/common/math:linear_interpolation"
,
"//modules/common/proto:pnc_point_proto"
,
"//modules/planning/common:planning_context"
,
"//modules/planning/common:planning_context"
,
],
],
)
)
...
@@ -39,6 +42,9 @@ cc_library(
...
@@ -39,6 +42,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"frenet_frame_path.h"
,
"frenet_frame_path.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
"//modules/common/math:linear_interpolation"
,
"//modules/common/math:linear_interpolation"
,
"//modules/common/proto:pnc_point_proto"
,
"//modules/common/proto:pnc_point_proto"
,
...
@@ -66,6 +72,9 @@ cc_library(
...
@@ -66,6 +72,9 @@ cc_library(
hdrs
=
[
hdrs
=
[
"path_data.h"
,
"path_data.h"
,
],
],
copts
=
[
"-DMODULE_NAME=
\\\"
planning
\\\"
"
,
],
deps
=
[
deps
=
[
":discretized_path"
,
":discretized_path"
,
":frenet_frame_path"
,
":frenet_frame_path"
,
...
...
modules/planning/common/path/path_data.cc
浏览文件 @
f5b34cc1
...
@@ -177,6 +177,7 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path,
...
@@ -177,6 +177,7 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path,
double
theta
=
CartesianFrenetConverter
::
CalculateTheta
(
double
theta
=
CartesianFrenetConverter
::
CalculateTheta
(
ref_point
.
heading
(),
ref_point
.
kappa
(),
frenet_point
.
l
(),
ref_point
.
heading
(),
ref_point
.
kappa
(),
frenet_point
.
l
(),
frenet_point
.
dl
());
frenet_point
.
dl
());
ADEBUG
<<
"frenet_point: "
<<
frenet_point
.
ShortDebugString
();
double
kappa
=
CartesianFrenetConverter
::
CalculateKappa
(
double
kappa
=
CartesianFrenetConverter
::
CalculateKappa
(
ref_point
.
kappa
(),
ref_point
.
dkappa
(),
frenet_point
.
l
(),
ref_point
.
kappa
(),
ref_point
.
dkappa
(),
frenet_point
.
l
(),
frenet_point
.
dl
(),
frenet_point
.
ddl
());
frenet_point
.
dl
(),
frenet_point
.
ddl
());
...
@@ -209,15 +210,23 @@ bool PathData::XYToSL(const DiscretizedPath &discretized_path,
...
@@ -209,15 +210,23 @@ bool PathData::XYToSL(const DiscretizedPath &discretized_path,
std
::
vector
<
common
::
FrenetFramePoint
>
frenet_frame_points
;
std
::
vector
<
common
::
FrenetFramePoint
>
frenet_frame_points
;
const
double
max_len
=
reference_line_
->
Length
();
const
double
max_len
=
reference_line_
->
Length
();
for
(
const
auto
&
path_point
:
discretized_path
.
path_points
())
{
for
(
const
auto
&
path_point
:
discretized_path
.
path_points
())
{
SLPoint
sl_point
;
common
::
FrenetFramePoint
frenet_point
=
if
(
!
reference_line_
->
XYToSL
({
path_point
.
x
(),
path_point
.
y
()},
&
sl_point
))
{
reference_line_
->
GetFrenetPoint
(
path_point
);
AERROR
<<
"Fail to transfer cartesian point to frenet point."
;
if
(
!
frenet_point
.
has_s
())
{
return
false
;
SLPoint
sl_point
;
if
(
!
reference_line_
->
XYToSL
({
path_point
.
x
(),
path_point
.
y
()},
&
sl_point
))
{
AERROR
<<
"Fail to transfer cartesian point to frenet point."
;
return
false
;
}
common
::
FrenetFramePoint
frenet_point
;
// NOTICE: does not set dl and ddl here. Add if needed.
frenet_point
.
set_s
(
std
::
max
(
0.0
,
std
::
min
(
sl_point
.
s
(),
max_len
)));
frenet_point
.
set_l
(
sl_point
.
l
());
frenet_frame_points
.
push_back
(
std
::
move
(
frenet_point
));
continue
;
}
}
common
::
FrenetFramePoint
frenet_point
;
frenet_point
.
set_s
(
std
::
max
(
0.0
,
std
::
min
(
frenet_point
.
s
(),
max_len
)));
// NOTICE: does not set dl and ddl here. Add if needed.
frenet_point
.
set_s
(
std
::
max
(
0.0
,
std
::
min
(
sl_point
.
s
(),
max_len
)));
frenet_point
.
set_l
(
sl_point
.
l
());
frenet_frame_points
.
push_back
(
std
::
move
(
frenet_point
));
frenet_frame_points
.
push_back
(
std
::
move
(
frenet_point
));
}
}
*
frenet_path
=
FrenetFramePath
(
std
::
move
(
frenet_frame_points
));
*
frenet_path
=
FrenetFramePath
(
std
::
move
(
frenet_frame_points
));
...
...
modules/planning/reference_line/reference_line.cc
浏览文件 @
f5b34cc1
...
@@ -180,15 +180,19 @@ bool ReferenceLine::Shrink(const double s, double look_backward,
...
@@ -180,15 +180,19 @@ bool ReferenceLine::Shrink(const double s, double look_backward,
}
}
common
::
FrenetFramePoint
ReferenceLine
::
GetFrenetPoint
(
common
::
FrenetFramePoint
ReferenceLine
::
GetFrenetPoint
(
const
common
::
TrajectoryPoint
&
traj_point
)
const
{
const
common
::
PathPoint
&
path_point
)
const
{
if
(
reference_points_
.
empty
())
{
return
common
::
FrenetFramePoint
();
}
common
::
SLPoint
sl_point
;
common
::
SLPoint
sl_point
;
XYToSL
({
traj_point
.
path_point
().
x
(),
traj_point
.
path_point
()
.
y
()},
&
sl_point
);
XYToSL
({
path_point
.
x
(),
path_point
.
y
()},
&
sl_point
);
common
::
FrenetFramePoint
frenet_frame_point
;
common
::
FrenetFramePoint
frenet_frame_point
;
frenet_frame_point
.
set_s
(
sl_point
.
s
());
frenet_frame_point
.
set_s
(
sl_point
.
s
());
frenet_frame_point
.
set_l
(
sl_point
.
l
());
frenet_frame_point
.
set_l
(
sl_point
.
l
());
const
double
theta
=
traj_point
.
path_point
()
.
theta
();
const
double
theta
=
path_point
.
theta
();
const
double
kappa
=
traj_point
.
path_point
()
.
kappa
();
const
double
kappa
=
path_point
.
kappa
();
const
double
l
=
frenet_frame_point
.
l
();
const
double
l
=
frenet_frame_point
.
l
();
ReferencePoint
ref_point
=
GetReferencePoint
(
frenet_frame_point
.
s
());
ReferencePoint
ref_point
=
GetReferencePoint
(
frenet_frame_point
.
s
());
...
...
modules/planning/reference_line/reference_line.h
浏览文件 @
f5b34cc1
...
@@ -81,7 +81,7 @@ class ReferenceLine {
...
@@ -81,7 +81,7 @@ class ReferenceLine {
ReferencePoint
GetReferencePoint
(
const
double
s
)
const
;
ReferencePoint
GetReferencePoint
(
const
double
s
)
const
;
common
::
FrenetFramePoint
GetFrenetPoint
(
common
::
FrenetFramePoint
GetFrenetPoint
(
const
common
::
TrajectoryPoint
&
traj
_point
)
const
;
const
common
::
PathPoint
&
path
_point
)
const
;
std
::
vector
<
ReferencePoint
>
GetReferencePoints
(
double
start_s
,
std
::
vector
<
ReferencePoint
>
GetReferencePoints
(
double
start_s
,
double
end_s
)
const
;
double
end_s
)
const
;
...
...
modules/planning/scenarios/side_pass/side_pass_stage.cc
浏览文件 @
f5b34cc1
...
@@ -78,14 +78,13 @@ Stage::StageStatus SidePassBackup::Process(
...
@@ -78,14 +78,13 @@ Stage::StageStatus SidePassBackup::Process(
double
lane_left_width
=
0.0
;
double
lane_left_width
=
0.0
;
double
lane_right_width
=
0.0
;
double
lane_right_width
=
0.0
;
reference_line
.
GetLaneWidth
(
obstacle
->
PerceptionSLBoundary
().
start_s
(),
reference_line
.
GetLaneWidth
(
obstacle
->
PerceptionSLBoundary
().
start_s
(),
&
lane_left_width
,
&
lane_left_width
,
&
lane_right_width
);
&
lane_right_width
);
double
driving_width
=
double
driving_width
=
std
::
max
(
std
::
max
(
lane_left_width
-
obstacle
->
PerceptionSLBoundary
().
end_l
(),
lane_left_width
-
obstacle
->
PerceptionSLBoundary
().
end_l
(),
lane_right_width
+
obstacle
->
PerceptionSLBoundary
().
start_l
());
lane_right_width
+
obstacle
->
PerceptionSLBoundary
().
start_l
());
driving_width
=
driving_width
=
std
::
min
(
lane_left_width
+
lane_right_width
,
driving_width
)
-
std
::
min
(
lane_left_width
+
lane_right_width
,
driving_width
)
-
FLAGS_static_decision_nudge_l_buffer
;
FLAGS_static_decision_nudge_l_buffer
;
ADEBUG
<<
"driving_width["
<<
driving_width
<<
"]"
;
ADEBUG
<<
"driving_width["
<<
driving_width
<<
"]"
;
if
(
driving_width
>
kLBufferThreshold
)
{
if
(
driving_width
>
kLBufferThreshold
)
{
continue
;
continue
;
...
@@ -244,7 +243,7 @@ Stage::StageStatus SidePassDetectSafety::Process(
...
@@ -244,7 +243,7 @@ Stage::StageStatus SidePassDetectSafety::Process(
const
auto
adc_frenet_frame_point_
=
const
auto
adc_frenet_frame_point_
=
reference_line_info
.
reference_line
().
GetFrenetPoint
(
reference_line_info
.
reference_line
().
GetFrenetPoint
(
frame
->
PlanningStartPoint
());
frame
->
PlanningStartPoint
()
.
path_point
()
);
bool
trim_success
=
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
bool
trim_success
=
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
adc_frenet_frame_point_
.
s
(),
adc_frenet_frame_point_
.
l
());
adc_frenet_frame_point_
.
s
(),
adc_frenet_frame_point_
.
l
());
...
@@ -305,7 +304,7 @@ Stage::StageStatus SidePassPassObstacle::Process(
...
@@ -305,7 +304,7 @@ Stage::StageStatus SidePassPassObstacle::Process(
const
auto
adc_frenet_frame_point_
=
const
auto
adc_frenet_frame_point_
=
reference_line_info
.
reference_line
().
GetFrenetPoint
(
reference_line_info
.
reference_line
().
GetFrenetPoint
(
frame
->
PlanningStartPoint
());
frame
->
PlanningStartPoint
()
.
path_point
()
);
bool
trim_success
=
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
bool
trim_success
=
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
adc_frenet_frame_point_
.
s
(),
adc_frenet_frame_point_
.
l
());
adc_frenet_frame_point_
.
s
(),
adc_frenet_frame_point_
.
l
());
...
...
modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.cc
浏览文件 @
f5b34cc1
...
@@ -56,7 +56,7 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
...
@@ -56,7 +56,7 @@ Stage::StageStatus SidePassStopOnWaitPoint::Process(
}
}
const
auto
adc_frenet_frame_point_
=
const
auto
adc_frenet_frame_point_
=
reference_line
.
GetFrenetPoint
(
frame
->
PlanningStartPoint
());
reference_line
.
GetFrenetPoint
(
frame
->
PlanningStartPoint
()
.
path_point
()
);
if
(
!
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
adc_frenet_frame_point_
.
s
(),
if
(
!
GetContext
()
->
path_data_
.
LeftTrimWithRefS
(
adc_frenet_frame_point_
.
s
(),
adc_frenet_frame_point_
.
l
()))
{
adc_frenet_frame_point_
.
l
()))
{
...
...
modules/planning/std_planning.cc
浏览文件 @
f5b34cc1
...
@@ -301,6 +301,9 @@ void StdPlanning::RunOnce(const LocalView& local_view,
...
@@ -301,6 +301,9 @@ void StdPlanning::RunOnce(const LocalView& local_view,
status
=
Plan
(
start_timestamp
,
stitching_trajectory
,
trajectory_pb
);
status
=
Plan
(
start_timestamp
,
stitching_trajectory
,
trajectory_pb
);
for
(
const
auto
&
p
:
trajectory_pb
->
trajectory_point
())
{
ADEBUG
<<
p
.
DebugString
();
}
const
auto
time_diff_ms
=
(
Clock
::
NowInSeconds
()
-
start_timestamp
)
*
1000
;
const
auto
time_diff_ms
=
(
Clock
::
NowInSeconds
()
-
start_timestamp
)
*
1000
;
ADEBUG
<<
"total planning time spend: "
<<
time_diff_ms
<<
" ms."
;
ADEBUG
<<
"total planning time spend: "
<<
time_diff_ms
<<
" ms."
;
...
...
modules/planning/toolkits/deciders/side_pass_path_decider.cc
浏览文件 @
f5b34cc1
...
@@ -43,7 +43,7 @@ using apollo::common::util::MakePointENU;
...
@@ -43,7 +43,7 @@ using apollo::common::util::MakePointENU;
using
apollo
::
hdmap
::
HDMapUtil
;
using
apollo
::
hdmap
::
HDMapUtil
;
constexpr
double
kRoadBuffer
=
0.2
;
constexpr
double
kRoadBuffer
=
0.2
;
constexpr
double
kObstacleLBuffer
=
0.2
;
constexpr
double
kObstacleLBuffer
=
1.0
;
constexpr
double
kObstacleSBuffer
=
0.5
;
constexpr
double
kObstacleSBuffer
=
0.5
;
constexpr
double
kSidePassPathLength
=
50.0
;
constexpr
double
kSidePassPathLength
=
50.0
;
...
@@ -76,7 +76,7 @@ Status SidePassPathDecider::Process(
...
@@ -76,7 +76,7 @@ Status SidePassPathDecider::Process(
adc_planning_start_point_
=
frame
->
PlanningStartPoint
();
adc_planning_start_point_
=
frame
->
PlanningStartPoint
();
adc_frenet_frame_point_
=
adc_frenet_frame_point_
=
reference_line_info
->
reference_line
().
GetFrenetPoint
(
reference_line_info
->
reference_line
().
GetFrenetPoint
(
frame
->
PlanningStartPoint
());
frame
->
PlanningStartPoint
()
.
path_point
()
);
InitSolver
();
InitSolver
();
nearest_obstacle_
=
nearest_obstacle_
=
...
@@ -205,7 +205,9 @@ bool SidePassPathDecider::GeneratePath(
...
@@ -205,7 +205,9 @@ bool SidePassPathDecider::GeneratePath(
for
(
size_t
i
=
0
;
i
<
fem_qp_
->
x
().
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
<
fem_qp_
->
x
().
size
();
++
i
)
{
common
::
FrenetFramePoint
frenet_frame_point
;
common
::
FrenetFramePoint
frenet_frame_point
;
ADEBUG
<<
"FrenetFramePath: s = "
<<
accumulated_s
ADEBUG
<<
"FrenetFramePath: s = "
<<
accumulated_s
<<
", l = "
<<
fem_qp_
->
x
()[
i
];
<<
", l = "
<<
fem_qp_
->
x
()[
i
]
<<
", dl = "
<<
fem_qp_
->
x_derivative
()[
i
]
<<
", ddl = "
<<
fem_qp_
->
x_second_order_derivative
()[
i
];
if
(
accumulated_s
>=
reference_line_info
->
reference_line
().
Length
())
{
if
(
accumulated_s
>=
reference_line_info
->
reference_line
().
Length
())
{
break
;
break
;
}
}
...
...
modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc
浏览文件 @
f5b34cc1
...
@@ -211,7 +211,8 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds(
...
@@ -211,7 +211,8 @@ QpPiecewiseJerkPathOptimizer::GetLateralBounds(
Status
QpPiecewiseJerkPathOptimizer
::
Process
(
Status
QpPiecewiseJerkPathOptimizer
::
Process
(
const
SpeedData
&
speed_data
,
const
ReferenceLine
&
reference_line
,
const
SpeedData
&
speed_data
,
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
init_point
,
PathData
*
const
path_data
)
{
const
common
::
TrajectoryPoint
&
init_point
,
PathData
*
const
path_data
)
{
const
auto
frenet_point
=
reference_line
.
GetFrenetPoint
(
init_point
);
const
auto
frenet_point
=
reference_line
.
GetFrenetPoint
(
init_point
.
path_point
());
const
auto
&
qp_config
=
config_
.
qp_piecewise_jerk_path_config
();
const
auto
&
qp_config
=
config_
.
qp_piecewise_jerk_path_config
();
const
auto
&
adc_sl
=
reference_line_info_
->
AdcSlBoundary
();
const
auto
&
adc_sl
=
reference_line_info_
->
AdcSlBoundary
();
const
double
qp_delta_s
=
qp_config
.
qp_delta_s
();
const
double
qp_delta_s
=
qp_config
.
qp_delta_s
();
...
...
modules/planning/toolkits/optimizers/qp_spline_path/qp_spline_path_generator.cc
浏览文件 @
f5b34cc1
...
@@ -92,7 +92,7 @@ bool QpSplinePathGenerator::Generate(
...
@@ -92,7 +92,7 @@ bool QpSplinePathGenerator::Generate(
last_discretized_path_
=
&
path_data_history
.
back
().
first
;
last_discretized_path_
=
&
path_data_history
.
back
().
first
;
}
}
init_frenet_point_
=
reference_line_
.
GetFrenetPoint
(
init_point
);
init_frenet_point_
=
reference_line_
.
GetFrenetPoint
(
init_point
.
path_point
()
);
if
(
is_change_lane_path_
)
{
if
(
is_change_lane_path_
)
{
ref_l_
=
init_frenet_point_
.
l
();
ref_l_
=
init_frenet_point_
.
l
();
...
...
modules/planning/toolkits/optimizers/road_graph/dp_road_graph.cc
浏览文件 @
f5b34cc1
...
@@ -69,7 +69,8 @@ bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
...
@@ -69,7 +69,8 @@ bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
<<
init_point
.
DebugString
();
<<
init_point
.
DebugString
();
return
false
;
return
false
;
}
}
init_frenet_frame_point_
=
reference_line_
.
GetFrenetPoint
(
init_point_
);
init_frenet_frame_point_
=
reference_line_
.
GetFrenetPoint
(
init_point_
.
path_point
());
waypoint_sampler_
->
Init
(
&
reference_line_info_
,
init_sl_point_
,
waypoint_sampler_
->
Init
(
&
reference_line_info_
,
init_sl_point_
,
init_frenet_frame_point_
);
init_frenet_frame_point_
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录