turtlebot_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 = (26, 10, 6, 10, 14, 12, 52) 175 176 # drive constants. 177 RADIUS_TURN_IN_PLACE_CW = -1 178 RADIUS_TURN_IN_PLACE_CCW = 1 179 RADIUS_STRAIGHT = 32768 180 RADIUS_MAX = 2000 181 182 VELOCITY_MAX = 500 # mm/s 183 VELOCITY_SLOW = int(VELOCITY_MAX * 0.33) 184 VELOCITY_FAST = int(VELOCITY_MAX * 0.66) 185 186 MAX_WHEEL_SPEED = 500 187 WHEEL_SEPARATION = 260 # mm 188 189 SERIAL_TIMEOUT = 2 # Number of seconds to wait for reads. 2 is generous. 190 START_DELAY = 5 # Time it takes the Roomba/Turtlebot to boot. 191 192 assert struct.calcsize('H') == 2, 'Expecting 2-byte shorts.' 193

194 -class DriverError(Exception):

195 pass

196

197 -class SerialCommandInterface(object):

198 199 """A higher-level wrapper around PySerial specifically designed for use with 200 iRobot's SCI. 201 202 """

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

204 self.ser = serial.Serial(tty, baudrate=baudrate, timeout=SERIAL_TIMEOUT) 205 self.ser.open() 206 self.wake() 207 self.opcodes = {} 208 209 #TODO: kwc all locking code should be outside of the driver. Instead, 210 #could place a lock object in Roomba and let people "with" it 211 self.lock = threading.RLock()

212

213 - def wake(self):

214 """wake up robot.""" 215 self.ser.setRTS(0) 216 time.sleep(0.1) 217 self.ser.setRTS(1) 218 time.sleep(0.75) # Technically it should wake after 500ms. 219 for i in range(3): 220 self.ser.setRTS(0) 221 time.sleep(0.25) 222 self.ser.setRTS(1) 223 time.sleep(0.25)

224

225 - def add_opcodes(self, opcodes):

226 """Add available opcodes to the SCI.""" 227 self.opcodes.update(opcodes)

228

229 - def send(self, bytes):

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

233 234 #TODO: kwc the locking should be done at a higher level

235 - def read(self, num_bytes):

236 """Read a string of 'num_bytes' bytes from the robot.""" 237 logging.debug('Attempting to read %d bytes from SCI port.' % num_bytes) 238 with self.lock: 239 data = self.ser.read(num_bytes) 240 logging.debug('Read %d bytes from SCI port.' % len(data)) 241 if not data: 242 raise DriverError('Error reading from SCI port. No data.') 243 if len(data) != num_bytes: 244 raise DriverError('Error reading from SCI port. Wrong data length.') 245 return data

246

247 - def flush_input(self):

248 """Flush input buffer, discarding all its contents.""" 249 logging.debug('Flushing serial input buffer.') 250 self.ser.flushInput()

251

252 - def __getattr__(self, name):

253 """Turtlebots methods for opcodes on the fly. 254 255 Each opcode method sends the opcode optionally followed by a string of 256 bytes. 257 258 """ 259 #TODO: kwc do static initialization instead 260 if name in self.opcodes: 261 def send_opcode(*bytes): 262 logging.debug('sending opcode %s.' % name) 263 self.send([self.opcodes[name]] + list(bytes))

264 return send_opcode 265 raise AttributeError

266 267

268 -class Roomba(object):

269 270 """Represents a Roomba robot.""" 271

272 - def __init__(self):

273 self.tty = None 274 self.sci = None 275 self.safe = True

276

277 - def start(self, tty='/dev/ttyUSB0'):

278 self.tty = tty 279 self.sci = SerialCommandInterface(tty, 57600) 280 self.sci.add_opcodes(ROOMBA_OPCODES)

281

282 - def change_baud_rate(self, baud_rate):

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

299

300 - def passive(self):

301 """Put the robot in passive mode.""" 302 self.sci.start() 303 time.sleep(0.5)

304

305 - def control(self):

306 """Start the robot's SCI interface and place it in safe mode.""" 307 self.passive() 308 self.sci.control() # Also puts the Roomba in to safe mode. 309 if not self.safe: 310 self.sci.full() 311 else: 312 self.passive() 313 time.sleep(0.5)

314

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

316 # Mask integers to 2 bytes. 317 vl = int(velocity_left) & 0xffff 318 vr = int(velocity_right) & 0xffff 319 self.sci.direct_drive(*struct.unpack('4B', struct.pack('>2H', vr, vl)))

320

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

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

353

354 - def stop(self):

355 """Set velocity and radius to 0 to stop movement.""" 356 self.drive(0, 0)

357

358 - def slow_stop(self, velocity):

359 """Slowly reduce the velocity to 0 to stop movement.""" 360 velocities = xrange(velocity, VELOCITY_SLOW, -25) 361 if velocity < 0: 362 velocities = xrange(velocity, -VELOCITY_SLOW, 25) 363 for v in velocities: 364 self.drive(v, RADIUS_STRAIGHT) 365 time.sleep(0.05) 366 self.stop()

367

368 - def drive_straight(self, velocity):

369 """drive in a straight line.""" 370 self.drive(velocity, RADIUS_STRAIGHT)

371

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

373 """Turn in place either clockwise or counter-clockwise.""" 374 valid_directions = {'cw': RADIUS_TURN_IN_PLACE_CW, 375 'ccw': RADIUS_TURN_IN_PLACE_CCW} 376 self.drive(velocity, valid_directions[direction])

377

378 - def dock(self):

379 """Start looking for the dock and then dock.""" 380 # NOTE(damonkohler): We should be able to call dock from any mode, however 381 # it only seems to work from passive. 382 self.sci.start() 383 time.sleep(0.5) 384 self.sci.force_seeking_dock()

385

386 -class Turtlebot(Roomba):

387 388 """Represents a Turtlebot robot.""" 389

390 - def __init__(self):

391 """ 392 @param sensor_class: Sensor class to use for fetching and decoding sensor data. 393 """ 394 super(Turtlebot, self).__init__()

395

396 - def start(self, tty='/dev/ttyUSB0'):

397 super(Turtlebot, self).start(tty) 398 self.sci.add_opcodes(CREATE_OPCODES)

399

400 - def control(self):

401 """Start the robot's SCI interface and place it in safe or full mode.""" 402 logging.info('sending control opcodes.') 403 self.passive() 404 if self.safe: 405 self.sci.safe() 406 self.passive() 407 else: 408 self.sci.full() 409 time.sleep(0.5)

410

411 - def power_low_side_drivers(self, drivers):

412 """Enable or disable power to low side drivers. 413 414 'drivers' should be a list of booleans indicating which low side drivers 415 should be powered. 416 417 """ 418 assert len(drivers) == 3, 'Expecting 3 low side driver power settings.' 419 byte = 0 420 for driver, power in enumerate(drivers): 421 byte += (2 ** driver) * int(power) 422 self.sci.low_side_drivers(byte)

423

424 - def set_digital_outputs(self, value):

425 """Enable or disable digital outputs.""" 426 self.sci.digital_outputs(value)

427

428 - def soft_reset(self):

429 """Do a soft reset of the Turtlebot.""" 430 logging.info('sending soft reset.') 431 self.sci.soft_reset() 432 time.sleep(START_DELAY) 433 self.passive()

434

Source Code for Module turtlebot_driver