create_driver

1 #!/usr/bin/python 2 3 # The MIT License 4 # 5 # Copyright (c) 2007 Damon Kohler 6 # 7 # Permission is hereby granted, free of charge, to any person obtaining a copy 8 # of this software and associated documentation files (the "Software"), to deal 9 # in the Software without restriction, including without limitation the rights 10 # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 # copies of the Software, and to permit persons to whom the Software is 12 # furnished to do so, subject to the following conditions: 13 # 14 # The above copyright notice and this permission notice shall be included in 15 # all copies or substantial portions of the Software. 16 # 17 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 23 # THE SOFTWARE. 24 # 25 # Modified for use in ROS by Ken Conley. API names have been modified 26 # for consistency ROS Python coding guidelines. 27 28 from __future__ import with_statement 29 30 """iRobot Roomba Serial control Interface (SCI) and Turtlebot Open Interface (OI). 31 32 turtlebot.py is a fork of PyRobot. 33 34 PyRobot was originally based on openinterface.py, developed by iRobot 35 Corporation. Many of the docstrings from openinterface.py, particularly those 36 which describe the specification, are also used here. Also, docstrings may 37 contain specification text copied directly from the Roomba SCI Spec Manual and 38 the Turtlebot Open Interface specification. 39 40 Since SCI is a subset of OI, PyRobot first defines the Roomba's functionality 41 in the Roomba class and then extends that with the Turtlebot's additional 42 functionality in the Turtlebot class. In addition, since OI is built on SCI the 43 SerialCommandInterface class is also used for OI. 44 45 """ 46 __author__ = "damonkohler@gmail.com (Damon Kohler)" 47 48 import logging 49 import math 50 import serial 51 import struct 52 import time 53 import threading 54 import traceback 55 import rospy 56 57 ROOMBA_OPCODES = dict( 58 start = 128, 59 baud = 129, 60 control = 130, 61 safe = 131, 62 full = 132, 63 power = 133, 64 spot = 134, 65 clean = 135, 66 max = 136, 67 drive = 137, 68 motors = 138, 69 leds = 139, 70 song = 140, 71 play = 141, 72 sensors = 142, 73 force_seeking_dock = 143, 74 ) 75 76 CREATE_OPCODES = dict( 77 soft_reset = 7, # Where is this documented? 78 low_side_drivers = 138, 79 song = 140, 80 play_song = 141, 81 pwm_low_side_drivers = 144, 82 direct_drive = 145, 83 digital_outputs = 147, 84 stream = 148, 85 query_list = 149, 86 pause_resume_stream = 150, 87 send_ir = 151, 88 script = 152, 89 play_script = 153, 90 show_script = 154, 91 wait_time = 155, 92 wait_distance = 156, 93 wait_angle = 157, 94 wait_event = 158, 95 ) 96 97 REMOTE_OPCODES = { 98 # Remote control. 99 129: 'left', 100 130: 'forward', 101 131: 'right', 102 132: 'spot', 103 133: 'max', 104 134: 'small', 105 135: 'medium', 106 136: 'large', 107 136: 'clean', 108 137: 'pause', 109 138: 'power', 110 139: 'arc-left', 111 140: 'arc-right', 112 141: 'drive-stop', 113 # Scheduling remote. 114 142: 'send-all', 115 143: 'seek-dock', 116 # Home base. 117 240: 'reserved', 118 242: 'force-field', 119 244: 'green-buoy', 120 246: 'green-buoy-and-force-field', 121 248: 'red-buoy', 122 250: 'red-buoy-and-force-field', 123 252: 'red-buoy-and-green-buoy', 124 254: 'red-buoy-and-green-buoy-and-force-field', 125 255: 'none', 126 } 127 128 BAUD_RATES = ( # In bits per second. 129 300, 130 600, 131 1200, 132 2400, 133 4800, 134 9600, 135 14400, 136 19200, 137 28800, 138 38400, 139 57600, # Default. 140 115200) 141 142 CHARGING_STATES = ( 143 'not-charging', 144 'charging-recovery', 145 'charging', 146 'trickle-charging', 147 'waiting', 148 'charging-error') 149 150 OI_MODES = ( 151 'off', 152 'passive', 153 'safe', 154 'full') 155 156 # Various state flag masks 157 WHEEL_DROP_CASTER = 0x10 158 WHEEL_DROP_LEFT = 0x08 159 WHEEL_DROP_RIGHT = 0x04 160 BUMP_LEFT = 0x02 161 BUMP_RIGHT = 0x01 162 163 OVERCURRENTS_DRIVE_LEFT = 0x10 164 OVERCURRENTS_DRIVE_RIGHT = 0x08 165 OVERCURRENTS_MAIN_BRUSH = 0x04 166 OVERCURRENTS_VACUUM = 0x02 167 OVERCURRENTS_SIDE_BRUSH = 0x01 168 169 BUTTON_POWER = 0x08 170 BUTTON_SPOT = 0x04 171 BUTTON_CLEAN = 0x02 172 BUTTON_MAX = 0x01 173 174 SENSOR_GROUP_PACKET_LENGTHS = { 175 0: 26, 176 1: 10, 177 2: 6, 178 3: 10, 179 4: 14, 180 5: 12, 181 6: 52, 182 100: 80 } 183 184 # drive constants. 185 RADIUS_TURN_IN_PLACE_CW = -1 186 RADIUS_TURN_IN_PLACE_CCW = 1 187 RADIUS_STRAIGHT = 32768 188 RADIUS_MAX = 2000 189 190 VELOCITY_MAX = 500 # mm/s 191 VELOCITY_SLOW = int(VELOCITY_MAX * 0.33) 192 VELOCITY_FAST = int(VELOCITY_MAX * 0.66) 193 194 MAX_WHEEL_SPEED = 500 195 WHEEL_SEPARATION = 260 # mm 196 197 SERIAL_TIMEOUT = 2 # Number of seconds to wait for reads. 2 is generous. 198 START_DELAY = 5 # Time it takes the Roomba/Turtlebot to boot. 199 200 assert struct.calcsize('H') == 2, 'Expecting 2-byte shorts.' 201

202 -class DriverError(Exception):

203 pass

204

205 -class SerialCommandInterface(object):

206 207 """A higher-level wrapper around PySerial specifically designed for use with 208 iRobot's SCI. 209 210 """

211 - def __init__(self, tty, baudrate):

212 self.ser = serial.Serial(tty, baudrate=baudrate, timeout=SERIAL_TIMEOUT) 213 self.ser.open() 214 self.wake() 215 self.opcodes = {} 216 217 #TODO: kwc all locking code should be outside of the driver. Instead, 218 #could place a lock object in Roomba and let people "with" it 219 self.lock = threading.RLock()

220

221 - def wake(self):

222 """wake up robot.""" 223 self.ser.setRTS(0) 224 time.sleep(0.1) 225 self.ser.setRTS(1) 226 time.sleep(0.75) # Technically it should wake after 500ms. 227 for i in range(3): 228 self.ser.setRTS(0) 229 time.sleep(0.25) 230 self.ser.setRTS(1) 231 time.sleep(0.25)

232

233 - def add_opcodes(self, opcodes):

234 """Add available opcodes to the SCI.""" 235 self.opcodes.update(opcodes)

236

237 - def send(self, bytes):

238 """send a string of bytes to the robot.""" 239 with self.lock: 240 self.ser.write(struct.pack('B' * len(bytes), *bytes))

241 242 #TODO: kwc the locking should be done at a higher level

243 - def read(self, num_bytes):

244 """Read a string of 'num_bytes' bytes from the robot.""" 245 logging.debug('Attempting to read %d bytes from SCI port.' % num_bytes) 246 with self.lock: 247 data = self.ser.read(num_bytes) 248 logging.debug('Read %d bytes from SCI port.' % len(data)) 249 if not data: 250 raise DriverError('Error reading from SCI port. No data.') 251 if len(data) != num_bytes: 252 raise DriverError('Error reading from SCI port. Wrong data length.') 253 return data

254

255 - def flush_input(self):

256 """Flush input buffer, discarding all its contents.""" 257 logging.debug('Flushing serial input buffer.') 258 self.ser.flushInput()

259

260 - def __getattr__(self, name):

261 """Turtlebots methods for opcodes on the fly. 262 263 Each opcode method sends the opcode optionally followed by a string of 264 bytes. 265 266 """ 267 #TODO: kwc do static initialization instead 268 if name in self.opcodes: 269 def send_opcode(*bytes): 270 logging.debug('sending opcode %s.' % name) 271 self.send([self.opcodes[name]] + list(bytes))

272 return send_opcode 273 raise AttributeError

274 275

276 -class Roomba(object):

277 278 """Represents a Roomba robot.""" 279

280 - def __init__(self):

281 self.tty = None 282 self.sci = None 283 self.safe = True

284

285 - def start(self, tty='/dev/ttyUSB0', baudrate=57600):

286 self.tty = tty 287 self.sci = SerialCommandInterface(tty, baudrate) 288 self.sci.add_opcodes(ROOMBA_OPCODES)

289

290 - def change_baud_rate(self, baud_rate):

291 """Sets the baud rate in bits per second (bps) at which SCI commands and 292 data are sent according to the baud code sent in the data byte. 293 294 The default baud rate at power up is 57600 bps. (See Serial Port Settings, 295 above.) Once the baud rate is changed, it will persist until Roomba is 296 power cycled by removing the battery (or until the battery voltage falls 297 below the minimum required for processor operation). You must wait 100ms 298 after sending this command before sending additional commands at the new 299 baud rate. The SCI must be in passive, safe, or full mode to accept this 300 command. This command puts the SCI in passive mode. 301 302 """ 303 if baud_rate not in BAUD_RATES: 304 raise DriverError('Invalid baud rate specified.') 305 self.sci.baud(baud_rate) 306 self.sci = SerialCommandInterface(self.tty, baud_rate)

307

308 - def passive(self):

309 """Put the robot in passive mode.""" 310 self.sci.start() 311 time.sleep(0.5)

312

313 - def control(self):

314 """Start the robot's SCI interface and place it in safe mode.""" 315 self.passive() 316 if not self.safe: 317 self.sci.full() 318 else: 319 self.sci.safe() 320 # self.sci.control() # Also puts the Roomba in to safe mode. 321 time.sleep(0.5)

322

323 - def direct_drive(self, velocity_left, velocity_right):

324 # Mask integers to 2 bytes. 325 vl = int(velocity_left) & 0xffff 326 vr = int(velocity_right) & 0xffff 327 self.sci.direct_drive(*struct.unpack('4B', struct.pack('>2H', vr, vl)))

328

329 - def drive(self, velocity, radius):

330 """controls Roomba's drive wheels. 331 332 NOTE(damonkohler): The following specification applies to both the Roomba 333 and the Turtlebot. 334 335 The Roomba takes four data bytes, interpreted as two 16-bit signed values 336 using two's complement. The first two bytes specify the average velocity 337 of the drive wheels in millimeters per second (mm/s), with the high byte 338 being sent first. The next two bytes specify the radius in millimeters at 339 which Roomba will turn. The longer radii make Roomba drive straighter, 340 while the shorter radii make Roomba turn more. The radius is measured from 341 the center of the turning circle to the center of Roomba. 342 343 A drive command with a positive velocity and a positive radius makes 344 Roomba drive forward while turning toward the left. A negative radius 345 makes Roomba turn toward the right. Special cases for the radius make 346 Roomba turn in place or drive straight, as specified below. A negative 347 velocity makes Roomba drive backward. 348 349 Also see drive_straight and turn_in_place convenience methods. 350 351 """ 352 # Mask integers to 2 bytes. 353 velocity = int(velocity) & 0xffff 354 radius = int(radius) & 0xffff 355 356 # Pack as shorts to get 2 x 2 byte integers. Unpack as 4 bytes to send. 357 # TODO(damonkohler): The 4 unpacked bytes will just be repacked later, 358 # that seems dumb to me. 359 bytes = struct.unpack('4B', struct.pack('>2H', velocity, radius)) 360 self.sci.drive(*bytes)

361

362 - def stop(self):

363 """Set velocity and radius to 0 to stop movement.""" 364 self.drive(0, 0)

365

366 - def slow_stop(self, velocity):

367 """Slowly reduce the velocity to 0 to stop movement.""" 368 velocities = xrange(velocity, VELOCITY_SLOW, -25) 369 if velocity < 0: 370 velocities = xrange(velocity, -VELOCITY_SLOW, 25) 371 for v in velocities: 372 self.drive(v, RADIUS_STRAIGHT) 373 time.sleep(0.05) 374 self.stop()

375

376 - def drive_straight(self, velocity):

377 """drive in a straight line.""" 378 self.drive(velocity, RADIUS_STRAIGHT)

379

380 - def turn_in_place(self, velocity, direction):

381 """Turn in place either clockwise or counter-clockwise.""" 382 valid_directions = {'cw': RADIUS_TURN_IN_PLACE_CW, 383 'ccw': RADIUS_TURN_IN_PLACE_CCW} 384 self.drive(velocity, valid_directions[direction])

385

386 - def dock(self):

387 """Start looking for the dock and then dock.""" 388 # NOTE(damonkohler): We should be able to call dock from any mode, however 389 # it only seems to work from passive. 390 self.sci.start() 391 time.sleep(0.5) 392 self.sci.force_seeking_dock()

393

394 -class Turtlebot(Roomba):

395 396 """Represents a Turtlebot robot.""" 397

398 - def __init__(self):

399 """ 400 @param sensor_class: Sensor class to use for fetching and decoding sensor data. 401 """ 402 super(Turtlebot, self).__init__()

403

404 - def start(self, tty='/dev/ttyUSB0', baudrate=57600):

405 super(Turtlebot, self).start(tty, baudrate) 406 self.sci.add_opcodes(CREATE_OPCODES)

407

408 - def control(self):

409 """Start the robot's SCI interface and place it in safe or full mode.""" 410 logging.info('sending control opcodes.') 411 self.passive() 412 if self.safe: 413 self.sci.safe() 414 else: 415 self.sci.full() 416 time.sleep(0.5)

417

418 - def power_low_side_drivers(self, drivers):

419 """Enable or disable power to low side drivers. 420 421 'drivers' should be a list of booleans indicating which low side drivers 422 should be powered. 423 424 """ 425 assert len(drivers) == 3, 'Expecting 3 low side driver power settings.' 426 byte = 0 427 for driver, power in enumerate(drivers): 428 byte += (2 ** driver) * int(power) 429 self.sci.low_side_drivers(byte)

430

431 - def set_digital_outputs(self, value):

432 """Enable or disable digital outputs.""" 433 self.sci.digital_outputs(value)

434

435 - def soft_reset(self):

436 """Do a soft reset of the Turtlebot.""" 437 logging.info('sending soft reset.') 438 self.sci.soft_reset() 439 time.sleep(START_DELAY) 440 self.passive()

441

Source Code for Module create_driver