Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f8a73cbd
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,发现更多精彩内容 >>
提交
f8a73cbd
编写于
7月 20, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
7月 20, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
moved path.h/cc to modules/map/pnc_map, changed namespace to apollo/hdmap, added reference line.
上级
daead0d7
变更
9
显示空白变更内容
内联
并排
Showing
9 changed file
with
322 addition
and
15 deletion
+322
-15
modules/map/pnc_map/BUILD
modules/map/pnc_map/BUILD
+0
-0
modules/map/pnc_map/path.cc
modules/map/pnc_map/path.cc
+3
-3
modules/map/pnc_map/path.h
modules/map/pnc_map/path.h
+2
-2
modules/planning/reference_line/.BUILD.swp
modules/planning/reference_line/.BUILD.swp
+0
-0
modules/planning/reference_line/BUILD
modules/planning/reference_line/BUILD
+7
-1
modules/planning/reference_line/reference_line.cc
modules/planning/reference_line/reference_line.cc
+226
-0
modules/planning/reference_line/reference_line.h
modules/planning/reference_line/reference_line.h
+75
-0
modules/planning/reference_line/reference_point.cc
modules/planning/reference_line/reference_point.cc
+5
-5
modules/planning/reference_line/reference_point.h
modules/planning/reference_line/reference_point.h
+4
-4
未找到文件。
modules/
common/path
/BUILD
→
modules/
map/pnc_map
/BUILD
浏览文件 @
f8a73cbd
文件已移动
modules/
common/path
/path.cc
→
modules/
map/pnc_map
/path.cc
浏览文件 @
f8a73cbd
...
...
@@ -14,7 +14,7 @@
* limitations under the License.
*****************************************************************************/
#include "modules/
common/path
/path.h"
#include "modules/
map/pnc_map
/path.h"
#include <cstdlib>
#include <limits>
...
...
@@ -27,7 +27,7 @@
#include "modules/common/math/vec2d.h"
namespace
apollo
{
namespace
common
{
namespace
hdmap
{
using
apollo
::
common
::
math
::
LineSegment2d
;
using
apollo
::
common
::
math
::
Polygon2d
;
...
...
@@ -934,5 +934,5 @@ bool PathApproximation::overlap_with(const Path& path, const Box2d& box,
return
false
;
}
}
// namespace
common
}
// namespace
hdmap
}
// namespace apollo
modules/
common/path
/path.h
→
modules/
map/pnc_map
/path.h
浏览文件 @
f8a73cbd
...
...
@@ -38,7 +38,7 @@
#include "modules/map/hdmap/hdmap_common.h"
namespace
apollo
{
namespace
common
{
namespace
hdmap
{
// class LaneInfo;
// class OverlapInfo;
...
...
@@ -322,7 +322,7 @@ class Path {
std
::
vector
<
PathOverlap
>
_speed_bump_overlaps
;
};
}
// namespace
common
}
// namespace
hdmap
}
// namespace apollo
#endif // MODULES_MAP_PNC_MAP_TRAJECTORY_H_
modules/planning/reference_line/.BUILD.swp
0 → 100644
浏览文件 @
f8a73cbd
文件已添加
modules/planning/reference_line/BUILD
浏览文件 @
f8a73cbd
...
...
@@ -4,12 +4,18 @@ cc_library(
name
=
"lib_reference_line"
,
srcs
=
[
"reference_point.cc"
,
"reference_line.cc"
,
],
hdrs
=
[
"reference_point.h"
,
"reference_line.h"
,
],
deps
=
[
"//modules/common/path:pnc_path"
,
"//modules/common:log"
,
"//modules/common/proto:path_point_proto"
,
"//modules/map/pnc_map:pnc_path"
,
"//modules/common/math:math"
,
"//modules/planning/math:double"
,
"//modules/planning/math:interpolation"
,
],
)
modules/planning/reference_line/reference_line.cc
0 → 100644
浏览文件 @
f8a73cbd
/******************************************************************************
* 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 reference_line.cc
**/
#include "modules/planning/reference_line/reference_line.h"
#include "boost/math/tools/minima.hpp"
#include "modules/common/log.h"
#include "modules/common/math/angle.h"
#include "modules/planning/math/double.h"
#include "modules/planning/math/interpolation.h"
namespace
apollo
{
namespace
planning
{
using
ReferenceMapLine
=
::
apollo
::
hdmap
::
Path
;
ReferenceLine
::
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
)
:
_reference_points
(
reference_points
)
{
std
::
vector
<::
apollo
::
hdmap
::
PathPoint
>
points
(
reference_points
.
begin
(),
reference_points
.
end
());
_reference_map_line
=
std
::
move
(
ReferenceMapLine
(
std
::
move
(
points
)));
}
ReferenceLine
::
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
const
std
::
vector
<::
apollo
::
hdmap
::
LaneSegment
>&
lane_segments
,
const
double
max_approximation_error
)
:
_reference_points
(
reference_points
)
{
std
::
vector
<::
apollo
::
hdmap
::
PathPoint
>
points
(
reference_points
.
begin
(),
reference_points
.
end
());
_reference_map_line
=
std
::
move
(
ReferenceMapLine
(
std
::
move
(
points
),
lane_segments
,
max_approximation_error
));
}
void
ReferenceLine
::
move
(
const
ReferenceLine
&
reference_line
)
{
_reference_map_line
=
ReferenceMapLine
(
reference_line
.
reference_map_line
().
path_points
());
_reference_points
=
reference_line
.
reference_points
();
}
void
ReferenceLine
::
move
(
const
ReferenceLine
&&
reference_line
)
{
_reference_map_line
=
ReferenceMapLine
(
reference_line
.
reference_map_line
().
path_points
());
_reference_points
=
reference_line
.
reference_points
();
}
ReferencePoint
ReferenceLine
::
get_reference_point
(
const
double
s
)
const
{
const
auto
&
accumulated_s
=
_reference_map_line
.
accumulated_s
();
if
(
s
<
accumulated_s
.
front
())
{
AWARN
<<
"The requested s is nearer than the start point of the reference "
"line; reference line starts at "
<<
accumulated_s
.
back
()
<<
", requested "
<<
s
<<
"."
;
return
_reference_points
.
front
();
}
if
(
s
>
accumulated_s
.
back
())
{
AWARN
<<
"The requested s exceeds the reference line; reference line ends at "
<<
accumulated_s
.
back
()
<<
"requested "
<<
s
<<
" ."
;
return
_reference_points
.
back
();
}
auto
it_lower
=
std
::
lower_bound
(
accumulated_s
.
begin
(),
accumulated_s
.
end
(),
s
);
if
(
it_lower
==
accumulated_s
.
begin
())
{
return
_reference_points
.
front
();
}
else
{
std
::
size_t
index
=
(
std
::
size_t
)(
it_lower
-
accumulated_s
.
begin
());
auto
p0
=
_reference_points
[
index
-
1
];
auto
p1
=
_reference_points
[
index
];
auto
s0
=
accumulated_s
[
index
-
1
];
auto
s1
=
accumulated_s
[
index
];
return
ReferenceLine
::
interpolate
(
p0
,
s0
,
p1
,
s1
,
s
);
}
}
double
ReferenceLine
::
find_min_distance_point
(
const
ReferencePoint
&
p0
,
const
double
s0
,
const
ReferencePoint
&
p1
,
const
double
s1
,
const
double
x
,
const
double
y
)
{
auto
func_dist_square
=
[
&
p0
,
&
p1
,
&
s0
,
&
s1
,
&
x
,
&
y
](
const
double
s
)
{
auto
p
=
ReferenceLine
::
interpolate
(
p0
,
s0
,
p1
,
s1
,
s
);
double
dx
=
p
.
x
()
-
x
;
double
dy
=
p
.
y
()
-
y
;
return
dx
*
dx
+
dy
*
dy
;
};
return
::
boost
::
math
::
tools
::
brent_find_minima
(
func_dist_square
,
s0
,
s1
,
8
)
.
first
;
}
ReferencePoint
ReferenceLine
::
get_reference_point
(
const
double
x
,
const
double
y
)
const
{
CHECK
(
_reference_points
.
size
()
>
0
);
auto
func_distance_square
=
[](
const
ReferencePoint
&
point
,
const
double
x
,
const
double
y
)
{
double
dx
=
point
.
x
()
-
x
;
double
dy
=
point
.
y
()
-
y
;
return
dx
*
dx
+
dy
*
dy
;
};
double
d_min
=
func_distance_square
(
_reference_points
.
front
(),
x
,
y
);
double
index_min
=
0
;
for
(
size_t
i
=
1
;
i
<
_reference_points
.
size
();
++
i
)
{
double
d_temp
=
func_distance_square
(
_reference_points
[
i
],
x
,
y
);
if
(
d_temp
<
d_min
)
{
d_min
=
d_temp
;
index_min
=
i
;
}
}
size_t
index_start
=
index_min
==
0
?
index_min
:
index_min
-
1
;
size_t
index_end
=
index_min
+
1
==
_reference_points
.
size
()
?
index_min
:
index_min
+
1
;
if
(
index_start
==
index_end
)
return
_reference_points
[
index_start
];
double
s0
=
_reference_map_line
.
accumulated_s
()[
index_start
];
double
s1
=
_reference_map_line
.
accumulated_s
()[
index_end
];
double
s
=
ReferenceLine
::
find_min_distance_point
(
_reference_points
[
index_start
],
s0
,
_reference_points
[
index_end
],
s1
,
x
,
y
);
return
ReferenceLine
::
interpolate
(
_reference_points
[
index_start
],
s0
,
_reference_points
[
index_end
],
s1
,
s
);
}
bool
ReferenceLine
::
get_point_in_Cartesian_frame
(
const
apollo
::
common
::
SLPoint
&
sl_point
,
Eigen
::
Vector2d
*
const
xy_point
)
const
{
CHECK_NOTNULL
(
xy_point
);
if
(
_reference_map_line
.
num_points
()
<
2
)
{
AERROR
<<
"The reference line has too few points."
;
return
false
;
}
const
auto
matched_point
=
get_reference_point
(
sl_point
.
s
());
const
auto
angle
=
::
apollo
::
common
::
math
::
Angle16
::
from_rad
(
matched_point
.
heading
());
(
*
xy_point
)[
0
]
=
(
matched_point
.
x
()
-
::
apollo
::
common
::
math
::
sin
(
angle
)
*
sl_point
.
l
());
(
*
xy_point
)[
1
]
=
(
matched_point
.
y
()
+
::
apollo
::
common
::
math
::
cos
(
angle
)
*
sl_point
.
l
());
return
true
;
}
bool
ReferenceLine
::
get_point_in_Frenet_frame
(
const
Eigen
::
Vector2d
&
xy_point
,
apollo
::
common
::
SLPoint
*
const
sl_point
)
const
{
CHECK_NOTNULL
(
sl_point
);
double
s
=
0
;
double
l
=
0
;
if
(
!
_reference_map_line
.
get_nearest_point
({
xy_point
.
x
(),
xy_point
.
y
()},
&
s
,
&
l
))
{
AERROR
<<
"Can't get nearest point from path."
;
return
false
;
}
if
(
s
>
_reference_map_line
.
accumulated_s
().
back
())
{
AERROR
<<
"The s of point is bigger than the length of current path."
;
return
false
;
}
sl_point
->
set_s
(
s
);
sl_point
->
set_l
(
l
);
return
true
;
}
ReferencePoint
ReferenceLine
::
interpolate
(
const
ReferencePoint
&
p0
,
const
double
s0
,
const
ReferencePoint
&
p1
,
const
double
s1
,
const
double
s
)
{
ReferencePoint
p
=
p1
;
p
.
set_x
(
Interpolation
::
lerp
(
p0
.
x
(),
s0
,
p1
.
x
(),
s1
,
s
));
p
.
set_y
(
Interpolation
::
lerp
(
p0
.
y
(),
s0
,
p1
.
y
(),
s1
,
s
));
p
.
set_heading
(
Interpolation
::
slerp
(
p0
.
heading
(),
s0
,
p1
.
heading
(),
s1
,
s
));
p
.
set_kappa
(
Interpolation
::
lerp
(
p0
.
kappa
(),
s0
,
p1
.
kappa
(),
s1
,
s
));
p
.
set_dkappa
(
Interpolation
::
lerp
(
p0
.
dkappa
(),
s0
,
p1
.
dkappa
(),
s1
,
s
));
// lane boundary info, lane info will be the same as the p1.
return
p
;
}
const
std
::
vector
<
ReferencePoint
>&
ReferenceLine
::
reference_points
()
const
{
return
_reference_points
;
}
const
ReferenceMapLine
&
ReferenceLine
::
reference_map_line
()
const
{
return
_reference_map_line
;
}
double
ReferenceLine
::
get_lane_width
(
const
double
s
)
const
{
// TODO : need implement;
return
4.0
;
}
bool
ReferenceLine
::
is_on_road
(
const
apollo
::
common
::
SLPoint
&
sl_point
)
const
{
// TODO : need implement;
return
true
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/reference_line/reference_line.h
0 → 100644
浏览文件 @
f8a73cbd
/******************************************************************************
* 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 reference_line.h
**/
#ifndef MODULES_PLANNING_MATH_REFERENCE_LINE_H_
#define MODULES_PLANNING_MATH_REFERENCE_LINE_H_
#include "Eigen/Dense"
#include "modules/common/proto/path_point.pb.h"
#include "modules/map/pnc_map/path.h"
#include "modules/planning/reference_line/reference_point.h"
namespace
apollo
{
namespace
planning
{
class
ReferenceLine
{
public:
ReferenceLine
()
=
default
;
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
);
ReferenceLine
(
const
std
::
vector
<
ReferencePoint
>&
reference_points
,
const
std
::
vector
<::
apollo
::
hdmap
::
LaneSegment
>&
lane_segments
,
const
double
max_approximation_error
);
void
move
(
const
ReferenceLine
&
reference_line
);
void
move
(
const
ReferenceLine
&&
reference_line
);
const
::
apollo
::
hdmap
::
Path
&
reference_map_line
()
const
;
const
std
::
vector
<
ReferencePoint
>&
reference_points
()
const
;
ReferencePoint
get_reference_point
(
const
double
s
)
const
;
ReferencePoint
get_reference_point
(
const
double
x
,
const
double
y
)
const
;
bool
get_point_in_Cartesian_frame
(
const
apollo
::
common
::
SLPoint
&
sl_point
,
Eigen
::
Vector2d
*
const
xy_point
)
const
;
bool
get_point_in_Frenet_frame
(
const
Eigen
::
Vector2d
&
xy_point
,
apollo
::
common
::
SLPoint
*
const
sl_point
)
const
;
double
get_lane_width
(
const
double
s
)
const
;
bool
is_on_road
(
const
apollo
::
common
::
SLPoint
&
sl_point
)
const
;
private:
static
ReferencePoint
interpolate
(
const
ReferencePoint
&
p0
,
const
double
s0
,
const
ReferencePoint
&
p1
,
const
double
s1
,
const
double
s
);
static
double
find_min_distance_point
(
const
ReferencePoint
&
p0
,
const
double
s0
,
const
ReferencePoint
&
p1
,
const
double
s1
,
const
double
x
,
const
double
y
);
std
::
vector
<
ReferencePoint
>
_reference_points
;
::
apollo
::
hdmap
::
Path
_reference_map_line
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_MATH_REFERENCE_LINE_H_
modules/planning/reference_line/reference_point.cc
浏览文件 @
f8a73cbd
...
...
@@ -27,7 +27,7 @@ ReferencePoint::ReferencePoint(const ::apollo::common::math::Vec2d& point,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
,
const
double
lower_bound
,
const
double
upper_bound
)
:
::
apollo
::
common
::
PathPoint
(
point
,
heading
),
:
::
apollo
::
hdmap
::
PathPoint
(
point
,
heading
),
_kappa
(
kappa
),
_dkappa
(
dkappa
),
_lower_bound
(
lower_bound
),
...
...
@@ -35,14 +35,14 @@ ReferencePoint::ReferencePoint(const ::apollo::common::math::Vec2d& point,
ReferencePoint
::
ReferencePoint
(
const
apollo
::
common
::
math
::
Vec2d
&
point
,
const
double
heading
,
const
::
apollo
::
common
::
LaneWaypoint
lane_waypoint
)
:
::
apollo
::
common
::
PathPoint
(
point
,
heading
,
lane_waypoint
)
{}
const
::
apollo
::
hdmap
::
LaneWaypoint
lane_waypoint
)
:
::
apollo
::
hdmap
::
PathPoint
(
point
,
heading
,
lane_waypoint
)
{}
ReferencePoint
::
ReferencePoint
(
const
apollo
::
common
::
math
::
Vec2d
&
point
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
,
const
::
apollo
::
common
::
LaneWaypoint
lane_waypoint
)
:
::
apollo
::
common
::
PathPoint
(
point
,
heading
,
lane_waypoint
),
const
::
apollo
::
hdmap
::
LaneWaypoint
lane_waypoint
)
:
::
apollo
::
hdmap
::
PathPoint
(
point
,
heading
,
lane_waypoint
),
_kappa
(
kappa
),
_dkappa
(
dkappa
)
{}
...
...
modules/planning/reference_line/reference_point.h
浏览文件 @
f8a73cbd
...
...
@@ -21,12 +21,12 @@
#ifndef MODULES_PLANNING_REFERENCE_LINE_REFERENCE_POINT_H_
#define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_POINT_H_
#include "modules/
common/path
/path.h"
#include "modules/
map/pnc_map
/path.h"
namespace
apollo
{
namespace
planning
{
class
ReferencePoint
:
public
::
apollo
::
common
::
PathPoint
{
class
ReferencePoint
:
public
::
apollo
::
hdmap
::
PathPoint
{
public:
ReferencePoint
()
=
default
;
...
...
@@ -35,11 +35,11 @@ class ReferencePoint : public ::apollo::common::PathPoint {
const
double
lower_bound
,
const
double
upper_bound
);
ReferencePoint
(
const
apollo
::
common
::
math
::
Vec2d
&
point
,
const
double
heading
,
const
::
apollo
::
common
::
LaneWaypoint
lane_waypoint
);
const
::
apollo
::
hdmap
::
LaneWaypoint
lane_waypoint
);
ReferencePoint
(
const
apollo
::
common
::
math
::
Vec2d
&
point
,
const
double
heading
,
const
double
kappa
,
const
double
dkappa
,
const
::
apollo
::
common
::
LaneWaypoint
lane_waypoint
);
const
::
apollo
::
hdmap
::
LaneWaypoint
lane_waypoint
);
void
set_kappa
(
const
double
kappa
);
void
set_dkappa
(
const
double
dkappa
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录