Source Code for Module neato_driver

  1  #!/usr/bin/env python 
  2   
  3  # Generic driver for the Neato XV-11 Robot Vacuum 
  4  # Copyright (c) 2010 University at Albany. All right reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions are met: 
  8  #     * Redistributions of source code must retain the above copyright 
  9  #       notice, this list of conditions and the following disclaimer. 
 10  #     * Redistributions in binary form must reproduce the above copyright 
 11  #       notice, this list of conditions and the following disclaimer in the 
 12  #       documentation and/or other materials provided with the distribution. 
 13  #     * Neither the name of the University at Albany nor the names of its 
 14  #       contributors may be used to endorse or promote products derived 
 15  #       from this software without specific prior written permission. 
 16  # 
 17  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
 18  # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
 19  # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 20  # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 21  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 22  # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
 23  # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
 24  # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
 25  # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 
 26  # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 27   
 28  """ 
 29  neato_driver.py is a generic driver for the Neato XV-11 Robotic Vacuum. 
 30  ROS Bindings can be found in the neato_node package. 
 31  """ 
 32   
 33  __author__ = "ferguson@cs.albany.edu (Michael Ferguson)" 
 34   
 35  import serial 
 36   
 37  BASE_WIDTH = 248    # millimeters 
 38  MAX_SPEED = 300     # millimeters/second 
 39   
 40  xv11_analog_sensors = [ "WallSensorInMM", 
 41                  "BatteryVoltageInmV", 
 42                  "LeftDropInMM", 
 43                  "RightDropInMM", 
 44                  "RightMagSensor", 
 45                  "LeftMagSensor", 
 46                  "XTemp0InC", 
 47                  "XTemp1InC", 
 48                  "VacuumCurrentInmA", 
 49                  "ChargeVoltInmV", 
 50                  "NotConnected1", 
 51                  "BatteryTemp1InC", 
 52                  "NotConnected2", 
 53                  "CurrentInmA", 
 54                  "NotConnected3", 
 55                  "BatteryTemp0InC" ] 
 56   
 57  xv11_digital_sensors = [ "SNSR_DC_JACK_CONNECT", 
 58                  "SNSR_DUSTBIN_IS_IN", 
 59                  "SNSR_LEFT_WHEEL_EXTENDED", 
 60                  "SNSR_RIGHT_WHEEL_EXTENDED", 
 61                  "LSIDEBIT", 
 62                  "LFRONTBIT", 
 63                  "RSIDEBIT", 
 64                  "RFRONTBIT" ] 
 65   
 66  xv11_motor_info = [ "Brush_MaxPWM", 
 67                  "Brush_PWM", 
 68                  "Brush_mVolts", 
 69                  "Brush_Encoder", 
 70                  "Brush_RPM", 
 71                  "Vacuum_MaxPWM", 
 72                  "Vacuum_PWM", 
 73                  "Vacuum_CurrentInMA", 
 74                  "Vacuum_Encoder", 
 75                  "Vacuum_RPM", 
 76                  "LeftWheel_MaxPWM", 
 77                  "LeftWheel_PWM", 
 78                  "LeftWheel_mVolts", 
 79                  "LeftWheel_Encoder", 
 80                  "LeftWheel_PositionInMM", 
 81                  "LeftWheel_RPM", 
 82                  "RightWheel_MaxPWM", 
 83                  "RightWheel_PWM", 
 84                  "RightWheel_mVolts", 
 85                  "RightWheel_Encoder", 
 86                  "RightWheel_PositionInMM", 
 87                  "RightWheel_RPM", 
 88                  "Laser_MaxPWM", 
 89                  "Laser_PWM", 
 90                  "Laser_mVolts", 
 91                  "Laser_Encoder", 
 92                  "Laser_RPM", 
 93                  "Charger_MaxPWM", 
 94                  "Charger_PWM", 
 95                  "Charger_mAH" ] 
 96   
 97  xv11_charger_info = [ "FuelPercent", 
 98                  "BatteryOverTemp", 
 99                  "ChargingActive", 
100                  "ChargingEnabled", 
101                  "ConfidentOnFuel", 
102                  "OnReservedFuel", 
103                  "EmptyFuel", 
104                  "BatteryFailure", 
105                  "ExtPwrPresent", 
106                  "ThermistorPresent[0]", 
107                  "ThermistorPresent[1]", 
108                  "BattTempCAvg[0]", 
109                  "BattTempCAvg[1]", 
110                  "VBattV", 
111                  "VExtV", 
112                  "Charger_mAH", 
113                  "MaxPWM" ] 
114   
115 -class xv11(): 
116   
117 -    def __init__(self, port="/dev/ttyUSB0"): 
118          self.port = serial.Serial(port,115200) 
119          # Storage for motor and sensor information 
120          self.state = {"LeftWheel_PositionInMM": 0, "RightWheel_PositionInMM": 0} 
121          self.stop_state = True 
122          # turn things on 
123          self.setTestMode("on") 
124          self.setLDS("on") 
125   
126 -    def exit(self): 
127          self.setLDS("off") 
128          self.setTestMode("off") 
129   
130 -    def setTestMode(self, value): 
131          """ Turn test mode on/off. """ 
132          self.port.write("testmode " + value + "\n") 
133   
134 -    def setLDS(self, value): 
135          self.port.write("setldsrotation " + value + "\n") 
136   
137 -    def requestScan(self): 
138          """ Ask neato for an array of scan reads. """ 
139          self.port.flushInput() 
140          self.port.write("getldsscan\n") 
141   
142 -    def getScanRanges(self): 
143          """ Read values of a scan -- call requestScan first! """ 
144          ranges = list() 
145          angle = 0 
146          try: 
147              line = self.port.readline() 
148          except: 
149              return [] 
150          while line.split(",")[0] != "AngleInDegrees": 
151              try: 
152                  line = self.port.readline() 
153              except: 
154                  return [] 
155          while angle < 360: 
156              try: 
157                  vals = self.port.readline() 
158              except: 
159                  pass 
160              vals = vals.split(",") 
161              #print angle, vals 
162              try: 
163                  a = int(vals[0]) 
164                  r = int(vals[1]) 
165                  ranges.append(r/1000.0) 
166              except: 
167                  ranges.append(0) 
168              angle += 1 
169          return ranges 
170   
171 -    def setMotors(self, l, r, s): 
172          """ Set motors, distance left & right + speed """ 
173          #This is a work-around for a bug in the Neato API. The bug is that the 
174          #robot won't stop instantly if a 0-velocity command is sent - the robot 
175          #could continue moving for up to a second. To work around this bug, the 
176          #first time a 0-velocity is sent in, a velocity of 1,1,1 is sent. Then,  
177          #the zero is sent. This effectively causes the robot to stop instantly. 
178          if (int(l) == 0 and int(r) == 0 and int(s) == 0): 
179              if (not self.stop_state): 
180                  self.stop_state = True 
181              l = 1 
182              r = 1 
183              s = 1 
184          else: 
185              self.stop_state = False 
186   
187          self.port.write("setmotor "+str(int(l))+" "+str(int(r))+" "+str(int(s))+"\n") 
188   
189 -    def getMotors(self): 
190          """ Update values for motors in the self.state dictionary. 
191              Returns current left, right encoder values. """ 
192          self.port.flushInput() 
193          self.port.write("getmotors\n") 
194          line = self.port.readline() 
195          while line.split(",")[0] != "Parameter": 
196              try: 
197                  line = self.port.readline() 
198              except: 
199                  return [0,0] 
200          for i in range(len(xv11_motor_info)): 
201              try: 
202                  values = self.port.readline().split(",") 
203                  self.state[values[0]] = int(values[1]) 
204              except: 
205                  pass 
206          return [self.state["LeftWheel_PositionInMM"],self.state["RightWheel_PositionInMM"]] 
207   
208 -    def getAnalogSensors(self): 
209          """ Update values for analog sensors in the self.state dictionary. """ 
210          self.port.write("getanalogsensors\n") 
211          line = self.port.readline() 
212          while line.split(",")[0] != "SensorName": 
213              try: 
214                  line = self.port.readline() 
215              except: 
216                  return 
217          for i in range(len(xv11_analog_sensors)): 
218              try: 
219                  values = self.port.readline().split(",") 
220                  self.state[values[0]] = int(values[1]) 
221              except: 
222                  pass 
223   
224 -    def getDigitalSensors(self): 
225          """ Update values for digital sensors in the self.state dictionary. """ 
226          self.port.write("getdigitalsensors\n") 
227          line = self.port.readline() 
228          while line.split(",")[0] != "Digital Sensor Name": 
229              try: 
230                  line = self.port.readline() 
231              except: 
232                  return 
233          for i in range(len(xv11_digital_sensors)): 
234              try: 
235                  values = self.port.readline().split(",") 
236                  self.state[values[0]] = int(values[1]) 
237              except: 
238                  pass 
239   
240 -    def getCharger(self): 
241          """ Update values for charger/battery related info in self.state dictionary. """ 
242          self.port.write("getcharger\n") 
243          line = self.port.readline() 
244          while line.split(",")[0] != "Label": 
245              line = self.port.readline() 
246          for i in range(len(xv11_charger_info)): 
247              values = self.port.readline().split(",") 
248              try: 
249                  self.state[values[0]] = int(values[1]) 
250              except: 
251                  pass 
252   
253 -    def setBacklight(self, value): 
254          if value > 0: 
255              self.port.write("setled backlighton") 
256          else: 
257              self.port.write("setled backlightoff") 
258   
259      #SetLED - Sets the specified LED to on,off,blink, or dim. (TestMode Only) 
260      #BacklightOn - LCD Backlight On  (mutually exclusive of BacklightOff) 
261      #BacklightOff - LCD Backlight Off (mutually exclusive of BacklightOn) 
262      #ButtonAmber - Start Button Amber (mutually exclusive of other Button options) 
263      #ButtonGreen - Start Button Green (mutually exclusive of other Button options) 
264      #LEDRed - Start Red LED (mutually exclusive of other Button options) 
265      #LEDGreen - Start Green LED (mutually exclusive of other Button options) 
266      #ButtonAmberDim - Start Button Amber Dim (mutually exclusive of other Button options) 
267      #ButtonGreenDim - Start Button Green Dim (mutually exclusive of other Button options) 
268      #ButtonOff - Start Button Off 
269