Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
e52f6f6d
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,发现更多精彩内容 >>
提交
e52f6f6d
编写于
9月 02, 2017
作者:
D
Dong Li
提交者:
Jiangtao Hu
9月 02, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: remove move fence behavior
上级
90760ec6
变更
14
展开全部
隐藏空白更改
内联
并排
Showing
14 changed file
with
55464 addition
and
1292 deletion
+55464
-1292
modules/common/data/global_flagfile.txt
modules/common/data/global_flagfile.txt
+2
-0
modules/common/math/qp_solver/active_set_qp_solver.cc
modules/common/math/qp_solver/active_set_qp_solver.cc
+3
-3
modules/planning/common/frame.cc
modules/planning/common/frame.cc
+77
-0
modules/planning/common/frame.h
modules/planning/common/frame.h
+5
-0
modules/planning/integration_tests/planning_test_base.cc
modules/planning/integration_tests/planning_test_base.cc
+1
-1
modules/planning/tasks/path_decider/path_decider.cc
modules/planning/tasks/path_decider/path_decider.cc
+2
-20
modules/planning/tasks/traffic_decider/traffic_decider.cc
modules/planning/tasks/traffic_decider/traffic_decider.cc
+0
-116
modules/planning/tasks/traffic_decider/traffic_decider.h
modules/planning/tasks/traffic_decider/traffic_decider.h
+0
-9
modules/planning/testdata/garage_test/result_follow_0.pb.txt
modules/planning/testdata/garage_test/result_follow_0.pb.txt
+17951
-0
modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
...s/planning/testdata/garage_test/result_stop_dest_0.pb.txt
+1133
-1140
modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
...anning/testdata/garage_test/result_stop_obstacle_0.pb.txt
+8
-0
modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
...nning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+18237
-0
modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
...g/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
+18036
-0
modules/tools/plot_trace/plot_planning_result.py
modules/tools/plot_trace/plot_planning_result.py
+9
-3
未找到文件。
modules/common/data/global_flagfile.txt
浏览文件 @
e52f6f6d
...
...
@@ -8,3 +8,5 @@
--vehicle_config_path=modules/common/data/mkz_config.pb.txt
--map_dir=modules/map/data/sunnyvale_loop
--map_dir=modules/map/data/sunnyvale_loop
modules/common/math/qp_solver/active_set_qp_solver.cc
浏览文件 @
e52f6f6d
...
...
@@ -115,11 +115,11 @@ bool ActiveSetQpSolver::Solve() {
lower_bound
,
upper_bound
,
constraint_lower_bound
,
constraint_upper_bound
,
max_iter
);
if
(
ret
!=
qpOASES
::
SUCCESSFUL_RETURN
)
{
AERROR
<<
"Fail to initialze qp problem"
;
AERROR
<<
"Fail to initial
i
ze qp problem"
;
if
(
ret
==
qpOASES
::
RET_MAX_NWSR_REACHED
)
{
AERROR
<<
"Qp solver failed due to reach max iteration"
;
AERROR
<<
"Qp solver failed due to reach
ed
max iteration"
;
}
else
{
AERROR
<<
"Qp
is failed due to infeasibility or other internal reason
"
;
AERROR
<<
"Qp
failed due to infeasibility or other internal reasons
"
;
}
return
false
;
}
...
...
modules/planning/common/frame.cc
浏览文件 @
e52f6f6d
...
...
@@ -20,6 +20,7 @@
#include "modules/planning/common/frame.h"
#include <cmath>
#include <functional>
#include <list>
#include <string>
#include <utility>
...
...
@@ -31,6 +32,7 @@
#include "modules/common/log.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/reference_line/reference_line_smoother.h"
...
...
@@ -112,6 +114,75 @@ bool Frame::InitReferenceLineInfo(
return
true
;
}
const
Obstacle
*
Frame
::
AddStaticVirtualObstacle
(
const
std
::
string
&
id
,
const
common
::
math
::
Box2d
&
box
)
{
const
auto
*
object
=
obstacles_
.
Find
(
id
);
if
(
object
)
{
AWARN
<<
"obstacle "
<<
id
<<
" already exist."
;
return
object
;
}
// create a "virtual" perception_obstacle
perception
::
PerceptionObstacle
perception_obstacle
;
// simulator needs a valid integer
perception_obstacle
.
set_id
(
-
(
std
::
hash
<
std
::
string
>
{}(
id
)
>>
1
));
perception_obstacle
.
mutable_position
()
->
set_x
(
box
.
center
().
x
());
perception_obstacle
.
mutable_position
()
->
set_y
(
box
.
center
().
y
());
perception_obstacle
.
set_theta
(
box
.
heading
());
perception_obstacle
.
mutable_velocity
()
->
set_x
(
0
);
perception_obstacle
.
mutable_velocity
()
->
set_y
(
0
);
perception_obstacle
.
set_length
(
box
.
length
());
perception_obstacle
.
set_width
(
box
.
width
());
perception_obstacle
.
set_height
(
FLAGS_virtual_stop_wall_height
);
perception_obstacle
.
set_type
(
perception
::
PerceptionObstacle
::
UNKNOWN_UNMOVABLE
);
perception_obstacle
.
set_tracking_time
(
1.0
);
std
::
vector
<
common
::
math
::
Vec2d
>
corner_points
;
box
.
GetAllCorners
(
&
corner_points
);
for
(
const
auto
&
corner_point
:
corner_points
)
{
auto
*
point
=
perception_obstacle
.
add_polygon_point
();
point
->
set_x
(
corner_point
.
x
());
point
->
set_y
(
corner_point
.
y
());
}
auto
ptr
=
std
::
unique_ptr
<
Obstacle
>
(
new
Obstacle
(
id
,
perception_obstacle
));
auto
*
obstacle_ptr
=
ptr
.
get
();
obstacles_
.
Add
(
id
,
std
::
move
(
ptr
));
return
obstacle_ptr
;
}
const
Obstacle
*
Frame
::
CreateDestinationObstacle
()
{
if
(
!
routing_response_
.
routing_request
().
has_end
())
{
ADEBUG
<<
"routing_request has no end"
;
return
nullptr
;
}
const
auto
&
routing_end
=
routing_response_
.
routing_request
().
end
();
common
::
PointENU
dest_point
=
common
::
util
::
MakePointENU
(
routing_end
.
pose
().
x
(),
routing_end
.
pose
().
y
(),
0.0
);
const
auto
lane
=
pnc_map_
->
HDMap
().
GetLaneById
(
hdmap
::
MakeMapId
(
routing_end
.
id
()));
if
(
!
lane
)
{
AERROR
<<
"Failed to find lane for destination : "
<<
routing_end
.
DebugString
();
return
nullptr
;
}
double
dest_s
=
0.0
;
double
dest_l
=
0.0
;
if
(
!
lane
->
GetProjection
({
dest_point
.
x
(),
dest_point
.
y
()},
&
dest_s
,
&
dest_l
))
{
AERROR
<<
"Failed to get projection for "
<<
dest_point
.
DebugString
()
<<
" on lane "
<<
lane
->
id
().
id
();
return
nullptr
;
}
// check if destination point is in planning range
common
::
math
::
Box2d
destination_box
{{
dest_point
.
x
(),
dest_point
.
y
()},
lane
->
Heading
(
dest_s
),
FLAGS_virtual_stop_wall_length
,
FLAGS_virtual_stop_wall_width
};
return
AddStaticVirtualObstacle
(
FLAGS_destination_obstacle_id
,
destination_box
);
}
Status
Frame
::
Init
(
const
PlanningConfig
&
config
,
const
double
current_time_stamp
)
{
if
(
!
pnc_map_
)
{
...
...
@@ -141,6 +212,12 @@ Status Frame::Init(const PlanningConfig &config,
if
(
FLAGS_enable_prediction
)
{
CreatePredictionObstacles
(
prediction_
);
}
if
(
!
CreateDestinationObstacle
())
{
AERROR
<<
"Failed to create the destination obstacle"
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"failed to find destination"
);
}
if
(
CheckCollision
())
{
AERROR
<<
"Found collision with obstacle: "
<<
collision_obstacle_id_
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
...
...
modules/planning/common/frame.h
浏览文件 @
e52f6f6d
...
...
@@ -114,6 +114,11 @@ class Frame {
*/
bool
CheckCollision
();
const
Obstacle
*
AddStaticVirtualObstacle
(
const
std
::
string
&
id
,
const
common
::
math
::
Box2d
&
box
);
const
Obstacle
*
CreateDestinationObstacle
();
private:
common
::
TrajectoryPoint
planning_start_point_
;
...
...
modules/planning/integration_tests/planning_test_base.cc
浏览文件 @
e52f6f6d
...
...
@@ -145,7 +145,7 @@ bool PlanningTestBase::RunPlanning(const std::string& test_case_name,
AERROR
<<
"Failed to write to file "
<<
tmp_fname
;
}
AERROR
<<
"found
\n
diff "
<<
tmp_fname
<<
" "
<<
full_golden_path
;
AERROR
<<
"visualize diff
\n
python "
AERROR
<<
"visualize diff
\n
python "
"modules/tools/plot_trace/plot_planning_result.py "
<<
tmp_fname
<<
" "
<<
full_golden_path
;
return
false
;
...
...
modules/planning/tasks/path_decider/path_decider.cc
浏览文件 @
e52f6f6d
...
...
@@ -175,27 +175,9 @@ bool PathDecider::MakeStaticObstacleDecision(
// STOP: and break
ObjectStop
*
object_stop_ptr
=
object_decision
.
mutable_stop
();
object_stop_ptr
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_OBSTACLE
);
const
auto
&
vehicle_param
=
common
::
VehicleConfigHelper
::
instance
()
->
GetConfig
()
.
vehicle_param
();
constexpr
double
kStopBuffer
=
1.0e-6
;
const
double
adc_v
=
common
::
VehicleState
::
instance
()
->
linear_velocity
();
const
double
distance_to_full_stop
=
0.5
*
(
adc_v
*
adc_v
)
/
std
::
fabs
(
vehicle_param
.
max_deceleration
());
double
stop_ref_s
=
std
::
fmax
(
reference_line_info_
->
AdcSlBoundary
().
end_s
()
+
distance_to_full_stop
+
kStopBuffer
,
sl_boundary
.
start_s
()
-
FLAGS_stop_distance_obstacle
);
constexpr
double
kMinStopDistanceToObstacle
=
1.0
;
if
(
stop_ref_s
+
kMinStopDistanceToObstacle
>
sl_boundary
.
start_s
())
{
stop_ref_s
=
std
::
fmax
(
reference_line_info_
->
AdcSlBoundary
().
end_s
()
+
kStopBuffer
,
sl_boundary
.
start_s
()
-
kMinStopDistanceToObstacle
);
}
object_stop_ptr
->
set_distance_s
(
stop_ref_s
-
sl_boundary
.
start_s
());
sl_boundary
.
start_s
()
-
FLAGS_stop_distance_obstacle
;
object_stop_ptr
->
set_distance_s
(
-
FLAGS_stop_distance_obstacle
);
auto
stop_ref_point
=
reference_line_
->
GetReferencePoint
(
stop_ref_s
);
object_stop_ptr
->
mutable_stop_point
()
->
set_x
(
stop_ref_point
.
x
());
...
...
modules/planning/tasks/traffic_decider/traffic_decider.cc
浏览文件 @
e52f6f6d
...
...
@@ -44,84 +44,10 @@ bool TrafficDecider::Init(const PlanningConfig &config) {
return
true
;
}
const
PathObstacle
*
TrafficDecider
::
CreateDestinationPathObstacle
()
{
// set destination point
const
auto
*
destination_obstacle
=
frame_
->
FindObstacle
(
FLAGS_destination_obstacle_id
);
if
(
!
destination_obstacle
)
{
destination_obstacle
=
CreateDestinationObstacle
();
ADEBUG
<<
"Created destination obstacle"
;
}
if
(
!
destination_obstacle
)
{
return
nullptr
;
}
else
{
const
auto
*
ptr
=
reference_line_info_
->
AddObstacle
(
destination_obstacle
);
if
(
!
ptr
)
{
AERROR
<<
"Failed to add destination obstacle's projection"
;
return
nullptr
;
}
return
ptr
;
}
}
const
Obstacle
*
TrafficDecider
::
CreateDestinationObstacle
()
{
const
auto
&
routing_response
=
frame_
->
routing_response
();
if
(
!
routing_response
.
routing_request
().
has_end
())
{
ADEBUG
<<
"routing_request has no end"
;
return
nullptr
;
}
common
::
math
::
Vec2d
destination
;
destination
.
set_x
(
routing_response
.
routing_request
().
end
().
pose
().
x
());
destination
.
set_y
(
routing_response
.
routing_request
().
end
().
pose
().
y
());
// check if destination point is in planning range
common
::
SLPoint
destination_sl
;
const
auto
&
reference_line
=
reference_line_info_
->
reference_line
();
reference_line
.
XYToSL
(
destination
,
&
destination_sl
);
double
destination_s
=
destination_sl
.
s
();
double
destination_l
=
destination_sl
.
l
();
double
left_bound
;
double
right_bound
;
if
(
!
reference_line
.
GetLaneWidth
(
destination_s
,
&
left_bound
,
&
right_bound
))
{
left_bound
=
right_bound
=
FLAGS_default_reference_line_width
/
2
;
}
if
(
destination_s
<
0
||
destination_s
>
reference_line
.
Length
()
||
destination_l
>
left_bound
||
destination_l
<
-
right_bound
)
{
ADEBUG
<<
"destination[s="
<<
destination_s
<<
"; l="
<<
destination_l
<<
"] out of planning range. Skip"
;
return
nullptr
;
}
// adjust destination based on adc front s
if
(
destination_sl
.
s
()
<=
reference_line_info_
->
AdcSlBoundary
().
end_s
())
{
destination_s
=
reference_line_info_
->
AdcSlBoundary
().
end_s
()
+
FLAGS_destination_adjust_distance_buffer
;
}
std
::
unique_ptr
<
Obstacle
>
obstacle_ptr
=
reference_line_info_
->
CreateVirtualObstacle
(
FLAGS_destination_obstacle_id
,
{
destination
.
x
(),
destination
.
y
()},
FLAGS_virtual_stop_wall_length
,
FLAGS_virtual_stop_wall_width
,
FLAGS_virtual_stop_wall_height
);
const
auto
*
obstacle
=
obstacle_ptr
.
get
();
if
(
!
frame_
->
AddObstacle
(
std
::
move
(
obstacle_ptr
)))
{
AERROR
<<
"Failed to add destination obstacle"
;
return
nullptr
;
}
return
obstacle
;
}
Status
TrafficDecider
::
Execute
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
)
{
Task
::
Execute
(
frame
,
reference_line_info
);
// 1. add destination stop
if
(
!
MakeDestinationStopDecision
())
{
ADEBUG
<<
"There is no destination stop"
;
}
else
{
ADEBUG
<<
"destination is created"
;
}
for
(
const
auto
rule_config
:
rule_configs_
)
{
auto
rule
=
rule_factory_
.
CreateObject
(
rule_config
.
name
());
if
(
!
rule
)
{
...
...
@@ -134,47 +60,5 @@ Status TrafficDecider::Execute(Frame *frame,
return
Status
::
OK
();
}
bool
TrafficDecider
::
MakeDestinationStopDecision
()
{
const
auto
*
path_obstacle
=
CreateDestinationPathObstacle
();
if
(
!
path_obstacle
)
{
ADEBUG
<<
"The path obstacle is not found"
;
return
false
;
}
const
auto
*
obstacle
=
path_obstacle
->
obstacle
();
const
auto
&
reference_line
=
reference_line_info_
->
reference_line
();
// check stop_posision on reference line
auto
stop_position
=
obstacle
->
Perception
().
position
();
common
::
SLPoint
stop_line_sl
;
reference_line
.
XYToSL
({
stop_position
.
x
(),
stop_position
.
y
()},
&
stop_line_sl
);
if
(
!
reference_line
.
IsOnRoad
(
stop_line_sl
))
{
return
false
;
}
// check stop_line_s vs adc_s. stop_line_s must be ahead of adc_front_s
if
(
stop_line_sl
.
s
()
<=
reference_line_info_
->
AdcSlBoundary
().
end_s
())
{
ADEBUG
<<
"skip: object:"
<<
obstacle
->
Id
()
<<
" fence route_s["
<<
stop_line_sl
.
s
()
<<
"] behind adc_front_s["
<<
reference_line_info_
->
AdcSlBoundary
().
end_s
()
<<
"]"
;
return
false
;
}
ObjectDecisionType
object_stop
;
ObjectStop
*
object_stop_ptr
=
object_stop
.
mutable_stop
();
object_stop_ptr
->
set_distance_s
(
FLAGS_stop_line_min_distance
);
object_stop_ptr
->
set_reason_code
(
StopReasonCode
::
STOP_REASON_DESTINATION
);
auto
stop_ref_point
=
reference_line
.
GetReferencePoint
(
stop_position
.
x
(),
stop_position
.
y
());
object_stop_ptr
->
mutable_stop_point
()
->
set_x
(
stop_ref_point
.
x
());
object_stop_ptr
->
mutable_stop_point
()
->
set_y
(
stop_ref_point
.
y
());
object_stop_ptr
->
set_stop_heading
(
stop_ref_point
.
heading
());
reference_line_info_
->
path_decision
()
->
AddLongitudinalDecision
(
"Destination"
,
obstacle
->
Id
(),
object_stop
);
return
true
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/tasks/traffic_decider/traffic_decider.h
浏览文件 @
e52f6f6d
...
...
@@ -56,15 +56,6 @@ class TrafficDecider : public Task {
private:
void
RegisterRules
();
bool
MakeDestinationStopDecision
();
/**
* @brief create destination obstacle based on routing end point
* @return the created path obstacle
*/
const
Obstacle
*
CreateDestinationObstacle
();
const
PathObstacle
*
CreateDestinationPathObstacle
();
apollo
::
common
::
util
::
Factory
<
std
::
string
,
TrafficRule
>
rule_factory_
;
google
::
protobuf
::
RepeatedPtrField
<
RuleConfig
>
rule_configs_
;
};
...
...
modules/planning/testdata/garage_test/result_follow_0.pb.txt
浏览文件 @
e52f6f6d
此差异已折叠。
点击以展开。
modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
浏览文件 @
e52f6f6d
此差异已折叠。
点击以展开。
modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
浏览文件 @
e52f6f6d
...
...
@@ -2285,5 +2285,13 @@ decision {
}
}
}
decision {
id: "DEST"
perception_id: -1109540454
object_decision {
ignore {
}
}
}
}
}
modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
浏览文件 @
e52f6f6d
此差异已折叠。
点击以展开。
modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
浏览文件 @
e52f6f6d
此差异已折叠。
点击以展开。
modules/tools/plot_trace/plot_planning_result.py
浏览文件 @
e52f6f6d
...
...
@@ -50,9 +50,15 @@ def plot_planning(ax, planning_file):
def
press_key
(
event
):
files
=
" "
.
join
(
g_args
.
planning_files
)
if
evnet
.
key
==
'f'
:
call
([
"cp"
,
files
])
if
event
.
key
==
'c'
:
files
=
g_args
.
planning_files
if
len
(
files
)
!=
2
:
print
"Need more than two files"
return
command
=
[
"cp"
]
for
f
in
files
:
command
.
append
(
f
)
call
(
command
)
if
__name__
==
'__main__'
:
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录