Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
7c67350d
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,发现更多精彩内容 >>
提交
7c67350d
编写于
7月 26, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
7月 26, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
removed veh_param in st_boundary_mapper.
上级
84c3af60
变更
9
隐藏空白更改
内联
并排
Showing
9 changed file
with
21 addition
and
34 deletion
+21
-34
modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc
...s/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc
+2
-5
modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.h
...es/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.h
+1
-2
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
...s/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
+1
-3
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc
...imizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc
+2
-2
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.h
...timizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.h
+1
-3
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
...imizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
+4
-3
modules/planning/optimizer/st_graph/BUILD
modules/planning/optimizer/st_graph/BUILD
+1
-0
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
+2
-9
modules/planning/optimizer/st_graph/st_boundary_mapper.h
modules/planning/optimizer/st_graph/st_boundary_mapper.h
+7
-7
未找到文件。
modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc
浏览文件 @
7c67350d
...
...
@@ -37,9 +37,8 @@ using apollo::common::PathPoint;
using
apollo
::
common
::
Status
;
DpStBoundaryMapper
::
DpStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
,
const
::
apollo
::
common
::
config
::
VehicleParam
&
veh_param
)
:
StBoundaryMapper
(
st_boundary_config
,
veh_param
)
{}
const
StBoundaryConfig
&
st_boundary_config
)
:
StBoundaryMapper
(
st_boundary_config
)
{}
Status
DpStBoundaryMapper
::
get_graph_boundary
(
const
common
::
TrajectoryPoint
&
initial_planning_point
,
...
...
@@ -131,7 +130,6 @@ Status DpStBoundaryMapper::map_obstacle_with_trajectory(
std
::
uint32_t
high_index
=
veh_path
.
size
()
-
1
;
bool
find_low
=
false
;
bool
find_high
=
false
;
while
(
high_index
>=
low_index
)
{
if
(
find_high
&&
find_low
)
{
break
;
...
...
@@ -202,7 +200,6 @@ Status DpStBoundaryMapper::map_obstacle_without_trajectory(
std
::
uint32_t
high_index
=
veh_path
.
size
()
-
1
;
bool
find_low
=
false
;
bool
find_high
=
false
;
while
(
high_index
>=
low_index
)
{
if
(
find_high
&&
find_low
)
{
break
;
...
...
modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.h
浏览文件 @
7c67350d
...
...
@@ -32,8 +32,7 @@ namespace planning {
class
DpStBoundaryMapper
:
public
StBoundaryMapper
{
public:
DpStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
,
const
::
apollo
::
common
::
config
::
VehicleParam
&
veh_param
);
DpStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
);
// TODO: combine two interfaces together to provide a st graph data type
apollo
::
common
::
Status
get_graph_boundary
(
...
...
modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc
浏览文件 @
7c67350d
...
...
@@ -62,10 +62,8 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
const
ReferenceLine
&
reference_line
,
DecisionData
*
const
decision_data
,
SpeedData
*
const
speed_data
)
{
const
auto
&
veh_param
=
VehicleConfigHelper
::
GetConfig
().
vehicle_param
();
// step 1 get boundaries
DpStBoundaryMapper
st_mapper
(
st_boundary_config_
,
veh_param
);
DpStBoundaryMapper
st_mapper
(
st_boundary_config_
);
double
planning_distance
=
std
::
min
(
dp_st_speed_config_
.
total_path_length
(),
path_data
.
path
().
param_length
());
std
::
vector
<
StGraphBoundary
>
boundaries
;
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc
浏览文件 @
7c67350d
...
...
@@ -44,8 +44,8 @@ using Box2d = apollo::common::math::Box2d;
using
Vec2d
=
apollo
::
common
::
math
::
Vec2d
;
QpSplineStBoundaryMapper
::
QpSplineStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
,
const
VehicleParam
&
veh_param
)
:
StBoundaryMapper
(
st_boundary_config
,
veh_param
)
{}
const
StBoundaryConfig
&
st_boundary_config
)
:
StBoundaryMapper
(
st_boundary_config
)
{}
Status
QpSplineStBoundaryMapper
::
get_graph_boundary
(
const
common
::
TrajectoryPoint
&
initial_planning_point
,
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.h
浏览文件 @
7c67350d
...
...
@@ -35,9 +35,7 @@ namespace planning {
class
QpSplineStBoundaryMapper
:
public
StBoundaryMapper
{
public:
QpSplineStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
,
const
apollo
::
common
::
config
::
VehicleParam
&
veh_param
);
QpSplineStBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
);
apollo
::
common
::
Status
get_graph_boundary
(
const
common
::
TrajectoryPoint
&
initial_planning_point
,
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc
浏览文件 @
7c67350d
...
...
@@ -67,13 +67,12 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
AERROR
<<
"Please call Init() before Process."
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"Not init."
);
}
double
total_length
=
std
::
min
(
qp_spline_st_speed_config_
.
total_path_length
(),
path_data
.
path
().
param_length
());
// step 1 get boundaries
const
auto
&
veh_param
=
common
::
config
::
VehicleConfigHelper
::
GetConfig
().
vehicle_param
();
QpSplineStBoundaryMapper
st_mapper
(
st_boundary_config_
,
veh_param
);
QpSplineStBoundaryMapper
st_mapper
(
st_boundary_config_
);
std
::
vector
<
StGraphBoundary
>
boundaries
;
if
(
st_mapper
.
get_graph_boundary
(
init_point
,
*
decision_data
,
path_data
,
reference_line
,
...
...
@@ -96,6 +95,8 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
}
// step 2 perform graph search
const
auto
&
veh_param
=
common
::
config
::
VehicleConfigHelper
::
GetConfig
().
vehicle_param
();
QpSplineStGraph
st_graph
(
qp_spline_st_speed_config_
,
veh_param
);
StGraphData
st_graph_data
(
boundaries
,
init_point
,
speed_limits
,
...
...
modules/planning/optimizer/st_graph/BUILD
浏览文件 @
7c67350d
...
...
@@ -57,6 +57,7 @@ cc_library(
],
deps
=
[
":st_graph_boundary"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/configs/proto:vehicle_config_proto"
,
"//modules/common/proto:path_point_proto"
,
"//modules/common/status"
,
...
...
modules/planning/optimizer/st_graph/st_boundary_mapper.cc
浏览文件 @
7c67350d
...
...
@@ -38,10 +38,8 @@ using apollo::common::Status;
using
apollo
::
hdmap
::
HDMap
;
using
apollo
::
localization
::
Pose
;
StBoundaryMapper
::
StBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
,
const
apollo
::
common
::
config
::
VehicleParam
&
veh_param
)
:
_st_boundary_config
(
st_boundary_config
),
_veh_param
(
veh_param
)
{}
StBoundaryMapper
::
StBoundaryMapper
(
const
StBoundaryConfig
&
st_boundary_config
)
:
_st_boundary_config
(
st_boundary_config
)
{}
double
StBoundaryMapper
::
get_area
(
const
std
::
vector
<
STPoint
>&
boundary_points
)
const
{
...
...
@@ -174,10 +172,5 @@ const StBoundaryConfig& StBoundaryMapper::st_boundary_config() const {
return
_st_boundary_config
;
}
const
apollo
::
common
::
config
::
VehicleParam
&
StBoundaryMapper
::
vehicle_param
()
const
{
return
_veh_param
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/optimizer/st_graph/st_boundary_mapper.h
浏览文件 @
7c67350d
...
...
@@ -23,12 +23,12 @@
#include <vector>
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/proto/path_point.pb.h"
#include "modules/common/status/status.h"
#include "modules/localization/proto/pose.pb.h"
#include "modules/planning/proto/st_boundary_config.pb.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/planning/common/decision_data.h"
#include "modules/planning/common/path/path_data.h"
...
...
@@ -42,8 +42,8 @@ namespace planning {
class
StBoundaryMapper
{
public:
StBoundaryMapper
(
const
apollo
::
planning
::
StBoundaryConfig
&
st_boundary_config
,
const
apollo
::
common
::
config
::
VehicleParam
&
veh_param
);
StBoundaryMapper
(
const
apollo
::
planning
::
StBoundaryConfig
&
st_boundary_config
);
virtual
~
StBoundaryMapper
()
=
default
;
...
...
@@ -60,12 +60,13 @@ class StBoundaryMapper {
const
std
::
uint32_t
matrix_dimension_s
,
const
double
default_speed_limit
,
SpeedLimit
*
const
speed_limit_data
);
protected:
const
apollo
::
planning
::
StBoundaryConfig
&
st_boundary_config
()
const
;
const
apollo
::
common
::
config
::
VehicleParam
&
vehicle_param
()
const
;
const
apollo
::
common
::
config
::
VehicleParam
&
vehicle_param
()
const
{
return
common
::
config
::
VehicleConfigHelper
::
GetConfig
().
vehicle_param
();
}
protected:
double
get_area
(
const
std
::
vector
<
STPoint
>&
boundary_points
)
const
;
bool
check_overlap
(
const
apollo
::
common
::
PathPoint
&
path_point
,
const
apollo
::
common
::
config
::
VehicleParam
&
params
,
const
apollo
::
common
::
math
::
Box2d
&
obs_box
,
...
...
@@ -73,7 +74,6 @@ class StBoundaryMapper {
private:
apollo
::
planning
::
StBoundaryConfig
_st_boundary_config
;
apollo
::
common
::
config
::
VehicleParam
_veh_param
;
};
}
// namespace planning
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录