Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
2a8623c6
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,发现更多精彩内容 >>
提交
2a8623c6
编写于
8月 07, 2017
作者:
L
lianglia-apollo
提交者:
Jiaming Tao
8月 07, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: fixed a bug on get_pathpoint_from_ref_s. (#587)
上级
49fb380c
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
25 addition
and
33 deletion
+25
-33
modules/planning/common/path/BUILD
modules/planning/common/path/BUILD
+1
-0
modules/planning/common/path/path_data.cc
modules/planning/common/path/path_data.cc
+16
-27
modules/planning/common/path/path_data.h
modules/planning/common/path/path_data.h
+3
-1
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
...anning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
+1
-2
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
+2
-1
modules/planning/optimizer/st_graph/st_graph_boundary.cc
modules/planning/optimizer/st_graph/st_graph_boundary.cc
+2
-2
未找到文件。
modules/planning/common/path/BUILD
浏览文件 @
2a8623c6
...
...
@@ -43,6 +43,7 @@ cc_library(
":frenet_frame_path"
,
"//modules/planning/common:planning_gflags"
,
"//modules/planning/math:double"
,
"//modules/planning/reference_line"
,
],
)
...
...
modules/planning/common/path/path_data.cc
浏览文件 @
2a8623c6
...
...
@@ -31,6 +31,9 @@
namespace
apollo
{
namespace
planning
{
using
SLPoint
=
apollo
::
common
::
SLPoint
;
using
Vec2d
=
apollo
::
common
::
math
::
Vec2d
;
void
PathData
::
set_discretized_path
(
const
DiscretizedPath
&
path
)
{
discretized_path_
=
path
;
}
...
...
@@ -59,33 +62,19 @@ bool PathData::get_path_point_with_path_s(
}
bool
PathData
::
get_path_point_with_ref_s
(
const
double
ref_s
,
common
::
PathPoint
*
const
path_point
)
const
{
const
auto
&
frenet_points
=
frenet_path_
.
points
();
if
(
frenet_points
.
size
()
<
2
||
ref_s
<
frenet_points
.
front
().
s
()
||
frenet_points
.
back
().
s
()
<
ref_s
)
{
return
false
;
}
auto
comp
=
[](
const
common
::
FrenetFramePoint
&
frenet_point
,
const
double
ref_s
)
{
return
frenet_point
.
s
()
<
ref_s
;
};
auto
it_lower
=
std
::
lower_bound
(
frenet_points
.
begin
(),
frenet_points
.
end
(),
ref_s
,
comp
);
if
(
it_lower
==
frenet_points
.
begin
())
{
*
path_point
=
discretized_path_
.
points
().
front
();
}
else
{
// std::uint32_t index_lower = (std::uint32_t)(it_lower -
// frenet_points.begin());
//
// double ref_s0 = (it_lower - 1)->s();
// double ref_s1 = it_lower->s();
//
// CHECK_LT(ref_s0, ref_s1);
// double weight = (ref_s - ref_s0) / (ref_s1 - ref_s0);
// *path_point =
// common::PathPoint::interpolate_linear_approximation(path_.path_point_at(index_lower
// - 1),
// path_.path_point_at(index_lower), weight);
const
ReferenceLine
&
reference_line
,
const
double
ref_s
,
common
::
PathPoint
*
const
path_point
)
const
{
*
path_point
=
discretized_path_
.
start_point
();
for
(
common
::
PathPoint
point
:
discretized_path_
.
points
())
{
SLPoint
sl
;
reference_line
.
get_point_in_frenet_frame
(
Vec2d
(
point
.
x
(),
point
.
y
()),
&
sl
);
if
(
Double
::
Compare
(
sl
.
s
(),
ref_s
)
==
0
)
{
path_point
->
CopyFrom
(
point
);
return
true
;
}
if
(
std
::
fabs
(
sl
.
s
()
-
ref_s
)
<
std
::
fabs
(
path_point
->
s
()
-
ref_s
))
{
path_point
->
CopyFrom
(
point
);
}
}
return
true
;
}
...
...
modules/planning/common/path/path_data.h
浏览文件 @
2a8623c6
...
...
@@ -25,6 +25,7 @@
#include "modules/planning/common/path/discretized_path.h"
#include "modules/planning/common/path/frenet_frame_path.h"
#include "modules/planning/reference_line/reference_line.h"
namespace
apollo
{
namespace
planning
{
...
...
@@ -46,7 +47,8 @@ class PathData {
bool
get_path_point_with_path_s
(
const
double
s
,
common
::
PathPoint
*
const
path_point
)
const
;
bool
get_path_point_with_ref_s
(
const
double
ref_s
,
bool
get_path_point_with_ref_s
(
const
ReferenceLine
&
reference_line
,
const
double
ref_s
,
common
::
PathPoint
*
const
path_point
)
const
;
void
Clear
();
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
浏览文件 @
2a8623c6
...
...
@@ -303,8 +303,7 @@ Status QpSplineStGraph::GetSConstraintByTime(
const
std
::
vector
<
StGraphBoundary
>&
boundaries
,
const
double
time
,
const
double
total_path_s
,
double
*
const
s_upper_bound
,
double
*
const
s_lower_bound
)
const
{
*
s_upper_bound
=
std
::
min
(
total_path_s
,
time
*
qp_spline_st_speed_config_
.
max_speed
());
*
s_upper_bound
=
total_path_s
;
for
(
const
StGraphBoundary
&
boundary
:
boundaries
)
{
double
s_upper
=
0.0
;
...
...
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
浏览文件 @
2a8623c6
...
...
@@ -172,7 +172,8 @@ bool StBoundaryMapper::MapObstacleWithStopDecision(
PathPoint
path_point
;
if
(
!
path_data_
.
get_path_point_with_ref_s
(
stop_obstacle
.
sl_boundary
().
start_s
(),
&
path_point
))
{
reference_line_
,
stop_obstacle
.
sl_boundary
().
start_s
(),
&
path_point
))
{
AERROR
<<
"Fail to get path point from reference s. The sl boundary of "
"stop obstacle is: "
<<
stop_obstacle
.
sl_boundary
().
DebugString
();
...
...
modules/planning/optimizer/st_graph/st_graph_boundary.cc
浏览文件 @
2a8623c6
...
...
@@ -113,6 +113,7 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
Vec2d
(
curr_time
,
s_high_limit_
)};
*
s_upper
=
s_high_limit_
;
*
s_lower
=
0.0
;
Vec2d
p_s_first
;
Vec2d
p_s_second
;
...
...
@@ -123,8 +124,7 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
}
if
(
boundary_type_
==
BoundaryType
::
STOP
||
boundary_type_
==
BoundaryType
::
YIELD
||
boundary_type_
==
BoundaryType
::
FOLLOW
||
boundary_type_
==
BoundaryType
::
UNKNOWN
)
{
boundary_type_
==
BoundaryType
::
FOLLOW
)
{
*
s_upper
=
std
::
fmin
(
*
s_upper
,
std
::
fmin
(
p_s_first
.
y
(),
p_s_second
.
y
()));
}
else
if
(
boundary_type_
==
BoundaryType
::
OVERTAKE
)
{
// overtake
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录