-
Notifications
You must be signed in to change notification settings - Fork 0
/
Plotter.py
117 lines (103 loc) · 3.79 KB
/
Plotter.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import rospy, time, math
from time import ctime,sleep
from glog import get_glogger
# project sensor_msgs/LaserScan.msg to sensor_msgs/PointCloud.msg
class Plotter:
pointcloud = np.zeros( (1040,2) )
robot = np.zeros( (40,2) )
trajectory = np.zeros((1,2))
trajectory2 = np.zeros((1,2))
pose_2d = (0, 0, 0)
angle_min = -1*math.pi/2
angle_max = math.pi/2
fig = plt.figure()
axes = fig.add_subplot(111, autoscale_on=True)
lines = []
aini = None
def __init__(self):
xdata = []
ydata = []
line, = self.axes.plot(xdata, ydata, 'r.') # point cloud
self.lines.append(line)
line, = self.axes.plot(xdata, ydata, 'b+') # plot_robot
self.lines.append(line)
line, = self.axes.plot(xdata, ydata, 'go') # trajectory
self.lines.append(line)
line, = self.axes.plot(xdata, ydata, 'y*') # trajectory2
self.lines.append(line)
self.axes.set_autoscale_on(True)
self.axes.relim()
self.axes.autoscale_view(True,True,True)
self.ani = animation.FuncAnimation(self.fig, self.update, self.data_gen, interval=1*200)
def show(self):
plt.show()
def set_angle(self, min, max):
self.angle_max = max
self.angle_min = min
def update(self, data):
# xdata = np.random.rand(1040)*1000
# ydata = np.random.rand(1040)*100
xdata = data[0][:,0]
ydata = data[0][:,1]
self.lines[0].set_data(xdata, ydata)
xdata = data[1][:,0]
ydata = data[1][:,1]
self.lines[1].set_data(xdata, ydata)
xdata = data[2][:,0]
ydata = data[2][:,1]
self.lines[2].set_data(xdata, ydata)
xdata = data[3][:,0]
ydata = data[3][:,1]
self.lines[3].set_data(xdata, ydata)
self.axes.axis('equal')
self.axes.relim()
self.axes.autoscale_view(True,True,True)
return self.lines,
def data_gen(self):
while True:
# rospy.loginfo("gen points %d", len(scan))
yield [self.pointcloud, self.robot, self.trajectory, self.trajectory2]
# this trajectory is true pose
def plot_trajectory(self, pose_2d=(0,0,0)):
# self.plot_robot(pose_2d)
self.trajectory = np.append(self.trajectory, np.mat(pose_2d[0:2]), axis=0)
# this trajectory is calc pose
def plot_trajectory2(self, pose_2d=(0,0,0)):
self.plot_robot(pose_2d)
self.trajectory2 = np.append(self.trajectory2, np.mat(pose_2d[0:2]), axis=0)
def plot_scan(self, points):
self.pointcloud = points
# self.plot_robot(pose_2d)
# print points[: , 0].
def plot_robot(self, pose_2d):
# del self.robot[:]
x0 = pose_2d[0]
y0 = pose_2d[1]
yaw = pose_2d[2]
robot = np.zeros( (1,2) )
for r in range(20):
point = [(x0 + r/4.0*np.cos(yaw), y0 + r/4.0*np.sin(yaw))]
robot = np.append(robot, np.mat(point), axis=0)
for r in range(10):
point = [(x0 + r/4.0*np.cos(yaw + self.angle_max), y0 + r/4.0*np.sin(yaw + self.angle_max))]
robot = np.append(robot, np.mat(point), axis=0)
for r in range(10):
point = [(x0 + r/4.0*np.cos(yaw + self.angle_min), y0 + r/4.0*np.sin(yaw + self.angle_min))]
robot = np.append(robot, np.mat(point), axis=0)
# self.data = np.append(self.data, robot, axis=0)
self.robot = robot
# print points[: , 0]
if __name__ == '__main__':
glogger = get_glogger(0, __file__)
ranges = range(10)
print ranges
for i, r in enumerate(ranges):
if i%10 == 0:
print i, r
ranges[0:2]+=[1, 2]
print ranges[0:2]