def executeRotateAndDrive(commandPayload):
maxVelocity = int(commandPayload[0])
pivotTurnSpeed = int(commandPayload[1])
optionByte1 = int(commandPayload[2])
optionByte2 = int(commandPayload[3])
piOptionByte1 = int(commandPayload[4])
rotationAngle = int(commandPayload[5])
distance = int(commandPayload[6])
waypoints = []
if globals.currentCommandPhase == 0:
globals.currentCommand = enums.PI_CMD_ROTATE_AND_DRIVE
globals.currentCommandPhase = 1
globals.currentCommandPayload = commandPayload
waypoint = (rotationAngle, 0)
waypoints.append(waypoint)
msg = ('R&D P1: Rotating [pivotSpeed: %d, opt1: %2X, opt2: %2X]' %
(pivotTurnSpeed, optionByte1, optionByte2))
elif globals.currentCommandPhase == 1:
globals.currentCommand = 0
globals.currentCommandPayload = 0
if distance:
targetX = globals.currentX + (distance *
math.cos(globals.currentHeading))
targetY = globals.currentY + (distance *
math.sin(globals.currentHeading))
waypoint = (int(targetX), int(targetY))
waypoints.append(waypoint)
optionByte2 = 0 # this carries the rotate command, need to switch back to waypoint transit
msg = (
'R&D P2: Driving %d cm from (%d,%d) to (%d,%d) [maxVelocity: %d]'
% (distance, globals.currentX, globals.currentY, targetX,
targetY, maxVelocity))
else:
return (True, '') # no drive phase
else:
globals.currentCommand = 0
globals.currentCommandPhase = 0
return (False, 'Unknown phase')
print(msg)
udp.logToBotlab(msg, False)
transmitSegments = i2c.buildTransmitSegments(waypoints, maxVelocity,
pivotTurnSpeed, optionByte1,
optionByte2)
i2c.registerTransmitSegments(transmitSegments)
return (True, '')
def executeSetBotOption(commandPayload):
maxVelocity = int(commandPayload[0])
pivotTurnSpeed = int(commandPayload[1])
optionByte1 = int(commandPayload[2])
optionByte2 = int(commandPayload[3])
piOptionByte1 = int(commandPayload[4])
packedOptions = commandPayload[5:]
waypoints = []
if len(packedOptions) % 2 != 0:
return (False, 'Received invalid packed options from botLab')
packedOptionCount = int(len(packedOptions) / 2)
print('Received %d packed options [%s]' %
(packedOptionCount, packedOptions))
for i in range(packedOptionCount):
packedOption = (int(packedOptions[i * 2]),
int(packedOptions[(i * 2) + 1]))
waypoints.append(packedOption)
msg = ''
if optionByte1 == enums.BOT_OPTION_CALIBRATE:
msg = ('Executing bot calibration [pivotSpeed: %d]' % (pivotTurnSpeed))
elif optionByte1 == enums.BOT_OPTION_RESET_TO_ORIGIN:
msg = 'Reset to origin'
globals.currentX = 0
globals.currentY = 0
globals.currentHeading = 0
elif optionByte1 == enums.BOT_OPTION_SET_PID_PARAMETERS:
msg = 'Updated PID parameters'
else:
return (False, 'Unknown bot option')
print(msg)
udp.logToBotlab(msg, False)
transmitSegments = i2c.buildTransmitSegments(waypoints, maxVelocity,
pivotTurnSpeed, optionByte1,
optionByte2)
i2c.registerTransmitSegments(transmitSegments)
return (True, '')
def executeTransit(commandPayload):
maxVelocity = int(commandPayload[0])
pivotTurnSpeed = int(commandPayload[1])
optionByte1 = int(commandPayload[2])
optionByte2 = int(commandPayload[3])
piOptionByte1 = int(commandPayload[4])
globals.followMeWaypoints = []
globals.followMeMaxVelocity = maxVelocity
globals.followMePivotTurnSpeed = pivotTurnSpeed
points = commandPayload[5:]
waypoints = []
if len(points) % 2 != 0:
return (False, 'Received invalid waypoint instructions from botLab')
waypointCount = int(len(points) / 2)
print('Received %d waypoints [%s]' % (waypointCount, points))
for i in range(waypointCount):
waypoint = (int(points[i * 2]), int(points[(i * 2) + 1]))
waypoints.append(waypoint)
if piOptionByte1 & enums.PI_OPTION_FOLLOW_ME_MODE:
globals.followMe = True
else:
globals.followMe = False
msg = (
'Transit between %d waypoints [maxVelocity: %d, pivotSpeed: %d, opt1: %2X, opt2: %2X, piOpt1: %2X]'
% (waypointCount, maxVelocity, pivotTurnSpeed, optionByte1,
optionByte2, piOptionByte1))
print(msg)
udp.logToBotlab(msg, False)
globals.currentCommand = enums.PI_CMD_TRANSIT_WAYPOINTS
transmitSegments = i2c.buildTransmitSegments(waypoints, maxVelocity,
pivotTurnSpeed, optionByte1,
optionByte2)
i2c.registerTransmitSegments(transmitSegments)
return (True, '')
def detectRedBox(currentX, currentY, currentHeading, uploadSnapshots):
global lowerColourRange
global upperColourRange
global cameraDimX
if lowerColourRange == None or upperColourRange == None:
msg = 'Camera not calibrated'
print(msg)
udp.logToBotlab(msg, False)
return (False, 0, 0)
camera = getCamera()
timestamp = time.time()
imageName = '%s/%u.jpg' % (config.imagePath, timestamp)
camera.capture(imageName, resize=(320, 240))
bgrImg = cv2.imread(imageName)
hsvImg = cv2.cvtColor(bgrImg, cv2.COLOR_BGR2HSV)
(foundObject, x, y, width, height,
aspectRatio) = detectTrackedObject(hsvImg, bgrImg, lowerColourRange,
upperColourRange)
if uploadSnapshots:
labelledImageName = '%s/%u-labelled.jpg' % (config.imagePath,
timestamp)
cv2.imwrite(labelledImageName, bgrImg)
uploadSnapshot(labelledImageName)
(targetX, targetY) = (0, 0)
if foundObject:
(targetX,
targetY) = calculatePositionOfTrackedObject(currentX, currentY,
currentHeading, x, y,
width, cameraDimX)
return (foundObject, targetX, targetY)
def calibrateCameraUntilObjectFound(acceptableHueRange, uploadSnapshots):
global lowerColourRange
global upperColourRange
global cameraDimX
if acceptableHueRange < 0:
acceptableHueRange = 40
camera = getCamera()
foundObject = False
while not foundObject:
timestamp = time.time()
imageName = '%s/%u.jpg' % (config.imagePath, timestamp)
camera.capture(imageName, resize=(320, 240))
bgrImg = cv2.imread(imageName)
hsvImg = cv2.cvtColor(bgrImg, cv2.COLOR_BGR2HSV)
(cameraDimX, cameraDimY, averageHue, lowerColourRange,
upperColourRange) = calibrateCamera(hsvImg, acceptableHueRange)
(foundObject, x, y, width, height,
aspectRatio) = detectTrackedObject(hsvImg, bgrImg, lowerColourRange,
upperColourRange)
if foundObject:
if uploadSnapshots:
labelledImageName = '%s/%u-labelled.jpg' % (config.imagePath,
timestamp)
cv2.imwrite(labelledImageName, bgrImg)
uploadSnapshot(labelledImageName)
msg = 'Calibrated with object of aspect ratio %.2f and average hue %.2f (scene width: %dpx)' % (
aspectRatio, averageHue, cameraDimX)
print(msg)
udp.logToBotlab(msg, False)
else:
time.sleep(1)
def botLabCallback(data):
dataString = data.decode('ascii')
payload = dataString.split()
if len(payload) < 7: # name + command + maxVelocity + pivotTurnSpeed + 2x bot option bytes + 1x Pi option byte
print('Received invalid command payload: [%s]' % dataString)
return
if payload[0] == config.name:
print('Ignoring broadcast command from myself')
return
command = int(payload[1])
globals.currentCommandPhase = 0
success = False
errMsg = ''
if command == enums.PI_CMD_TRANSIT_WAYPOINTS:
(success, errMsg) = commands.executeTransit(payload[2:])
elif command == enums.PI_CMD_SET_BOT_OPTION:
(success, errMsg) = commands.executeSetBotOption(payload[2:])
elif command == enums.PI_CMD_ROTATE_AND_DRIVE:
(success, errMsg) = commands.executeRotateAndDrive(payload[2:])
elif command == enums.PI_CMD_FIND_OBJECT:
(success, errMsg) = commands.executeFindObject(payload[2:])
elif command == enums.PI_CMD_CALIBRATE_CAMERA:
(success, errMsg) = commands.executeCalibrateCamera(payload[2:])
elif command == enums.PI_CMD_REPORT_STATUS:
(success, errMsg) = commands.executeReportStatus(payload[2:])
else:
errMsg = 'Unknown command'
if not success:
print('Failed to execute command [%2X]: %s (payload: %s)' % (command, errMsg, dataString))
msg = 'Failed to execute cmd [%2X]: %s' % (command, errMsg)
udp.logToBotlab(msg, False)
return 0
def i2cInterrupt(id, tick):
"""Handle an I2C interrupt to either send or receive data from the bot.
"""
global localPi
global slaveAddr
global transmitSegments
global segmentsSent
s, b, d = localPi.bsc_i2c(slaveAddr)
if b:
if d[0] == ord(
'p') and len(transmitSegments) != 0 and segmentsSent < len(
transmitSegments):
"""CMD: PING: Send the next waypoint I2C segment."""
s, b, d = localPi.bsc_i2c(slaveAddr,
transmitSegments[segmentsSent])
segmentsSent += 1
elif d[0] == ord('s') and b == 3:
"""CMD: REPORT STATUS: """
mv = struct.unpack_from('>H', d, 1)[0]
msg = 'Battery Level: %d mV' % mv
print(msg)
udp.sendPong('255.255.255.255', config.udpBotLabPort, mv)
elif d[0] == ord('r'):
"""CMD: REPORT SNAPSHOT: Receive the next pose snapshot report."""
if b == 14:
x = struct.unpack_from('>h', d, 2)[0]
y = struct.unpack_from('>h', d, 4)[0]
headingPacked = struct.unpack_from('B', d, 6)[0]
headingFloat = struct.unpack_from('B', d, 7)[0]
distanceToObstacle = struct.unpack_from('>H', d, 8)[0]
timestamp = struct.unpack_from('>H', d, 10)[0]
detailByte = struct.unpack_from('>B', d, 12)[0]
if headingPacked & 0x80:
heading = 0
heading -= (headingFloat / 100.0)
heading -= (headingPacked & 0x7F)
else:
heading = (headingPacked & 0x7F) + (headingFloat / 100.0)
# If ranging was disabled the distance to obstacle reported will always be 0
if distanceToObstacle:
obstacleX = x + (distanceToObstacle * math.cos(heading))
obstacleY = y + (distanceToObstacle * math.sin(heading))
else:
obstacleX = 0 # ensure that botlab doesn't draw any obstacles, we weren't measuring them
obstacleY = 0 # ensure that botlab doesn't draw any obstacles, we weren't measuring them
print('%lu\t%d\t%d\t%f\t%d\t%d\t%d\t%u' %
(timestamp, x, y, heading, distanceToObstacle, obstacleX,
obstacleY, detailByte))
msg = ('%lu\t%d\t%d\t%.2f\t%d\t%d\t%u' %
(timestamp, x, y, heading, obstacleX, obstacleY,
detailByte))
udp.logToBotlab(msg, True)
globals.currentX = x
globals.currentY = y
globals.currentHeading = heading
if globals.currentCommand == enums.PI_CMD_TRANSIT_WAYPOINTS:
# If this is the last snapshot of a waypoint and it wasn't interrupted by an obstacle, it's a good one to follow
if (detailByte &
POSE_SNAPSHOT_DETAILBYTE_LAST_SNAPSHOT_FOR_WAYPOINT
) and not (detailByte &
POSE_SNAPSHOT_DETAILBYTE_ABORTED_WAYPOINT):
print(' above snapshot is follow me candidate')
waypoint = (x, y)
globals.followMeWaypoints.append(waypoint)
# If the last snapshot of the last waypoint and follow me mode is on, send all good waypoints
if (detailByte &
POSE_SNAPSHOT_DETAILBYTE_LAST_SNAPSHOT_FOR_JOURNEY
) and globals.followMe:
# TODO: propagate optionByte1
udp.sendFollowMeCommand(globals.followMeWaypoints,
globals.followMeMaxVelocity,
globals.followMePivotTurnSpeed,
0)
elif globals.currentCommand == enums.PI_CMD_ROTATE_AND_DRIVE:
if detailByte & POSE_SNAPSHOT_DETAILBYTE_LAST_SNAPSHOT_FOR_WAYPOINT:
commands.executeRotateAndDrive(
globals.currentCommandPayload)
elif globals.currentCommand == enums.PI_CMD_FIND_OBJECT:
if detailByte & POSE_SNAPSHOT_DETAILBYTE_LAST_SNAPSHOT_FOR_WAYPOINT:
commands.executeFindObject(
globals.currentCommandPayload)
def executeFindObject(commandPayload):
maxVelocity = int(commandPayload[0])
pivotTurnSpeed = int(commandPayload[1])
optionByte1 = int(commandPayload[2])
optionByte2 = int(commandPayload[3])
piOptionByte1 = int(commandPayload[4])
rotationAngle = int(commandPayload[5])
rotationIterations = int(commandPayload[6])
objectName = commandPayload[7]
distance = int(commandPayload[8])
if not config.supportsCamera:
return ('False', 'Camera not supported')
if globals.currentCommandPhase >= rotationIterations:
globals.currentCommand = 0
globals.currentCommandPhase = 0
return (False, 'Object not found')
msg = ('Looking for [%s] (attempt %d of %d)') % (
objectName, globals.currentCommandPhase + 1, rotationIterations)
print(msg)
udp.logToBotlab(msg, False)
foundObject = False
(targetX, targetY) = (0, 0)
labels = ''
# Look for the object, there may be no need to rotate
if objectName == 'redbox':
(foundObject, targetX, targetY) = imgrecognition.detectRedBox(
globals.currentX, globals.currentY, globals.currentHeading,
piOptionByte1 & enums.PI_OPTION_UPLOAD_SNAPSHOTS)
else:
(foundObject, labels) = imgrecognition.detectNamedObjectInSnapshot(
objectName, piOptionByte1 & enums.PI_OPTION_UPLOAD_SNAPSHOTS)
if foundObject and distance:
# We've been commanded to travel this far toward the object we've found
targetX = globals.currentX + (distance *
math.cos(globals.currentHeading))
targetY = globals.currentY + (distance *
math.sin(globals.currentHeading))
if foundObject:
globals.currentCommand = 0
globals.currentCommandPhase = 0
if objectName == 'redbox':
msg = 'Found [redbox] at (%d, %d)' % (targetX, targetY)
else:
msg = 'Found [%s]' % objectName
if distance:
waypoints = []
waypoint = (int(targetX), int(targetY))
waypoints.append(waypoint)
if piOptionByte1 & enums.PI_OPTION_FOLLOW_ME_MODE:
# Send another bot there, not me
msg = (
'Found [%s], directing others to (%d,%d) [maxVelocity: %d, pivotSpeed: %d, optionByte1: %d]'
% (objectName, targetX, targetY, maxVelocity,
pivotTurnSpeed, optionByte1))
udp.sendFollowMeCommand(waypoints, maxVelocity, pivotTurnSpeed,
optionByte1)
else:
msg = (
'Found [%s], driving %d cm from (%d,%d) to (%d,%d) [maxVelocity: %d]'
% (objectName, distance, globals.currentX,
globals.currentY, targetX, targetY, maxVelocity))
transmitSegments = i2c.buildTransmitSegments(
waypoints, maxVelocity, pivotTurnSpeed, optionByte1,
optionByte2)
i2c.registerTransmitSegments(transmitSegments)
print(msg)
udp.logToBotlab(msg, False)
return (True, '')
else:
msg = 'Saw: %s' % labels
udp.logToBotlab(msg, False)
globals.currentCommand = enums.PI_CMD_FIND_OBJECT
globals.currentCommandPayload = commandPayload
globals.currentCommandPhase += 1
waypoints = []
waypoint = (rotationAngle, 0)
waypoints.append(waypoint)
transmitSegments = i2c.buildTransmitSegments(waypoints, maxVelocity,
pivotTurnSpeed, optionByte1,
enums.BOT_OPTION2_ROTATE)
i2c.registerTransmitSegments(transmitSegments)
return (True, '')