Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e0e911ab
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,发现更多精彩内容 >>
提交
e0e911ab
编写于
12月 25, 2017
作者:
Z
Zhang Liangliang
提交者:
Qi Luo
12月 25, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: implemented poly_st_graph. Added init files for speed_profile_cost.h/cc
上级
a3c0cbf4
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
164 addition
and
13 deletion
+164
-13
modules/planning/tasks/poly_st_speed/BUILD
modules/planning/tasks/poly_st_speed/BUILD
+17
-0
modules/planning/tasks/poly_st_speed/poly_st_graph.cc
modules/planning/tasks/poly_st_speed/poly_st_graph.cc
+52
-7
modules/planning/tasks/poly_st_speed/poly_st_graph.h
modules/planning/tasks/poly_st_speed/poly_st_graph.h
+4
-5
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
...s/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
+2
-1
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
+39
-0
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
+50
-0
未找到文件。
modules/planning/tasks/poly_st_speed/BUILD
浏览文件 @
e0e911ab
...
@@ -2,6 +2,22 @@ load("//tools:cpplint.bzl", "cpplint")
...
@@ -2,6 +2,22 @@ load("//tools:cpplint.bzl", "cpplint")
package
(
default_visibility
=
[
"//visibility:public"
])
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"speed_profile_cost"
,
srcs
=
[
"speed_profile_cost.cc"
,
],
hdrs
=
[
"speed_profile_cost.h"
,
],
deps
=
[
"//modules/planning/proto:planning_config_proto"
,
"//modules/planning/proto:poly_st_speed_config_proto"
,
"//modules/planning/tasks:speed_optimizer"
,
"//modules/planning/tasks/st_graph:st_boundary_mapper"
,
],
)
cc_library
(
cc_library
(
name
=
"poly_st_graph"
,
name
=
"poly_st_graph"
,
srcs
=
[
srcs
=
[
...
@@ -11,6 +27,7 @@ cc_library(
...
@@ -11,6 +27,7 @@ cc_library(
"poly_st_graph.h"
,
"poly_st_graph.h"
,
],
],
deps
=
[
deps
=
[
":speed_profile_cost"
,
"//modules/common:log"
,
"//modules/common:log"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/configs:vehicle_config_helper"
,
"//modules/common/configs/proto:vehicle_config_proto"
,
"//modules/common/configs/proto:vehicle_config_proto"
,
...
...
modules/planning/tasks/poly_st_speed/poly_st_graph.cc
浏览文件 @
e0e911ab
...
@@ -32,6 +32,7 @@
...
@@ -32,6 +32,7 @@
#include "modules/common/util/util.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/tasks/poly_st_speed/speed_profile_cost.h"
namespace
apollo
{
namespace
apollo
{
namespace
planning
{
namespace
planning
{
...
@@ -40,10 +41,12 @@ using apollo::common::ErrorCode;
...
@@ -40,10 +41,12 @@ using apollo::common::ErrorCode;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
Status
;
PolyStGraph
::
PolyStGraph
(
const
PolyStSpeedConfig
&
config
,
PolyStGraph
::
PolyStGraph
(
const
PolyStSpeedConfig
&
config
,
const
ReferenceLineInfo
*
reference_line_info
)
const
ReferenceLineInfo
*
reference_line_info
,
const
SpeedLimit
&
speed_limit
)
:
config_
(
config
),
:
config_
(
config
),
reference_line_info_
(
reference_line_info
),
reference_line_info_
(
reference_line_info
),
reference_line_
(
reference_line_info
->
reference_line
())
{}
reference_line_
(
reference_line_info
->
reference_line
()),
speed_limit_
(
speed_limit
)
{}
bool
PolyStGraph
::
FindStTunnel
(
bool
PolyStGraph
::
FindStTunnel
(
const
common
::
TrajectoryPoint
&
init_point
,
const
common
::
TrajectoryPoint
&
init_point
,
...
@@ -57,15 +60,57 @@ bool PolyStGraph::FindStTunnel(
...
@@ -57,15 +60,57 @@ bool PolyStGraph::FindStTunnel(
AERROR
<<
"Fail to generate graph!"
;
AERROR
<<
"Fail to generate graph!"
;
return
false
;
return
false
;
}
}
// TODO(All): implement this function
std
::
vector
<
PolyStGraphNode
>
min_cost_speed_profile
;
if
(
!
GenerateMinCostSpeedProfile
(
obstacles
,
&
min_cost_speed_profile
))
{
AERROR
<<
"Fail to search min cost speed profile."
;
return
false
;
}
constexpr
double
delta_t
=
0.1
;
// output resolution, in seconds
const
auto
curve
=
min_cost_speed_profile
.
back
().
speed_profile
;
for
(
double
t
=
0.0
;
t
<
planning_time_
;
t
+=
delta_t
)
{
const
double
s
=
curve
.
Evaluate
(
0
,
t
);
const
double
v
=
curve
.
Evaluate
(
1
,
t
);
const
double
a
=
curve
.
Evaluate
(
2
,
t
);
const
double
da
=
curve
.
Evaluate
(
3
,
t
);
speed_data
->
AppendSpeedPoint
(
s
,
t
,
v
,
a
,
da
);
}
return
true
;
return
true
;
}
}
bool
PolyStGraph
::
GenerateMinCostSpeedProfile
(
bool
PolyStGraph
::
GenerateMinCostSpeedProfile
(
const
std
::
vector
<
const
PathObstacle
*>
&
obstacles
,
const
std
::
vector
<
const
PathObstacle
*>
&
obstacles
,
std
::
vector
<
PolyStGraphNode
>
*
min_cost_path
)
{
std
::
vector
<
PolyStGraphNode
>
*
min_cost_speed_profile
)
{
CHECK
(
min_cost_path
!=
nullptr
);
CHECK
(
min_cost_speed_profile
!=
nullptr
);
// TODO(All): implement this function
std
::
vector
<
std
::
vector
<
STPoint
>>
points
;
if
(
!
SampleStPoints
(
&
points
))
{
AERROR
<<
"Fail to sample st points."
;
return
false
;
}
PolyStGraphNode
start_node
=
{
STPoint
(
0.0
,
0.0
),
init_point_
.
v
(),
init_point_
.
a
()};
min_cost_speed_profile
->
resize
(
2
);
min_cost_speed_profile
->
front
()
=
start_node
;
SpeedProfileCost
cost
(
config_
,
obstacles
);
double
min_cost
=
std
::
numeric_limits
<
double
>::
max
();
for
(
const
auto
&
level
:
points
)
{
for
(
const
auto
&
st_point
:
level
)
{
const
double
speed_limit
=
speed_limit_
.
GetSpeedLimitByS
(
st_point
.
s
());
constexpr
int
num_speed
=
5
;
for
(
double
v
=
0
;
v
<
speed_limit
;
v
=
std
::
fmin
(
speed_limit
,
v
+
speed_limit
/
num_speed
))
{
PolyStGraphNode
node
=
{
st_point
,
v
,
0.0
};
node
.
speed_profile
=
QuinticPolynomialCurve1d
(
0.0
,
start_node
.
speed
,
start_node
.
accel
,
node
.
st_point
.
s
(),
node
.
speed
,
node
.
accel
,
node
.
st_point
.
t
());
const
double
c
=
cost
.
Calculate
(
node
.
speed_profile
);
if
(
c
<
min_cost
)
{
min_cost_speed_profile
->
back
()
=
node
;
min_cost
=
c
;
}
}
}
}
return
true
;
return
true
;
}
}
...
@@ -77,7 +122,7 @@ bool PolyStGraph::SampleStPoints(
...
@@ -77,7 +122,7 @@ bool PolyStGraph::SampleStPoints(
for
(
double
t
=
start_t
;
t
<
planning_time_
;
t
+=
unit_t_
)
{
for
(
double
t
=
start_t
;
t
<
planning_time_
;
t
+=
unit_t_
)
{
std
::
vector
<
STPoint
>
level_points
;
std
::
vector
<
STPoint
>
level_points
;
for
(
double
s
=
start_s
;
s
<
planning_distance_
;
s
+=
unit_s_
)
{
for
(
double
s
=
start_s
;
s
<
planning_distance_
;
s
+=
unit_s_
)
{
level_points
.
emplace_back
(
t
,
s
);
level_points
.
emplace_back
(
s
,
t
);
}
}
points
->
push_back
(
std
::
move
(
level_points
));
points
->
push_back
(
std
::
move
(
level_points
));
}
}
...
...
modules/planning/tasks/poly_st_speed/poly_st_graph.h
浏览文件 @
e0e911ab
...
@@ -35,6 +35,7 @@
...
@@ -35,6 +35,7 @@
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/path_obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/reference_line/reference_point.h"
#include "modules/planning/reference_line/reference_point.h"
...
@@ -45,7 +46,8 @@ namespace planning {
...
@@ -45,7 +46,8 @@ namespace planning {
class
PolyStGraph
{
class
PolyStGraph
{
public:
public:
explicit
PolyStGraph
(
const
PolyStSpeedConfig
&
config
,
explicit
PolyStGraph
(
const
PolyStSpeedConfig
&
config
,
const
ReferenceLineInfo
*
reference_line_info
);
const
ReferenceLineInfo
*
reference_line_info
,
const
SpeedLimit
&
speed_limit
);
~
PolyStGraph
()
=
default
;
~
PolyStGraph
()
=
default
;
...
@@ -54,9 +56,6 @@ class PolyStGraph {
...
@@ -54,9 +56,6 @@ class PolyStGraph {
SpeedData
*
const
speed_data
);
SpeedData
*
const
speed_data
);
private:
private:
/**
* an private inner struct for the poly st graph
*/
struct
PolyStGraphNode
{
struct
PolyStGraphNode
{
public:
public:
PolyStGraphNode
()
=
default
;
PolyStGraphNode
()
=
default
;
...
@@ -82,7 +81,7 @@ class PolyStGraph {
...
@@ -82,7 +81,7 @@ class PolyStGraph {
common
::
TrajectoryPoint
init_point_
;
common
::
TrajectoryPoint
init_point_
;
const
ReferenceLineInfo
*
reference_line_info_
=
nullptr
;
const
ReferenceLineInfo
*
reference_line_info_
=
nullptr
;
const
ReferenceLine
&
reference_line_
;
const
ReferenceLine
&
reference_line_
;
SpeedData
speed_data
_
;
const
SpeedLimit
&
speed_limit
_
;
double
unit_t_
=
1.0
;
double
unit_t_
=
1.0
;
double
unit_s_
=
5.0
;
double
unit_s_
=
5.0
;
...
...
modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
浏览文件 @
e0e911ab
...
@@ -110,7 +110,8 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
...
@@ -110,7 +110,8 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
// step 2 perform graph search
// step 2 perform graph search
// make a poly_st_graph and perform search here.
// make a poly_st_graph and perform search here.
PolyStGraph
poly_st_graph
(
poly_st_speed_config_
,
reference_line_info_
);
PolyStGraph
poly_st_graph
(
poly_st_speed_config_
,
reference_line_info_
,
speed_limits
);
auto
ret
=
poly_st_graph
.
FindStTunnel
(
auto
ret
=
poly_st_graph
.
FindStTunnel
(
init_point
,
init_point
,
reference_line_info_
->
path_decision
()
->
path_obstacles
().
Items
(),
reference_line_info_
->
path_decision
()
->
path_obstacles
().
Items
(),
...
...
modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
0 → 100644
浏览文件 @
e0e911ab
/******************************************************************************
* 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
**/
#include "modules/planning/tasks/poly_st_speed/speed_profile_cost.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
common
::
TrajectoryPoint
;
SpeedProfileCost
::
SpeedProfileCost
(
const
PolyStSpeedConfig
&
config
,
const
std
::
vector
<
const
PathObstacle
*>
&
obstacles
)
:
config_
(
config
),
obstacles_
(
obstacles
)
{}
double
SpeedProfileCost
::
Calculate
(
const
QuinticPolynomialCurve1d
&
curve
)
const
{
return
0.0
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/tasks/poly_st_speed/speed_profile_cost.h
0 → 100644
浏览文件 @
e0e911ab
/******************************************************************************
* 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 trajectory_cost.h
**/
#ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
#define MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
#include <vector>
#include "modules/planning/proto/poly_st_speed_config.pb.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
namespace
apollo
{
namespace
planning
{
class
SpeedProfileCost
{
public:
explicit
SpeedProfileCost
(
const
PolyStSpeedConfig
&
config
,
const
std
::
vector
<
const
PathObstacle
*>
&
obstacles
);
double
Calculate
(
const
QuinticPolynomialCurve1d
&
curve
)
const
;
private:
const
PolyStSpeedConfig
config_
;
const
std
::
vector
<
const
PathObstacle
*>
&
obstacles_
;
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录