~mrol-dev/mrol/trunk
» Revision
49
(compared to revision 54)
: mrol_mapping/visualiser/robotvisualiser.py
« back to all changes in this revision
Viewing changes to mrol_mapping/visualiser/robotvisualiser.py
- browse files at revision 49
- compare with another revision
- download diff
- download tarball
- view history from revision 49
- Committer: Vikas Dhiman
- Date: 2012-11-16 19:59:46 UTC
- Revision ID: vikasdhi@buffalo.edu-20121116195946-fwfdqevt3h3eqoyg
Manually added julians changes after merge failed. "Added function to save point cloud in the .ply format"; "Got speed_test working again after API changes.
- files added:
- mrol_mapping/visualiser
- files removed:
- mrol_mapping/visualiser
- files modified:
- mrol_mapping/log.py
added
removed
mrol_mapping/visualiser/robotvisualiser.py
1
'''
2
3D visualiser
3
Author: Julian Ryde and Nick Hillier
4
'''
5
6
#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
7
#
8
#This program is distributed in the hope that it will be useful,
9
#but WITHOUT ANY WARRANTY; without even the implied warranty of
10
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11
#GNU Lesser General Public License for more details.
12
#
13
#You should have received a copy of the GNU Lesser General Public License
14
#along with this program. If not, see <http://www.gnu.org/licenses/>.
15
16
from __future__ import division
17
18
import visual
19
import numpy as np
20
import pylab
21
import time
22
import mrol_mapping.visualiser.dispxyz as dispxyz
23
from threading import Thread
24
25
showlocal = False
26
27
def _draw_axes(position, size=1):
28
'''Draws a small set of x,y,z axes coloured red, green, blue and the
29
position specified.'''
30
# TODO make it account for orientation and take a pose
31
position = np.array(position)
32
visual.curve(pos=(position, position + (size,0,0)), color=(1,0,0))
33
visual.curve(pos=(position, position + (0,size,0)), color=(0,1,0))
34
visual.curve(pos=(position, position + (0,0,size)), color=(0,0,1))
35
36
class RotateVisual(Thread):
37
def __init__(self, scene):
38
Thread.__init__(self)
39
self.scene_angle = 0
40
self.scene = scene
41
42
def run(self):
43
while True:
44
self.scene_angle += 0.005
45
self.scene.forward = (np.cos(self.scene_angle), np.sin(self.scene_angle), -0.5)
46
self.scene.up = (0, 0, 1)
47
#self.scene.center += (0.1, 0, 0)
48
time.sleep(0.01)
49
50
class Visualiser:
51
def __init__(self, npts=100000, Rotate=False, title=''):
52
ptsize = 3
53
mapptsize = 2
54
self.scene_angle = 0
55
if showlocal:
56
self.localscene = visual.display(title=title + ' Local Coordinate System')
57
# generate new window
58
#self.globalscene = visual.display()
59
# reuse existing scene window
60
self.globalscene = visual.display.get_selected()
61
62
# cannot seem to set title on active window
63
#self.globalscene.title = title + ' Global Coordinate System'
64
self.globalscene.background = (1, 1, 1)
65
#self.globalscene.up = (0, 0, 1)
66
#self.globalscene.forward = (0, 1, -0.4)
67
self.currentrow = 0
68
69
self.auto_colour = False
70
71
# Set up the global coordinate frame visualiser
72
self.globalscene.select()
73
# initial view is looking down from top
74
#self.globalscene.forward = (0,1.0,0)
75
#self.globalscene.up =(0, 0, 1.0)
76
#self.globalscene.forward =(0, 0, -1.0)
77
78
#self.visualrobot = visual.box(length=3, height=2, width=1.5)
79
w = 1
80
wid = 0.2
81
self.robotpts = np.array(( (0, -wid, 0, w), (0, wid, 0, w), (3*wid, 0, 0, w), (0, -wid, 0, w) ))
82
#self.visualrobot = visual.points(pos=self.robotpts[:, :3], size=4, shape='round', color=(0, 1, 1))
83
self.visualrobot = visual.curve(pos=self.robotpts[:, :3], color=(0, 1, 1))
84
85
# draw the origin coordinate system
86
_draw_axes((0,0,0))
87
88
89
90
X = np.array([(-1, -1), (-1, 1), (1, 1), (1, -1), (-1, -1)])
91
square = visual.curve(pos=50*X)
92
self.leftmappts = visual.points(size=ptsize, shape='square', color=(1, 0, 0))
93
self.rightmappts = visual.points(size=ptsize, shape='square', color=(0, 1, 0))
94
self.spinmappts = visual.points(size=ptsize, shape='square', color=(0, 0, 1))
95
X = np.zeros((npts, 3))
96
self.mappts = visual.points(pos=X, size=mapptsize, shape='square', color=(1, 1, 1))
97
self.trajpts_ind = 0
98
self.trajpts = visual.points(pos=np.zeros((10000, 3)), color=(0, 0, 1), size=3)
99
visual.scene.show_rendertime = True
100
101
if Rotate:
102
# Enable continuous rotation
103
RV = RotateVisual(self.globalscene)
104
RV.start()
105
else:
106
# Enable mouse panning
107
MT = dispxyz.EnableMouseThread(self.globalscene)
108
MT.start()
109
110
# Set up the local coordinate frame visualiser
111
if showlocal:
112
self.localscene.select()
113
self.leftpts = visual.points(color=(1, 0, 0), size=ptsize, shape='square')
114
self.rightpts = visual.points(color=(0, 1, 0), size=ptsize, shape='square')
115
self.spinpts = visual.points(color=(0, 0, 1), size=ptsize, shape='square')
116
visual.scene.show_rendertime = True
117
118
self.colourmin = -2
119
self.colourmax = 2
120
121
def getkeypress(self):
122
key = None
123
if self.globalscene.kb.keys:
124
key = self.globalscene.kb.getkey()
125
return key
126
127
def setminmax(self,colourmin,colourmax):
128
self.colourmin = colourmin
129
self.colourmax = colourmax
130
131
def setautominmax(self, Bool=True):
132
self.auto_colour = Bool
133
134
def set_orthographic(self, Bool=False):
135
if Bool:
136
self.globalscene.fov = 1e-5
137
138
def setrobotpose(self, transform_mat): # TODO is there are more elegant way of doing this?
139
self.visualrobot.pos[:] = np.dot(transform_mat, self.robotpts.T).T[:, :3]
140
141
# TODO refactor this left and right
142
def setleftpts(self, xyzs, M=None):
143
if showlocal: self.leftpts.pos = xyzs[:, :3]
144
if M != None:
145
xyzs = np.dot(M, xyzs.T).T
146
self.leftmappts.pos = xyzs[:, :3]
147
#for i in X[:, :3]:
148
#self.leftmappts.append(i)
149
150
def setrightpts(self, xyzs, M=None):
151
if showlocal: self.rightpts.pos = xyzs[:, :3]
152
if M != None:
153
xyzs = np.dot(M, xyzs.T).T
154
self.rightmappts.pos = xyzs[:, :3]
155
#for i in X[:, :3]:
156
#self.rightmappts.append(i)
157
158
def setspinpts(self, xyzs, M=None):
159
if showlocal: self.spinpts.pos = xyzs[:, :3]
160
if M != None:
161
xyzs = np.dot(M, xyzs.T).T
162
self.spinmappts.pos = xyzs[:, :3]
163
164
# TODO make sure the ids of the pos arrays do not change to avoid memory leaks
165
166
def clear(self):
167
self.mappts.pos[:] = 0
168
self.currentrow = 0
169
170
def addmappts(self, xyzs, colour=None):
171
# add this to the array of points to visualise
172
# TODO make this more elegant so that you do not waste space at the end and
173
# do not leave points hanging around?
174
# something like
175
# X[:, 0].put([9, 10], [99, 98], mode='wrap') for the X, Y and Z coords
176
177
# TODO fix bug if xyzs is longer than self.npts
178
assert len(xyzs) < len(self.mappts.pos), 'Adding more points than visualiser was initialised with'
179
# handle cases when xzys is empty
180
if len(xyzs) < 1:
181
return
182
if self.currentrow+len(xyzs) > len(self.mappts.pos):
183
# if adding the points would go off the end of the array start
184
# adding at the beginning again
185
self.currentrow = 0
186
cr = self.currentrow
187
self.mappts.pos[cr:cr+len(xyzs)] = xyzs[:, :3]
188
if colour == None:
189
X = xyzs[:, 2]
190
if self.auto_colour == True:
191
self.colourmin = np.min(X)
192
self.colourmax = np.max(X)
193
colourmin = self.colourmax#-4
194
colourmax = self.colourmin#4
195
colours = (X-colourmin)/(colourmax-colourmin)
196
self.mappts.color[cr:cr+len(xyzs)] = pylab.cm.gist_rainbow(colours)[:, :3]
197
# jet, Accent, Blues, copper, autumn, cool, gist_earth, gist_heat,
198
# gist_rainbow, spectral, spring, summer, winter
199
else:
200
self.mappts.color[cr:cr+len(xyzs)] = colour
201
self.currentrow += len(xyzs)
202
203
def setinfostr(self, labeltext):
204
self.infolabel = visual.label()
205
self.infolabel.pos = (-20, 0, 20)
206
# TODO is this a memory leak?
207
self.infolabel.text = str(labeltext)
208
209
# TODO make a circular buffer type array as this is often needed by this
210
# visualiser code
211
def addtrajectorypoint(self, xyz):
212
self.trajpts.pos[self.trajpts_ind, :] = xyz
213
self.trajpts_ind += 1
214
_draw_axes(xyz, size=0.1)
215
if self.trajpts_ind >= len(self.trajpts.pos):
216
self.trajpts_ind = 0
Loggerhead is a web-based interface for Breezy
Version: 2.0.1