Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
997dd4d5
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,发现更多精彩内容 >>
提交
997dd4d5
编写于
7月 18, 2017
作者:
D
Dong Li
提交者:
Jiangtao Hu
7月 19, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
added planning/common/path
上级
8cac0222
变更
31
展开全部
隐藏空白更改
内联
并排
Showing
31 changed file
with
1495 addition
and
707 deletion
+1495
-707
modules/common/proto/path_point.proto
modules/common/proto/path_point.proto
+12
-0
modules/map/hdmap/hdmap_common.cc
modules/map/hdmap/hdmap_common.cc
+143
-143
modules/map/hdmap/hdmap_common_test.cc
modules/map/hdmap/hdmap_common_test.cc
+322
-326
modules/planning/common/BUILD
modules/planning/common/BUILD
+4
-0
modules/planning/common/logic_point.cc
modules/planning/common/logic_point.cc
+11
-27
modules/planning/common/logic_point.h
modules/planning/common/logic_point.h
+6
-8
modules/planning/common/path/BUILD
modules/planning/common/path/BUILD
+75
-0
modules/planning/common/path/discretized_path.cc
modules/planning/common/path/discretized_path.cc
+120
-0
modules/planning/common/path/discretized_path.h
modules/planning/common/path/discretized_path.h
+70
-0
modules/planning/common/path/frenet_frame_path.cc
modules/planning/common/path/frenet_frame_path.cc
+62
-0
modules/planning/common/path/frenet_frame_path.h
modules/planning/common/path/frenet_frame_path.h
+58
-0
modules/planning/common/path/path.h
modules/planning/common/path/path.h
+47
-0
modules/planning/common/path/path_data.cc
modules/planning/common/path/path_data.cc
+107
-0
modules/planning/common/path/path_data.h
modules/planning/common/path/path_data.h
+64
-0
modules/planning/common/path/path_point_util.cc
modules/planning/common/path/path_point_util.cc
+98
-0
modules/planning/common/path/path_point_util.h
modules/planning/common/path/path_point_util.h
+44
-0
modules/planning/common/path/sl_point_util.cc
modules/planning/common/path/sl_point_util.cc
+39
-0
modules/planning/common/path/sl_point_util.h
modules/planning/common/path/sl_point_util.h
+38
-0
modules/planning/common/speed/speed_data.cc
modules/planning/common/speed/speed_data.cc
+51
-46
modules/planning/common/speed/speed_data.h
modules/planning/common/speed/speed_data.h
+17
-17
modules/planning/common/speed/speed_point.cc
modules/planning/common/speed/speed_point.cc
+39
-57
modules/planning/common/speed/speed_point.h
modules/planning/common/speed/speed_point.h
+25
-27
modules/planning/common/speed/st_point.cc
modules/planning/common/speed/st_point.cc
+12
-22
modules/planning/common/speed/st_point.h
modules/planning/common/speed/st_point.h
+11
-12
modules/planning/math/spiral_curve/quintic_spiral_curve.cc
modules/planning/math/spiral_curve/quintic_spiral_curve.cc
+7
-6
modules/planning/math/spiral_curve/spiral_curve.cc
modules/planning/math/spiral_curve/spiral_curve.cc
+5
-3
modules/planning/planner/em_planner.cc
modules/planning/planner/em_planner.cc
+3
-6
modules/planning/planner/em_planner.h
modules/planning/planner/em_planner.h
+2
-3
modules/planning/planning.cc
modules/planning/planning.cc
+1
-0
modules/planning/planning.h
modules/planning/planning.h
+1
-3
modules/planning/planning_test.cc
modules/planning/planning_test.cc
+1
-1
未找到文件。
modules/common/proto/path_point.proto
浏览文件 @
997dd4d5
...
@@ -2,6 +2,18 @@ syntax = "proto2";
...
@@ -2,6 +2,18 @@ syntax = "proto2";
package
apollo
.
common
;
package
apollo
.
common
;
message
SLPoint
{
optional
double
s
=
1
;
optional
double
l
=
2
;
}
message
FrenetFramePoint
{
optional
double
s
=
1
;
optional
double
l
=
2
;
optional
double
dl
=
3
;
optional
double
ddl
=
4
;
}
message
PathPoint
{
message
PathPoint
{
// coordinates
// coordinates
optional
double
x
=
1
;
optional
double
x
=
1
;
...
...
modules/map/hdmap/hdmap_common.cc
浏览文件 @
997dd4d5
...
@@ -13,11 +13,11 @@ See the License for the specific language governing permissions and
...
@@ -13,11 +13,11 @@ See the License for the specific language governing permissions and
limitations under the License.
limitations under the License.
=========================================================================*/
=========================================================================*/
#include <iostream>
#include <algorithm>
#include <algorithm>
#include <iostream>
#include "modules/map/hdmap/hdmap_common.h"
#include "glog/logging.h"
#include "glog/logging.h"
#include "modules/map/hdmap/hdmap_common.h"
namespace
{
namespace
{
...
@@ -28,57 +28,58 @@ const double kSegmentationEpsilon = 0.2;
...
@@ -28,57 +28,58 @@ const double kSegmentationEpsilon = 0.2;
const
double
kDuplicatedPointsEpsilon
=
1e-7
;
const
double
kDuplicatedPointsEpsilon
=
1e-7
;
void
remove_duplicates
(
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
*
points
)
{
void
remove_duplicates
(
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
*
points
)
{
CHECK_NOTNULL
(
points
);
CHECK_NOTNULL
(
points
);
int
count
=
0
;
int
count
=
0
;
const
double
limit
=
kDuplicatedPointsEpsilon
*
kDuplicatedPointsEpsilon
;
const
double
limit
=
kDuplicatedPointsEpsilon
*
kDuplicatedPointsEpsilon
;
for
(
size_t
i
=
0
;
i
<
points
->
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
<
points
->
size
();
++
i
)
{
if
(
count
==
0
||
if
(
count
==
0
||
(
*
points
)[
i
].
DistanceSquareTo
((
*
points
)[
count
-
1
])
>
limit
)
{
(
*
points
)[
i
].
DistanceSquareTo
((
*
points
)[
count
-
1
])
>
limit
)
{
(
*
points
)[
count
++
]
=
(
*
points
)[
i
];
(
*
points
)[
count
++
]
=
(
*
points
)[
i
];
}
}
}
points
->
resize
(
count
);
}
points
->
resize
(
count
);
}
}
void
points_from_curve
(
const
apollo
::
hdmap
::
Curve
&
input_curve
,
void
points_from_curve
(
const
apollo
::
hdmap
::
Curve
&
input_curve
,
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
*
points
)
{
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
*
points
)
{
CHECK_NOTNULL
(
points
)
->
clear
();
CHECK_NOTNULL
(
points
)
->
clear
();
for
(
const
auto
&
curve
:
input_curve
.
segment
())
{
for
(
const
auto
&
curve
:
input_curve
.
segment
())
{
if
(
curve
.
has_line_segment
())
{
if
(
curve
.
has_line_segment
())
{
for
(
const
auto
&
point
:
curve
.
line_segment
().
point
())
{
for
(
const
auto
&
point
:
curve
.
line_segment
().
point
())
{
points
->
emplace_back
(
point
.
x
(),
point
.
y
());
points
->
emplace_back
(
point
.
x
(),
point
.
y
());
}
}
}
else
{
}
else
{
LOG
(
FATAL
)
<<
"Can not handle curve type."
;
LOG
(
FATAL
)
<<
"Can not handle curve type."
;
}
}
}
remove_duplicates
(
points
);
}
remove_duplicates
(
points
);
}
}
apollo
::
common
::
math
::
Polygon2d
convert_to_polygon2d
(
apollo
::
common
::
math
::
Polygon2d
convert_to_polygon2d
(
const
apollo
::
hdmap
::
Polygon
&
polygon
)
{
const
apollo
::
hdmap
::
Polygon
&
polygon
)
{
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
points
.
reserve
(
polygon
.
point_size
());
points
.
reserve
(
polygon
.
point_size
());
for
(
const
auto
&
point
:
polygon
.
point
())
{
for
(
const
auto
&
point
:
polygon
.
point
())
{
points
.
emplace_back
(
point
.
x
(),
point
.
y
());
points
.
emplace_back
(
point
.
x
(),
point
.
y
());
}
}
remove_duplicates
(
&
points
);
remove_duplicates
(
&
points
);
while
(
points
.
size
()
>=
2
&&
while
(
points
.
size
()
>=
2
&&
points
[
0
].
DistanceTo
(
points
.
back
())
points
[
0
].
DistanceTo
(
points
.
back
())
<=
<=
apollo
::
common
::
math
::
kMathEpsilon
)
{
apollo
::
common
::
math
::
kMathEpsilon
)
{
points
.
pop_back
();
points
.
pop_back
();
}
}
return
apollo
::
common
::
math
::
Polygon2d
(
points
);
return
apollo
::
common
::
math
::
Polygon2d
(
points
);
}
}
void
segments_from_curve
(
const
apollo
::
hdmap
::
Curve
&
curve
,
void
segments_from_curve
(
std
::
vector
<
apollo
::
common
::
math
::
LineSegment2d
>
*
segments
)
{
const
apollo
::
hdmap
::
Curve
&
curve
,
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
std
::
vector
<
apollo
::
common
::
math
::
LineSegment2d
>
*
segments
)
{
points_from_curve
(
curve
,
&
points
);
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
for
(
size_t
i
=
0
;
i
+
1
<
points
.
size
();
++
i
)
{
points_from_curve
(
curve
,
&
points
);
segments
->
emplace_back
(
points
[
i
],
points
[
i
+
1
]);
for
(
size_t
i
=
0
;
i
+
1
<
points
.
size
();
++
i
)
{
segments
->
emplace_back
(
points
[
i
],
points
[
i
+
1
]);
}
}
}
}
}
// namespace
}
// namespace
...
@@ -89,165 +90,164 @@ namespace hdmap {
...
@@ -89,165 +90,164 @@ namespace hdmap {
LaneInfo
::
LaneInfo
(
const
apollo
::
hdmap
::
Lane
&
lane
)
:
_lane
(
lane
)
{
init
();
}
LaneInfo
::
LaneInfo
(
const
apollo
::
hdmap
::
Lane
&
lane
)
:
_lane
(
lane
)
{
init
();
}
void
LaneInfo
::
init
()
{
void
LaneInfo
::
init
()
{
points_from_curve
(
_lane
.
central_curve
(),
&
_points
);
points_from_curve
(
_lane
.
central_curve
(),
&
_points
);
CHECK_GE
(
_points
.
size
(),
2
);
CHECK_GE
(
_points
.
size
(),
2
);
_segments
.
clear
();
_segments
.
clear
();
_accumulated_s
.
clear
();
_accumulated_s
.
clear
();
_unit_directions
.
clear
();
_unit_directions
.
clear
();
_headings
.
clear
();
_headings
.
clear
();
double
s
=
0
;
double
s
=
0
;
for
(
size_t
i
=
0
;
i
+
1
<
_points
.
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
+
1
<
_points
.
size
();
++
i
)
{
_segments
.
emplace_back
(
_points
[
i
],
_points
[
i
+
1
]);
_segments
.
emplace_back
(
_points
[
i
],
_points
[
i
+
1
]);
_accumulated_s
.
push_back
(
s
);
_unit_directions
.
push_back
(
_segments
.
back
().
unit_direction
());
s
+=
_segments
.
back
().
length
();
}
_accumulated_s
.
push_back
(
s
);
_accumulated_s
.
push_back
(
s
);
_total_length
=
s
;
_unit_directions
.
push_back
(
_segments
.
back
().
unit_direction
());
CHECK
(
!
_unit_directions
.
empty
());
s
+=
_segments
.
back
().
length
();
_unit_directions
.
push_back
(
_unit_directions
.
back
());
}
for
(
const
auto
&
direction
:
_unit_directions
)
{
_accumulated_s
.
push_back
(
s
);
_headings
.
push_back
(
direction
.
Angle
());
_total_length
=
s
;
}
CHECK
(
!
_unit_directions
.
empty
());
CHECK
(
!
_segments
.
empty
());
_unit_directions
.
push_back
(
_unit_directions
.
back
());
for
(
const
auto
&
direction
:
_unit_directions
)
{
_headings
.
push_back
(
direction
.
Angle
());
}
CHECK
(
!
_segments
.
empty
());
_sampled_left_width
.
clear
();
_sampled_left_width
.
clear
();
_sampled_right_width
.
clear
();
_sampled_right_width
.
clear
();
for
(
const
auto
&
sample
:
_lane
.
left_sample
())
{
for
(
const
auto
&
sample
:
_lane
.
left_sample
())
{
_sampled_left_width
.
emplace_back
(
sample
.
s
(),
sample
.
width
());
_sampled_left_width
.
emplace_back
(
sample
.
s
(),
sample
.
width
());
}
}
for
(
const
auto
&
sample
:
_lane
.
right_sample
())
{
for
(
const
auto
&
sample
:
_lane
.
right_sample
())
{
_sampled_right_width
.
emplace_back
(
sample
.
s
(),
sample
.
width
());
_sampled_right_width
.
emplace_back
(
sample
.
s
(),
sample
.
width
());
}
}
}
}
void
LaneInfo
::
get_width
(
const
double
s
,
double
*
left_width
,
void
LaneInfo
::
get_width
(
const
double
s
,
double
*
left_width
,
double
*
right_width
)
const
{
double
*
right_width
)
const
{
if
(
left_width
!=
nullptr
)
{
if
(
left_width
!=
nullptr
)
{
*
left_width
=
get_width_from_sample
(
_sampled_left_width
,
s
);
*
left_width
=
get_width_from_sample
(
_sampled_left_width
,
s
);
}
}
if
(
right_width
!=
nullptr
)
{
if
(
right_width
!=
nullptr
)
{
*
right_width
=
get_width_from_sample
(
_sampled_right_width
,
s
);
*
right_width
=
get_width_from_sample
(
_sampled_right_width
,
s
);
}
}
}
}
double
LaneInfo
::
get_width
(
const
double
s
)
const
{
double
LaneInfo
::
get_width
(
const
double
s
)
const
{
double
left_width
=
0.0
;
double
left_width
=
0.0
;
double
right_width
=
0.0
;
double
right_width
=
0.0
;
get_width
(
s
,
&
left_width
,
&
right_width
);
get_width
(
s
,
&
left_width
,
&
right_width
);
return
left_width
+
right_width
;
return
left_width
+
right_width
;
}
}
double
LaneInfo
::
get_effective_width
(
const
double
s
)
const
{
double
LaneInfo
::
get_effective_width
(
const
double
s
)
const
{
double
left_width
=
0.0
;
double
left_width
=
0.0
;
double
right_width
=
0.0
;
double
right_width
=
0.0
;
get_width
(
s
,
&
left_width
,
&
right_width
);
get_width
(
s
,
&
left_width
,
&
right_width
);
return
2
*
std
::
min
(
left_width
,
right_width
);
return
2
*
std
::
min
(
left_width
,
right_width
);
}
}
double
LaneInfo
::
get_width_from_sample
(
double
LaneInfo
::
get_width_from_sample
(
const
std
::
vector
<
LaneInfo
::
SampledWidth
>&
samples
,
const
std
::
vector
<
LaneInfo
::
SampledWidth
>
&
samples
,
const
double
s
)
const
{
const
double
s
)
const
{
if
(
samples
.
empty
())
{
if
(
samples
.
empty
())
{
return
0.0
;
return
0.0
;
}
}
if
(
s
<=
samples
[
0
].
first
)
{
if
(
s
<=
samples
[
0
].
first
)
{
return
samples
[
0
].
second
;
return
samples
[
0
].
second
;
}
}
if
(
s
>=
samples
.
back
().
first
)
{
if
(
s
>=
samples
.
back
().
first
)
{
return
samples
.
back
().
second
;
return
samples
.
back
().
second
;
}
}
int
low
=
0
;
int
low
=
0
;
int
high
=
static_cast
<
int
>
(
samples
.
size
());
int
high
=
static_cast
<
int
>
(
samples
.
size
());
while
(
low
+
1
<
high
)
{
while
(
low
+
1
<
high
)
{
const
int
mid
=
(
low
+
high
)
/
2
;
const
int
mid
=
(
low
+
high
)
/
2
;
if
(
samples
[
mid
].
first
<=
s
)
{
if
(
samples
[
mid
].
first
<=
s
)
{
low
=
mid
;
low
=
mid
;
}
else
{
}
else
{
high
=
mid
;
high
=
mid
;
}
}
}
const
LaneInfo
::
SampledWidth
&
sample1
=
samples
[
low
];
}
const
LaneInfo
::
SampledWidth
&
sample2
=
samples
[
high
];
const
LaneInfo
::
SampledWidth
&
sample1
=
samples
[
low
];
const
double
ratio
=
(
sample2
.
first
-
s
)
/
(
sample2
.
first
-
sample1
.
first
);
const
LaneInfo
::
SampledWidth
&
sample2
=
samples
[
high
];
return
sample1
.
second
*
ratio
+
sample2
.
second
*
(
1.0
-
ratio
);
const
double
ratio
=
(
sample2
.
first
-
s
)
/
(
sample2
.
first
-
sample1
.
first
);
return
sample1
.
second
*
ratio
+
sample2
.
second
*
(
1.0
-
ratio
);
}
}
JunctionInfo
::
JunctionInfo
(
const
apollo
::
hdmap
::
Junction
&
junction
)
JunctionInfo
::
JunctionInfo
(
const
apollo
::
hdmap
::
Junction
&
junction
)
:
_junction
(
junction
)
{
:
_junction
(
junction
)
{
init
();
init
();
}
}
void
JunctionInfo
::
init
()
{
void
JunctionInfo
::
init
()
{
_polygon
=
convert_to_polygon2d
(
_junction
.
polygon
());
_polygon
=
convert_to_polygon2d
(
_junction
.
polygon
());
CHECK_GT
(
_polygon
.
num_points
(),
2
);
CHECK_GT
(
_polygon
.
num_points
(),
2
);
}
}
SignalInfo
::
SignalInfo
(
const
apollo
::
hdmap
::
Signal
&
signal
)
:
_signal
(
signal
)
{
SignalInfo
::
SignalInfo
(
const
apollo
::
hdmap
::
Signal
&
signal
)
:
_signal
(
signal
)
{
init
();
init
();
}
}
void
SignalInfo
::
init
()
{
void
SignalInfo
::
init
()
{
for
(
const
auto
&
stop_line
:
_signal
.
stop_line
())
{
for
(
const
auto
&
stop_line
:
_signal
.
stop_line
())
{
segments_from_curve
(
stop_line
,
&
_segments
);
segments_from_curve
(
stop_line
,
&
_segments
);
}
}
CHECK
(
!
_segments
.
empty
());
CHECK
(
!
_segments
.
empty
());
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
for
(
const
auto
&
segment
:
_segments
)
{
for
(
const
auto
&
segment
:
_segments
)
{
points
.
emplace_back
(
segment
.
start
());
points
.
emplace_back
(
segment
.
start
());
points
.
emplace_back
(
segment
.
end
());
points
.
emplace_back
(
segment
.
end
());
}
}
CHECK_GT
(
points
.
size
(),
0
);
CHECK_GT
(
points
.
size
(),
0
);
}
}
CrosswalkInfo
::
CrosswalkInfo
(
const
apollo
::
hdmap
::
Crosswalk
&
crosswalk
)
CrosswalkInfo
::
CrosswalkInfo
(
const
apollo
::
hdmap
::
Crosswalk
&
crosswalk
)
:
_crosswalk
(
crosswalk
)
{
:
_crosswalk
(
crosswalk
)
{
init
();
init
();
}
}
void
CrosswalkInfo
::
init
()
{
void
CrosswalkInfo
::
init
()
{
_polygon
=
convert_to_polygon2d
(
_crosswalk
.
polygon
());
_polygon
=
convert_to_polygon2d
(
_crosswalk
.
polygon
());
CHECK_GT
(
_polygon
.
num_points
(),
2
);
CHECK_GT
(
_polygon
.
num_points
(),
2
);
}
}
StopSignInfo
::
StopSignInfo
(
const
apollo
::
hdmap
::
StopSign
&
stop_sign
)
StopSignInfo
::
StopSignInfo
(
const
apollo
::
hdmap
::
StopSign
&
stop_sign
)
:
_stop_sign
(
stop_sign
)
{
:
_stop_sign
(
stop_sign
)
{
init
();
init
();
}
}
void
StopSignInfo
::
init
()
{
void
StopSignInfo
::
init
()
{
// for (const auto &stop_line : _stop_sign.stop_line()) {
// for (const auto &stop_line : _stop_sign.stop_line()) {
// segments_from_curve(stop_line, &_segments);
// segments_from_curve(stop_line, &_segments);
// }
// }
segments_from_curve
(
_stop_sign
.
stop_line
(),
&
_segments
);
segments_from_curve
(
_stop_sign
.
stop_line
(),
&
_segments
);
CHECK
(
!
_segments
.
empty
());
CHECK
(
!
_segments
.
empty
());
}
}
YieldSignInfo
::
YieldSignInfo
(
const
apollo
::
hdmap
::
YieldSign
&
yield_sign
)
YieldSignInfo
::
YieldSignInfo
(
const
apollo
::
hdmap
::
YieldSign
&
yield_sign
)
:
_yield_sign
(
yield_sign
)
{
:
_yield_sign
(
yield_sign
)
{
init
();
init
();
}
}
void
YieldSignInfo
::
init
()
{
void
YieldSignInfo
::
init
()
{
// for (const auto &stop_line : _yield_sign.stop_line()) {
// for (const auto &stop_line : _yield_sign.stop_line()) {
// segments_from_curve(stop_line, &_segments);
// segments_from_curve(stop_line, &_segments);
// }
// }
segments_from_curve
(
_yield_sign
.
stop_line
(),
&
_segments
);
segments_from_curve
(
_yield_sign
.
stop_line
(),
&
_segments
);
CHECK
(
!
_segments
.
empty
());
CHECK
(
!
_segments
.
empty
());
}
}
OverlapInfo
::
OverlapInfo
(
const
apollo
::
hdmap
::
Overlap
&
overlap
)
OverlapInfo
::
OverlapInfo
(
const
apollo
::
hdmap
::
Overlap
&
overlap
)
:
_overlap
(
overlap
)
{}
:
_overlap
(
overlap
)
{}
const
ObjectOverlapInfo
*
OverlapInfo
::
get_object_overlap_info
(
const
ObjectOverlapInfo
*
OverlapInfo
::
get_object_overlap_info
(
const
apollo
::
hdmap
::
Id
&
id
)
const
{
const
apollo
::
hdmap
::
Id
&
id
)
const
{
for
(
const
auto
&
object
:
_overlap
.
object
())
{
for
(
const
auto
&
object
:
_overlap
.
object
())
{
if
(
object
.
id
().
id
()
==
id
.
id
())
{
if
(
object
.
id
().
id
()
==
id
.
id
())
{
return
&
object
;
return
&
object
;
}
}
}
return
nullptr
;
}
return
nullptr
;
}
}
}
// namespace hdmap
}
// namespace hdmap
...
...
modules/map/hdmap/hdmap_common_test.cc
浏览文件 @
997dd4d5
此差异已折叠。
点击以展开。
modules/planning/common/BUILD
浏览文件 @
997dd4d5
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
cc_library
(
...
@@ -36,3 +38,5 @@ cc_library(
...
@@ -36,3 +38,5 @@ cc_library(
"//modules/localization/proto:localization_proto"
,
"//modules/localization/proto:localization_proto"
,
],
],
)
)
cpplint
()
modules/planning/common/logic_point.cc
浏览文件 @
997dd4d5
...
@@ -28,43 +28,27 @@
...
@@ -28,43 +28,27 @@
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
LogicPoint
::
LogicPoint
(
const
double
x
,
LogicPoint
::
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
y
,
const
double
heading
,
const
double
kappa
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
)
const
double
dkappa
)
:
::
apollo
::
common
::
math
::
Vec2d
(
x
,
y
),
:
::
apollo
::
common
::
math
::
Vec2d
(
x
,
y
),
_s
(
s
),
_s
(
s
),
_heading
(
heading
),
_heading
(
heading
),
_kappa
(
kappa
),
_kappa
(
kappa
),
_dkappa
(
dkappa
),
_dkappa
(
dkappa
),
_lane_id
(
""
)
{
_lane_id
(
""
)
{}
}
double
LogicPoint
::
s
()
const
{
double
LogicPoint
::
s
()
const
{
return
_s
;
}
return
_s
;
}
double
LogicPoint
::
heading
()
const
{
double
LogicPoint
::
heading
()
const
{
return
_heading
;
}
return
_heading
;
}
double
LogicPoint
::
kappa
()
const
{
double
LogicPoint
::
kappa
()
const
{
return
_kappa
;
}
return
_kappa
;
}
double
LogicPoint
::
dkappa
()
const
{
double
LogicPoint
::
dkappa
()
const
{
return
_dkappa
;
}
return
_dkappa
;
}
void
LogicPoint
::
set_lane_id
(
const
std
::
string
&
lane_id
)
{
void
LogicPoint
::
set_lane_id
(
const
std
::
string
&
lane_id
)
{
_lane_id
=
lane_id
;
}
_lane_id
=
lane_id
;
}
const
std
::
string
&
LogicPoint
::
lane_id
()
const
{
const
std
::
string
&
LogicPoint
::
lane_id
()
const
{
return
_lane_id
;
}
return
_lane_id
;
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/common/logic_point.h
浏览文件 @
997dd4d5
...
@@ -33,11 +33,8 @@ namespace planning {
...
@@ -33,11 +33,8 @@ namespace planning {
class
LogicPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
class
LogicPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
public:
public:
explicit
LogicPoint
(
const
double
x
,
explicit
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
y
,
const
double
heading
,
const
double
kappa
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
);
const
double
dkappa
);
double
s
()
const
;
double
s
()
const
;
double
heading
()
const
;
double
heading
()
const
;
...
@@ -45,6 +42,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
...
@@ -45,6 +42,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
double
dkappa
()
const
;
double
dkappa
()
const
;
const
std
::
string
&
lane_id
()
const
;
const
std
::
string
&
lane_id
()
const
;
void
set_lane_id
(
const
std
::
string
&
lane_id
);
void
set_lane_id
(
const
std
::
string
&
lane_id
);
private:
private:
double
_s
;
double
_s
;
double
_heading
;
double
_heading
;
...
@@ -53,7 +51,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
...
@@ -53,7 +51,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
std
::
string
_lane_id
;
std
::
string
_lane_id
;
};
};
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_LOGIC_POINT_H
#endif
// MODULES_PLANNING_COMMON_LOGIC_POINT_H
modules/planning/common/path/BUILD
0 → 100644
浏览文件 @
997dd4d5
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"path_point_util"
,
srcs
=
[
"path_point_util.cc"
,
"sl_point_util.cc"
,
],
hdrs
=
[
"path_point_util.h"
,
"sl_point_util.h"
,
],
deps
=
[
"//modules/common/proto:path_point_proto"
,
"//modules/planning/math:hermite_spline"
,
"//modules/planning/math:integration"
,
"@eigen//:eigen"
,
],
)
cc_library
(
name
=
"path"
,
hdrs
=
[
"path.h"
,
],
)
cc_library
(
name
=
"discretized_path"
,
srcs
=
[
"discretized_path.cc"
,
],
hdrs
=
[
"discretized_path.h"
,
],
deps
=
[
":path"
,
":path_point_util"
,
"//modules/common/proto:path_point_proto"
,
],
)
cc_library
(
name
=
"frenet_frame_path"
,
srcs
=
[
"frenet_frame_path.cc"
,
],
hdrs
=
[
"frenet_frame_path.h"
,
],
deps
=
[
":path"
,
":path_point_util"
,
"//modules/common/proto:path_point_proto"
,
],
)
cc_library
(
name
=
"path_data"
,
srcs
=
[
"path_data.cc"
,
],
hdrs
=
[
"path_data.h"
,
],
deps
=
[
":discretized_path"
,
":frenet_frame_path"
,
"//modules/planning/math:double"
,
],
)
cpplint
()
modules/planning/common/path/discretized_path.cc
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path.cc
**/
#include "modules/planning/common/path/discretized_path.h"
#include <utility>
#include "glog/logging.h"
#include "modules/planning/common/path/path_point_util.h"
namespace
apollo
{
namespace
planning
{
DiscretizedPath
::
DiscretizedPath
(
std
::
vector
<
common
::
PathPoint
>
path_points
)
{
path_points_
=
std
::
move
(
path_points
);
}
common
::
PathPoint
DiscretizedPath
::
evaluate
(
const
double
param
)
const
{
CHECK_GT
(
path_points_
.
size
(),
1
);
CHECK
(
path_points_
.
front
().
s
()
<=
param
&&
path_points_
.
back
().
s
()
<=
param
);
auto
it_lower
=
query_lower_bound
(
param
);
if
(
it_lower
==
path_points_
.
begin
())
{
return
path_points_
.
front
();
}
return
util
::
interpolate
(
*
(
it_lower
-
1
),
*
it_lower
,
param
);
}
double
DiscretizedPath
::
param_length
()
const
{
if
(
path_points_
.
empty
())
{
return
0.0
;
}
return
path_points_
.
back
().
s
()
-
path_points_
.
front
().
s
();
}
int
DiscretizedPath
::
query_closest_point
(
const
double
param
)
const
{
if
(
path_points_
.
empty
())
{
return
-
1
;
}
auto
it_lower
=
query_lower_bound
(
param
);
if
(
it_lower
==
path_points_
.
begin
())
{
return
0
;
}
if
(
it_lower
==
path_points_
.
end
())
{
return
path_points_
.
size
()
-
1
;
}
double
d0
=
param
-
(
it_lower
-
1
)
->
s
();
double
d1
=
it_lower
->
s
()
-
param
;
if
(
d0
<
d1
)
{
return
static_cast
<
int
>
(
it_lower
-
path_points_
.
begin
())
-
1
;
}
else
{
return
static_cast
<
int
>
(
it_lower
-
path_points_
.
begin
());
}
}
std
::
vector
<
common
::
PathPoint
>*
DiscretizedPath
::
mutable_path_points
()
{
return
&
path_points_
;
}
const
std
::
vector
<
common
::
PathPoint
>&
DiscretizedPath
::
path_points
()
const
{
return
path_points_
;
}
std
::
size_t
DiscretizedPath
::
num_of_points
()
const
{
return
path_points_
.
size
();
}
const
common
::
PathPoint
&
DiscretizedPath
::
path_point_at
(
const
std
::
size_t
index
)
const
{
CHECK
(
index
<
path_points_
.
size
());
return
path_points_
[
index
];
}
common
::
PathPoint
DiscretizedPath
::
start_point
()
const
{
CHECK
(
!
path_points_
.
empty
());
return
path_points_
.
front
();
}
common
::
PathPoint
DiscretizedPath
::
end_point
()
const
{
CHECK
(
!
path_points_
.
empty
());
return
path_points_
.
back
();
}
common
::
PathPoint
&
DiscretizedPath
::
path_point_at
(
const
std
::
size_t
index
)
{
CHECK
(
index
<
path_points_
.
size
());
return
path_points_
[
index
];
}
std
::
vector
<
common
::
PathPoint
>::
const_iterator
DiscretizedPath
::
query_lower_bound
(
const
double
param
)
const
{
auto
func
=
[](
const
common
::
PathPoint
&
tp
,
const
double
param
)
{
return
tp
.
s
()
<
param
;
};
return
std
::
lower_bound
(
path_points_
.
begin
(),
path_points_
.
end
(),
param
,
func
);
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/path/discretized_path.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file discretized_path.h
**/
#ifndef MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_
#define MODULES_PLANNING_COMMON_PATH_DISCRETIZED_PATH_H_
#include <vector>
#include "modules/planning/common/path/path.h"
#include "modules/planning/common/path/path_point_util.h"
namespace
apollo
{
namespace
planning
{
class
DiscretizedPath
:
public
Path
{
public:
DiscretizedPath
()
=
default
;
explicit
DiscretizedPath
(
std
::
vector
<
common
::
PathPoint
>
path_points
);
virtual
~
DiscretizedPath
()
=
default
;
common
::
PathPoint
evaluate
(
const
double
param
)
const
override
;
double
param_length
()
const
override
;
common
::
PathPoint
start_point
()
const
override
;
common
::
PathPoint
end_point
()
const
override
;
int
query_closest_point
(
const
double
param
)
const
;
std
::
vector
<
common
::
PathPoint
>*
mutable_path_points
();
const
std
::
vector
<
common
::
PathPoint
>&
path_points
()
const
;
std
::
size_t
num_of_points
()
const
;
const
common
::
PathPoint
&
path_point_at
(
const
std
::
size_t
index
)
const
;
common
::
PathPoint
&
path_point_at
(
const
std
::
size_t
index
);
private:
std
::
vector
<
common
::
PathPoint
>::
const_iterator
query_lower_bound
(
const
double
param
)
const
;
std
::
vector
<
common
::
PathPoint
>
path_points_
;
};
}
// namespace planning
}
// namespace apollo
#endif
/* MODULES_PLANNING_COMMON_PATH_PATH_H_ */
modules/planning/common/path/frenet_frame_path.cc
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file frenet_frame_path.cc
**/
#include "modules/planning/common/path/frenet_frame_path.h"
#include <utility>
#include "glog/logging.h"
#include "modules/common/proto/path_point.pb.h"
namespace
apollo
{
namespace
planning
{
FrenetFramePath
::
FrenetFramePath
(
std
::
vector
<
common
::
FrenetFramePoint
>
sl_points
)
{
points_
=
std
::
move
(
sl_points
);
}
void
FrenetFramePath
::
set_frenet_points
(
const
std
::
vector
<
common
::
FrenetFramePoint
>&
points
)
{
points_
=
points
;
}
std
::
vector
<
common
::
FrenetFramePoint
>*
FrenetFramePath
::
mutable_points
()
{
return
&
points_
;
}
const
std
::
vector
<
common
::
FrenetFramePoint
>&
FrenetFramePath
::
points
()
const
{
return
points_
;
}
std
::
size_t
FrenetFramePath
::
num_points
()
const
{
return
points_
.
size
();
}
const
common
::
FrenetFramePoint
&
FrenetFramePath
::
point_at
(
const
std
::
size_t
index
)
const
{
CHECK
(
index
<
points_
.
size
());
return
points_
[
index
];
}
common
::
FrenetFramePoint
&
FrenetFramePath
::
point_at
(
const
std
::
size_t
index
)
{
CHECK
(
index
<
points_
.
size
());
return
points_
[
index
];
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/path/frenet_frame_path.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file frenet_frame_path.h
**/
#ifndef BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_
#define BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_
#include <vector>
#include "modules/common/proto/path_point.pb.h"
namespace
apollo
{
namespace
planning
{
class
FrenetFramePath
{
public:
FrenetFramePath
()
=
default
;
explicit
FrenetFramePath
(
std
::
vector
<
common
::
FrenetFramePoint
>
sl_points
);
virtual
~
FrenetFramePath
()
=
default
;
void
set_frenet_points
(
const
std
::
vector
<
common
::
FrenetFramePoint
>&
points
);
std
::
vector
<
common
::
FrenetFramePoint
>*
mutable_points
();
const
std
::
vector
<
common
::
FrenetFramePoint
>&
points
()
const
;
std
::
size_t
num_points
()
const
;
const
common
::
FrenetFramePoint
&
point_at
(
const
std
::
size_t
index
)
const
;
common
::
FrenetFramePoint
&
point_at
(
const
std
::
size_t
index
);
private:
std
::
vector
<
common
::
FrenetFramePoint
>
points_
;
};
}
// namespace planning
}
// namespace apollo
#endif
/* BAIDU_ADU_HOUSTON_COMMON_PATH_FRENET_FRAME_PATH_H_ */
modules/planning/common/path/path.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path.h
**/
#ifndef MODULES_PLANNING_COMMON_PATH_PATH_H_
#define MODULES_PLANNING_COMMON_PATH_PATH_H_
#include "modules/common/proto/path_point.pb.h"
namespace
apollo
{
namespace
planning
{
class
Path
{
public:
Path
()
=
default
;
virtual
~
Path
()
=
default
;
virtual
common
::
PathPoint
evaluate
(
const
double
param
)
const
=
0
;
virtual
double
param_length
()
const
=
0
;
virtual
common
::
PathPoint
start_point
()
const
=
0
;
virtual
common
::
PathPoint
end_point
()
const
=
0
;
};
}
// namespace planning
}
// namespace apollo
#endif
/* MODULES_PLANNING_COMMON_PATH_PATH_H_ */
modules/planning/common/path/path_data.cc
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path_data.cc
**/
#include "modules/planning/common/path/path_data.h"
#include <sstream>
#include "glog/logging.h"
#include "modules/planning/math/double.h"
namespace
apollo
{
namespace
planning
{
void
PathData
::
set_path
(
const
DiscretizedPath
&
path
)
{
path_
=
path
;
}
void
PathData
::
set_frenet_path
(
const
FrenetFramePath
&
frenet_path
)
{
frenet_path_
=
frenet_path
;
}
DiscretizedPath
*
PathData
::
mutable_path
()
{
return
&
path_
;
}
const
DiscretizedPath
&
PathData
::
path
()
const
{
return
path_
;
}
FrenetFramePath
*
PathData
::
mutable_frenet_frame_path
()
{
return
&
frenet_path_
;
}
const
FrenetFramePath
&
PathData
::
frenet_frame_path
()
const
{
return
frenet_path_
;
}
bool
PathData
::
get_path_point_with_path_s
(
const
double
s
,
common
::
PathPoint
*
const
path_point
)
const
{
const
auto
&
path_points
=
path_
.
path_points
();
if
(
path_points
.
size
()
<
2
||
s
<
path_points
.
front
().
s
()
||
path_points
.
back
().
s
()
<
s
)
{
return
false
;
}
auto
comp
=
[](
const
common
::
PathPoint
&
path_point
,
const
double
s
)
{
return
path_point
.
s
()
<
s
;
};
auto
it_lower
=
std
::
lower_bound
(
path_points
.
begin
(),
path_points
.
end
(),
s
,
comp
);
if
(
it_lower
==
path_points
.
begin
())
{
*
path_point
=
*
it_lower
;
}
else
{
double
s0
=
(
it_lower
-
1
)
->
s
();
double
s1
=
it_lower
->
s
();
CHECK
(
s0
<
s1
);
*
path_point
=
util
::
interpolate_linear_approximation
(
*
(
it_lower
-
1
),
*
it_lower
,
s
);
}
return
true
;
}
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
=
path_
.
path_points
().
front
();
}
else
{
// std::size_t index_lower = (std::size_t)(it_lower -
// frenet_points.begin());
//
// double ref_s0 = (it_lower - 1)->s();
// double ref_s1 = it_lower->s();
//
// CHECK(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);
}
return
true
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/path/path_data.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path_data.h
**/
#ifndef MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_
#define MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_
#include <vector>
#include "modules/planning/common/path/discretized_path.h"
#include "modules/planning/common/path/frenet_frame_path.h"
namespace
apollo
{
namespace
planning
{
class
PathData
{
public:
PathData
()
=
default
;
void
set_path
(
const
DiscretizedPath
&
path
);
void
set_frenet_path
(
const
FrenetFramePath
&
frenet_path
);
DiscretizedPath
*
mutable_path
();
const
DiscretizedPath
&
path
()
const
;
FrenetFramePath
*
mutable_frenet_frame_path
();
const
FrenetFramePath
&
frenet_frame_path
()
const
;
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
,
common
::
PathPoint
*
const
path_point
)
const
;
// TODO(fanhaoyang) add check if the path data is valid
private:
DiscretizedPath
path_
;
FrenetFramePath
frenet_path_
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_PATH_PATH_DATA_H_
modules/planning/common/path/path_point_util.cc
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path_point_util.cc
**/
#include "modules/planning/common/path/path_point_util.h"
#include <utility>
#include "modules/planning/math/hermite_spline.h"
#include "modules/planning/math/integration.h"
namespace
apollo
{
namespace
planning
{
namespace
util
{
common
::
PathPoint
interpolate
(
const
common
::
PathPoint
&
p0
,
const
common
::
PathPoint
&
p1
,
const
double
s
)
{
double
s0
=
p0
.
s
();
double
s1
=
p1
.
s
();
CHECK
(
s0
<=
s
&&
s
<=
s1
);
std
::
array
<
double
,
2
>
gx0
{{
p0
.
theta
(),
p0
.
kappa
()}};
std
::
array
<
double
,
2
>
gx1
{{
p1
.
theta
(),
p1
.
kappa
()}};
HermiteSpline
<
double
,
3
>
geometry_spline
(
gx0
,
gx1
,
s0
,
s1
);
auto
func_cos_theta
=
[
&
geometry_spline
](
const
double
s
)
{
auto
theta
=
geometry_spline
.
evaluate
(
0
,
s
);
return
std
::
cos
(
theta
);
};
auto
func_sin_theta
=
[
&
geometry_spline
](
const
double
s
)
{
auto
theta
=
geometry_spline
.
evaluate
(
0
,
s
);
return
std
::
sin
(
theta
);
};
double
x
=
p0
.
x
()
+
Integration
::
gauss_legendre
(
func_cos_theta
,
s0
,
s
);
double
y
=
p0
.
y
()
+
Integration
::
gauss_legendre
(
func_sin_theta
,
s0
,
s
);
double
theta
=
geometry_spline
.
evaluate
(
0
,
s
);
double
kappa
=
geometry_spline
.
evaluate
(
1
,
s
);
double
dkappa
=
geometry_spline
.
evaluate
(
2
,
s
);
double
d2kappa
=
geometry_spline
.
evaluate
(
3
,
s
);
common
::
PathPoint
p
;
p
.
set_x
(
x
);
p
.
set_y
(
y
);
p
.
set_theta
(
theta
);
p
.
set_kappa
(
kappa
);
p
.
set_dkappa
(
dkappa
);
p
.
set_ddkappa
(
d2kappa
);
p
.
set_s
(
s
);
return
std
::
move
(
p
);
}
common
::
PathPoint
interpolate_linear_approximation
(
const
common
::
PathPoint
&
left
,
const
common
::
PathPoint
&
right
,
const
double
s
)
{
double
s0
=
left
.
s
();
double
s1
=
right
.
s
();
CHECK
(
s0
<
s1
);
common
::
PathPoint
path_point
;
double
weight
=
(
s
-
s0
)
/
(
s1
-
s0
);
double
x
=
(
1
-
weight
)
*
left
.
x
()
+
weight
*
right
.
x
();
double
y
=
(
1
-
weight
)
*
left
.
y
()
+
weight
*
right
.
y
();
double
cos_heading
=
(
1
-
weight
)
*
std
::
cos
(
left
.
theta
())
+
weight
*
std
::
cos
(
right
.
theta
());
double
sin_heading
=
(
1
-
weight
)
*
std
::
sin
(
left
.
theta
())
+
weight
*
std
::
sin
(
right
.
theta
());
double
theta
=
std
::
atan2
(
sin_heading
,
cos_heading
);
double
kappa
=
(
1
-
weight
)
*
left
.
kappa
()
+
weight
*
right
.
kappa
();
double
dkappa
=
(
1
-
weight
)
*
left
.
dkappa
()
+
weight
*
right
.
dkappa
();
double
ddkappa
=
(
1
-
weight
)
*
left
.
ddkappa
()
+
weight
*
right
.
ddkappa
();
path_point
.
set_x
(
x
);
path_point
.
set_y
(
y
);
path_point
.
set_theta
(
theta
);
path_point
.
set_kappa
(
kappa
);
path_point
.
set_dkappa
(
dkappa
);
path_point
.
set_ddkappa
(
ddkappa
);
return
path_point
;
}
}
// namespace util
}
// namespace planning
}
// namespace apollo
modules/planning/common/path/path_point_util.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file path_point_util.h
**/
#ifndef MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_
#define MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_
#include "Eigen/Dense"
#include "modules/common/proto/path_point.pb.h"
namespace
apollo
{
namespace
planning
{
namespace
util
{
common
::
PathPoint
interpolate
(
const
common
::
PathPoint
&
p0
,
const
common
::
PathPoint
&
p1
,
const
double
s
);
// @ weight shall between 1 and 0
common
::
PathPoint
interpolate_linear_approximation
(
const
common
::
PathPoint
&
left
,
const
common
::
PathPoint
&
right
,
const
double
s
);
}
// namespace util
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_PATH_PATH_POINT_H_
modules/planning/common/path/sl_point_util.cc
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file sl_point_util.cc
**/
#include "modules/planning/common/path/sl_point_util.h"
namespace
apollo
{
namespace
planning
{
namespace
util
{
common
::
SLPoint
interpolate
(
const
common
::
SLPoint
&
start
,
const
common
::
SLPoint
&
end
,
const
double
weight
)
{
common
::
SLPoint
point
;
double
s
=
start
.
s
()
*
(
1
-
weight
)
+
end
.
s
()
*
weight
;
double
l
=
start
.
l
()
*
(
1
-
weight
)
+
end
.
l
()
*
weight
;
point
.
set_s
(
s
);
point
.
set_l
(
l
);
return
point
;
}
}
// namespace util
}
// namespace planning
}
// namespace apollo
modules/planning/common/path/sl_point_util.h
0 → 100644
浏览文件 @
997dd4d5
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file sl_point_util.h
**/
#ifndef MODULES_PLANNING_COMMON_PATH_SL_POINT_H_
#define MODULES_PLANNING_COMMON_PATH_SL_POINT_H_
#include "modules/common/proto/path_point.pb.h"
namespace
apollo
{
namespace
planning
{
namespace
util
{
static
common
::
SLPoint
interpolate
(
const
common
::
SLPoint
&
start
,
const
common
::
SLPoint
&
end
,
const
double
weight
);
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_PATH_SL_POINT_H_
modules/planning/common/speed/speed_data.cc
浏览文件 @
997dd4d5
...
@@ -29,75 +29,80 @@ namespace apollo {
...
@@ -29,75 +29,80 @@ namespace apollo {
namespace
planning
{
namespace
planning
{
namespace
{
namespace
{
bool
speed_time_comp
(
const
double
t
,
const
SpeedPoint
&
speed_point
)
{
bool
speed_time_comp
(
const
double
t
,
const
SpeedPoint
&
speed_point
)
{
return
t
<
speed_point
.
t
();
return
t
<
speed_point
.
t
();
}
}
}
}
SpeedData
::
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
)
{
SpeedData
::
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
)
{
speed_vector_
=
std
::
move
(
speed_points
);
speed_vector_
=
std
::
move
(
speed_points
);
}
}
std
::
vector
<
SpeedPoint
>*
SpeedData
::
mutable_speed_vector
()
{
std
::
vector
<
SpeedPoint
>*
SpeedData
::
mutable_speed_vector
()
{
return
&
speed_vector_
;
return
&
speed_vector_
;
}
}
const
std
::
vector
<
SpeedPoint
>&
SpeedData
::
speed_vector
()
const
{
const
std
::
vector
<
SpeedPoint
>&
SpeedData
::
speed_vector
()
const
{
return
speed_vector_
;
return
speed_vector_
;
}
}
void
SpeedData
::
set_speed_vector
(
const
std
::
vector
<
SpeedPoint
>&
speed_points
)
{
void
SpeedData
::
set_speed_vector
(
const
std
::
vector
<
SpeedPoint
>&
speed_points
)
{
speed_vector_
=
std
::
move
(
speed_points
);
speed_vector_
=
std
::
move
(
speed_points
);
}
}
bool
SpeedData
::
get_speed_point_with_time
(
const
double
t
,
SpeedPoint
*
const
speed_point
)
const
{
bool
SpeedData
::
get_speed_point_with_time
(
const
double
t
,
if
(
speed_vector_
.
size
()
<
2
)
{
SpeedPoint
*
const
speed_point
)
const
{
return
false
;
if
(
speed_vector_
.
size
()
<
2
)
{
}
return
false
;
std
::
size_t
index
=
find_index
(
t
);
}
if
(
Double
::
compare
(
t
,
speed_vector_
[
index
].
t
())
<
0
||
index
+
1
>=
speed_vector_
.
size
())
{
std
::
size_t
index
=
find_index
(
t
);
return
false
;
if
(
Double
::
compare
(
t
,
speed_vector_
[
index
].
t
())
<
0
||
}
index
+
1
>=
speed_vector_
.
size
())
{
return
false
;
// index index + 1
}
double
weight
=
0.0
;
if
(
Double
::
compare
(
speed_vector_
[
index
+
1
].
t
(),
speed_vector_
[
index
].
t
())
>
0
)
{
// index index + 1
weight
=
(
t
-
speed_vector_
[
index
].
t
())
/
double
weight
=
0.0
;
(
speed_vector_
[
index
].
t
()
-
speed_vector_
[
index
+
1
].
t
());
if
(
Double
::
compare
(
speed_vector_
[
index
+
1
].
t
(),
speed_vector_
[
index
].
t
())
>
}
0
)
{
weight
=
(
t
-
speed_vector_
[
index
].
t
())
/
*
speed_point
=
SpeedPoint
::
interpolate
(
speed_vector_
[
index
],
speed_vector_
[
index
+
1
],
weight
);
(
speed_vector_
[
index
].
t
()
-
speed_vector_
[
index
+
1
].
t
());
return
true
;
}
*
speed_point
=
SpeedPoint
::
interpolate
(
speed_vector_
[
index
],
speed_vector_
[
index
+
1
],
weight
);
return
true
;
}
}
double
SpeedData
::
total_time
()
const
{
double
SpeedData
::
total_time
()
const
{
if
(
speed_vector_
.
empty
())
{
if
(
speed_vector_
.
empty
())
{
return
0.0
;
return
0.0
;
}
}
return
speed_vector_
.
back
().
t
()
-
speed_vector_
.
front
().
t
();
return
speed_vector_
.
back
().
t
()
-
speed_vector_
.
front
().
t
();
}
}
std
::
string
SpeedData
::
DebugString
()
const
{
std
::
string
SpeedData
::
DebugString
()
const
{
std
::
ostringstream
sout
;
std
::
ostringstream
sout
;
sout
<<
"["
<<
std
::
endl
;
sout
<<
"["
<<
std
::
endl
;
for
(
std
::
size_t
i
=
0
;
i
<
speed_vector_
.
size
();
++
i
)
{
for
(
std
::
size_t
i
=
0
;
i
<
speed_vector_
.
size
();
++
i
)
{
if
(
i
>
0
)
{
if
(
i
>
0
)
{
sout
<<
","
<<
std
::
endl
;
sout
<<
","
<<
std
::
endl
;
}
sout
<<
speed_vector_
[
i
].
DebugString
();
}
}
sout
<<
"]"
<<
std
::
endl
;
sout
<<
speed_vector_
[
i
].
DebugString
();
sout
.
flush
();
}
return
sout
.
str
();
sout
<<
"]"
<<
std
::
endl
;
sout
.
flush
();
return
sout
.
str
();
}
}
std
::
size_t
SpeedData
::
find_index
(
const
double
t
)
const
{
std
::
size_t
SpeedData
::
find_index
(
const
double
t
)
const
{
auto
upper_bound
=
std
::
upper_bound
(
speed_vector_
.
begin
()
+
1
,
speed_vector_
.
end
(),
auto
upper_bound
=
std
::
upper_bound
(
speed_vector_
.
begin
()
+
1
,
t
,
speed_time_comp
);
speed_vector_
.
end
(),
t
,
speed_time_comp
);
return
std
::
min
(
speed_vector_
.
size
()
-
1
,
return
std
::
min
(
static_cast
<
std
::
size_t
>
(
upper_bound
-
speed_vector_
.
begin
()))
-
1
;
speed_vector_
.
size
()
-
1
,
static_cast
<
std
::
size_t
>
(
upper_bound
-
speed_vector_
.
begin
()))
-
1
;
}
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/common/speed/speed_data.h
浏览文件 @
997dd4d5
...
@@ -28,31 +28,31 @@ namespace apollo {
...
@@ -28,31 +28,31 @@ namespace apollo {
namespace
planning
{
namespace
planning
{
class
SpeedData
{
class
SpeedData
{
public:
public:
SpeedData
()
=
default
;
SpeedData
()
=
default
;
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
);
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
);
std
::
vector
<
SpeedPoint
>*
mutable_speed_vector
();
std
::
vector
<
SpeedPoint
>*
mutable_speed_vector
();
const
std
::
vector
<
SpeedPoint
>&
speed_vector
()
const
;
const
std
::
vector
<
SpeedPoint
>&
speed_vector
()
const
;
void
set_speed_vector
(
const
std
::
vector
<
SpeedPoint
>&
speed_points
);
void
set_speed_vector
(
const
std
::
vector
<
SpeedPoint
>&
speed_points
);
virtual
std
::
string
DebugString
()
const
;
virtual
std
::
string
DebugString
()
const
;
bool
get_speed_point_with_time
(
const
double
time
,
SpeedPoint
*
const
speed_point
)
const
;
bool
get_speed_point_with_time
(
const
double
time
,
SpeedPoint
*
const
speed_point
)
const
;
double
total_time
()
const
;
double
total_time
()
const
;
private:
std
::
size_t
find_index
(
const
double
s
)
const
;
std
::
vector
<
SpeedPoint
>
speed_vector_
;
private:
};
std
::
size_t
find_index
(
const
double
s
)
const
;
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H
std
::
vector
<
SpeedPoint
>
speed_vector_
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_DATA_H
modules/planning/common/speed/speed_point.cc
浏览文件 @
997dd4d5
/******************************************************************************
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
*
* Licensed under the Apache License, Version 2.0 (the "License");
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
* You may obtain a copy of the License at
*
*
* http://www.apache.org/licenses/LICENSE-2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
*
* Unless required by applicable law or agreed to in writing, software
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* See the License for the specific language governing permissions and
* limitations under the License.
* limitations under the License.
*****************************************************************************/
*****************************************************************************/
/**
/**
* @file
* @file
...
@@ -25,56 +25,38 @@
...
@@ -25,56 +25,38 @@
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
SpeedPoint
::
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
)
:
SpeedPoint
::
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
STPoint
(
st_point
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
const
double
j
)
}
:
STPoint
(
st_point
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
}
SpeedPoint
::
SpeedPoint
(
SpeedPoint
::
SpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
s
,
const
double
a
,
const
double
j
)
const
double
t
,
:
STPoint
(
s
,
t
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{}
const
double
v
,
const
double
a
,
const
double
j
)
:
STPoint
(
s
,
t
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
}
void
SpeedPoint
::
set_v
(
const
double
v
)
{
void
SpeedPoint
::
set_v
(
const
double
v
)
{
v_
=
v
;
}
v_
=
v
;
}
void
SpeedPoint
::
set_a
(
const
double
a
)
{
a_
=
a
;
}
void
SpeedPoint
::
set_j
(
const
double
j
)
{
void
SpeedPoint
::
set_a
(
const
double
a
)
{
a_
=
a
;
}
j_
=
j
;
}
double
SpeedPoint
::
v
()
const
{
void
SpeedPoint
::
set_j
(
const
double
j
)
{
j_
=
j
;
}
return
v_
;
}
double
SpeedPoint
::
a
()
const
{
double
SpeedPoint
::
v
()
const
{
return
v_
;
}
return
a_
;
}
double
SpeedPoint
::
j
()
const
{
double
SpeedPoint
::
a
()
const
{
return
a_
;
}
return
j_
;
}
SpeedPoint
SpeedPoint
::
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
double
SpeedPoint
::
j
()
const
{
return
j_
;
}
const
double
weight
)
{
double
s
=
(
1
-
weight
)
*
left
.
s
()
+
weight
*
right
.
s
();
double
t
=
(
1
-
weight
)
*
left
.
t
()
+
weight
*
right
.
t
();
double
v
=
(
1
-
weight
)
*
left
.
v
()
+
weight
*
right
.
v
();
double
a
=
(
1
-
weight
)
*
left
.
a
()
+
weight
*
right
.
a
();
double
j
=
(
1
-
weight
)
*
left
.
j
()
+
weight
*
right
.
j
();
return
SpeedPoint
(
s
,
t
,
v
,
a
,
j
);
}
std
::
string
SpeedPoint
::
DebugString
()
const
{
SpeedPoint
SpeedPoint
::
interpolate
(
const
SpeedPoint
&
left
,
return
""
;
const
SpeedPoint
&
right
,
const
double
weight
)
{
double
s
=
(
1
-
weight
)
*
left
.
s
()
+
weight
*
right
.
s
();
double
t
=
(
1
-
weight
)
*
left
.
t
()
+
weight
*
right
.
t
();
double
v
=
(
1
-
weight
)
*
left
.
v
()
+
weight
*
right
.
v
();
double
a
=
(
1
-
weight
)
*
left
.
a
()
+
weight
*
right
.
a
();
double
j
=
(
1
-
weight
)
*
left
.
j
()
+
weight
*
right
.
j
();
return
SpeedPoint
(
s
,
t
,
v
,
a
,
j
);
}
}
}
// namespace planning
std
::
string
SpeedPoint
::
DebugString
()
const
{
return
""
;
}
}
// namespace apollo
}
// namespace planning
}
// namespace apollo
modules/planning/common/speed/speed_point.h
浏览文件 @
997dd4d5
...
@@ -27,33 +27,31 @@ namespace apollo {
...
@@ -27,33 +27,31 @@ namespace apollo {
namespace
planning
{
namespace
planning
{
class
SpeedPoint
:
public
STPoint
{
class
SpeedPoint
:
public
STPoint
{
public:
public:
SpeedPoint
()
=
default
;
SpeedPoint
()
=
default
;
SpeedPoint
(
const
double
s
,
SpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
const
double
t
,
const
double
j
);
const
double
v
,
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
a
,
const
double
j
);
const
double
j
);
void
set_v
(
const
double
v
);
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
);
void
set_a
(
const
double
a
);
void
set_v
(
const
double
v
);
void
set_j
(
const
double
j
);
void
set_a
(
const
double
a
);
void
set_j
(
const
double
j
);
double
v
()
const
;
double
a
()
const
;
double
v
()
const
;
double
j
()
const
;
double
a
()
const
;
double
j
()
const
;
std
::
string
DebugString
()
const
;
static
SpeedPoint
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
std
::
string
DebugString
()
const
;
const
double
weight
);
static
SpeedPoint
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
const
double
weight
);
private:
private:
double
v_
=
0.0
;
double
v_
=
0.0
;
double
a_
=
0.0
;
double
a_
=
0.0
;
double
j_
=
0.0
;
double
j_
=
0.0
;
};
};
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H
#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H
modules/planning/common/speed/st_point.cc
浏览文件 @
997dd4d5
...
@@ -26,33 +26,23 @@
...
@@ -26,33 +26,23 @@
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
STPoint
::
STPoint
(
const
double
s
,
const
double
t
)
:
Vec2d
(
t
,
s
)
{
STPoint
::
STPoint
(
const
double
s
,
const
double
t
)
:
Vec2d
(
t
,
s
){};
};
double
STPoint
::
s
()
const
{
double
STPoint
::
s
()
const
{
return
y_
;
}
return
y_
;
}
double
STPoint
::
t
()
const
{
double
STPoint
::
t
()
const
{
return
x_
;
}
return
x_
;
}
void
STPoint
::
set_s
(
const
double
s
)
{
void
STPoint
::
set_s
(
const
double
s
)
{
return
set_y
(
s
);
}
return
set_y
(
s
);
}
void
STPoint
::
set_t
(
const
double
t
)
{
void
STPoint
::
set_t
(
const
double
t
)
{
return
set_x
(
t
);
}
return
set_x
(
t
);
}
std
::
string
STPoint
::
DebugString
()
const
{
std
::
string
STPoint
::
DebugString
()
const
{
std
::
ostringstream
sout
;
std
::
ostringstream
sout
;
sout
<<
"{
\"
s
\"
: "
<<
std
::
setprecision
(
6
)
<<
s
()
sout
<<
"{
\"
s
\"
: "
<<
std
::
setprecision
(
6
)
<<
s
()
<<
",
\"
t
\"
: "
<<
std
::
setprecision
(
6
)
<<
t
()
<<
" }"
;
<<
",
\"
t
\"
: "
<<
std
::
setprecision
(
6
)
<<
t
()
<<
" }"
;
sout
.
flush
();
sout
.
flush
();
return
sout
.
str
();
return
sout
.
str
();
}
}
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
modules/planning/common/speed/st_point.h
浏览文件 @
997dd4d5
...
@@ -27,18 +27,17 @@ namespace apollo {
...
@@ -27,18 +27,17 @@ namespace apollo {
namespace
planning
{
namespace
planning
{
class
STPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
class
STPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
public:
public:
STPoint
()
=
default
;
STPoint
()
=
default
;
STPoint
(
const
double
s
,
const
double
t
);
STPoint
(
const
double
s
,
const
double
t
);
double
s
()
const
;
double
s
()
const
;
double
t
()
const
;
double
t
()
const
;
void
set_s
(
const
double
s
);
void
set_s
(
const
double
s
);
void
set_t
(
const
double
t
);
void
set_t
(
const
double
t
);
std
::
string
DebugString
()
const
;
std
::
string
DebugString
()
const
;
};
};
}
// namespace planning
}
// namespace planning
}
// namespace apollo
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H
#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H
modules/planning/math/spiral_curve/quintic_spiral_curve.cc
浏览文件 @
997dd4d5
...
@@ -33,7 +33,8 @@ namespace planning {
...
@@ -33,7 +33,8 @@ namespace planning {
using
::
apollo
::
common
::
ErrorCode
;
using
::
apollo
::
common
::
ErrorCode
;
using
::
apollo
::
common
::
PathPoint
;
using
::
apollo
::
common
::
PathPoint
;
QuinticSpiralCurve
::
QuinticSpiralCurve
(
const
PathPoint
&
s
,
const
PathPoint
&
e
)
QuinticSpiralCurve
::
QuinticSpiralCurve
(
const
common
::
PathPoint
&
s
,
const
common
::
PathPoint
&
e
)
:
SpiralCurve
(
s
,
e
,
5
)
{
:
SpiralCurve
(
s
,
e
,
5
)
{
// generate an order 5 spiral path with four parameters
// generate an order 5 spiral path with four parameters
}
}
...
@@ -191,7 +192,7 @@ bool QuinticSpiralCurve::calculate_path() {
...
@@ -191,7 +192,7 @@ bool QuinticSpiralCurve::calculate_path() {
}
}
ErrorCode
QuinticSpiralCurve
::
get_path_vec
(
ErrorCode
QuinticSpiralCurve
::
get_path_vec
(
const
std
::
size_t
n
,
std
::
vector
<
PathPoint
>*
path_points
)
const
{
const
std
::
size_t
n
,
std
::
vector
<
common
::
PathPoint
>*
path_points
)
const
{
CHECK_NOTNULL
(
path_points
);
CHECK_NOTNULL
(
path_points
);
if
(
n
<=
1
||
error
()
>
spiral_config
().
newton_raphson_tol
())
{
if
(
n
<=
1
||
error
()
>
spiral_config
().
newton_raphson_tol
())
{
...
@@ -199,7 +200,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
...
@@ -199,7 +200,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
}
}
path_points
->
resize
(
n
);
path_points
->
resize
(
n
);
std
::
vector
<
PathPoint
>&
result
=
*
path_points
;
std
::
vector
<
common
::
PathPoint
>&
result
=
*
path_points
;
result
[
0
].
set_x
(
start_point
().
x
());
result
[
0
].
set_x
(
start_point
().
x
());
result
[
0
].
set_y
(
start_point
().
y
());
result
[
0
].
set_y
(
start_point
().
y
());
...
@@ -247,7 +248,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
...
@@ -247,7 +248,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
ErrorCode
QuinticSpiralCurve
::
get_path_vec_with_s
(
ErrorCode
QuinticSpiralCurve
::
get_path_vec_with_s
(
const
std
::
vector
<
double
>&
vec_s
,
const
std
::
vector
<
double
>&
vec_s
,
std
::
vector
<
PathPoint
>*
path_points
)
const
{
std
::
vector
<
common
::
PathPoint
>*
path_points
)
const
{
CHECK_NOTNULL
(
path_points
);
CHECK_NOTNULL
(
path_points
);
if
(
error
()
>
spiral_config
().
newton_raphson_tol
())
{
if
(
error
()
>
spiral_config
().
newton_raphson_tol
())
{
...
@@ -260,10 +261,10 @@ ErrorCode QuinticSpiralCurve::get_path_vec_with_s(
...
@@ -260,10 +261,10 @@ ErrorCode QuinticSpiralCurve::get_path_vec_with_s(
const
std
::
size_t
n
=
vec_s
.
size
();
const
std
::
size_t
n
=
vec_s
.
size
();
PathPoint
ref_point
=
start_point
();
common
::
PathPoint
ref_point
=
start_point
();
path_points
->
resize
(
n
);
path_points
->
resize
(
n
);
std
::
vector
<
PathPoint
>&
result
=
*
path_points
;
std
::
vector
<
common
::
PathPoint
>&
result
=
*
path_points
;
std
::
array
<
double
,
6
>
p_value
;
std
::
array
<
double
,
6
>
p_value
;
std
::
copy
(
p_params
().
begin
(),
p_params
().
end
(),
p_value
.
begin
());
std
::
copy
(
p_params
().
begin
(),
p_params
().
end
(),
p_value
.
begin
());
std
::
array
<
double
,
6
>
a_params
=
SpiralFormula
::
p_to_a_k5
(
sg
(),
p_value
);
std
::
array
<
double
,
6
>
a_params
=
SpiralFormula
::
p_to_a_k5
(
sg
(),
p_value
);
...
...
modules/planning/math/spiral_curve/spiral_curve.cc
浏览文件 @
997dd4d5
...
@@ -27,7 +27,7 @@ namespace planning {
...
@@ -27,7 +27,7 @@ namespace planning {
using
apollo
::
common
::
PathPoint
;
using
apollo
::
common
::
PathPoint
;
SpiralCurve
::
SpiralCurve
(
const
PathPoint
&
s
,
const
PathPoint
&
e
,
SpiralCurve
::
SpiralCurve
(
const
common
::
PathPoint
&
s
,
const
common
::
PathPoint
&
e
,
const
std
::
size_t
order
)
const
std
::
size_t
order
)
:
p_params_
(
order
+
1
,
0.0
),
:
p_params_
(
order
+
1
,
0.0
),
sg_
(
0.0
),
sg_
(
0.0
),
...
@@ -42,9 +42,11 @@ void SpiralCurve::set_spiral_config(const SpiralCurveConfig& spiral_config) {
...
@@ -42,9 +42,11 @@ void SpiralCurve::set_spiral_config(const SpiralCurveConfig& spiral_config) {
}
}
// output params
// output params
const
PathPoint
&
SpiralCurve
::
start_point
()
const
{
return
*
start_point_
;
}
const
common
::
PathPoint
&
SpiralCurve
::
start_point
()
const
{
return
*
start_point_
;
}
const
PathPoint
&
SpiralCurve
::
end_point
()
const
{
return
*
end_point_
;
}
const
common
::
PathPoint
&
SpiralCurve
::
end_point
()
const
{
return
*
end_point_
;
}
double
SpiralCurve
::
sg
()
const
{
return
sg_
;
}
double
SpiralCurve
::
sg
()
const
{
return
sg_
;
}
...
...
modules/planning/planner/em_planner.cc
浏览文件 @
997dd4d5
...
@@ -28,13 +28,10 @@ namespace planning {
...
@@ -28,13 +28,10 @@ namespace planning {
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
vehicle_state
::
VehicleState
;
using
apollo
::
common
::
vehicle_state
::
VehicleState
;
EMPlanner
::
EMPlanner
()
{
EMPlanner
::
EMPlanner
()
{}
}
bool
EMPlanner
::
Plan
(
const
TrajectoryPoint
&
start_point
,
std
::
vector
<
TrajectoryPoint
>*
discretized_trajectory
)
{
bool
EMPlanner
::
Plan
(
const
TrajectoryPoint
&
start_point
,
std
::
vector
<
TrajectoryPoint
>*
discretized_trajectory
)
{
return
true
;
return
true
;
}
}
...
...
modules/planning/planner/em_planner.h
浏览文件 @
997dd4d5
...
@@ -52,9 +52,8 @@ class EMPlanner : public Planner {
...
@@ -52,9 +52,8 @@ class EMPlanner : public Planner {
* @param discretized_trajectory The computed trajectory
* @param discretized_trajectory The computed trajectory
* @return true if planning succeeds; false otherwise.
* @return true if planning succeeds; false otherwise.
*/
*/
bool
Plan
(
bool
Plan
(
const
apollo
::
common
::
TrajectoryPoint
&
start_point
,
const
apollo
::
common
::
TrajectoryPoint
&
start_point
,
std
::
vector
<
apollo
::
common
::
TrajectoryPoint
>*
trajectory
)
override
;
std
::
vector
<
apollo
::
common
::
TrajectoryPoint
>*
trajectory
)
override
;
private:
private:
};
};
...
...
modules/planning/planning.cc
浏览文件 @
997dd4d5
...
@@ -13,6 +13,7 @@
...
@@ -13,6 +13,7 @@
* See the License for the specific language governing permissions and
* See the License for the specific language governing permissions and
* limitations under the License.
* limitations under the License.
*****************************************************************************/
*****************************************************************************/
#include "modules/planning/planning.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
...
...
modules/planning/planning.h
浏览文件 @
997dd4d5
...
@@ -102,9 +102,7 @@ class Planning : public apollo::common::ApolloApp {
...
@@ -102,9 +102,7 @@ class Planning : public apollo::common::ApolloApp {
const
double
header_time
,
const
double
header_time
,
const
std
::
vector
<
common
::
TrajectoryPoint
>&
discretized_trajectory
);
const
std
::
vector
<
common
::
TrajectoryPoint
>&
discretized_trajectory
);
apollo
::
common
::
util
::
Factory
<
PlanningConfig
::
PlannerType
,
Planner
>
apollo
::
common
::
util
::
Factory
<
PlanningConfig
::
PlannerType
,
Planner
>
planner_factory_
;
planner_factory_
;
PlanningConfig
config_
;
PlanningConfig
config_
;
...
...
modules/planning/planning_test.cc
浏览文件 @
997dd4d5
...
@@ -14,9 +14,9 @@
...
@@ -14,9 +14,9 @@
* limitations under the License.
* limitations under the License.
*****************************************************************************/
*****************************************************************************/
#include "modules/planning/planning.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planning.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/planning.pb.h"
#include "gmock/gmock.h"
#include "gmock/gmock.h"
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录