Source Code for Module nxt.server

  1  # nxt.server module -- LEGO Mindstorms NXT socket interface module
 
  2  # Copyright (C) 2009  Marcus Wanner
 
  3  #
 
  4  # This program is free software: you can redistribute it and/or modify
 
  5  # it under the terms of the GNU General Public License as published by
 
  6  # the Free Software Foundation, either version 3 of the License, or
 
  7  # (at your option) any later version.
 
  8  #
 
  9  # This program is distributed in the hope that it will be useful,
 
 10  # but WITHOUT ANY WARRANTY; without even the implied warranty of
 
 11  # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
 12  # GNU General Public License for more details.
 
 13  
 
 14  '''Use for a socket-interface NXT driver. Command and protocol docs at:
 
 15  http://code.google.com/p/nxt-python/wiki/ServerUsage''' 
 16  
 
 17  import nxt.locator 
 18  from nxt.motor import * 
 19  from nxt.sensor import * 
 20  from nxt.compass import * 
 21  import socket, string, sys 
 22  global brick 
 23  
 
 24  host = '' 
 25  port = 54174 
 26  outport = 54374 
 27  
 
 28 -def _process_port(nxtport): 
 29      if nxtport == 'A' or nxtport == 'a': 
 30          nxtport = PORT_A 
 31      elif nxtport == 'B' or nxtport == 'b': 
 32          nxtport = PORT_B 
 33      elif nxtport == 'C' or nxtport == 'c': 
 34          nxtport = PORT_C 
 35      elif nxtport == 'ALL' or nxtport == 'All' or nxtport == 'all': 
 36          nxtport = PORT_ALL 
 37  
 
 38      elif nxtport == '1': 
 39          nxtport = PORT_1 
 40      elif nxtport == '2': 
 41          nxtport = PORT_2 
 42      elif nxtport == '3': 
 43          nxtport = PORT_3 
 44      elif nxtport == '4': 
 45          nxtport = PORT_4 
 46  
 
 47      else: 
 48          raise ValueError, 'Invalid port: '+nxtport 
 49  
 
 50      return nxtport 
 51  
 
 52 -def _process_command(cmd): 
 53      global brick 
 54      retcode = 0 
 55      retmsg = '' 
 56      #act on messages, these conditions can be in no particular order
 
 57      #it should send a return code on port 54374. 0 for success, 1 for failure
 
 58      #then an error message
 
 59  
 
 60      if cmd.startswith('find_brick'): 
 61          try: 
 62              brick = nxt.locator.find_one_brick() 
 63              brick = brick.connect() 
 64              retmsg = 'Connected to brick.' 
 65              retcode = 0 
 66          except: 
 67              retcode = 1 
 68              retmsg = str(sys.exc_info()[1]) 
 69  
 
 70      elif cmd.startswith('get_touch_sample'): 
 71          try: 
 72              port = string.split(cmd, ':')[1] 
 73              port = _process_port(port) 
 74              retmsg = str(TouchSensor(brick, port).get_sample()) 
 75              retcode = 0 
 76          except: 
 77              retcode = 1 
 78              retmsg = str(sys.exc_info()[1]) 
 79  
 
 80      elif cmd.startswith('get_sound_sample'): 
 81          try: 
 82              port = string.split(cmd, ':')[1] 
 83              port = _process_port(port) 
 84              retmsg = str(SoundSensor(brick, port).get_sample()) 
 85              retcode = 0 
 86          except: 
 87              retcode = 1 
 88              retmsg = str(sys.exc_info()[1]) 
 89  
 
 90      elif cmd.startswith('get_light_sample'): 
 91          try: 
 92              data = string.split(cmd, ':')[1] 
 93              data = string.split(data, ',') 
 94              if len(data) > 1: 
 95                  #there is emit light data
 
 96                  port, emit = data 
 97              else: 
 98                  port, emit = data[0], False 
 99              port = _process_port(port) 
100              light = LightSensor(brick, port) 
101              light.set_illuminated(emit) 
102              retmsg = str(light.get_sample()) 
103              light.set_illuminated(False) 
104              retcode = 0 
105          except: 
106              retcode = 1 
107              retmsg = str(sys.exc_info()[1]) 
108  
 
109      elif cmd.startswith('get_ultrasonic_sample'): 
110          try: 
111              port = string.split(cmd, ':')[1] 
112              port = _process_port(port) 
113              retmsg = str(UltrasonicSensor(brick, port).get_sample()) 
114              retcode = 0 
115          except: 
116              retcode = 1 
117              retmsg = str(sys.exc_info()[1]) 
118  
 
119      elif cmd.startswith('get_accelerometer_sample'): 
120          try: 
121              port = string.split(cmd, ':')[1] 
122              port = _process_port(port) 
123              retmsg = str(AccelerometerSensor(brick, port).get_sample()) 
124              retcode = 0 
125          except: 
126              retcode = 1 
127              retmsg = str(sys.exc_info()[1]) 
128  
 
129      elif cmd.startswith('get_compass_sample'): 
130          try: 
131              port = string.split(cmd, ':')[1] 
132              port = _process_port(port) 
133              retmsg = str(CompassSensor(brick, port).get_sample()) 
134              retcode = 0 
135          except: 
136              retcode = 1 
137              retmsg = str(sys.exc_info()[1]) 
138  
 
139      elif cmd.startswith('update_motor:'): 
140          try: 
141              #separate the information from the command keyword
 
142              info = string.split(cmd, ':')[1] 
143              [port, power, tacholim] = string.split(info, ',') 
144              portarray = [] 
145              if port.count('(') > 0 and port.count(')') > 0: 
146                  #there are more than 1 ports, separate them
 
147                  port = port.strip('()') 
148                  #port.strip(')')
 
149                  port.replace(' ', '') 
150                  for separateport in string.split(port, ';'): 
151                      portarray.append(separateport) 
152              else: 
153                  #one port, just use that
 
154                  portarray.append(port) 
155          
 
156              #process the port
 
157              for currentport in portarray: 
158                  processedport = _process_port(currentport) 
159                  Motor(brick, processedport).update(int(power), int(tacholim)) 
160              retmsg = 'Motor command succeded.' 
161              retcode = 0 
162          except: 
163              retcode = 1 
164              retmsg = str(sys.exc_info()[1]) 
165  
 
166      elif cmd.startswith('run_motor:'): 
167          try: 
168              #separate the information from the command keyword
 
169              info = string.split(cmd, ':')[1] 
170              [port, power, regulated] = string.split(info, ',') 
171              port = _process_port(port) 
172              Motor(brick, port).run(int(power), int(regulated)) 
173              retmsg = 'Motor run command succeded.' 
174          except: 
175              retcode = 1 
176              retmsg = str(sys.exc_info()[1]) 
177  
 
178      elif cmd.startswith('stop_motor:'): 
179          try: 
180              #separate the information from the command keyword
 
181              info = string.split(cmd, ':')[1] 
182              [port, braking] = string.split(info, ',') 
183              port = _process_port(port) 
184              Motor(brick, port).stop(int(braking)) 
185              retmsg = 'Motor stop command succeded.' 
186          except: 
187              retcode = 1 
188              retmsg = str(sys.exc_info()[1]) 
189  
 
190      elif cmd.startswith('play_tone:'): 
191          try: 
192              #separate the information from the command keyword
 
193              info = string.split(cmd, ':')[1] 
194              [freq, dur] = string.split(info, ',') 
195  
 
196              #call the function
 
197  
 
198              brick.play_tone_and_wait(int(freq), int(dur)) 
199              retmsg = 'Tone command succeded.' 
200              retcode = 0 
201          except: 
202              retcode = 1 
203              retmsg = str(sys.exc_info()[1]) 
204  
 
205      #close_brick
 
206      elif cmd == 'close_brick': 
207          try: 
208              brick.close() 
209              retcode = 0 
210          except: 
211              retcode = 1 
212              retmsg = str(sys.exc_info()[1]) 
213      
 
214      #command not recognised
 
215      else: 
216          retmsg = 'Command not found.' 
217          retcode = 1 
218      
 
219      #then return 1 or 0 and a message
 
220      return retcode, retmsg 
221  
 
222 -def serve_forever(password=None, authorizedips = []): 
223      '''Serve clients until the window is closed or there is an unhandled error.
 
224  If you supply a password, then any ip that wants to control the NXT will have
 
225  to send the password once to be authorized before any of the commands it sends
 
226  will be carried out.
 
227  authorizedips is a list of the ips that can have access to the NXT without
 
228  supplying a password. Normally, this is left blank.''' 
229      #make sockets
 
230      outsock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 
231      insock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 
232      insock.bind((host, port)) 
233      while 1: 
234          #get a message from port on any host
 
235          inmsg, (clientip,assignedport) = insock.recvfrom(100) #no commands can be longer than 100 chars 
236  
 
237          #print a helpful message to the console.
 
238          print 'Got command '+inmsg+' from '+clientip 
239  
 
240          #process command
 
241          if password: 
242              #password protection enabled
 
243              try: 
244                  authorizedips.index(clientip) 
245                  #ip is authorized, and is therefore in the list of authorized ip
 
246                  code, message = _process_command(inmsg) #process the command as normal 
247              except ValueError: 
248                  #ip not authorized, and therefore cannot be found in the list of authorized ips
 
249                  if inmsg == str(password): 
250                      #command is the correct password
 
251                      authorizedips.append(clientip) 
252                      code = 0 
253                      message = 'Authorization successful.' 
254                  else: 
255                      #command is not the password
 
256                      code = 1 
257                      message = 'NXT access on this server is password protected, please send correct password to be authorized.' 
258  
 
259          else: 
260              #not password protected
 
261              code, message = _process_command(inmsg) 
262          
 
263          #send return code to the computer that send the request
 
264          outsock.sendto(str(code) + message, (clientip, 54374)) 
265  
 
266          #print a summany of the response
 
267          print 'Sent return code '+str(code)+' with message "'+message+'" to '+clientip 
268          print '' 
269          
 
270          #do again
 
271  
 
272  #serve automatically if the script is started
 
273  #by double-clicking or by command line.
 
274  if __name__ == '__main__': 
275      try: 
276          password = sys.argv[1] 
277      except: 
278          password = None 
279      
 
280      serve_forever(password) 
281