Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
d4c51919
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,发现更多精彩内容 >>
提交
d4c51919
编写于
9月 12, 2017
作者:
J
jiangyifei
提交者:
Dong Li
9月 12, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
planning updated plot planning.
上级
42111c5a
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
254 addition
and
0 deletion
+254
-0
modules/tools/mapshow/planning.py
modules/tools/mapshow/planning.py
+46
-0
modules/tools/mapshow/plot_smoothness.py
modules/tools/mapshow/plot_smoothness.py
+40
-0
modules/tools/mapshow/subplot_traj_acc.py
modules/tools/mapshow/subplot_traj_acc.py
+84
-0
modules/tools/mapshow/subplot_traj_speed.py
modules/tools/mapshow/subplot_traj_speed.py
+84
-0
未找到文件。
modules/tools/mapshow/planning.py
浏览文件 @
d4c51919
...
...
@@ -32,6 +32,14 @@ class Planning:
self
.
speed_data_time
=
{}
self
.
speed_data_val
=
{}
self
.
traj_data_lock
=
threading
.
Lock
()
self
.
traj_speed_history_len
=
30
self
.
traj_speed_t_history
=
[]
self
.
traj_speed_v_history
=
[]
self
.
traj_acc_history_len
=
30
self
.
traj_acc_t_history
=
[]
self
.
traj_acc_a_history
=
[]
self
.
st_data_lock
=
threading
.
Lock
()
self
.
st_curve_s
=
{}
self
.
st_curve_t
=
{}
...
...
@@ -233,6 +241,44 @@ class Planning:
self
.
st_data_lock
.
release
()
def
compute_traj_data
(
self
):
traj_speed_t
=
[]
traj_speed_v
=
[]
traj_acc_t
=
[]
traj_acc_a
=
[]
base_time
=
self
.
planning_pb
.
header
.
timestamp_sec
for
trajectory_point
in
self
.
planning_pb
.
trajectory_point
:
traj_acc_t
.
append
(
base_time
+
trajectory_point
.
relative_time
)
traj_acc_a
.
append
(
trajectory_point
.
a
)
for
trajectory_point
in
self
.
planning_pb
.
trajectory_point
:
traj_speed_t
.
append
(
base_time
+
trajectory_point
.
relative_time
)
traj_speed_v
.
append
(
trajectory_point
.
v
)
self
.
traj_data_lock
.
acquire
()
self
.
traj_speed_t_history
.
append
(
traj_speed_t
)
self
.
traj_speed_v_history
.
append
(
traj_speed_v
)
if
len
(
self
.
traj_speed_t_history
)
>
self
.
traj_speed_history_len
:
self
.
traj_speed_t_history
=
\
self
.
traj_speed_t_history
[
len
(
self
.
traj_speed_t_history
)
-
self
.
traj_speed_history_len
:]
self
.
traj_speed_v_history
=
\
self
.
traj_speed_v_history
[
len
(
self
.
traj_speed_v_history
)
-
self
.
traj_speed_history_len
:]
self
.
traj_acc_t_history
.
append
(
traj_acc_t
)
self
.
traj_acc_a_history
.
append
(
traj_acc_a
)
if
len
(
self
.
traj_acc_t_history
)
>
self
.
traj_acc_history_len
:
self
.
traj_acc_t_history
=
\
self
.
traj_acc_t_history
[
len
(
self
.
traj_acc_t_history
)
-
self
.
traj_acc_history_len
:]
self
.
traj_acc_a_history
=
\
self
.
traj_acc_a_history
[
len
(
self
.
traj_acc_a_history
)
-
self
.
traj_acc_history_len
:]
self
.
traj_data_lock
.
release
()
def
replot_sl_data
(
self
,
sl_static_obstacle_lower_boundary
,
sl_static_obstacle_upper_boundary
,
...
...
modules/tools/mapshow/plot_smoothness.py
0 → 100644
浏览文件 @
d4c51919
import
rospy
from
modules.planning.proto
import
planning_pb2
import
matplotlib.pyplot
as
plt
from
planning
import
Planning
import
matplotlib.animation
as
animation
from
subplot_traj_speed
import
TrajSpeedSubplot
from
subplot_traj_acc
import
TrajAccSubplot
planning
=
Planning
()
def
update
(
frame_number
):
traj_speed_subplot
.
show
(
planning
)
traj_acc_subplot
.
show
(
planning
)
def
planning_callback
(
planning_pb
):
planning
.
update_planning_pb
(
planning_pb
)
planning
.
compute_traj_data
()
def
add_listener
():
rospy
.
init_node
(
'st_plot'
,
anonymous
=
True
)
rospy
.
Subscriber
(
'/apollo/planning'
,
planning_pb2
.
ADCTrajectory
,
planning_callback
)
def
press_key
():
pass
if
__name__
==
'__main__'
:
add_listener
()
fig
=
plt
.
figure
(
figsize
=
(
14
,
6
))
fig
.
canvas
.
mpl_connect
(
'key_press_event'
,
press_key
)
ax
=
plt
.
subplot2grid
((
1
,
2
),
(
0
,
0
))
traj_speed_subplot
=
TrajSpeedSubplot
(
ax
)
ax2
=
plt
.
subplot2grid
((
1
,
2
),
(
0
,
1
))
traj_acc_subplot
=
TrajAccSubplot
(
ax2
)
ani
=
animation
.
FuncAnimation
(
fig
,
update
,
interval
=
100
)
plt
.
show
()
modules/tools/mapshow/subplot_traj_acc.py
0 → 100644
浏览文件 @
d4c51919
#!/usr/bin/env python
###############################################################################
# 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.
###############################################################################
import
matplotlib.pyplot
as
plt
from
matplotlib
import
cm
as
cmx
from
matplotlib
import
colors
as
mcolors
class
TrajAccSubplot
:
def
__init__
(
self
,
ax
):
self
.
ax
=
ax
self
.
acc_lines
=
[]
self
.
acc_lines_size
=
30
self
.
colors
=
[]
self
.
init_colors
()
#self.colors = ['b','r', 'y', 'k']
for
i
in
range
(
self
.
acc_lines_size
):
line
,
=
ax
.
plot
(
[
0
],
[
0
],
c
=
self
.
colors
[
i
%
len
(
self
.
colors
)],
ls
=
"-"
,
marker
=
''
,
lw
=
3
,
alpha
=
0.8
)
self
.
acc_lines
.
append
(
line
)
ax
.
set_xlabel
(
"t (second)"
)
#ax.set_xlim([-2, 10])
ax
.
set_ylim
([
-
6
,
6
])
self
.
ax
.
autoscale_view
()
#self.ax.relim()
ax
.
set_ylabel
(
"acc (m/s^2)"
)
ax
.
set_title
(
"PLANNING ACC"
)
self
.
set_visible
(
False
)
def
init_colors
(
self
):
self
.
colors
=
[]
values
=
range
(
self
.
acc_lines_size
)
jet
=
plt
.
get_cmap
(
'brg'
)
color_norm
=
mcolors
.
Normalize
(
vmin
=
0
,
vmax
=
values
[
-
1
])
scalar_map
=
cmx
.
ScalarMappable
(
norm
=
color_norm
,
cmap
=
jet
)
for
val
in
values
:
color_val
=
scalar_map
.
to_rgba
(
val
)
self
.
colors
.
append
(
color_val
)
def
set_visible
(
self
,
visible
):
for
line
in
self
.
acc_lines
:
line
.
set_visible
(
visible
)
def
show
(
self
,
planning
):
planning
.
traj_data_lock
.
acquire
()
for
i
in
range
(
len
(
planning
.
traj_speed_t_history
)):
if
i
>=
self
.
acc_lines_size
:
print
"WARNING: number of path lines is more than "
\
+
str
(
self
.
acc_lines_size
)
continue
speed_line
=
self
.
acc_lines
[
self
.
acc_lines_size
-
i
-
1
]
speed_line
.
set_xdata
(
planning
.
traj_acc_t_history
[
i
])
speed_line
.
set_ydata
(
planning
.
traj_acc_a_history
[
i
])
#speed_line.set_xdata([1,2,3,4])
#speed_line.set_ydata([1,2,3,4])
#speed_line.set_label(name[0:5])
speed_line
.
set_visible
(
True
)
#self.ax.legend(loc="upper left", borderaxespad=0., ncol=5)
#self.ax.axis('equal')
planning
.
traj_data_lock
.
release
()
self
.
ax
.
autoscale_view
()
self
.
ax
.
relim
()
\ No newline at end of file
modules/tools/mapshow/subplot_traj_speed.py
0 → 100644
浏览文件 @
d4c51919
#!/usr/bin/env python
###############################################################################
# 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.
###############################################################################
import
matplotlib.pyplot
as
plt
from
matplotlib
import
cm
as
cmx
from
matplotlib
import
colors
as
mcolors
class
TrajSpeedSubplot
:
def
__init__
(
self
,
ax
):
self
.
ax
=
ax
self
.
speed_lines
=
[]
self
.
speed_lines_size
=
30
self
.
colors
=
[]
self
.
init_colors
()
#self.colors = ['b','r', 'y', 'k']
for
i
in
range
(
self
.
speed_lines_size
):
line
,
=
ax
.
plot
(
[
0
],
[
0
],
c
=
self
.
colors
[
i
%
len
(
self
.
colors
)],
ls
=
"-"
,
marker
=
''
,
lw
=
3
,
alpha
=
0.8
)
self
.
speed_lines
.
append
(
line
)
ax
.
set_xlabel
(
"t (second)"
)
#ax.set_xlim([-2, 10])
ax
.
set_ylim
([
-
1
,
13
])
self
.
ax
.
autoscale_view
()
#self.ax.relim()
ax
.
set_ylabel
(
"speed (m/s)"
)
ax
.
set_title
(
"PLANNING SPEED"
)
self
.
set_visible
(
False
)
def
init_colors
(
self
):
self
.
colors
=
[]
values
=
range
(
self
.
speed_lines_size
)
jet
=
plt
.
get_cmap
(
'brg'
)
color_norm
=
mcolors
.
Normalize
(
vmin
=
0
,
vmax
=
values
[
-
1
])
scalar_map
=
cmx
.
ScalarMappable
(
norm
=
color_norm
,
cmap
=
jet
)
for
val
in
values
:
color_val
=
scalar_map
.
to_rgba
(
val
)
self
.
colors
.
append
(
color_val
)
def
set_visible
(
self
,
visible
):
for
line
in
self
.
speed_lines
:
line
.
set_visible
(
visible
)
def
show
(
self
,
planning
):
planning
.
traj_data_lock
.
acquire
()
for
i
in
range
(
len
(
planning
.
traj_speed_t_history
)):
if
i
>=
self
.
speed_lines_size
:
print
"WARNING: number of path lines is more than "
\
+
str
(
self
.
speed_lines_size
)
continue
speed_line
=
self
.
speed_lines
[
self
.
speed_lines_size
-
i
-
1
]
speed_line
.
set_xdata
(
planning
.
traj_speed_t_history
[
i
])
speed_line
.
set_ydata
(
planning
.
traj_speed_v_history
[
i
])
#speed_line.set_xdata([1,2,3,4])
#speed_line.set_ydata([1,2,3,4])
#speed_line.set_label(name[0:5])
speed_line
.
set_visible
(
True
)
#self.ax.legend(loc="upper left", borderaxespad=0., ncol=5)
#self.ax.axis('equal')
planning
.
traj_data_lock
.
release
()
self
.
ax
.
autoscale_view
()
self
.
ax
.
relim
()
\ No newline at end of file
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录