Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
6b439cc8
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,发现更多精彩内容 >>
提交
6b439cc8
编写于
8月 05, 2017
作者:
D
Dong Li
提交者:
Jiangtao Hu
8月 05, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
refactor reference line smoother
上级
82952d2b
变更
11
隐藏空白更改
内联
并排
Showing
11 changed file
with
52 addition
and
56 deletion
+52
-56
modules/planning/common/frame.cc
modules/planning/common/frame.cc
+9
-37
modules/planning/common/frame.h
modules/planning/common/frame.h
+4
-4
modules/planning/integration_tests/planning_test_base.cc
modules/planning/integration_tests/planning_test_base.cc
+2
-1
modules/planning/optimizer/qp_spline_path/BUILD
modules/planning/optimizer/qp_spline_path/BUILD
+3
-0
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer_test.cc
...optimizer/qp_spline_path/qp_spline_path_optimizer_test.cc
+3
-0
modules/planning/planning_test.cc
modules/planning/planning_test.cc
+2
-0
modules/planning/reference_line/reference_line.cc
modules/planning/reference_line/reference_line.cc
+10
-0
modules/planning/reference_line/reference_line.h
modules/planning/reference_line/reference_line.h
+2
-0
modules/planning/reference_line/reference_line_smoother.cc
modules/planning/reference_line/reference_line_smoother.cc
+10
-8
modules/planning/reference_line/reference_line_smoother.h
modules/planning/reference_line/reference_line_smoother.h
+1
-1
modules/planning/reference_line/reference_line_smoother_test.cc
...s/planning/reference_line/reference_line_smoother_test.cc
+6
-5
未找到文件。
modules/planning/common/frame.cc
浏览文件 @
6b439cc8
...
@@ -40,7 +40,12 @@ void Frame::SetMap(hdmap::PncMap *pnc_map) { pnc_map_ = pnc_map; }
...
@@ -40,7 +40,12 @@ void Frame::SetMap(hdmap::PncMap *pnc_map) { pnc_map_ = pnc_map; }
FrameHistory
::
FrameHistory
()
FrameHistory
::
FrameHistory
()
:
IndexedQueue
<
uint32_t
,
Frame
>
(
FLAGS_max_history_frame_num
)
{}
:
IndexedQueue
<
uint32_t
,
Frame
>
(
FLAGS_max_history_frame_num
)
{}
Frame
::
Frame
(
const
uint32_t
sequence_num
)
:
sequence_num_
(
sequence_num
)
{}
Frame
::
Frame
(
const
uint32_t
sequence_num
)
:
sequence_num_
(
sequence_num
)
{
DCHECK
(
common
::
util
::
GetProtoFromFile
(
FLAGS_reference_line_smoother_config_file
,
&
smoother_config_
))
<<
"Failed to init reference line smoother config with file "
<<
FLAGS_reference_line_smoother_config_file
;
}
void
Frame
::
SetVehicleInitPose
(
const
localization
::
Pose
&
pose
)
{
void
Frame
::
SetVehicleInitPose
(
const
localization
::
Pose
&
pose
)
{
init_pose_
=
pose
;
init_pose_
=
pose
;
...
@@ -151,45 +156,12 @@ bool Frame::CreateReferenceLineFromRouting(
...
@@ -151,45 +156,12 @@ bool Frame::CreateReferenceLineFromRouting(
return
false
;
return
false
;
}
}
reference_lines
->
emplace_back
(
ReferenceLine
());
reference_lines
->
emplace_back
(
ReferenceLine
());
if
(
!
SmoothReferenceLine
(
hdmap_path
,
&
reference_lines
->
back
()))
{
AERROR
<<
"Failed to smooth reference line"
;
return
false
;
}
return
true
;
}
bool
Frame
::
SmoothReferenceLine
(
const
hdmap
::
Path
&
hdmap_path
,
ReferenceLine
*
const
reference_line
)
{
CHECK_NOTNULL
(
reference_line
);
std
::
vector
<
ReferencePoint
>
ref_points
;
for
(
const
auto
&
point
:
hdmap_path
.
path_points
())
{
if
(
point
.
lane_waypoints
().
empty
())
{
AERROR
<<
"path point has no lane_waypoint"
;
return
false
;
}
const
auto
&
lane_waypoint
=
point
.
lane_waypoints
()[
0
];
ref_points
.
emplace_back
(
point
,
point
.
heading
(),
0.0
,
0.0
,
lane_waypoint
);
}
if
(
ref_points
.
empty
())
{
AERROR
<<
"Found no reference points from map"
;
return
false
;
}
*
reference_line
=
ReferenceLine
(
ref_points
);
std
::
vector
<
ReferencePoint
>
smoothed_ref_points
;
ReferenceLineSmoother
smoother
;
ReferenceLineSmoother
smoother
;
if
(
!
smoother
.
Init
(
FLAGS_reference_line_smoother_config_file
))
{
smoother
.
Init
(
smoother_config_
);
AERROR
<<
"Failed to load file "
if
(
!
smoother
.
smooth
(
ReferenceLine
(
hdmap_path
),
&
reference_lines
->
back
()))
{
<<
FLAGS_reference_line_smoother_config_file
;
AERROR
<<
"Failed to smooth reference line"
;
return
false
;
}
if
(
!
smoother
.
smooth
(
*
reference_line
,
&
smoothed_ref_points
))
{
AERROR
<<
"Fail to smooth a reference line from map"
;
return
false
;
return
false
;
}
}
*
reference_line
=
ReferenceLine
(
smoothed_ref_points
);
ADEBUG
<<
"smooth reference points num:"
<<
smoothed_ref_points
.
size
();
return
true
;
return
true
;
}
}
...
...
modules/planning/common/frame.h
浏览文件 @
6b439cc8
...
@@ -27,6 +27,9 @@
...
@@ -27,6 +27,9 @@
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/proto/geometry.pb.h"
#include "modules/localization/proto/pose.pb.h"
#include "modules/localization/proto/pose.pb.h"
#include "modules/planning/proto/reference_line_smoother_config.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/routing/proto/routing.pb.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/map/pnc_map/pnc_map.h"
...
@@ -35,8 +38,6 @@
...
@@ -35,8 +38,6 @@
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/planning_data.h"
#include "modules/planning/common/planning_data.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/routing/proto/routing.pb.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -96,8 +97,6 @@ class Frame {
...
@@ -96,8 +97,6 @@ class Frame {
* TODO move this function to a helper class in
* TODO move this function to a helper class in
* modules/planning/reference_line folder
* modules/planning/reference_line folder
*/
*/
bool
SmoothReferenceLine
(
const
hdmap
::
Path
&
hdmap_path
,
ReferenceLine
*
const
reference_line
);
/**
/**
* @brief create obstacles from prediction input.
* @brief create obstacles from prediction input.
...
@@ -133,6 +132,7 @@ class Frame {
...
@@ -133,6 +132,7 @@ class Frame {
ReferenceLine
reference_line_
;
ReferenceLine
reference_line_
;
PlanningData
_planning_data
;
PlanningData
_planning_data
;
static
const
hdmap
::
PncMap
*
pnc_map_
;
static
const
hdmap
::
PncMap
*
pnc_map_
;
ReferenceLineSmootherConfig
smoother_config_
;
ADCTrajectory
trajectory_pb_
;
// planning output pb
ADCTrajectory
trajectory_pb_
;
// planning output pb
};
};
...
...
modules/planning/integration_tests/planning_test_base.cc
浏览文件 @
6b439cc8
...
@@ -68,7 +68,8 @@ bool PlanningTestBase::SetUpAdapters() {
...
@@ -68,7 +68,8 @@ bool PlanningTestBase::SetUpAdapters() {
<<
FLAGS_adapter_config_path
;
<<
FLAGS_adapter_config_path
;
return
false
;
return
false
;
}
}
if
(
!
AdapterManager
::
FeedRoutingResponseFile
(
FLAGS_test_routing_result_file
))
{
if
(
!
AdapterManager
::
FeedRoutingResponseFile
(
FLAGS_test_routing_result_file
))
{
AERROR
<<
"failed to routing file: "
<<
FLAGS_test_routing_result_file
;
AERROR
<<
"failed to routing file: "
<<
FLAGS_test_routing_result_file
;
return
false
;
return
false
;
}
}
...
...
modules/planning/optimizer/qp_spline_path/BUILD
浏览文件 @
6b439cc8
...
@@ -50,6 +50,9 @@ cc_test(
...
@@ -50,6 +50,9 @@ cc_test(
srcs
=
[
srcs
=
[
"qp_spline_path_optimizer_test.cc"
,
"qp_spline_path_optimizer_test.cc"
,
],
],
data
=
[
"//modules/planning:planning_testdata"
,
],
deps
=
[
deps
=
[
":qp_spline_path"
,
":qp_spline_path"
,
"@gtest//:main"
,
"@gtest//:main"
,
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer_test.cc
浏览文件 @
6b439cc8
...
@@ -18,6 +18,7 @@
...
@@ -18,6 +18,7 @@
#include <utility>
#include <utility>
#include "gtest/gtest.h"
#include "gtest/gtest.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
namespace
apollo
{
namespace
apollo
{
...
@@ -26,6 +27,8 @@ namespace planning {
...
@@ -26,6 +27,8 @@ namespace planning {
class
QpSplinePathOptimizerTest
:
public
::
testing
::
Test
{
class
QpSplinePathOptimizerTest
:
public
::
testing
::
Test
{
public:
public:
virtual
void
SetUp
()
{
virtual
void
SetUp
()
{
FLAGS_reference_line_smoother_config_file
=
"modules/planning/testdata/conf/reference_line_smoother_config.pb.txt"
;
qp_spline_path_optimizer_
.
reset
(
new
QpSplinePathOptimizer
(
"QP_PATH"
));
qp_spline_path_optimizer_
.
reset
(
new
QpSplinePathOptimizer
(
"QP_PATH"
));
}
}
...
...
modules/planning/planning_test.cc
浏览文件 @
6b439cc8
...
@@ -36,6 +36,8 @@ class PlanningTest : public ::testing::Test {
...
@@ -36,6 +36,8 @@ class PlanningTest : public ::testing::Test {
"modules/planning/testdata/conf/planning_config.pb.txt"
;
"modules/planning/testdata/conf/planning_config.pb.txt"
;
FLAGS_adapter_config_path
=
"modules/planning/testdata/conf/adapter.conf"
;
FLAGS_adapter_config_path
=
"modules/planning/testdata/conf/adapter.conf"
;
FLAGS_reference_line_smoother_config_file
=
"modules/planning/testdata/conf/reference_line_smoother_config.pb.txt"
;
FLAGS_enable_record_debug
=
false
;
FLAGS_enable_record_debug
=
false
;
}
}
...
...
modules/planning/reference_line/reference_line.cc
浏览文件 @
6b439cc8
...
@@ -46,6 +46,16 @@ ReferenceLine::ReferenceLine(
...
@@ -46,6 +46,16 @@ ReferenceLine::ReferenceLine(
map_path_
(
MapPath
(
std
::
vector
<
hdmap
::
MapPathPoint
>
(
map_path_
(
MapPath
(
std
::
vector
<
hdmap
::
MapPathPoint
>
(
reference_points
.
begin
(),
reference_points
.
end
())))
{}
reference_points
.
begin
(),
reference_points
.
end
())))
{}
ReferenceLine
::
ReferenceLine
(
const
MapPath
&
hdmap_path
)
:
map_path_
(
hdmap_path
)
{
for
(
const
auto
&
point
:
hdmap_path
.
path_points
())
{
DCHECK
(
!
point
.
lane_waypoints
().
empty
());
const
auto
&
lane_waypoint
=
point
.
lane_waypoints
()[
0
];
reference_points_
.
emplace_back
(
point
,
point
.
heading
(),
0.0
,
0.0
,
lane_waypoint
);
}
}
ReferenceLine
::
ReferenceLine
(
ReferenceLine
::
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
const
std
::
vector
<
hdmap
::
LaneSegment
>&
lane_segments
,
const
std
::
vector
<
hdmap
::
LaneSegment
>&
lane_segments
,
...
...
modules/planning/reference_line/reference_line.h
浏览文件 @
6b439cc8
...
@@ -38,6 +38,7 @@ class ReferenceLine {
...
@@ -38,6 +38,7 @@ class ReferenceLine {
public:
public:
ReferenceLine
()
=
default
;
ReferenceLine
()
=
default
;
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
);
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
);
ReferenceLine
(
const
hdmap
::
Path
&
hdmap_path
);
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
const
std
::
vector
<
hdmap
::
LaneSegment
>&
lane_segments
,
const
std
::
vector
<
hdmap
::
LaneSegment
>&
lane_segments
,
const
double
max_approximation_error
);
const
double
max_approximation_error
);
...
@@ -80,6 +81,7 @@ class ReferenceLine {
...
@@ -80,6 +81,7 @@ class ReferenceLine {
const
double
s1
,
const
double
x
,
const
double
s1
,
const
double
x
,
const
double
y
);
const
double
y
);
private:
std
::
vector
<
ReferencePoint
>
reference_points_
;
std
::
vector
<
ReferencePoint
>
reference_points_
;
hdmap
::
Path
map_path_
;
hdmap
::
Path
map_path_
;
};
};
...
...
modules/planning/reference_line/reference_line_smoother.cc
浏览文件 @
6b439cc8
...
@@ -54,8 +54,9 @@ void ReferenceLineSmoother::Reset() {
...
@@ -54,8 +54,9 @@ void ReferenceLineSmoother::Reset() {
bool
ReferenceLineSmoother
::
smooth
(
bool
ReferenceLineSmoother
::
smooth
(
const
ReferenceLine
&
raw_reference_line
,
const
ReferenceLine
&
raw_reference_line
,
std
::
vector
<
ReferencePoint
>*
const
smoothed_ref
_line
)
{
ReferenceLine
*
const
smoothed_reference
_line
)
{
Reset
();
Reset
();
std
::
vector
<
ReferencePoint
>
ref_points
;
if
(
!
sampling
(
raw_reference_line
))
{
if
(
!
sampling
(
raw_reference_line
))
{
AERROR
<<
"Fail to sample reference line smoother points!"
;
AERROR
<<
"Fail to sample reference line smoother points!"
;
return
false
;
return
false
;
...
@@ -85,8 +86,8 @@ bool ReferenceLineSmoother::smooth(
...
@@ -85,8 +86,8 @@ bool ReferenceLineSmoother::smooth(
const
double
resolution
=
const
double
resolution
=
(
end_t
-
start_t
)
/
(
smoother_config_
.
num_of_total_points
()
-
1
);
(
end_t
-
start_t
)
/
(
smoother_config_
.
num_of_total_points
()
-
1
);
double
t
=
start_t
;
double
t
=
start_t
;
for
(
std
::
uint32_t
i
=
0
;
i
<
smoother_config_
.
num_of_total_points
()
&&
for
(
std
::
uint32_t
i
=
0
;
t
<
end_t
;
++
i
)
{
i
<
smoother_config_
.
num_of_total_points
()
&&
t
<
end_t
;
++
i
)
{
std
::
pair
<
double
,
double
>
xy
=
spline_solver_
->
spline
()(
t
);
std
::
pair
<
double
,
double
>
xy
=
spline_solver_
->
spline
()(
t
);
const
double
heading
=
std
::
atan2
(
spline_solver_
->
spline
().
derivative_y
(
t
),
const
double
heading
=
std
::
atan2
(
spline_solver_
->
spline
().
derivative_y
(
t
),
spline_solver_
->
spline
().
derivative_x
(
t
));
spline_solver_
->
spline
().
derivative_x
(
t
));
...
@@ -108,19 +109,20 @@ bool ReferenceLineSmoother::smooth(
...
@@ -108,19 +109,20 @@ bool ReferenceLineSmoother::smooth(
return
false
;
return
false
;
}
}
ReferencePoint
rlp
=
raw_reference_line
.
get_reference_point
(
s
);
ReferencePoint
rlp
=
raw_reference_line
.
get_reference_point
(
s
);
ReferencePoint
new_rlp
(
common
::
math
::
Vec2d
(
xy
.
first
,
xy
.
second
),
heading
,
ref_points
.
emplace_back
(
kappa
,
dkappa
,
rlp
.
lane_waypoints
());
ReferencePoint
(
common
::
math
::
Vec2d
(
xy
.
first
,
xy
.
second
),
heading
,
kappa
,
smoothed_ref_line
->
push_back
(
std
::
move
(
new_rlp
));
dkappa
,
rlp
.
lane_waypoints
()
));
t
=
start_t
+
(
i
+
1
)
*
resolution
;
t
=
start_t
+
(
i
+
1
)
*
resolution
;
}
}
*
smoothed_reference_line
=
ReferenceLine
(
ref_points
);
return
true
;
return
true
;
}
}
bool
ReferenceLineSmoother
::
sampling
(
const
ReferenceLine
&
raw_reference_line
)
{
bool
ReferenceLineSmoother
::
sampling
(
const
ReferenceLine
&
raw_reference_line
)
{
const
double
length
=
raw_reference_line
.
length
();
const
double
length
=
raw_reference_line
.
length
();
const
double
resolution
=
length
/
smoother_config_
.
num_spline
();
const
double
resolution
=
length
/
smoother_config_
.
num_spline
();
for
(
std
::
uint32_t
i
=
0
;
i
<=
smoother_config_
.
num_spline
()
&&
for
(
std
::
uint32_t
i
=
0
;
i
*
resolution
<=
length
;
++
i
)
{
i
<=
smoother_config_
.
num_spline
()
&&
i
*
resolution
<=
length
;
++
i
)
{
ReferencePoint
rlp
=
raw_reference_line
.
get_reference_point
(
resolution
*
i
);
ReferencePoint
rlp
=
raw_reference_line
.
get_reference_point
(
resolution
*
i
);
common
::
PathPoint
path_point
;
common
::
PathPoint
path_point
;
path_point
.
set_x
(
rlp
.
x
());
path_point
.
set_x
(
rlp
.
x
());
...
...
modules/planning/reference_line/reference_line_smoother.h
浏览文件 @
6b439cc8
...
@@ -40,7 +40,7 @@ class ReferenceLineSmoother {
...
@@ -40,7 +40,7 @@ class ReferenceLineSmoother {
bool
Init
(
const
std
::
string
&
config_file
);
bool
Init
(
const
std
::
string
&
config_file
);
void
Init
(
const
ReferenceLineSmootherConfig
&
refline_smooth_config
);
void
Init
(
const
ReferenceLineSmootherConfig
&
refline_smooth_config
);
bool
smooth
(
const
ReferenceLine
&
raw_reference_line
,
bool
smooth
(
const
ReferenceLine
&
raw_reference_line
,
std
::
vector
<
ReferencePoint
>*
const
smoothed_ref
_line
);
ReferenceLine
*
const
smoothed_reference
_line
);
private:
private:
void
Reset
();
void
Reset
();
...
...
modules/planning/reference_line/reference_line_smoother_test.cc
浏览文件 @
6b439cc8
...
@@ -43,7 +43,8 @@ class ReferenceLineSmootherTest : public ::testing::Test {
...
@@ -43,7 +43,8 @@ class ReferenceLineSmootherTest : public ::testing::Test {
<<
map_file
;
<<
map_file
;
return
;
return
;
}
}
smoother_
.
Init
(
config_
);
// use the default value in config.
ReferenceLineSmootherConfig
config
;
smoother_
.
Init
(
config
);
// use the default value in config.
std
::
vector
<
ReferencePoint
>
ref_points
;
std
::
vector
<
ReferencePoint
>
ref_points
;
const
auto
&
points
=
lane_info_ptr
->
points
();
const
auto
&
points
=
lane_info_ptr
->
points
();
...
@@ -56,7 +57,6 @@ class ReferenceLineSmootherTest : public ::testing::Test {
...
@@ -56,7 +57,6 @@ class ReferenceLineSmootherTest : public ::testing::Test {
}
}
const
std
::
string
map_file
=
"modules/planning/testdata/base_map.txt"
;
const
std
::
string
map_file
=
"modules/planning/testdata/base_map.txt"
;
ReferenceLineSmootherConfig
config_
;
hdmap
::
HDMap
hdmap_
;
hdmap
::
HDMap
hdmap_
;
ReferenceLineSmoother
smoother_
;
ReferenceLineSmoother
smoother_
;
...
@@ -66,9 +66,10 @@ class ReferenceLineSmootherTest : public ::testing::Test {
...
@@ -66,9 +66,10 @@ class ReferenceLineSmootherTest : public ::testing::Test {
};
};
TEST_F
(
ReferenceLineSmootherTest
,
smooth
)
{
TEST_F
(
ReferenceLineSmootherTest
,
smooth
)
{
std
::
vector
<
ReferencePoint
>
smoothed_ref_points
;
ReferenceLine
smoothed_reference_line
;
EXPECT_TRUE
(
smoother_
.
smooth
(
*
reference_line_
,
&
smoothed_ref_points
));
EXPECT_FLOAT_EQ
(
153.87421
,
reference_line_
->
length
());
EXPECT_FALSE
(
smoothed_ref_points
.
empty
());
EXPECT_TRUE
(
smoother_
.
smooth
(
*
reference_line_
,
&
smoothed_reference_line
));
EXPECT_FLOAT_EQ
(
153.84697
,
smoothed_reference_line
.
length
());
}
}
}
// namespace planning
}
// namespace planning
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录