Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
aabf0612
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,体验更适合开发者的 AI 搜索 >>
提交
aabf0612
编写于
7月 26, 2017
作者:
D
Dong Li
提交者:
Jiangtao Hu
7月 26, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
added path_sampler_test
上级
dc09f9e5
变更
11
隐藏空白更改
内联
并排
Showing
11 changed file
with
208 addition
and
28 deletion
+208
-28
modules/planning/common/data_center.cc
modules/planning/common/data_center.cc
+1
-2
modules/planning/common/data_center.h
modules/planning/common/data_center.h
+1
-1
modules/planning/common/frame.cc
modules/planning/common/frame.cc
+2
-12
modules/planning/common/frame.h
modules/planning/common/frame.h
+1
-2
modules/planning/optimizer/dp_poly_path/BUILD
modules/planning/optimizer/dp_poly_path/BUILD
+21
-0
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
+2
-2
modules/planning/optimizer/dp_poly_path/path_sampler.cc
modules/planning/optimizer/dp_poly_path/path_sampler.cc
+2
-3
modules/planning/optimizer/dp_poly_path/path_sampler.h
modules/planning/optimizer/dp_poly_path/path_sampler.h
+4
-5
modules/planning/optimizer/dp_poly_path/path_sampler_test.cc
modules/planning/optimizer/dp_poly_path/path_sampler_test.cc
+150
-0
modules/planning/planning.cc
modules/planning/planning.cc
+1
-1
modules/planning/testdata/garage_routing.pb.txt
modules/planning/testdata/garage_routing.pb.txt
+23
-0
未找到文件。
modules/planning/common/data_center.cc
浏览文件 @
aabf0612
...
@@ -114,9 +114,8 @@ bool DataCenter::CreateReferenceLineFromMap() {
...
@@ -114,9 +114,8 @@ bool DataCenter::CreateReferenceLineFromMap() {
return
true
;
return
true
;
}
}
bool
DataCenter
::
init_frame
(
const
uint32_t
sequence_num
)
{
bool
DataCenter
::
init_
current_
frame
(
const
uint32_t
sequence_num
)
{
_frame
.
reset
(
new
Frame
(
sequence_num
));
_frame
.
reset
(
new
Frame
(
sequence_num
));
_frame
->
set_planning_data
(
new
PlanningData
());
if
(
!
CreateReferenceLineFromMap
())
{
if
(
!
CreateReferenceLineFromMap
())
{
AERROR
<<
"failed to create reference line"
;
AERROR
<<
"failed to create reference line"
;
...
...
modules/planning/common/data_center.h
浏览文件 @
aabf0612
...
@@ -41,7 +41,7 @@ class DataCenter {
...
@@ -41,7 +41,7 @@ class DataCenter {
Frame
*
frame
(
const
uint32_t
sequence_num
)
const
;
Frame
*
frame
(
const
uint32_t
sequence_num
)
const
;
public:
public:
bool
init_frame
(
const
uint32_t
sequence_num
);
bool
init_
current_
frame
(
const
uint32_t
sequence_num
);
Frame
*
current_frame
()
const
;
Frame
*
current_frame
()
const
;
void
save_frame
();
void
save_frame
();
const
::
apollo
::
hdmap
::
HDMap
&
map
()
{
return
map_
;
}
const
::
apollo
::
hdmap
::
HDMap
&
map
()
{
return
map_
;
}
...
...
modules/planning/common/frame.cc
浏览文件 @
aabf0612
...
@@ -33,21 +33,11 @@ void Frame::set_sequence_num(const uint32_t sequence_num) {
...
@@ -33,21 +33,11 @@ void Frame::set_sequence_num(const uint32_t sequence_num) {
_sequence_num
=
sequence_num
;
_sequence_num
=
sequence_num
;
}
}
void
Frame
::
set_planning_data
(
PlanningData
*
const
planning_data
)
{
_planning_data
.
reset
(
planning_data
);
}
uint32_t
Frame
::
sequence_num
()
const
{
return
_sequence_num
;
}
uint32_t
Frame
::
sequence_num
()
const
{
return
_sequence_num
;
}
const
PlanningData
&
Frame
::
planning_data
()
const
{
const
PlanningData
&
Frame
::
planning_data
()
const
{
return
_planning_data
;
}
CHECK_NOTNULL
(
_planning_data
.
get
());
return
*
(
_planning_data
.
get
());
}
PlanningData
*
Frame
::
mutable_planning_data
()
{
PlanningData
*
Frame
::
mutable_planning_data
()
{
return
&
_planning_data
;
}
CHECK_NOTNULL
(
_planning_data
.
get
());
return
_planning_data
.
get
();
}
void
Frame
::
set_computed_trajectory
(
const
PublishableTrajectory
&
trajectory
)
{
void
Frame
::
set_computed_trajectory
(
const
PublishableTrajectory
&
trajectory
)
{
_computed_trajectory
=
trajectory
;
_computed_trajectory
=
trajectory
;
...
...
modules/planning/common/frame.h
浏览文件 @
aabf0612
...
@@ -35,7 +35,6 @@ class Frame {
...
@@ -35,7 +35,6 @@ class Frame {
explicit
Frame
(
uint32_t
sequence_num
);
explicit
Frame
(
uint32_t
sequence_num
);
void
set_sequence_num
(
const
uint32_t
sequence_num
);
void
set_sequence_num
(
const
uint32_t
sequence_num
);
void
set_planning_data
(
PlanningData
*
const
planning_data
);
uint32_t
sequence_num
()
const
;
uint32_t
sequence_num
()
const
;
const
PlanningData
&
planning_data
()
const
;
const
PlanningData
&
planning_data
()
const
;
...
@@ -49,7 +48,7 @@ class Frame {
...
@@ -49,7 +48,7 @@ class Frame {
private:
private:
uint32_t
_sequence_num
;
uint32_t
_sequence_num
;
PublishableTrajectory
_computed_trajectory
;
PublishableTrajectory
_computed_trajectory
;
std
::
unique_ptr
<
PlanningData
>
_planning_data
;
PlanningData
_planning_data
;
};
};
}
// namespace planning
}
// namespace planning
...
...
modules/planning/optimizer/dp_poly_path/BUILD
浏览文件 @
aabf0612
...
@@ -54,4 +54,25 @@ cc_library(
...
@@ -54,4 +54,25 @@ cc_library(
],
],
)
)
cc_test
(
name
=
"path_sampler_test"
,
size
=
"small"
,
srcs
=
[
"path_sampler_test.cc"
,
],
data
=
[
"//modules/map:map_data"
,
"//modules/planning:planning_testdata"
,
],
deps
=
[
":dp_poly_path"
,
"//modules/common:log"
,
"//modules/common/adapters:adapter_manager"
,
"//modules/common/util"
,
"//modules/planning/common:data_center"
,
"//modules/planning/proto:dp_poly_path_config_proto"
,
"@gtest//:main"
,
],
)
cpplint
()
cpplint
()
modules/planning/optimizer/dp_poly_path/dp_road_graph.cc
浏览文件 @
aabf0612
...
@@ -183,8 +183,8 @@ bool DpRoadGraph::init(const ReferenceLine &reference_line) {
...
@@ -183,8 +183,8 @@ bool DpRoadGraph::init(const ReferenceLine &reference_line) {
Status
DpRoadGraph
::
generate_graph
(
const
ReferenceLine
&
reference_line
)
{
Status
DpRoadGraph
::
generate_graph
(
const
ReferenceLine
&
reference_line
)
{
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
points
;
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
points
;
PathSampler
path_sampler
(
_config
);
PathSampler
path_sampler
(
_config
);
if
(
!
path_sampler
.
sample
(
reference_line
,
_init_point
,
_init_sl_point
,
&
points
)
if
(
!
path_sampler
.
sample
(
reference_line
,
_init_point
,
_init_sl_point
,
.
ok
(
))
{
&
points
))
{
const
std
::
string
msg
=
"Fail to sampling point with path sampler!"
;
const
std
::
string
msg
=
"Fail to sampling point with path sampler!"
;
AERROR
<<
msg
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
...
...
modules/planning/optimizer/dp_poly_path/path_sampler.cc
浏览文件 @
aabf0612
...
@@ -36,7 +36,7 @@ using SLPoint = apollo::common::SLPoint;
...
@@ -36,7 +36,7 @@ using SLPoint = apollo::common::SLPoint;
PathSampler
::
PathSampler
(
const
DpPolyPathConfig
&
config
)
:
_config
(
config
)
{}
PathSampler
::
PathSampler
(
const
DpPolyPathConfig
&
config
)
:
_config
(
config
)
{}
Status
PathSampler
::
sample
(
bool
PathSampler
::
sample
(
const
ReferenceLine
&
reference_line
,
const
ReferenceLine
&
reference_line
,
const
::
apollo
::
common
::
TrajectoryPoint
&
init_point
,
const
::
apollo
::
common
::
TrajectoryPoint
&
init_point
,
const
::
apollo
::
common
::
SLPoint
&
init_sl_point
,
const
::
apollo
::
common
::
SLPoint
&
init_sl_point
,
...
@@ -67,8 +67,7 @@ Status PathSampler::sample(
...
@@ -67,8 +67,7 @@ Status PathSampler::sample(
}
}
points
->
push_back
(
level_points
);
points
->
push_back
(
level_points
);
}
}
return
true
;
return
Status
::
OK
();
}
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/optimizer/dp_poly_path/path_sampler.h
浏览文件 @
aabf0612
...
@@ -34,11 +34,10 @@ class PathSampler {
...
@@ -34,11 +34,10 @@ class PathSampler {
public:
public:
explicit
PathSampler
(
const
DpPolyPathConfig
&
config
);
explicit
PathSampler
(
const
DpPolyPathConfig
&
config
);
~
PathSampler
()
=
default
;
~
PathSampler
()
=
default
;
apollo
::
common
::
Status
sample
(
bool
sample
(
const
ReferenceLine
&
reference_line
,
const
ReferenceLine
&
reference_line
,
const
common
::
TrajectoryPoint
&
init_point
,
const
::
apollo
::
common
::
TrajectoryPoint
&
init_point
,
const
common
::
SLPoint
&
init_sl_point
,
const
common
::
SLPoint
&
init_sl_point
,
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
*
const
points
);
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
*
const
points
);
private:
private:
DpPolyPathConfig
_config
;
DpPolyPathConfig
_config
;
...
...
modules/planning/optimizer/dp_poly_path/path_sampler_test.cc
0 → 100644
浏览文件 @
aabf0612
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "ros/include/ros/ros.h"
#include "modules/planning/proto/dp_poly_path_config.pb.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/util/file.h"
#include "modules/planning/common/data_center.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
namespace
apollo
{
namespace
planning
{
using
common
::
adapter
::
AdapterManager
;
class
PathSamplerTest
:
public
::
testing
::
Test
{
public:
virtual
void
SetUp
()
{
FLAGS_planning_config_file
=
"modules/planning/testdata/conf/planning_config.pb.txt"
;
FLAGS_adapter_config_path
=
"modules/planning/testdata/conf/adapter.conf"
;
FLAGS_map_filename
=
"modules/planning/testdata/base_map.txt"
;
FLAGS_reference_line_smoother_config_file
=
"modules/planning/testdata/conf/reference_line_smoother_config.pb.txt"
;
FLAGS_dp_poly_path_config_file
=
"modules/planning/testdata/conf/dp_poly_path_config.pb.txt"
;
AdapterManager
::
Init
(
FLAGS_adapter_config_path
);
if
(
!
AdapterManager
::
GetRoutingResult
())
{
AERROR
<<
"routing is not registered in adapter manager, check adapter "
"config file "
<<
FLAGS_adapter_config_path
;
return
;
}
AdapterManager
::
FeedRoutingResultFile
(
routing_file_
);
AdapterManager
::
Observe
();
auto
*
data_center
=
DataCenter
::
instance
();
if
(
!
data_center
->
init_current_frame
(
0
))
{
AERROR
<<
"Failed to init frame"
;
return
;
}
if
(
!
common
::
util
::
GetProtoFromFile
(
FLAGS_dp_poly_path_config_file
,
&
config_
))
{
AERROR
<<
"Failed to load file "
<<
FLAGS_dp_poly_path_config_file
;
return
;
}
frame_
=
data_center
->
current_frame
();
}
void
export_sl_points
(
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>&
points
,
const
std
::
string
&
filename
)
{
std
::
ofstream
ofs
(
filename
);
ofs
<<
"level, s, l"
<<
std
::
endl
;
int
level
=
0
;
for
(
const
auto
&
level_points
:
points
)
{
for
(
const
auto
&
point
:
level_points
)
{
ofs
<<
level
<<
", "
<<
point
.
s
()
<<
", "
<<
point
.
l
()
<<
std
::
endl
;
}
++
level
;
}
ofs
.
close
();
}
protected:
const
std
::
string
routing_file_
=
"modules/planning/testdata/garage_routing.pb.txt"
;
DpPolyPathConfig
config_
;
Frame
*
frame_
=
nullptr
;
};
TEST_F
(
PathSamplerTest
,
sample_one_point
)
{
ASSERT_TRUE
(
frame_
!=
nullptr
);
config_
.
set_sample_points_num_each_level
(
1
);
PathSampler
sampler
(
config_
);
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
sampled_points
;
common
::
TrajectoryPoint
init_point
;
init_point
.
mutable_path_point
()
->
set_x
(
586392.84003
);
init_point
.
mutable_path_point
()
->
set_y
(
4140673.01232
);
common
::
SLPoint
init_sl_point
;
init_sl_point
.
set_s
(
0.0
);
init_sl_point
.
set_l
(
0.0
);
const
auto
&
reference_line
=
frame_
->
planning_data
().
reference_line
();
const
double
reference_line_length
=
reference_line
.
reference_map_line
().
length
();
EXPECT_FLOAT_EQ
(
70.1723
,
reference_line_length
);
bool
sample_result
=
sampler
.
sample
(
reference_line
,
init_point
,
init_sl_point
,
&
sampled_points
);
EXPECT_TRUE
(
sample_result
);
EXPECT_EQ
(
8
,
sampled_points
.
size
());
EXPECT_EQ
(
1
,
sampled_points
[
0
].
size
());
EXPECT_EQ
(
1
,
sampled_points
[
7
].
size
());
EXPECT_FLOAT_EQ
(
40
,
sampled_points
[
7
][
0
].
s
());
EXPECT_FLOAT_EQ
(
0
,
sampled_points
[
7
][
0
].
l
());
// export_sl_points(sampled_points, "/tmp/points.txt");
}
TEST_F
(
PathSamplerTest
,
sample_three_points
)
{
ASSERT_TRUE
(
frame_
!=
nullptr
);
config_
.
set_sample_points_num_each_level
(
3
);
PathSampler
sampler
(
config_
);
std
::
vector
<
std
::
vector
<
common
::
SLPoint
>>
sampled_points
;
common
::
TrajectoryPoint
init_point
;
init_point
.
mutable_path_point
()
->
set_x
(
586392.84003
);
init_point
.
mutable_path_point
()
->
set_y
(
4140673.01232
);
common
::
SLPoint
init_sl_point
;
init_sl_point
.
set_s
(
0.0
);
init_sl_point
.
set_l
(
0.0
);
const
auto
&
reference_line
=
frame_
->
planning_data
().
reference_line
();
const
double
reference_line_length
=
reference_line
.
reference_map_line
().
length
();
EXPECT_FLOAT_EQ
(
70.1723
,
reference_line_length
);
bool
sample_result
=
sampler
.
sample
(
reference_line
,
init_point
,
init_sl_point
,
&
sampled_points
);
EXPECT_TRUE
(
sample_result
);
EXPECT_EQ
(
8
,
sampled_points
.
size
());
EXPECT_EQ
(
3
,
sampled_points
[
0
].
size
());
ASSERT_EQ
(
3
,
sampled_points
[
7
].
size
());
EXPECT_FLOAT_EQ
(
40
,
sampled_points
[
7
][
0
].
s
());
EXPECT_FLOAT_EQ
(
-
0.5
,
sampled_points
[
7
][
0
].
l
());
EXPECT_FLOAT_EQ
(
40
,
sampled_points
[
7
][
1
].
s
());
EXPECT_FLOAT_EQ
(
0
,
sampled_points
[
7
][
1
].
l
());
EXPECT_FLOAT_EQ
(
40
,
sampled_points
[
7
][
2
].
s
());
EXPECT_FLOAT_EQ
(
0.5
,
sampled_points
[
7
][
2
].
l
());
// export_sl_points(sampled_points, "/tmp/points.txt");
}
}
// namespace planning
}
// namespace apollo
modules/planning/planning.cc
浏览文件 @
aabf0612
...
@@ -139,7 +139,7 @@ void Planning::RunOnce() {
...
@@ -139,7 +139,7 @@ void Planning::RunOnce() {
apollo
::
common
::
time
::
ToSecond
(
apollo
::
common
::
time
::
Clock
::
Now
())
+
apollo
::
common
::
time
::
ToSecond
(
apollo
::
common
::
time
::
Clock
::
Now
())
+
planning_cycle_time
;
planning_cycle_time
;
if
(
!
DataCenter
::
instance
()
->
init_frame
(
if
(
!
DataCenter
::
instance
()
->
init_
current_
frame
(
AdapterManager
::
GetPlanning
()
->
GetSeqNum
()
+
1
))
{
AdapterManager
::
GetPlanning
()
->
GetSeqNum
()
+
1
))
{
AERROR
<<
"DataCenter init frame failed"
;
AERROR
<<
"DataCenter init frame failed"
;
return
;
return
;
...
...
modules/planning/testdata/garage_routing.pb.txt
0 → 100644
浏览文件 @
aabf0612
header {
module_name: "routing"
timestamp_sec: 1234.5
sequence_num: 1
}
routing_request {
end {
id: "1_-1"
s: 153.87421245705966
pose {
x: 200
y: 2
}
}
}
route {
id: "1_-1"
start_s: 0.0
end_s: 153.87421245705966
}
measurement {
distance: 153.87421245705966
}
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录