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";
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
{
// coordinates
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
limitations under the License.
=========================================================================*/
#include <iostream>
#include <algorithm>
#include <iostream>
#include "modules/map/hdmap/hdmap_common.h"
#include "glog/logging.h"
#include "modules/map/hdmap/hdmap_common.h"
namespace
{
...
...
@@ -66,14 +66,15 @@ apollo::common::math::Polygon2d convert_to_polygon2d(
}
remove_duplicates
(
&
points
);
while
(
points
.
size
()
>=
2
&&
points
[
0
].
DistanceTo
(
points
.
back
())
<=
apollo
::
common
::
math
::
kMathEpsilon
)
{
points
[
0
].
DistanceTo
(
points
.
back
())
<=
apollo
::
common
::
math
::
kMathEpsilon
)
{
points
.
pop_back
();
}
return
apollo
::
common
::
math
::
Polygon2d
(
points
);
}
void
segments_from_curve
(
const
apollo
::
hdmap
::
Curve
&
curve
,
void
segments_from_curve
(
const
apollo
::
hdmap
::
Curve
&
curve
,
std
::
vector
<
apollo
::
common
::
math
::
LineSegment2d
>
*
segments
)
{
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>
points
;
points_from_curve
(
curve
,
&
points
);
...
...
@@ -147,8 +148,7 @@ double LaneInfo::get_effective_width(const double s) const {
}
double
LaneInfo
::
get_width_from_sample
(
const
std
::
vector
<
LaneInfo
::
SampledWidth
>&
samples
,
const
double
s
)
const
{
const
std
::
vector
<
LaneInfo
::
SampledWidth
>
&
samples
,
const
double
s
)
const
{
if
(
samples
.
empty
())
{
return
0.0
;
}
...
...
@@ -168,8 +168,8 @@ double LaneInfo::get_width_from_sample(
high
=
mid
;
}
}
const
LaneInfo
::
SampledWidth
&
sample1
=
samples
[
low
];
const
LaneInfo
::
SampledWidth
&
sample2
=
samples
[
high
];
const
LaneInfo
::
SampledWidth
&
sample1
=
samples
[
low
];
const
LaneInfo
::
SampledWidth
&
sample2
=
samples
[
high
];
const
double
ratio
=
(
sample2
.
first
-
s
)
/
(
sample2
.
first
-
sample1
.
first
);
return
sample1
.
second
*
ratio
+
sample2
.
second
*
(
1.0
-
ratio
);
}
...
...
@@ -194,7 +194,7 @@ void SignalInfo::init() {
}
CHECK
(
!
_segments
.
empty
());
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
.
end
());
}
...
...
@@ -240,7 +240,7 @@ void YieldSignInfo::init() {
OverlapInfo
::
OverlapInfo
(
const
apollo
::
hdmap
::
Overlap
&
overlap
)
:
_overlap
(
overlap
)
{}
const
ObjectOverlapInfo
*
OverlapInfo
::
get_object_overlap_info
(
const
ObjectOverlapInfo
*
OverlapInfo
::
get_object_overlap_info
(
const
apollo
::
hdmap
::
Id
&
id
)
const
{
for
(
const
auto
&
object
:
_overlap
.
object
())
{
if
(
object
.
id
().
id
()
==
id
.
id
())
{
...
...
modules/map/hdmap/hdmap_common_test.cc
浏览文件 @
997dd4d5
...
...
@@ -28,10 +28,8 @@ class HDMapCommonTestSuite : public ::testing::Test {
protected:
HDMapCommonTestSuite
()
{}
virtual
~
HDMapCommonTestSuite
()
{}
virtual
void
SetUp
()
{
}
virtual
void
TearDown
()
{
}
virtual
void
SetUp
()
{}
virtual
void
TearDown
()
{}
void
init_lane_obj
(
apollo
::
hdmap
::
Lane
*
lane
);
void
init_junction_obj
(
apollo
::
hdmap
::
Junction
*
junction
);
void
init_signal_obj
(
apollo
::
hdmap
::
Signal
*
signal
);
...
...
@@ -292,8 +290,8 @@ TEST_F(HDMapCommonTestSuite, lane_info) {
ASSERT_EQ
(
lane
.
central_curve
().
segment
(
0
).
line_segment
().
point_size
(),
lane_info
.
headings
().
size
());
for
(
std
::
size_t
i
=
0
;
i
<
lane_info
.
headings
().
size
();
++
i
)
{
ASSERT_NEAR
(
lane_info
.
unit_directions
()[
i
].
Angle
()
,
lane_info
.
headings
()[
i
],
1E-3
);
ASSERT_NEAR
(
lane_info
.
unit_directions
()[
i
].
Angle
(),
lane_info
.
headings
()[
i
]
,
1E-3
);
}
double
left_width
=
0.0
;
double
right_width
=
0.0
;
...
...
@@ -331,8 +329,7 @@ TEST_F(HDMapCommonTestSuite, junction_info) {
ASSERT_STREQ
(
junction
.
id
().
id
().
c_str
(),
junction_info
.
id
().
id
().
c_str
());
ASSERT_EQ
(
7
,
junction_info
.
polygon
().
points
().
size
());
for
(
std
::
size_t
i
=
0
;
i
<
5
;
++
i
)
{
ASSERT_NEAR
((
i
+
1
)
*
1.0
,
junction_info
.
polygon
().
points
()[
i
].
x
(),
1E-3
);
ASSERT_NEAR
((
i
+
1
)
*
1.0
,
junction_info
.
polygon
().
points
()[
i
].
x
(),
1E-3
);
}
ASSERT_NEAR
(
5.0
,
junction_info
.
polygon
().
points
()[
5
].
x
(),
1E-3
);
ASSERT_NEAR
(
2.0
,
junction_info
.
polygon
().
points
()[
5
].
y
(),
1E-3
);
...
...
@@ -390,8 +387,7 @@ TEST_F(HDMapCommonTestSuite, yield_sign_info) {
apollo
::
hdmap
::
YieldSign
yield_sign
;
init_yield_sign_obj
(
&
yield_sign
);
YieldSignInfo
yield_sign_info
(
yield_sign
);
ASSERT_STREQ
(
yield_sign
.
id
().
id
().
c_str
(),
yield_sign_info
.
id
().
id
().
c_str
());
ASSERT_STREQ
(
yield_sign
.
id
().
id
().
c_str
(),
yield_sign_info
.
id
().
id
().
c_str
());
ASSERT_EQ
(
yield_sign
.
stop_line
().
segment
(
0
).
line_segment
().
point_size
()
-
1
,
yield_sign_info
.
segments
().
size
());
for
(
std
::
size_t
i
=
0
;
i
<
yield_sign_info
.
segments
().
size
();
++
i
)
{
...
...
modules/planning/common/BUILD
浏览文件 @
997dd4d5
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
...
...
@@ -36,3 +38,5 @@ cc_library(
"//modules/localization/proto:localization_proto"
,
],
)
cpplint
()
modules/planning/common/logic_point.cc
浏览文件 @
997dd4d5
...
...
@@ -28,43 +28,27 @@
namespace
apollo
{
namespace
planning
{
LogicPoint
::
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
LogicPoint
::
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
)
:
::
apollo
::
common
::
math
::
Vec2d
(
x
,
y
),
_s
(
s
),
_heading
(
heading
),
_kappa
(
kappa
),
_dkappa
(
dkappa
),
_lane_id
(
""
)
{
}
_lane_id
(
""
)
{}
double
LogicPoint
::
s
()
const
{
return
_s
;
}
double
LogicPoint
::
s
()
const
{
return
_s
;
}
double
LogicPoint
::
heading
()
const
{
return
_heading
;
}
double
LogicPoint
::
heading
()
const
{
return
_heading
;
}
double
LogicPoint
::
kappa
()
const
{
return
_kappa
;
}
double
LogicPoint
::
kappa
()
const
{
return
_kappa
;
}
double
LogicPoint
::
dkappa
()
const
{
return
_dkappa
;
}
double
LogicPoint
::
dkappa
()
const
{
return
_dkappa
;
}
void
LogicPoint
::
set_lane_id
(
const
std
::
string
&
lane_id
)
{
_lane_id
=
lane_id
;
}
void
LogicPoint
::
set_lane_id
(
const
std
::
string
&
lane_id
)
{
_lane_id
=
lane_id
;
}
const
std
::
string
&
LogicPoint
::
lane_id
()
const
{
return
_lane_id
;
}
const
std
::
string
&
LogicPoint
::
lane_id
()
const
{
return
_lane_id
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/logic_point.h
浏览文件 @
997dd4d5
...
...
@@ -33,11 +33,8 @@ namespace planning {
class
LogicPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
public:
explicit
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
explicit
LogicPoint
(
const
double
x
,
const
double
y
,
const
double
s
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
);
double
s
()
const
;
double
heading
()
const
;
...
...
@@ -45,6 +42,7 @@ class LogicPoint : public ::apollo::common::math::Vec2d {
double
dkappa
()
const
;
const
std
::
string
&
lane_id
()
const
;
void
set_lane_id
(
const
std
::
string
&
lane_id
);
private:
double
_s
;
double
_heading
;
...
...
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,9 +29,9 @@ namespace apollo {
namespace
planning
{
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
();
}
}
}
SpeedData
::
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
)
{
...
...
@@ -50,23 +50,27 @@ void SpeedData::set_speed_vector(const std::vector<SpeedPoint>& 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
,
SpeedPoint
*
const
speed_point
)
const
{
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
())
{
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
)
{
if
(
Double
::
compare
(
speed_vector_
[
index
+
1
].
t
(),
speed_vector_
[
index
].
t
())
>
0
)
{
weight
=
(
t
-
speed_vector_
[
index
].
t
())
/
(
speed_vector_
[
index
].
t
()
-
speed_vector_
[
index
+
1
].
t
());
}
*
speed_point
=
SpeedPoint
::
interpolate
(
speed_vector_
[
index
],
speed_vector_
[
index
+
1
],
weight
);
*
speed_point
=
SpeedPoint
::
interpolate
(
speed_vector_
[
index
],
speed_vector_
[
index
+
1
],
weight
);
return
true
;
}
...
...
@@ -92,12 +96,13 @@ std::string SpeedData::DebugString() const {
}
std
::
size_t
SpeedData
::
find_index
(
const
double
t
)
const
{
auto
upper_bound
=
std
::
upper_bound
(
speed_vector_
.
begin
()
+
1
,
speed_vector_
.
end
(),
t
,
speed_time_comp
);
return
std
::
min
(
speed_vector_
.
size
()
-
1
,
static_cast
<
std
::
size_t
>
(
upper_bound
-
speed_vector_
.
begin
()))
-
1
;
auto
upper_bound
=
std
::
upper_bound
(
speed_vector_
.
begin
()
+
1
,
speed_vector_
.
end
(),
t
,
speed_time_comp
);
return
std
::
min
(
speed_vector_
.
size
()
-
1
,
static_cast
<
std
::
size_t
>
(
upper_bound
-
speed_vector_
.
begin
()))
-
1
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/speed/speed_data.h
浏览文件 @
997dd4d5
...
...
@@ -28,7 +28,7 @@ namespace apollo {
namespace
planning
{
class
SpeedData
{
public:
public:
SpeedData
()
=
default
;
SpeedData
(
std
::
vector
<
SpeedPoint
>
speed_points
);
...
...
@@ -41,10 +41,12 @@ public:
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
;
private:
private:
std
::
size_t
find_index
(
const
double
s
)
const
;
std
::
vector
<
SpeedPoint
>
speed_vector_
;
...
...
@@ -54,5 +56,3 @@ private:
}
// 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.
*
* 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.
*****************************************************************************/
/******************************************************************************
* 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
...
...
@@ -25,43 +25,28 @@
namespace
apollo
{
namespace
planning
{
SpeedPoint
::
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
)
:
STPoint
(
st_point
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
}
SpeedPoint
::
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
)
:
STPoint
(
st_point
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
}
SpeedPoint
::
SpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
const
double
j
)
:
STPoint
(
s
,
t
),
v_
(
v
),
a_
(
a
),
j_
(
j
)
{
}
SpeedPoint
::
SpeedPoint
(
const
double
s
,
const
double
t
,
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
)
{
v_
=
v
;
}
void
SpeedPoint
::
set_v
(
const
double
v
)
{
v_
=
v
;
}
void
SpeedPoint
::
set_a
(
const
double
a
)
{
a_
=
a
;
}
void
SpeedPoint
::
set_a
(
const
double
a
)
{
a_
=
a
;
}
void
SpeedPoint
::
set_j
(
const
double
j
)
{
j_
=
j
;
}
void
SpeedPoint
::
set_j
(
const
double
j
)
{
j_
=
j
;
}
double
SpeedPoint
::
v
()
const
{
return
v_
;
}
double
SpeedPoint
::
v
()
const
{
return
v_
;
}
double
SpeedPoint
::
a
()
const
{
return
a_
;
}
double
SpeedPoint
::
a
()
const
{
return
a_
;
}
double
SpeedPoint
::
j
()
const
{
return
j_
;
}
double
SpeedPoint
::
j
()
const
{
return
j_
;
}
SpeedPoint
SpeedPoint
::
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
SpeedPoint
SpeedPoint
::
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
const
double
weight
)
{
double
s
=
(
1
-
weight
)
*
left
.
s
()
+
weight
*
right
.
s
();
double
t
=
(
1
-
weight
)
*
left
.
t
()
+
weight
*
right
.
t
();
...
...
@@ -71,10 +56,7 @@ SpeedPoint SpeedPoint::interpolate(const SpeedPoint& left, const SpeedPoint& rig
return
SpeedPoint
(
s
,
t
,
v
,
a
,
j
);
}
std
::
string
SpeedPoint
::
DebugString
()
const
{
return
""
;
}
std
::
string
SpeedPoint
::
DebugString
()
const
{
return
""
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/common/speed/speed_point.h
浏览文件 @
997dd4d5
...
...
@@ -27,14 +27,12 @@ namespace apollo {
namespace
planning
{
class
SpeedPoint
:
public
STPoint
{
public:
public:
SpeedPoint
()
=
default
;
SpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
SpeedPoint
(
const
double
s
,
const
double
t
,
const
double
v
,
const
double
a
,
const
double
j
);
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
);
SpeedPoint
(
const
STPoint
&
st_point
,
const
double
v
,
const
double
a
,
const
double
j
);
void
set_v
(
const
double
v
);
void
set_a
(
const
double
a
);
void
set_j
(
const
double
j
);
...
...
@@ -46,7 +44,8 @@ public:
std
::
string
DebugString
()
const
;
static
SpeedPoint
interpolate
(
const
SpeedPoint
&
left
,
const
SpeedPoint
&
right
,
const
double
weight
);
private:
private:
double
v_
=
0.0
;
double
a_
=
0.0
;
double
j_
=
0.0
;
...
...
@@ -56,4 +55,3 @@ private:
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_SPEED_POINT_H
modules/planning/common/speed/st_point.cc
浏览文件 @
997dd4d5
...
...
@@ -26,24 +26,15 @@
namespace
apollo
{
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
{
return
y_
;
}
double
STPoint
::
s
()
const
{
return
y_
;
}
double
STPoint
::
t
()
const
{
return
x_
;
}
double
STPoint
::
t
()
const
{
return
x_
;
}
void
STPoint
::
set_s
(
const
double
s
)
{
return
set_y
(
s
);
}
void
STPoint
::
set_s
(
const
double
s
)
{
return
set_y
(
s
);
}
void
STPoint
::
set_t
(
const
double
t
)
{
return
set_x
(
t
);
}
void
STPoint
::
set_t
(
const
double
t
)
{
return
set_x
(
t
);
}
std
::
string
STPoint
::
DebugString
()
const
{
std
::
ostringstream
sout
;
...
...
@@ -55,4 +46,3 @@ std::string STPoint::DebugString() const {
}
// namespace planning
}
// namespace apollo
modules/planning/common/speed/st_point.h
浏览文件 @
997dd4d5
...
...
@@ -27,7 +27,7 @@ namespace apollo {
namespace
planning
{
class
STPoint
:
public
::
apollo
::
common
::
math
::
Vec2d
{
public:
public:
STPoint
()
=
default
;
STPoint
(
const
double
s
,
const
double
t
);
double
s
()
const
;
...
...
@@ -41,4 +41,3 @@ public:
}
// namespace apollo
#endif // MODULES_PLANNING_COMMON_SPEED_ST_POINT_H
modules/planning/math/spiral_curve/quintic_spiral_curve.cc
浏览文件 @
997dd4d5
...
...
@@ -33,7 +33,8 @@ namespace planning {
using
::
apollo
::
common
::
ErrorCode
;
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
)
{
// generate an order 5 spiral path with four parameters
}
...
...
@@ -191,7 +192,7 @@ bool QuinticSpiralCurve::calculate_path() {
}
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
);
if
(
n
<=
1
||
error
()
>
spiral_config
().
newton_raphson_tol
())
{
...
...
@@ -199,7 +200,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
}
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_y
(
start_point
().
y
());
...
...
@@ -247,7 +248,7 @@ ErrorCode QuinticSpiralCurve::get_path_vec(
ErrorCode
QuinticSpiralCurve
::
get_path_vec_with_s
(
const
std
::
vector
<
double
>&
vec_s
,
std
::
vector
<
PathPoint
>*
path_points
)
const
{
std
::
vector
<
common
::
PathPoint
>*
path_points
)
const
{
CHECK_NOTNULL
(
path_points
);
if
(
error
()
>
spiral_config
().
newton_raphson_tol
())
{
...
...
@@ -260,10 +261,10 @@ ErrorCode QuinticSpiralCurve::get_path_vec_with_s(
const
std
::
size_t
n
=
vec_s
.
size
();
PathPoint
ref_point
=
start_point
();
common
::
PathPoint
ref_point
=
start_point
();
path_points
->
resize
(
n
);
std
::
vector
<
PathPoint
>&
result
=
*
path_points
;
std
::
vector
<
common
::
PathPoint
>&
result
=
*
path_points
;
std
::
array
<
double
,
6
>
p_value
;
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
);
...
...
modules/planning/math/spiral_curve/spiral_curve.cc
浏览文件 @
997dd4d5
...
...
@@ -27,7 +27,7 @@ namespace planning {
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
)
:
p_params_
(
order
+
1
,
0.0
),
sg_
(
0.0
),
...
...
@@ -42,9 +42,11 @@ void SpiralCurve::set_spiral_config(const SpiralCurveConfig& spiral_config) {
}
// 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_
;
}
...
...
modules/planning/planner/em_planner.cc
浏览文件 @
997dd4d5
...
...
@@ -28,13 +28,10 @@ namespace planning {
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
vehicle_state
::
VehicleState
;
EMPlanner
::
EMPlanner
()
{
}
EMPlanner
::
EMPlanner
()
{}
bool
EMPlanner
::
Plan
(
const
TrajectoryPoint
&
start_point
,
bool
EMPlanner
::
Plan
(
const
TrajectoryPoint
&
start_point
,
std
::
vector
<
TrajectoryPoint
>*
discretized_trajectory
)
{
return
true
;
}
...
...
modules/planning/planner/em_planner.h
浏览文件 @
997dd4d5
...
...
@@ -52,8 +52,7 @@ class EMPlanner : public Planner {
* @param discretized_trajectory The computed trajectory
* @return true if planning succeeds; false otherwise.
*/
bool
Plan
(
const
apollo
::
common
::
TrajectoryPoint
&
start_point
,
bool
Plan
(
const
apollo
::
common
::
TrajectoryPoint
&
start_point
,
std
::
vector
<
apollo
::
common
::
TrajectoryPoint
>*
trajectory
)
override
;
private:
...
...
modules/planning/planning.cc
浏览文件 @
997dd4d5
...
...
@@ -13,6 +13,7 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/planning/planning.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
...
...
modules/planning/planning.h
浏览文件 @
997dd4d5
...
...
@@ -102,9 +102,7 @@ class Planning : public apollo::common::ApolloApp {
const
double
header_time
,
const
std
::
vector
<
common
::
TrajectoryPoint
>&
discretized_trajectory
);
apollo
::
common
::
util
::
Factory
<
PlanningConfig
::
PlannerType
,
Planner
>
apollo
::
common
::
util
::
Factory
<
PlanningConfig
::
PlannerType
,
Planner
>
planner_factory_
;
PlanningConfig
config_
;
...
...
modules/planning/planning_test.cc
浏览文件 @
997dd4d5
...
...
@@ -14,9 +14,9 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/planning.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planning.h"
#include "modules/planning/proto/planning.pb.h"
#include "gmock/gmock.h"
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录