Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
afe3ea37
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,发现更多精彩内容 >>
提交
afe3ea37
编写于
8月 10, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
8月 10, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: refactored speed_limit, removed t info.
上级
91db13db
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
52 addition
and
87 deletion
+52
-87
modules/planning/common/speed_limit.cc
modules/planning/common/speed_limit.cc
+14
-27
modules/planning/common/speed_limit.h
modules/planning/common/speed_limit.h
+7
-5
modules/planning/common/speed_limit_test.cc
modules/planning/common/speed_limit_test.cc
+12
-30
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
...anning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
+10
-9
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
...imizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
+8
-2
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
+1
-14
未找到文件。
modules/planning/common/speed_limit.cc
浏览文件 @
afe3ea37
...
...
@@ -32,42 +32,29 @@ namespace planning {
using
common
::
SpeedPoint
;
void
SpeedLimit
::
AddSpeedLimit
(
const
SpeedPoint
&
speed_point
)
{
if
(
!
speed_points_
.
empty
())
{
DCHECK_GE
(
speed_point
.
s
(),
speed_points_
.
back
().
s
());
DCHECK_GE
(
speed_point
.
t
(),
speed_points_
.
back
().
t
());
void
SpeedLimit
::
AddSpeedLimit
(
const
double
s
,
const
double
v
)
{
if
(
!
speed_limit_points_
.
empty
())
{
DCHECK_GE
(
s
,
speed_limit_points_
.
back
().
first
);
}
speed_
points_
.
push_back
(
speed_point
);
speed_
limit_points_
.
emplace_back
(
s
,
v
);
}
const
std
::
vector
<
SpeedPoint
>&
SpeedLimit
::
speed_points
()
const
{
return
speed_points_
;
const
std
::
vector
<
std
::
pair
<
double
,
double
>>&
SpeedLimit
::
speed_limit_points
()
const
{
return
speed_limit_points_
;
}
double
SpeedLimit
::
GetSpeedLimitByS
(
const
double
s
)
const
{
DCHECK_GE
(
speed_points_
.
size
(),
2
);
DCHECK_GE
(
s
,
speed_
points_
.
front
().
s
()
);
DCHECK_GE
(
speed_
limit_
points_
.
size
(),
2
);
DCHECK_GE
(
s
,
speed_
limit_points_
.
front
().
first
);
auto
compare_s
=
[](
const
SpeedPoint
&
sp
,
const
double
s
)
{
return
sp
.
s
()
<
s
;
auto
compare_s
=
[](
const
std
::
pair
<
double
,
double
>
point
,
const
double
s
)
{
return
point
.
first
<
s
;
};
auto
it_lower
=
std
::
lower_bound
(
speed_points_
.
begin
(),
speed_points_
.
end
(),
s
,
compare_s
);
return
it_lower
->
v
();
}
double
SpeedLimit
::
GetSpeedLimitByT
(
const
double
t
)
const
{
DCHECK_GE
(
speed_points_
.
size
(),
2
);
DCHECK_GE
(
t
,
speed_points_
.
front
().
t
());
auto
compare_t
=
[](
const
SpeedPoint
&
sp
,
const
double
t
)
{
return
sp
.
t
()
<
t
;
};
auto
it_lower
=
std
::
lower_bound
(
speed_points_
.
begin
(),
speed_points_
.
end
(),
t
,
compare_t
);
return
it_lower
->
v
();
auto
it_lower
=
std
::
lower_bound
(
speed_limit_points_
.
begin
(),
speed_limit_points_
.
end
(),
s
,
compare_s
);
return
it_lower
->
second
;
}
}
// namespace planning
...
...
modules/planning/common/speed_limit.h
浏览文件 @
afe3ea37
...
...
@@ -31,16 +31,18 @@ namespace planning {
class
SpeedLimit
{
public:
SpeedLimit
()
=
default
;
void
AddSpeedLimit
(
const
common
::
SpeedPoint
&
speed_point
);
void
AddSpeedLimit
(
const
double
s
,
const
double
v
);
const
std
::
vector
<
common
::
SpeedPoint
>&
speed
_points
()
const
;
const
std
::
vector
<
std
::
pair
<
double
,
double
>>&
speed_limit
_points
()
const
;
double
GetSpeedLimitByS
(
const
double
s
)
const
;
double
GetSpeedLimitByT
(
const
double
t
)
const
;
void
Clear
()
{
speed_points_
.
clear
();
}
void
Clear
()
{
speed_
limit_
points_
.
clear
();
}
private:
std
::
vector
<
common
::
SpeedPoint
>
speed_points_
;
// use a vector to represent speed limit
// the first number is s, the second number is v
// It means at distance s from the start point, the speed limit is v.
std
::
vector
<
std
::
pair
<
double
,
double
>>
speed_limit_points_
;
};
}
// namespace planning
...
...
modules/planning/common/speed_limit_test.cc
浏览文件 @
afe3ea37
...
...
@@ -32,11 +32,10 @@ class SpeedLimitTest : public ::testing::Test {
virtual
void
SetUp
()
{
speed_limit_
.
Clear
();
for
(
int
i
=
0
;
i
<
100
;
++
i
)
{
SpeedPoint
sp
;
sp
.
set_s
(
i
*
1.0
);
sp
.
set_t
(
i
*
0.5
);
sp
.
set_v
((
i
%
2
==
0
)
?
5.0
:
10.0
);
speed_limit_
.
AddSpeedLimit
(
std
::
move
(
sp
));
std
::
pair
<
double
,
double
>
sp
;
sp
.
first
=
i
*
1.0
;
sp
.
second
=
(
i
%
2
==
0
)
?
5.0
:
10.0
;
speed_limit_
.
AddSpeedLimit
(
sp
.
first
,
sp
.
second
);
}
}
...
...
@@ -46,45 +45,28 @@ class SpeedLimitTest : public ::testing::Test {
TEST_F
(
SpeedLimitTest
,
SimpleSpeedLimitCreation
)
{
SpeedLimit
simple_speed_limit
;
EXPECT_TRUE
(
simple_speed_limit
.
speed_points
().
empty
());
EXPECT_EQ
(
speed_limit_
.
speed_points
().
size
(),
100
);
EXPECT_TRUE
(
simple_speed_limit
.
speed_
limit_
points
().
empty
());
EXPECT_EQ
(
speed_limit_
.
speed_
limit_
points
().
size
(),
100
);
}
TEST_F
(
SpeedLimitTest
,
GetSpeedLimitByS
)
{
EXPECT_EQ
(
speed_limit_
.
speed_points
().
size
(),
100
);
EXPECT_EQ
(
speed_limit_
.
speed_
limit_
points
().
size
(),
100
);
double
s
=
0.0
;
const
double
ds
=
0.01
;
while
(
s
<
99.0
)
{
double
v_limit
=
speed_limit_
.
GetSpeedLimitByS
(
s
);
auto
it_lower
=
std
::
lower_bound
(
speed_limit_
.
speed_points
().
begin
(),
speed_limit_
.
speed_points
().
end
(),
s
,
[](
const
SpeedPoint
&
point
,
const
double
curr_s
)
{
return
point
.
s
()
<
curr_s
;
speed_limit_
.
speed_limit_points
().
begin
(),
speed_limit_
.
speed_limit_points
().
end
(),
s
,
[](
const
std
::
pair
<
double
,
double
>&
point
,
const
double
curr_s
)
{
return
point
.
first
<
curr_s
;
});
EXPECT_DOUBLE_EQ
(
v_limit
,
it_lower
->
v
()
);
EXPECT_DOUBLE_EQ
(
v_limit
,
it_lower
->
second
);
s
+=
ds
;
}
}
TEST_F
(
SpeedLimitTest
,
GetSpeedLimitByT
)
{
EXPECT_EQ
(
speed_limit_
.
speed_points
().
size
(),
100
);
double
t
=
0.0
;
const
double
dt
=
0.01
;
while
(
t
<
49.0
)
{
double
v_limit
=
speed_limit_
.
GetSpeedLimitByT
(
t
);
auto
it_lower
=
std
::
lower_bound
(
speed_limit_
.
speed_points
().
begin
(),
speed_limit_
.
speed_points
().
end
(),
t
,
[](
const
SpeedPoint
&
point
,
const
double
curr_t
)
{
return
point
.
t
()
<
curr_t
;
});
EXPECT_DOUBLE_EQ
(
v_limit
,
it_lower
->
v
());
t
+=
dt
;
}
}
}
// namespace planning
}
// namespace apollo
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
浏览文件 @
afe3ea37
...
...
@@ -21,9 +21,9 @@
#include "modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h"
#include <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <limits>
#include "modules/common/log.h"
...
...
@@ -254,7 +254,7 @@ Status QpSplineStGraph::AddCruiseReferenceLineKernel(
const
std
::
vector
<
double
>&
evaluate_t
,
const
SpeedLimit
&
speed_limit
)
{
auto
*
spline_kernel
=
spline_generator_
->
mutable_spline_kernel
();
std
::
vector
<
double
>
s_vec
;
if
(
speed_limit
.
speed_points
().
size
()
==
0
)
{
if
(
speed_limit
.
speed_
limit_
points
().
size
()
==
0
)
{
std
::
string
msg
=
"Fail to apply_kernel due to empty speed limits."
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
...
...
@@ -344,17 +344,18 @@ Status QpSplineStGraph::EstimateSpeedUpperBound(
uint32_t
j
=
0
;
double
distance
=
0.0
;
const
double
kDistanceEpsilon
=
1e-6
;
while
(
i
<
t_evaluated_
.
size
()
&&
j
+
1
<
speed_limit
.
speed_points
().
size
())
{
while
(
i
<
t_evaluated_
.
size
()
&&
j
+
1
<
speed_limit
.
speed_limit_points
().
size
())
{
distance
=
v
*
t_evaluated_
[
i
];
if
(
fabs
(
distance
-
speed_limit
.
speed_points
()[
j
].
s
())
<
kDistanceEpsilon
)
{
speed_upper_bound
->
push_back
(
speed_limit
.
speed_points
()[
j
].
v
());
if
(
fabs
(
distance
-
speed_limit
.
speed_limit_points
()[
j
].
first
)
<
kDistanceEpsilon
)
{
speed_upper_bound
->
push_back
(
speed_limit
.
speed_limit_points
()[
j
].
second
);
++
i
;
ADEBUG
<<
"speed upper bound:"
<<
speed_upper_bound
->
back
();
}
else
if
(
distance
<
speed_limit
.
speed_points
()[
j
].
s
()
)
{
}
else
if
(
distance
<
speed_limit
.
speed_limit_points
()[
j
].
first
)
{
++
i
;
}
else
if
(
distance
<=
speed_limit
.
speed_points
()[
j
+
1
].
s
())
{
speed_upper_bound
->
push_back
(
speed_limit
.
GetSpeedLimitByT
(
t_evaluated_
[
i
]));
}
else
if
(
distance
<=
speed_limit
.
speed_limit_points
()[
j
+
1
].
first
)
{
speed_upper_bound
->
push_back
(
speed_limit
.
GetSpeedLimitByS
(
distance
));
ADEBUG
<<
"speed upper bound:"
<<
speed_upper_bound
->
back
();
++
i
;
}
else
{
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @
afe3ea37
...
...
@@ -23,6 +23,8 @@
#include <algorithm>
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/file.h"
#include "modules/common/vehicle_state/vehicle_state.h"
...
...
@@ -80,8 +82,12 @@ void QpSplineStSpeedOptimizer::RecordSTGraphDebug(
}
}
st_graph_debug
->
mutable_speed_limit
()
->
CopyFrom
(
{
speed_limits
.
speed_points
().
begin
(),
speed_limits
.
speed_points
().
end
()});
for
(
const
auto
&
point
:
speed_limits
.
speed_limit_points
())
{
common
::
SpeedPoint
speed_point
;
speed_point
.
set_s
(
point
.
first
);
speed_point
.
set_v
(
point
.
second
);
st_graph_debug
->
add_speed_limit
()
->
CopyFrom
(
speed_point
);
}
st_graph_debug
->
mutable_speed_profile
()
->
CopyFrom
(
{
speed_data
.
speed_vector
().
begin
(),
speed_data
.
speed_vector
().
end
()});
...
...
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
浏览文件 @
afe3ea37
...
...
@@ -467,7 +467,6 @@ Status StBoundaryMapper::GetSpeedLimits(
CHECK_NOTNULL
(
speed_limit_data
);
std
::
vector
<
double
>
speed_limits
;
double
curr_t
=
0.0
;
for
(
const
auto
&
path_point
:
path_data_
.
discretized_path
().
path_points
())
{
if
(
Double
::
Compare
(
path_point
.
s
(),
reference_line_
.
length
())
>
0
)
{
std
::
string
msg
=
common
::
util
::
StrCat
(
...
...
@@ -491,19 +490,7 @@ Status StBoundaryMapper::GetSpeedLimits(
st_boundary_config_
.
lowest_speed
(),
std
::
fmin
(
speed_limit_on_path
,
speed_limit_on_reference_line
));
common
::
SpeedPoint
speed_point
;
double
delta_t
=
0.0
;
if
(
!
speed_limit_data
->
speed_points
().
empty
())
{
delta_t
=
(
path_point
.
s
()
-
speed_limit_data
->
speed_points
().
back
().
s
())
/
curr_speed_limit
;
}
speed_point
.
set_s
(
path_point
.
s
());
speed_point
.
set_v
(
curr_speed_limit
);
speed_point
.
set_t
(
curr_t
);
speed_limit_data
->
AddSpeedLimit
(
speed_point
);
curr_t
+=
delta_t
;
speed_limit_data
->
AddSpeedLimit
(
path_point
.
s
(),
curr_speed_limit
);
}
return
Status
::
OK
();
}
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录