Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
bb288ab9
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,发现更多精彩内容 >>
提交
bb288ab9
编写于
3月 19, 2019
作者:
D
deidaraho
提交者:
Xiangquan Xiao
3月 20, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning: visualize open space trajectory
上级
f1b34a8d
变更
8
隐藏空白更改
内联
并排
Showing
8 changed file
with
196 addition
and
35 deletion
+196
-35
modules/planning/common/open_space_info.cc
modules/planning/common/open_space_info.cc
+1
-1
modules/planning/common/open_space_info.h
modules/planning/common/open_space_info.h
+36
-1
modules/planning/on_lane_planning.cc
modules/planning/on_lane_planning.cc
+118
-24
modules/planning/on_lane_planning.h
modules/planning/on_lane_planning.h
+6
-2
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.cc
..._trajectory_generation/open_space_trajectory_optimizer.cc
+7
-7
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.h
...e_trajectory_generation/open_space_trajectory_optimizer.h
+4
-0
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.cc
...e_trajectory_generation/open_space_trajectory_provider.cc
+22
-0
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h
...ce_trajectory_generation/open_space_trajectory_provider.h
+2
-0
未找到文件。
modules/planning/common/open_space_info.cc
浏览文件 @
bb288ab9
...
...
@@ -22,6 +22,6 @@
namespace
apollo
{
namespace
planning
{
OpenSpaceInfo
::
OpenSpaceInfo
()
{}
// to-do[runxin]: record more trajectory information to info debug
}
// namespace planning
}
// namespace apollo
modules/planning/common/open_space_info.h
浏览文件 @
bb288ab9
...
...
@@ -41,9 +41,11 @@
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/common/trajectory/publishable_trajectory.h"
#include "modules/planning/proto/planning_internal.pb.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
planning_internal
::
Debug
;
typedef
std
::
pair
<
DiscretizedTrajectory
,
canbus
::
Chassis
::
GearPosition
>
TrajGearPair
;
...
...
@@ -60,7 +62,7 @@ struct GearSwitchStates {
class
OpenSpaceInfo
{
public:
OpenSpaceInfo
();
OpenSpaceInfo
()
=
default
;
~
OpenSpaceInfo
()
=
default
;
bool
open_space_pre_stop_finished
()
const
{
...
...
@@ -274,6 +276,32 @@ class OpenSpaceInfo {
return
publishable_trajectory_data_
;
}
apollo
::
planning_internal
::
Debug
*
mutable_debug
()
{
return
debug_
;
}
void
set_debug
(
apollo
::
planning_internal
::
Debug
*
debug
)
{
debug_
=
debug
;
}
const
apollo
::
planning_internal
::
Debug
&
debug
()
const
{
return
*
debug_
;
}
const
apollo
::
planning_internal
::
Debug
debug_instance
()
const
{
return
debug_instance_
;
}
apollo
::
planning_internal
::
Debug
*
mutable_debug_instance
()
{
return
&
debug_instance_
;
}
void
sync_debug_instance
()
{
debug_instance_
=
*
debug_
;
}
void
RecordDebug
();
private:
// @brief vehicle needs to stop first in open space related scenarios
bool
open_space_pre_stop_finished_
=
true
;
...
...
@@ -344,6 +372,13 @@ class OpenSpaceInfo {
std
::
pair
<
PublishableTrajectory
,
canbus
::
Chassis
::
GearPosition
>
publishable_trajectory_data_
;
// the pointer from ADCtrajectory
apollo
::
planning_internal
::
Debug
*
debug_
;
// the instance inside debug,
// if ADCtrajectory is NULL, blank; else same to ADCtrajectory
apollo
::
planning_internal
::
Debug
debug_instance_
;
};
}
// namespace planning
...
...
modules/planning/on_lane_planning.cc
浏览文件 @
bb288ab9
...
...
@@ -383,7 +383,8 @@ Status OnLanePlanning::Plan(
if
(
FLAGS_enable_record_debug
)
{
ptr_debug
->
mutable_planning_data
()
->
mutable_init_point
()
->
CopyFrom
(
stitching_trajectory
.
back
());
// 1, ptr_debug -> open_space_info
frame_
->
mutable_open_space_info
()
->
set_debug
(
ptr_debug
);
frame_
->
mutable_open_space_info
()
->
sync_debug_instance
();
}
auto
status
=
...
...
@@ -399,6 +400,12 @@ Status OnLanePlanning::Plan(
frame_
->
open_space_info
().
publishable_trajectory_data
().
second
;
publishable_trajectory
.
PopulateTrajectoryProtobuf
(
trajectory_pb
);
trajectory_pb
->
set_gear
(
publishable_trajectory_gear
);
if
(
FLAGS_enable_record_debug
&&
FLAGS_export_chart
)
{
// call open space info load debug
ExportOpenSpaceChart
(
frame_
->
open_space_info
().
debug_instance
(),
ptr_debug
);
}
}
else
{
const
auto
*
best_ref_info
=
frame_
->
FindDriveReferenceLineInfo
();
if
(
!
best_ref_info
)
{
...
...
@@ -410,7 +417,7 @@ Status OnLanePlanning::Plan(
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
msg
);
}
if
(
FLAGS_export_chart
)
{
ExportChart
(
best_ref_info
->
debug
(),
ptr_debug
);
Export
OnLane
Chart
(
best_ref_info
->
debug
(),
ptr_debug
);
}
else
{
ptr_debug
->
MergeFrom
(
best_ref_info
->
debug
());
ExportReferenceLineDebug
(
ptr_debug
);
...
...
@@ -493,14 +500,25 @@ bool OnLanePlanning::CheckPlanningConfig(const PlanningConfig& config) {
return
true
;
}
void
PopulateChartOptions
(
double
x_min
,
double
x_max
,
std
::
string
x_label
,
double
y_min
,
double
y_max
,
std
::
string
y_label
,
bool
display
,
Chart
*
chart
)
{
auto
*
options
=
chart
->
mutable_options
();
options
->
mutable_x
()
->
set_min
(
x_min
);
options
->
mutable_x
()
->
set_max
(
x_max
);
options
->
mutable_y
()
->
set_min
(
y_min
);
options
->
mutable_y
()
->
set_max
(
y_max
);
options
->
mutable_x
()
->
set_label_string
(
x_label
);
options
->
mutable_y
()
->
set_label_string
(
y_label
);
options
->
set_legend_display
(
display
);
}
void
AddStGraph
(
const
STGraphDebug
&
st_graph
,
Chart
*
chart
)
{
chart
->
set_title
(
st_graph
.
name
());
auto
*
options
=
chart
->
mutable_options
();
options
->
mutable_x
()
->
set_min
(
-
2.0
);
options
->
mutable_x
()
->
set_max
(
10.0
);
options
->
mutable_x
()
->
set_label_string
(
"t(second)"
);
options
->
mutable_y
()
->
set_label_string
(
"s(meter)"
);
options
->
set_legend_display
(
false
);
PopulateChartOptions
(
-
2.0
,
10.0
,
"t (second)"
,
0.0
,
80.0
,
"s (meter)"
,
true
,
chart
);
for
(
const
auto
&
boundary
:
st_graph
.
boundary
())
{
auto
*
boundary_chart
=
chart
->
add_polygon
();
...
...
@@ -518,13 +536,8 @@ void AddStGraph(const STGraphDebug& st_graph, Chart* chart) {
void
AddSlFrame
(
const
SLFrameDebug
&
sl_frame
,
Chart
*
chart
)
{
chart
->
set_title
(
sl_frame
.
name
());
auto
*
options
=
chart
->
mutable_options
();
options
->
mutable_x
()
->
set_min
(
0.0
);
options
->
mutable_x
()
->
set_max
(
80.0
);
options
->
mutable_x
()
->
set_label_string
(
"s (meter)"
);
options
->
mutable_y
()
->
set_min
(
-
8.0
);
options
->
mutable_y
()
->
set_max
(
8.0
);
options
->
mutable_y
()
->
set_label_string
(
"l (meter)"
);
PopulateChartOptions
(
0.0
,
80.0
,
"s (meter)"
,
-
8.0
,
8.0
,
"l (meter)"
,
false
,
chart
);
auto
*
sl_line
=
chart
->
add_line
();
sl_line
->
set_label
(
"SL Path"
);
for
(
const
auto
&
sl_point
:
sl_frame
.
sl_path
())
{
...
...
@@ -538,13 +551,9 @@ void AddSpeedPlan(
const
::
google
::
protobuf
::
RepeatedPtrField
<
SpeedPlan
>&
speed_plans
,
Chart
*
chart
)
{
chart
->
set_title
(
"Speed Plan"
);
auto
*
options
=
chart
->
mutable_options
();
options
->
mutable_x
()
->
set_min
(
0.0
);
options
->
mutable_x
()
->
set_max
(
80.0
);
options
->
mutable_x
()
->
set_label_string
(
"s (meter)"
);
options
->
mutable_y
()
->
set_min
(
0.0
);
options
->
mutable_y
()
->
set_max
(
50.0
);
options
->
mutable_y
()
->
set_label_string
(
"v (m/s)"
);
PopulateChartOptions
(
0.0
,
80.0
,
"s (meter)"
,
0.0
,
50.0
,
"v (m/s)"
,
false
,
chart
);
for
(
const
auto
&
speed_plan
:
speed_plans
)
{
auto
*
line
=
chart
->
add_line
();
line
->
set_label
(
speed_plan
.
name
());
...
...
@@ -568,8 +577,9 @@ void AddSpeedPlan(
}
}
void
OnLanePlanning
::
ExportChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
)
{
void
OnLanePlanning
::
ExportOnLaneChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
)
{
for
(
const
auto
&
st_graph
:
debug_info
.
planning_data
().
st_graph
())
{
AddStGraph
(
st_graph
,
debug_chart
->
mutable_planning_data
()
->
add_chart
());
}
...
...
@@ -581,5 +591,89 @@ void OnLanePlanning::ExportChart(const planning_internal::Debug& debug_info,
debug_chart
->
mutable_planning_data
()
->
add_chart
());
}
void
OnLanePlanning
::
ExportOpenSpaceChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
)
{
// Export Trajectory Visualization Chart.
if
(
FLAGS_enable_record_debug
)
{
AddOpenSpaceOptimizerResult
(
debug_info
,
debug_chart
);
// AddStitchSpeedProfile(debug);
// AddPublishedSpeed(debug, ptr_trajectory_pb);
// AddPublishedAcceleration(debug, ptr_trajectory_pb);
}
}
void
OnLanePlanning
::
AddOpenSpaceOptimizerResult
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
)
{
// if open space info provider success run
if
(
!
frame_
->
open_space_info
().
open_space_provider_success
())
{
return
;
}
auto
chart
=
debug_chart
->
mutable_planning_data
()
->
add_chart
();
auto
open_space_debug
=
debug_info
.
planning_data
().
open_space
();
chart
->
set_title
(
"Open Space Trajectory Visualization"
);
PopulateChartOptions
(
open_space_debug
.
xy_boundary
(
0
)
-
1.0
,
open_space_debug
.
xy_boundary
(
1
)
+
1.0
,
"x (meter)"
,
open_space_debug
.
xy_boundary
(
2
)
-
1.0
,
open_space_debug
.
xy_boundary
(
3
)
+
1.0
,
"y (meter)"
,
false
,
chart
);
int
obstacle_index
=
1
;
for
(
const
auto
&
obstacle
:
open_space_debug
.
obstacles
())
{
auto
*
obstacle_outline
=
chart
->
add_line
();
obstacle_outline
->
set_label
(
"boundary_"
+
std
::
to_string
(
obstacle_index
));
obstacle_index
+=
1
;
for
(
int
vertice_index
=
0
;
vertice_index
<
obstacle
.
vertices_x_coords_size
();
vertice_index
++
)
{
auto
*
point_debug
=
obstacle_outline
->
add_point
();
point_debug
->
set_x
(
obstacle
.
vertices_x_coords
(
vertice_index
));
point_debug
->
set_y
(
obstacle
.
vertices_y_coords
(
vertice_index
));
}
// Set chartJS's dataset properties
auto
*
obstacle_properties
=
obstacle_outline
->
mutable_properties
();
(
*
obstacle_properties
)[
"borderWidth"
]
=
"2"
;
(
*
obstacle_properties
)[
"pointRadius"
]
=
"0"
;
(
*
obstacle_properties
)[
"lineTension"
]
=
"0"
;
(
*
obstacle_properties
)[
"fill"
]
=
"false"
;
(
*
obstacle_properties
)[
"showLine"
]
=
"true"
;
}
auto
smoothed_trajectory
=
open_space_debug
.
smoothed_trajectory
();
auto
*
smoothed_line
=
chart
->
add_line
();
smoothed_line
->
set_label
(
"smoothed"
);
for
(
const
auto
&
point
:
smoothed_trajectory
.
vehicle_motion_point
())
{
auto
*
point_debug
=
smoothed_line
->
add_point
();
point_debug
->
set_x
(
point
.
trajectory_point
().
path_point
().
x
());
point_debug
->
set_y
(
point
.
trajectory_point
().
path_point
().
y
());
}
// Set chartJS's dataset properties
auto
*
smoothed_properties
=
smoothed_line
->
mutable_properties
();
(
*
smoothed_properties
)[
"borderWidth"
]
=
"2"
;
(
*
smoothed_properties
)[
"pointRadius"
]
=
"0"
;
(
*
smoothed_properties
)[
"lineTension"
]
=
"0"
;
(
*
smoothed_properties
)[
"fill"
]
=
"false"
;
(
*
smoothed_properties
)[
"showLine"
]
=
"true"
;
auto
warm_start_trajectory
=
open_space_debug
.
warm_start_trajectory
();
auto
*
warm_start_line
=
chart
->
add_line
();
warm_start_line
->
set_label
(
"warm_start"
);
for
(
const
auto
&
point
:
warm_start_trajectory
.
vehicle_motion_point
())
{
auto
*
point_debug
=
warm_start_line
->
add_point
();
point_debug
->
set_x
(
point
.
trajectory_point
().
path_point
().
x
());
point_debug
->
set_y
(
point
.
trajectory_point
().
path_point
().
y
());
}
// Set chartJS's dataset properties
auto
*
warm_start_properties
=
warm_start_line
->
mutable_properties
();
(
*
warm_start_properties
)[
"borderWidth"
]
=
"2"
;
(
*
warm_start_properties
)[
"pointRadius"
]
=
"0"
;
(
*
warm_start_properties
)[
"lineTension"
]
=
"0"
;
(
*
warm_start_properties
)[
"fill"
]
=
"false"
;
(
*
warm_start_properties
)[
"showLine"
]
=
"true"
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/on_lane_planning.h
浏览文件 @
bb288ab9
...
...
@@ -76,8 +76,12 @@ class OnLanePlanning : public PlanningBase {
void
ExportReferenceLineDebug
(
planning_internal
::
Debug
*
debug
);
bool
CheckPlanningConfig
(
const
PlanningConfig
&
config
);
void
GenerateStopTrajectory
(
ADCTrajectory
*
trajectory_pb
);
void
ExportChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
);
void
ExportOnLaneChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
);
void
ExportOpenSpaceChart
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
);
void
AddOpenSpaceOptimizerResult
(
const
planning_internal
::
Debug
&
debug_info
,
planning_internal
::
Debug
*
debug_chart
);
private:
routing
::
RoutingResponse
last_routing_
;
...
...
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.cc
浏览文件 @
bb288ab9
...
...
@@ -219,9 +219,10 @@ void OpenSpaceTrajectoryOptimizer::RecordDebugInfo(
obstacles_vertices_vec
)
{
// load translation origin and heading angle
auto
*
roi_shift_point
=
open_space_debug_
.
mutable_roi_shift_point
();
roi_shift_point
->
set_x
(
translate_origin
.
x
());
roi_shift_point
->
set_y
(
translate_origin
.
y
());
roi_shift_point
->
set_theta
(
rotate_angle
);
// pathpoint
roi_shift_point
->
mutable_path_point
()
->
set_x
(
translate_origin
.
x
());
roi_shift_point
->
mutable_path_point
()
->
set_y
(
translate_origin
.
y
());
roi_shift_point
->
mutable_path_point
()
->
set_theta
(
rotate_angle
);
// load warm start trajectory
size_t
horizon
=
uWS
.
cols
();
...
...
@@ -316,10 +317,9 @@ void OpenSpaceTrajectoryOptimizer::RecordDebugInfo(
// load obstacles
for
(
const
auto
&
obstacle_vertices
:
obstacles_vertices_vec
)
{
auto
*
obstacle_ptr
=
open_space_debug_
.
add_obstacles
();
size_t
vertices_size
=
obstacle_vertices
.
size
();
for
(
size_t
i
=
0
;
i
<
vertices_size
;
++
i
)
{
obstacle_ptr
->
add_vertices_x_coords
(
obstacle_vertices
[
i
].
x
());
obstacle_ptr
->
add_vertices_y_coords
(
obstacle_vertices
[
i
].
y
());
for
(
const
auto
&
vertex
:
obstacle_vertices
)
{
obstacle_ptr
->
add_vertices_x_coords
(
vertex
.
x
());
obstacle_ptr
->
add_vertices_y_coords
(
vertex
.
y
());
}
}
}
...
...
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.h
浏览文件 @
bb288ab9
...
...
@@ -79,6 +79,10 @@ class OpenSpaceTrajectoryOptimizer {
void
UpdateDebugInfo
(
::
apollo
::
planning_internal
::
OpenSpaceDebug
*
open_space_debug
);
apollo
::
planning_internal
::
OpenSpaceDebug
*
mutable_open_space_debug
()
{
return
&
open_space_debug_
;
}
private:
bool
IsInitPointNearDestination
(
const
common
::
TrajectoryPoint
&
planning_init_point
,
...
...
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.cc
浏览文件 @
bb288ab9
...
...
@@ -144,6 +144,14 @@ Status OpenSpaceTrajectoryProvider::Process() {
if
(
trajectory_updated_
)
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
open_space_mutex_
);
LoadResult
(
trajectory_data
);
if
(
FLAGS_enable_record_debug
)
{
// call merge debug ptr, open_space_trajectory_optimizer_
auto
*
debug_ptr
=
frame_
->
mutable_open_space_info
()
->
mutable_debug
();
open_space_trajectory_optimizer_
->
UpdateDebugInfo
(
debug_ptr
->
mutable_planning_data
()
->
mutable_open_space
());
// sync debug instance
frame_
->
mutable_open_space_info
()
->
sync_debug_instance
();
}
trajectory_updated_
.
store
(
false
);
return
Status
::
OK
();
}
...
...
@@ -163,6 +171,10 @@ Status OpenSpaceTrajectoryProvider::Process() {
if
(
previous_frame
->
open_space_info
().
open_space_provider_success
())
{
ReuseLastFrameResult
(
previous_frame
,
trajectory_data
);
if
(
FLAGS_enable_record_debug
)
{
// copy previous debug to current frame
ReuseLastFrameDebug
(
previous_frame
);
}
// reuse last frame debug when use last frame traj
return
Status
(
ErrorCode
::
OK
,
"Waiting for open_space_trajectory_optimizer in "
...
...
@@ -329,5 +341,15 @@ void OpenSpaceTrajectoryProvider::ReuseLastFrameResult(
frame_
->
mutable_open_space_info
()
->
set_open_space_provider_success
(
true
);
}
void
OpenSpaceTrajectoryProvider
::
ReuseLastFrameDebug
(
const
Frame
*
last_frame
)
{
// reuse last frame's instance
auto
*
debug_ptr
=
frame_
->
mutable_open_space_info
()
->
mutable_debug_instance
();
debug_ptr
->
mutable_planning_data
()
->
mutable_open_space
()
->
MergeFrom
(
last_frame
->
open_space_info
().
debug_instance
().
planning_data
().
open_space
());
// frame_->mutable_open_space_info()->sync_debug_instance();
}
}
// namespace planning
}
// namespace apollo
modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h
浏览文件 @
bb288ab9
...
...
@@ -72,6 +72,8 @@ class OpenSpaceTrajectoryProvider : public TrajectoryOptimizer {
void
ReuseLastFrameResult
(
const
Frame
*
last_frame
,
DiscretizedTrajectory
*
const
trajectory_data
);
void
ReuseLastFrameDebug
(
const
Frame
*
last_frame
);
private:
bool
thread_init_flag_
=
false
;
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录