~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/robotvisualiser.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-20 22:12:16 UTC
  • mfrom: (0.5.3 mrol)
  • Revision ID: vikasdhi@buffalo.edu-20121120221216-nunda3pv0n8jsf4e
MergingĀ lp:~wecacuee/mrol/mrol-devĀ 

Show diffs side-by-side

added added

removed removed

Lines of Context:
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