Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
74715d42
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,发现更多精彩内容 >>
提交
74715d42
编写于
8月 01, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
8月 01, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
added get_speed_limit_by_s and by_t functions.
上级
c6e56fda
变更
6
显示空白变更内容
内联
并排
Showing
6 changed file
with
63 addition
and
10 deletion
+63
-10
modules/planning/common/speed_limit.cc
modules/planning/common/speed_limit.cc
+37
-2
modules/planning/common/speed_limit.h
modules/planning/common/speed_limit.h
+2
-1
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
+2
-1
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
...s/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
+3
-3
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
...anning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
+12
-2
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
...lanning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
+7
-1
未找到文件。
modules/planning/common/speed_limit.cc
浏览文件 @
74715d42
...
@@ -18,9 +18,9 @@
...
@@ -18,9 +18,9 @@
* @file speed_limit.cc
* @file speed_limit.cc
**/
**/
#include "modules/planning/common/speed_limit.h"
#include <algorithm>
#include <algorithm>
#include "modules/planning/common/planning_util.h"
#include "modules/planning/common/planning_util.h"
#include "modules/planning/common/speed_limit.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -46,7 +46,7 @@ const std::vector<SpeedPoint>& SpeedLimit::speed_limits() const {
...
@@ -46,7 +46,7 @@ const std::vector<SpeedPoint>& SpeedLimit::speed_limits() const {
return
_speed_point
;
return
_speed_point
;
}
}
double
SpeedLimit
::
get_speed_limit
(
const
double
s
)
const
{
double
SpeedLimit
::
get_speed_limit
_by_s
(
const
double
s
)
const
{
double
ref_s
=
s
;
double
ref_s
=
s
;
if
(
_speed_point
.
size
()
==
0
)
{
if
(
_speed_point
.
size
()
==
0
)
{
return
0.0
;
return
0.0
;
...
@@ -79,5 +79,40 @@ double SpeedLimit::get_speed_limit(const double s) const {
...
@@ -79,5 +79,40 @@ double SpeedLimit::get_speed_limit(const double s) const {
}
}
return
util
::
interpolate
(
*
(
it_lower
-
1
),
*
it_lower
,
weight
).
v
();
return
util
::
interpolate
(
*
(
it_lower
-
1
),
*
it_lower
,
weight
).
v
();
}
}
double
SpeedLimit
::
get_speed_limit_by_t
(
const
double
t
)
const
{
double
ref_t
=
t
;
if
(
_speed_point
.
size
()
==
0
)
{
return
0.0
;
}
if
(
_speed_point
.
size
()
==
1
)
{
return
_speed_point
.
front
().
v
();
}
if
(
ref_t
>
_speed_point
.
back
().
t
())
{
return
_speed_point
.
back
().
v
();
}
if
(
ref_t
<
_speed_point
.
front
().
t
())
{
return
_speed_point
.
front
().
v
();
}
auto
func
=
[](
const
SpeedPoint
&
sp
,
const
double
t
)
{
return
sp
.
t
()
<
t
;
};
auto
it_lower
=
std
::
lower_bound
(
_speed_point
.
begin
(),
_speed_point
.
end
(),
t
,
func
);
if
(
it_lower
==
_speed_point
.
begin
())
{
return
_speed_point
.
front
().
v
();
}
double
weight
=
0.0
;
double
range
=
(
*
it_lower
).
t
()
-
(
*
(
it_lower
-
1
)).
t
();
if
(
range
>
0
)
{
weight
=
(
t
-
(
*
(
it_lower
-
1
)).
t
())
/
range
;
}
return
util
::
interpolate
(
*
(
it_lower
-
1
),
*
it_lower
,
weight
).
v
();
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/common/speed_limit.h
浏览文件 @
74715d42
...
@@ -35,7 +35,8 @@ class SpeedLimit {
...
@@ -35,7 +35,8 @@ class SpeedLimit {
void
add_speed_limit
(
const
double
s
,
const
double
t
,
const
double
v
,
void
add_speed_limit
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
const
double
da
);
const
double
a
,
const
double
da
);
const
std
::
vector
<
common
::
SpeedPoint
>&
speed_limits
()
const
;
const
std
::
vector
<
common
::
SpeedPoint
>&
speed_limits
()
const
;
double
get_speed_limit
(
const
double
s
)
const
;
double
get_speed_limit_by_s
(
const
double
s
)
const
;
double
get_speed_limit_by_t
(
const
double
t
)
const
;
private:
private:
std
::
vector
<
common
::
SpeedPoint
>
_speed_point
;
std
::
vector
<
common
::
SpeedPoint
>
_speed_point
;
...
...
modules/planning/optimizer/dp_st_speed/dp_st_graph.cc
浏览文件 @
74715d42
...
@@ -150,7 +150,8 @@ void DpStGraph::CalculateCostAt(const StGraphData& st_graph_data,
...
@@ -150,7 +150,8 @@ void DpStGraph::CalculateCostAt(const StGraphData& st_graph_data,
}
}
// TODO(all): get speed limit from mapper
// TODO(all): get speed limit from mapper
double
speed_limit
=
st_graph_data
.
speed_limit
().
get_speed_limit
(
unit_s_
*
r
);
double
speed_limit
=
st_graph_data
.
speed_limit
().
get_speed_limit_by_s
(
unit_s_
*
r
);
if
(
c
==
1
)
{
if
(
c
==
1
)
{
cost_table_
[
c
][
r
].
set_total_cost
(
cost_table_
[
c
][
r
].
set_total_cost
(
cost_table_
[
c
][
r
].
obstacle_cost
()
+
cost_table_
[
0
][
0
].
total_cost
()
+
cost_table_
[
c
][
r
].
obstacle_cost
()
+
cost_table_
[
0
][
0
].
total_cost
()
+
...
...
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @
74715d42
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
浏览文件 @
74715d42
...
@@ -74,7 +74,8 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
...
@@ -74,7 +74,8 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
}
if
(
!
ApplyKernel
(
st_graph_data
.
speed_limit
()).
ok
())
{
if
(
!
ApplyKernel
(
st_graph_data
.
obs_boundary
(),
st_graph_data
.
speed_limit
())
.
ok
())
{
const
std
::
string
msg
=
"Apply kernel failed!"
;
const
std
::
string
msg
=
"Apply kernel failed!"
;
AERROR
<<
msg
;
AERROR
<<
msg
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
...
@@ -211,7 +212,7 @@ Status QpSplineStGraph::ApplyKernel(
...
@@ -211,7 +212,7 @@ Status QpSplineStGraph::ApplyKernel(
for
(
uint32_t
i
=
0
;
for
(
uint32_t
i
=
0
;
i
<=
qp_spline_st_speed_config_
.
number_of_discrete_graph_t
();
++
i
)
{
i
<=
qp_spline_st_speed_config_
.
number_of_discrete_graph_t
();
++
i
)
{
s_vec
.
push_back
(
dist_ref
);
s_vec
.
push_back
(
dist_ref
);
dist_ref
+=
time_resolution_
*
speed_limit
.
get_speed_limit
(
dist_ref
);
dist_ref
+=
time_resolution_
*
speed_limit
.
get_speed_limit
_by_s
(
dist_ref
);
}
}
// TODO: change reference line kernel to configurable version
// TODO: change reference line kernel to configurable version
spline_kernel
->
add_reference_line_kernel_matrix
(
t_knots_
,
s_vec
,
1
);
spline_kernel
->
add_reference_line_kernel_matrix
(
t_knots_
,
s_vec
,
1
);
...
@@ -277,5 +278,14 @@ Status QpSplineStGraph::GetSConstraintByTime(
...
@@ -277,5 +278,14 @@ Status QpSplineStGraph::GetSConstraintByTime(
return
Status
::
OK
();
return
Status
::
OK
();
}
}
Status
QpSplineStGraph
::
EstimateSpeedConstraint
(
const
StGraphData
&
st_graph_data
,
const
PathData
&
path_data
,
const
SpeedLimit
&
speed_limit
,
std
::
vector
<
double
>*
speed_constraint
)
const
{
// TODO: (Liangliang) implement this function.
return
Status
::
OK
();
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h
浏览文件 @
74715d42
...
@@ -56,7 +56,8 @@ class QpSplineStGraph {
...
@@ -56,7 +56,8 @@ class QpSplineStGraph {
const
std
::
vector
<
StGraphBoundary
>&
boundaries
);
const
std
::
vector
<
StGraphBoundary
>&
boundaries
);
// apply objective function
// apply objective function
common
::
Status
ApplyKernel
(
const
SpeedLimit
&
speed_limit
);
common
::
Status
ApplyKernel
(
const
std
::
vector
<
StGraphBoundary
>&
boundaries
,
const
SpeedLimit
&
speed_limit
);
// solve
// solve
common
::
Status
Solve
();
common
::
Status
Solve
();
...
@@ -73,6 +74,11 @@ class QpSplineStGraph {
...
@@ -73,6 +74,11 @@ class QpSplineStGraph {
common
::
Status
AddFollowReferenceLineKernel
(
common
::
Status
AddFollowReferenceLineKernel
(
const
StGraphBoundary
&
follow_boundary
);
const
StGraphBoundary
&
follow_boundary
);
common
::
Status
EstimateSpeedConstraint
(
const
StGraphData
&
st_graph_data
,
const
PathData
&
path_data
,
const
SpeedLimit
&
speed_limit
,
std
::
vector
<
double
>*
speed_constraint
)
const
;
private:
private:
// qp st configuration
// qp st configuration
QpSplineStSpeedConfig
qp_spline_st_speed_config_
;
QpSplineStSpeedConfig
qp_spline_st_speed_config_
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录