class Pioneer:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.portName = settings.SERIAL_PORT_NAME
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0])
self.oldsonars = SharedVar(8 * [0]) #hz
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.oldsonars.get, #hz
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("Pioneer variables set up", 2)
self.serialready = False
try:
self.open(Serial)
debug("Serial Connection started", 2)
except:
debug("Could not open the serial port", 0)
raise CancelGUIAction("Error opening serial port")
self.cmdEnable(1)
debug("Robot cmdEnabled", 2)
app.soar.addFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if (self.serialready):
self.cmdEnable(0)
self.stopmoving()
self.sendPacket([ArcosCmd.CLOSE])
# self.flushSerialPort()
self.port.close()
app.soar.removeFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
if self.serialready:
self.cmdEnable(0)
self.stopmoving()
self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
from soar.serial import Serial
self.open(Serial)
self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
# Open connection to robot
def open(self, Serial):
#baudRates = [9600,38400,19200,115200,57600]
baudRates = [115200, 9600] #,38400,19200,57600]
curBaudRate = 0
self.port = Serial(self.portName, baudRates[0])
self.port._timeout = 1.0
self.port._writeTimout = 1.0
self.port.flushInput()
self.port.flushOutput()
numAttempts = 3
timeout = 5.0
startt = time()
while (self.connectionState != STATE_READY):
if (self.connectionState == STATE_NO_SYNC):
self.sendPacket([CMD_SYNC0])
elif (self.connectionState == STATE_FIRST_SYNC):
self.sendPacket([CMD_SYNC1])
elif (self.connectionState == STATE_SECOND_SYNC):
self.sendPacket([CMD_SYNC2])
elif (self.connectionState == STATE_READY):
pass
try:
pkt = self.recvPacket()
except:
if (self.connectionState == STATE_NO_SYNC
and numAttempts >= 0):
numAttempts -= 1
else:
curBaudRate += 1
if (curBaudRate < len(baudRates)):
self.port.close()
self.port = Serial(self.portName,
baudRates[curBaudRate])
debug(
"Changing to baud rate: " +
` baudRates[curBaudRate] `, 1)
self.port.flushInput()
self.port.flushOutput()
else:
self.port.close()
sys.stderr.write(
"Could not open serial port. Is robot turned on and connected?\n"
)
raise Exception("No Robot Found")
continue
if (pkt[3] == CMD_SYNC0):
self.connectionState = STATE_FIRST_SYNC
elif (pkt[3] == CMD_SYNC1):
self.connectionState = STATE_SECOND_SYNC
elif (pkt[3] == CMD_SYNC2):
self.connectionState = STATE_READY
if (time() - startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
botName = ""
botType = ""
botSubType = ""
i = 4
while (pkt[i]):
botName += chr(pkt[i])
i += 1
i += 1
while (pkt[i]):
botType += chr(pkt[i])
i += 1
i += 1
while (pkt[i]):
botSubType += chr(pkt[i])
i += 1
self.sendPacket([ArcosCmd.OPEN])
debug(
"P2OS Interface Ready - connected to " + ` botName ` + " " +
` botType ` + " " + ` botSubType `, 1)
print "P2OS Interface Ready - connected to " + ` botType ` + ` botName `
changed = [0, 0, 0, 0, 0, 0, 0, 0]
while 0 in changed:
self.cmdPulse()
self.sipRecv()
self.storedsonars.get()
changed = [
changed[i] or self.sonarsChanged[i]
for i in range(len(changed))
]
self.serialready = True
# turn on io packets
self.sendPacket([ArcosCmd.IOREQUEST, ArgType.ARGINT, 2, 0])
pkt = self.recvPacket()
print 'denny: battery = {0:d}'.format(pkt[14])
self.sendPacket([ArcosCmd.IOREQUEST, ArgType.ARGINT, 2, 0])
def getPose(self):
return self.odpose.get()
#def initGlobals(self):
# self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write(
"Trying to set maximum translational velocity too high for real robot\n"
)
if maxRotVel > 1.75:
sys.stderr.write(
"Trying to set maximum rotational velocity too high for real robot\n"
)
def enableTeleportation(self, prob, pose):
sys.stderr.write(
"Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if (not self.asynchronous):
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v, w)
sleep(dt)
self.motorOutput(0, 0)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get() * 1000.0))
self.cmdRvel(int(self.rot.get() * 180 / pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [
ArcosCmd.DIGOUT,
ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get() * 25.5))) & 0xFF),
0xFF
]
self.sipSend(data)
# update sonars, odometry and stalled
def update(self, dummyparameter=0):
if (self.serialready):
self.sendMotorCmd()
self.sendDigitalOutCmd()
# if we receive no packets for a few cycles, we've lost touch with the robot
if (self.sipRecv() > 0):
self.zero_recv_cnt = 0
else:
self.zero_recv_cnt += 1
if (self.zero_recv_cnt > 4):
self.serialready = False
app.soar.closePioneer()
sys.stderr.write(
"Robot turned off or no longer connected.\n Dead reckoning is no longer valid.\n"
)
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self, data):
c = 0
i = 3
n = data[2] - 2
while (n > 1):
c += (data[i] << 8) | (data[i + 1])
c = c & 0xFFFF
n -= 2
i += 2
if (n > 0):
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if (self.asynchronous):
pass
# self.stopSerialThread()
else:
self.sendMotorCmd()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self, data):
pkt = [0xFA, 0xFB, len(data) + 2]
for d in data:
pkt.append(d)
pkt.append(0)
pkt.append(0)
chk = self.calcChecksum(pkt)
pkt[len(pkt) - 2] = (chk >> 8)
pkt[len(pkt) - 1] = (chk & 0xFF)
s = reduce(lambda x, y: x + chr(y), pkt, "")
try:
self.port.write(s)
except:
sys.stderr.write(
"Could not write to serial port. Is robot turned on and connected?\n"
)
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0, 0, 0]
while (1):
cnt = 0
tstart = time()
while (time() - tstart) < timeout:
if (self.port.inWaiting() > 0):
data[2] = ord(self.port.read())
break
if (time() - tstart) > timeout:
raise Exception("Read timeout")
if (data[0] == 0xFA and data[1] == 0xFB):
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2]):
data.append(ord(self.port.read()))
crc = self.calcChecksum(data)
if data[len(data) - 1] != (crc & 0xFF) or data[len(data) -
2] != (crc >> 8):
self.port.flushInput()
raise Exception("Checksum failure")
# if self.port.inWaiting() > 0:
# self.port.flushInput()
return data
# Send a packet
def sipSend(self, data):
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
while (self.port.inWaiting() > 0):
try:
recv = self.recvPacket()
except:
break
iters += 1
# 0x3s means sip packet
if (recv[3] & 0xF0) == 0x30:
self.parseSip(recv)
# 0xF0 means io packet
elif recv[3] == 0xF0:
self.parseIO(recv)
return iters
def parseIO(self, recv):
analogInputs = [0, 0, 0, 0, 0, 0, 0, 0]
bufferidx = 12
for aninputidx in range(len(analogInputs)):
analogInputs[aninputidx] = (recv[bufferidx]
| recv[bufferidx + 1] << 8)
bufferidx += 2
analogInputs = [a * 10.0 / 1023.0 for a in analogInputs]
self.analogInputs.set(analogInputs[4:len(analogInputs)])
def parseSip(self, recv):
# parse all data
xpos = recv[4] | (recv[5] << 8)
ypos = recv[6] | (recv[7] << 8)
thpos = recv[8] | (recv[9] << 8)
#lvel = recv[10] | (recv[11]<<8)
#rvel = recv[12] | (recv[13]<<8)
#battery = recv[14]
stallbump = recv[15] | (recv[16] << 8)
#control = recv[17] | (recv[18]<<8)
#flags = recv[19] | (recv[20]<<8)
#compass = recv[21]
sonarcount = recv[22]
sonars = [-1, -1, -1, -1, -1, -1, -1, -1]
for i in range(sonarcount):
num = recv[23 + 3 * i]
sonars[num] = recv[24 + 3 * i] | (recv[25 + 3 * i] << 8)
#indx = 23 + 3*sonarcount
#gripstate = recv[indx]
#anport = recv[indx+1]
#analog = recv[indx+2]
#digin = recv[indx+3]
#digout = recv[indx+4]
#batteryx10 = recv[indx+5] | (recv[indx+6]<<8)
#chargestate = recv[indx+7]
# deal with the fact that x and y odometry roll over
dx, dy = xpos - self.lastxpos, ypos - self.lastypos
if dx > 60000:
dx -= 65536
elif dx < -60000:
dx += 65536
if dy > 60000:
dy -= 65536
elif dy < -60000:
dy += 65536
lastpose = self.odpose.get()
self.odpose.set((lastpose[0] + dx * METER_SCALE,
lastpose[1] + dy * METER_SCALE, thpos * RADIAN_SCALE))
self.lastxpos, self.lastypos = xpos, ypos
#self.battery = battery/10.0
stall = [((stallbump & 0x0001) == 0x0001),
((stallbump & 0x0100) == 0x0100)]
bump = [((stallbump & 0x00FE) >> 1), ((stallbump & 0xFE00) >> 9)]
self.stalled.set(stall[0] or stall[1] or bump[0] or bump[1])
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i] != -1:
storedsonars[i] = sonars[i] * METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
self.oldsonars.set(self.storedsonars.get()) #hz
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self, data):
self.sipSend(data)
self.sipRecv()
def cmdPulse(self):
self.sipSendReceive([ArcosCmd.PULSE])
def cmdPulseSend(self):
self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self, v):
self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0])
def cmdVel(self, v):
absv = int(abs(v))
if (v >= 0):
data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8]
else:
data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdVel2(self, l, r):
self.sipSendReceive([ArcosCmd.VEL2, ArgType.ARGINT, int(r), int(l)])
def cmdRvel(self, rv):
absrv = abs(rv)
if (rv >= 0):
data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8]
else:
data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self, note, duration):
data = [ArcosCmd.SAY, ArgType.ARGSTR]
for d in ("%s,%s" % (note, duration)):
data.append(ord(d))
data.append(0)
self.sendPacket(data)
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
class Simulator(object):
#DON'T change the dimensions on the fly, just make a new world
class World(object):
def wall(self, start, end):
self.walls.append((start,end))
def movingObstacle(self, vertices, com, d):
self.moving_obstacles.append((vertices, com, d))
def dimensions(self, width, height):
self.width = width
self.height = height
# borders to make collision detection/sonars easier:
self.wall((0,0),(0,self.height))
self.wall((0,0),(self.width,0))
self.wall((self.width,self.height),(0,self.height))
self.wall((self.width,self.height),(self.width,0))
def initialRobotLocation(self, x, y, theta=0):
self.init = x,y,theta
self.initset = True
def __init__(self, worldfile):
self.walls = []
self.moving_obstacles = []
self.initset = False
envin = {}
envin["dimensions"] = self.dimensions
envin["wall"] = self.wall
envin["initialRobotLoc"] = self.initialRobotLocation
envin["initialRobotLocation"] = self.initialRobotLocation
envin["movingObstacle"] = self.movingObstacle
parseFile(worldfile, envin)
if not self.initset:
self.init = self.width/2.0, self.height/2.0, 0.0
def __init__(self, worldfile, geom=None):
if len(worldfile) == 0:
raise CancelGUIAction
self.worldfile = worldfile
self.world = self.World(self.worldfile)
self.win = Toplevel(form.main.tk)
self.win.wm_title("Simulator")
self.win.protocol("WM_DELETE_WINDOW", skip)
# only pay attention to the position part, since the size is determined
# by the world file
self.geom = None
if geom:
self.geom = geom[geom.find('+'):]
self.win.geometry(self.geom)
debug("world loaded", 2)
self.initGlobals()
debug("simulator globals initialized", 2)
self.setters = {"motorOutput":self.motorOutput,
"discreteMotorOutput":self.discreteMotorOutput,
"analogOutput":self.analogOutput,
"setMaxEffectiveSonarDistance":self.setMaxEffectiveSonarDistance,
"enableAccelerationCap":self.enableAccelerationCap,
"setMaxVelocities":self.setMaxVelocities}
self.getters = {"sonarDistances":self.reportSonars,
"pose":self.odpose.get, "stalled":self.stalled.get,
"analogInputs":self.analogInputs.get}
debug("module data set up", 2)
#self.resetbutton = Button(self.win, text="Reset", command=lambda: self.initGlobals(True))
#self.resetbutton.pack(side="top")
self.initCanvas()
debug("drawing area initialized", 2)
self.drawWorld()
self.drawRobot()
debug("updating sonars...", 2)
self.updateSonars()
debug("...updated!", 2)
app.soar.addFlowTriplet((self.startmoving, self.onestep, self.stopmoving))
debug("finished settup up simulator", 2)
# Gets called by the Start button
def startmoving(self):
self.stopmoving()
self.currentthread = Stepper(self.onestep, SIMULATOR_PERIOD)
self.currentthread.start()
# Gets called by the Stop button
def stopmoving(self):
self.motorOutput(0,0)
try:
self.currentthread.stop()
except AttributeError:
pass
#Clean up before closing`
def destroy(self):
# tell the app what the current window geometry is so that we can
# restore it the next time soar is run
app.soar.simulator_geom = self.win.geometry()
app.soar.removeFlowTriplet((self.startmoving, self.onestep, self.stopmoving))
self.stopmoving()
self.win.destroy()
def initGlobals(self, reset=False):
self.storedsonars = SharedVar()
self.oldsonars = SharedVar()
self.reallyoldsonars = SharedVar()
self.v = SharedVar(0.0)
self.w = SharedVar(0.0)
self.abspose = SharedVar((self.world.init[0], self.world.init[1],
self.world.init[2]))
self.odpose = SharedVar((0.0, 0.0, 0.0))
self.analogInputs = SharedVar((0.0, 0.0, 0.0, 0.0))
if not reset:
self.obstacles = []
if self.world.width > self.world.height:
self.simheight = MAX_SIM_WIDTH*self.world.height/self.world.width
self.simwidth = MAX_SIM_WIDTH
else:
self.simwidth = MAX_SIM_HEIGHT*self.world.width/self.world.height
self.simheight = MAX_SIM_HEIGHT
self.pxmax = float(self.world.width)
self.pxmin = 0.0
self.pymax = float(self.world.height)
self.pymin = 0.0
self.cxmax = float(self.simwidth-5)
self.cxmin = 5.0
self.cymax = 5.0
self.cymin = float(self.simheight-5)
self.pxtocx = (self.cxmax-self.cxmin)/(self.pxmax-self.pxmin)
self.pytocy = (self.cymax-self.cymin)/(self.pymax-self.pymin)
self.cxtopx = (self.pxmax-self.pxmin)/(self.cxmax-self.cxmin)
self.cytopy = (self.pymax-self.pymin)/(self.cymax-self.cymin)
self.pxcxconstant = self.cxmin-self.pxmin*self.pxtocx
self.pycyoffset = self.cymin-self.pymin*self.pytocy
self.cxpxoffset = self.pxmin-self.cxmin*self.cxtopx
self.cypyoffset = self.pymin-self.cymin*self.cytopy
else:
self.drawRobot()
self.updateSonars()
self.inrobot = False
self.stalled = SharedVar(False)
# Get the drawable space ready
def initCanvas(self):
self.canvas = Canvas(self.win, width = self.simwidth, height = self.simheight, background = "white")
self.canvas.bind("<Button-1>", self.left_click_down)
self.canvas.bind("<B1-Motion>", self.left_click_moved)
self.canvas.bind("<ButtonRelease-1>", self.left_click_up)
#Thanks for making the right-button different on Macs, but not Linux.
#Thanks for making os.uname not exist on windows... ugh!
try:
if os.uname()[0] == 'Darwin':
self.canvas.bind("<Button-2>", self.right_click_down)
self.canvas.bind("<B2-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-2>", self.right_click_up)
else:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
except AttributeError:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
self.canvas.pack(side = "top")
self.odometrytext = StringVar()
self.odometrylabel = Label(self.win, textvariable=self.odometrytext)
self.odometrylabel.pack(side = "top")
# The next two functions are inverses of each other
# A mapping from the space the robot lives in to the space that is displayed onto the computer screen
def PtoC(self, (px, py)):
cx = px*self.pxtocx+self.pxcxconstant
cy = py*self.pytocy+self.cymin-self.pymin*self.pytocy
return (cx, cy)
def startmoving(self): self.stopmoving() self.currentthread = Stepper(self.onestep, SIMULATOR_PERIOD) self.currentthread.start()
def startmoving(self):
self.stopmoving()
self.currentthread = Stepper(self.onestep, SIMULATOR_PERIOD)
self.currentthread.start()
class eBot:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0., 0.], 0.)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] #Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0,
0]) #Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write(
"- Check if eBot is on, paired and connected. If not, power up and try again. \n"
)
#sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if (self.serialReady):
#self.cmdEnable(0)
self.stopmoving()
self.sendOutputs()
self.sendPacket([CMD.CLOSE])
# self.flushSerialPort()
try:
self.port.close()
except:
pass
app.soar.removeFlowTriplet(
(self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
#if self.serialReady:
#self.cmdEnable(0)
#self.stopmoving()
#self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
from soar.serial import Serial
self.connect(Serial)
self.open()
#self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
def getOpenPorts(self):
"""
This Function Returns a list of tuples with the port number and its description. Used for Windows only
"""
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
#maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
#return ports
#ports = getOpenPorts()
devicePorts = []
for port in ports:
#Just because it is formatted that way...
if 'BthModem' in port[1][8:] or 'VCP' in port[1][
8:] or 'ProlificSerial' in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def connect(self, Serial):
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
#usbSerial = glob.glob('/dev/ttyUSB*')
ports = glob.glob('/dev/rfcomm*')
elif sys.platform == "darwin":
ports = glob.glob('/dev/tty.eBot*')
#usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
#ports = ['COM' + str(i + 1) for i in range(256)]
#EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
for port in ports:
try:
s = Serial(port, baudRate, timeout=1, writeTimeout=1)
s._timeout = 1.0
s._writeTimeout = 1.0
#try:
# s.open()
#except:
# continue
line = "aaaa"
while line[:2] != "eB":
if (s.inWaiting() > 0):
line = s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if (line[:2] == "eB"):
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 1.0
self.port._writeTimeout = 1.0
self.port.flushInput()
self.port.flushOutput()
if (line[:2] == "eB"):
break
#s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if (connect == 0):
try:
self.port.close()
except:
pass
#sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
# Open connection to robot
def open(self):
numAttempts = 3
timeout = 5.0
startt = time()
while (self.connectionState != STATE_READY):
if (self.connectionState == STATE_NO_SYNC):
self.sendPacket([CMD_SYNC_SEND.CHECK1])
elif (self.connectionState == STATE_SYNC):
self.sendPacket([CMD_SYNC_SEND.CHECK2])
self.connectionState = STATE_READY
break
#elif (self.connectionState == STATE_READY):
# pass
try:
pkt = self.recvPacket()
except:
if (self.connectionState == STATE_NO_SYNC
and numAttempts >= 0):
numAttempts -= 1
else:
self.port.close()
sys.stderr.write(
"Could not open serial port. Is robot turned on and connected?\n"
)
raise Exception("No Robot Found")
continue
if (pkt[3] == CMD_SYNC_RECV.CHECK1):
self.connectionState = STATE_SYNC
# elif (pkt[3]==CMD_SYNC_RECV.CHECK2):
# self.connectionState = STATE_READY
if (time() - startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
sleep(.01)
try:
self.port.write('A')
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write(
"Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n"
)
#changed = [0, 0, 0, 0, 0, 0]
#while 0 in changed:
# #self.cmdPulse()
# self.sipRecv()
# self.storedsonars.get()
# changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))]
self.serialReady = True
# turn on io packets
#self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0])
def getPose(self):
return self.odpose.get()
def initGlobals(self):
self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write(
"Trying to set maximum translational velocity too high for real robot\n"
)
if maxRotVel > 1.75:
sys.stderr.write(
"Trying to set maximum rotational velocity too high for real robot\n"
)
def enableTeleportation(self, prob, pose):
sys.stderr.write(
"Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if (not self.asynchronous):
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v, w)
sleep(dt)
self.motorOutput(0, 0)
def sendOutputs(self):
data = [CMD.RECV]
v = int(self.trans.get() * 1000.0)
if (v >= 0):
#data = [abs(v) & 0xFF, v >> 8, 0x01]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x01)
else:
#data = [abs(v) & 0xFF, v >> 8, 0x10]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x10)
rv = int(self.rot.get() * 220 / pi) # 180*1.5
if (rv >= 0):
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x01)
else:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x10)
self.sipSend(data)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get() * 1000.0))
self.cmdRvel(int(self.rot.get() * 180 / pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [
ArcosCmd.DIGOUT,
ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get() * 25.5))) & 0xFF),
0xFF
]
self.sipSend(data)
# update sonars, odometry and stalled
def read(self):
self.incoming = None
if self.offset_counter > 1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting() != 0:
self.incoming = self.port.readline().rstrip('\n')
self.offset_counter += 1
try:
self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \
self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \
self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \
self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \
self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";")
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
print "eBot.read(): bad formatted data received"
print self.incoming
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = (self.Ax + self.Ax_offset)
self.Ay_offset = (self.Ay + self.Ay_offset)
self.Az_offset = (self.Az + self.Az_offset)
self.Gx_offset = (self.Gx + self.Gx_offset)
self.Gy_offset = (self.Gy + self.Gy_offset)
self.Gz_offset = (self.Gz + self.Gz_offset)
if self.offset_counter == self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset / self.offset_counter_iteration
self.Ay_offset = self.Ay_offset / self.offset_counter_iteration
self.Az_offset = self.Az_offset / self.offset_counter_iteration
self.Gx_offset = self.Gx_offset / self.offset_counter_iteration
self.Gy_offset = self.Gy_offset / self.offset_counter_iteration
self.Gz_offset = self.Gz_offset / self.offset_counter_iteration
if self.offset_counter > self.offset_counter_iteration:
sampling_time = (self.time_stamp -
self.prev_time_stamp) / 1000.
if sampling_time > 0:
if abs(self.Gz -
self.Gz_offset) > 50: # to remove the noise
self.heading = self.heading + sampling_time * (
self.Gz - self.Gz_offset
) # the integration to get the heading
self.heading_scaled = self.heading / 130.5 #128.7
self.heading_scaled = self.heading_scaled
self.pos_values[0],self.pos_values[1],self.pos_values[2] = \
self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time)
return self.incoming
def update(self, dummyparameter=0):
if (self.serialReady):
#self.sendPacket([CMD.RECV])
#self.sendMotorCmd()
#self.sendDigitalOutCmd()
# temp = self.port.inWaiting()
self.sendOutputs()
#sleep(.005)
#self.sendPacket([CMD.SEND])
self.read()
sleep(.005)
self.parseData()
self.from_update = 1
#if (self.sipRecv() > 0):
# self.zero_recv_cnt = 0
#else:
# self.zero_recv_cnt += 1
# if (self.zero_recv_cnt > 400):
# self.serialReady = False
# app.soar.closeEBot()
# sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self, data):
c = 0
i = 3
n = data[2] - ord('0') - 2
while (n > 1):
c += (data[i] << 8) | (data[i + 1])
c = c & 0xFFFF
n -= 2
i += 2
if (n > 0):
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if (self.asynchronous):
pass
# self.stopSerialThread()
else:
#self.sendMotorCmd()
self.sendOutputs()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self, data):
#pkt = [0x3C,0x3C,len(data)+2]
pkt = [0x3C, 0x3C, len(data)]
for d in data:
pkt.append(d)
#pkt.append(0)
#pkt.append(0)
#chk = self.calcChecksum(pkt)
#pkt[len(pkt)-2] = (chk>>8)
#pkt[len(pkt)-1] = (chk&0xFF)
s = reduce(lambda x, y: x + chr(y), pkt, "")
try:
self.port.write(s)
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write(
"Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n"
)
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0, 0, 0]
while (1):
cnt = 0
tstart = time()
while (time() - tstart) < timeout:
if (self.port.inWaiting() > 0):
data[2] = ord(self.port.read())
break
if (time() - tstart) > timeout:
raise Exception("Read timeout")
if (data[0] == 0x3E and data[1] == 0x3E):
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2] - ord('0')):
data.append(ord(self.port.read()))
#if self.from_update:
#crc = self.calcChecksum(data)
#if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8):
# self.port.flushInput()
# raise Exception("Checksum failure")
#if self.port.inWaiting() > 0:
# self.port.flushInput()
#self.from_update=0
return data
# Send a packet
def sipSend(self, data):
if self.serialReady:
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
if self.serialReady:
while (self.port.inWaiting() > 0):
try:
recv = self.recvPacket()
except:
print 'no recv packet'
#continue
break
iters += 1
self.parseData(recv)
# 0x3s means sip packet
#if (recv[3]&0xF0)==0x30:
# self.parseSip(recv)
# 0xF0 means io packet
#elif recv[3]==0xF0:
# self.parseIO(recv)
return iters
def parseData(self):
# parse all data
stall = 0
bump = 0
sonars = [-1, -1, -1, -1, -1, -1]
#Right Sonar
sonars[4] = float(self.Ultrasonic_rear_right)
#if -0.5<self.prev_sonar[4]- sonars[4]<0.5:
# sonars[4] = self.prev_sonar4
#else:
# self.prev_sonar4=sonars[4]
#print 'Sonar 1: ', sonars[0]
#Right Front Sonar
sonars[3] = float(self.Ultrasonic_right)
#print 'Sonar 2: ', sonars[1]
#Front Sonar
sonars[2] = float(self.Ultrasonic_front)
#print 'Sonar 3: ', sonars[2]
#Left Front Sonar
sonars[1] = float(self.Ultrasonic_left)
#print 'Sonar 4: ', sonars[3
#Left Sonar
sonars[0] = float(self.Ultrasonic_rear_left)
#print 'Sonar 5: ', sonars[4]
#Back Sonar
sonars[5] = float(self.Ultrasonic_back)
#print 'Sonar 6: ', sonars[5]
self.pos_values[0], self.pos_values[1] = self.EKF.get_position()
self.pos_values[2] = self.EKF.get_heading()
self.pos_values[2] = self.pos_values[2] % (2 * pi)
if self.pos_values[2] > 2 * pi:
self.pos_values[2] -= 2 * pi
elif self.pos_values[2] < 0:
self.pos_values[2] += 2 * pi
self.odpose.set(
(self.pos_values[0], self.pos_values[1], self.pos_values[2]))
self.stalled.set((stall != 0x00) or (bump != 0x00))
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i] != -1:
storedsonars[i] = sonars[i] * METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self, data):
self.sipSend(data)
self.sipRecv()
# def cmdPulse(self):
# self.sipSendReceive([ArcosCmd.PULSE])
# def cmdPulseSend(self):
# self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self, v):
#self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0])
pass
def cmdVel(self, v):
absv = int(abs(v))
if (v >= 0):
data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8]
else:
data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
# def cmdVel2(self,l,r):
# self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)])
def cmdRvel(self, rv):
absrv = abs(rv)
if (rv >= 0):
data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8]
else:
data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self, note, duration):
data = [ArcosCmd.SAY, ArgType.ARGSTR]
for d in ("%s,%s" % (note, duration)):
data.append(ord(d))
data.append(0)
class amigo_simulator(object):
#DON'T change the dimensions on the fly, just make a new world
class World(object):
def wall(self, start, end):
self.walls.append((start, end))
def movingObstacle(self, vertices, com, d):
self.moving_obstacles.append((vertices, com, d))
def dimensions(self, width, height):
self.width = width
self.height = height
# borders to make collision detection/sonars easier:
self.wall((0, 0), (0, self.height))
self.wall((0, 0), (self.width, 0))
self.wall((self.width, self.height), (0, self.height))
self.wall((self.width, self.height), (self.width, 0))
def initialRobotLocation(self, x, y, theta=0):
self.init = x, y, theta
self.initset = True
def __init__(self, worldfile):
self.walls = []
self.moving_obstacles = []
self.initset = False
envin = {}
envin["dimensions"] = self.dimensions
envin["wall"] = self.wall
envin["initialRobotLoc"] = self.initialRobotLocation
envin["initialRobotLocation"] = self.initialRobotLocation
envin["movingObstacle"] = self.movingObstacle
parseFile(worldfile, envin)
if not self.initset:
self.init = self.width / 2.0, self.height / 2.0, 0.0
def __init__(self, worldfile, geom=None):
if len(worldfile) == 0:
raise CancelGUIAction
self.worldfile = worldfile
self.world = self.World(self.worldfile)
self.win = Toplevel(form.main.tk)
self.win.wm_title("Simulator")
self.win.protocol("WM_DELETE_WINDOW", skip)
# only pay attention to the position part, since the size is determined
# by the world file
self.geom = None
if geom:
self.geom = geom[geom.find('+'):]
self.win.geometry(self.geom)
debug("world loaded", 2)
self.initGlobals()
debug("simulator globals initialized", 2)
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"sonarDistances": self.reportSonars,
"pose": self.odpose.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("module data set up", 2)
#self.resetbutton = Button(self.win, text="Reset", command=lambda: self.initGlobals(True))
#self.resetbutton.pack(side="top")
self.initCanvas()
debug("drawing area initialized", 2)
self.drawWorld()
self.drawRobot()
debug("updating sonars...", 2)
self.updateSonars()
debug("...updated!", 2)
app.soar.addFlowTriplet(
(self.startmoving, self.onestep, self.stopmoving))
debug("finished settup up simulator", 2)
# Gets called by the Start button
def startmoving(self):
self.stopmoving()
self.currentthread = Stepper(self.onestep, SIMULATOR_PERIOD)
self.currentthread.start()
# Gets called by the Stop button
def stopmoving(self):
self.motorOutput(0, 0)
try:
self.currentthread.stop()
except AttributeError:
pass
#Clean up before closing`
def destroy(self):
# tell the app what the current window geometry is so that we can
# restore it the next time soar is run
app.soar.simulator_geom = self.win.geometry()
app.soar.removeFlowTriplet(
(self.startmoving, self.onestep, self.stopmoving))
self.stopmoving()
self.win.destroy()
def initGlobals(self, reset=False):
self.storedsonars = SharedVar()
self.oldsonars = SharedVar()
self.reallyoldsonars = SharedVar()
self.v = SharedVar(0.0)
self.w = SharedVar(0.0)
self.abspose = SharedVar(
(self.world.init[0], self.world.init[1], self.world.init[2]))
self.odpose = SharedVar((0.0, 0.0, 0.0))
self.analogInputs = SharedVar((0.0, 0.0, 0.0, 0.0))
if not reset:
self.obstacles = []
if self.world.width > self.world.height:
self.simheight = MAX_SIM_WIDTH * self.world.height / self.world.width
self.simwidth = MAX_SIM_WIDTH
else:
self.simwidth = MAX_SIM_HEIGHT * self.world.width / self.world.height
self.simheight = MAX_SIM_HEIGHT
self.pxmax = float(self.world.width)
self.pxmin = 0.0
self.pymax = float(self.world.height)
self.pymin = 0.0
self.cxmax = float(self.simwidth - 5)
self.cxmin = 5.0
self.cymax = 5.0
self.cymin = float(self.simheight - 5)
self.pxtocx = (self.cxmax - self.cxmin) / (self.pxmax - self.pxmin)
self.pytocy = (self.cymax - self.cymin) / (self.pymax - self.pymin)
self.cxtopx = (self.pxmax - self.pxmin) / (self.cxmax - self.cxmin)
self.cytopy = (self.pymax - self.pymin) / (self.cymax - self.cymin)
self.pxcxconstant = self.cxmin - self.pxmin * self.pxtocx
self.pycyoffset = self.cymin - self.pymin * self.pytocy
self.cxpxoffset = self.pxmin - self.cxmin * self.cxtopx
self.cypyoffset = self.pymin - self.cymin * self.cytopy
else:
self.drawRobot()
self.updateSonars()
self.inrobot = False
self.stalled = SharedVar(False)
# Get the drawable space ready
def initCanvas(self):
self.canvas = Canvas(self.win,
width=self.simwidth,
height=self.simheight,
background="white")
self.canvas.bind("<Button-1>", self.left_click_down)
self.canvas.bind("<B1-Motion>", self.left_click_moved)
self.canvas.bind("<ButtonRelease-1>", self.left_click_up)
#Thanks for making the right-button different on Macs, but not Linux.
#Thanks for making os.uname not exist on windows... ugh!
try:
if os.uname()[0] == 'Darwin':
self.canvas.bind("<Button-2>", self.right_click_down)
self.canvas.bind("<B2-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-2>", self.right_click_up)
else:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
except AttributeError:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
self.canvas.pack(side="top")
self.odometrytext = StringVar()
self.odometrylabel = Label(self.win, textvariable=self.odometrytext)
self.odometrylabel.pack(side="top")
# The next two functions are inverses of each other
# A mapping from the space the robot lives in to the space that is displayed onto the computer screen
def PtoC(self, (px, py)):
cx = px * self.pxtocx + self.pxcxconstant
cy = py * self.pytocy + self.cymin - self.pymin * self.pytocy
return (cx, cy)
def startSerialThread(self): self.flushSerial() self.currentthread = Stepper(self.update, 50) self.currentthread.start()
class Pioneer:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.portName = settings.SERIAL_PORT_NAME
self.sonarsChanged = [0,0,0,0,0,0,0,0]
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0,0
self.storedsonars = SharedVar([0,0,0,0,0,0,0,0])
self.trans, self.rot = SharedVar(0),SharedVar(0)
self.odpose = SharedVar((0,0,0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0,0,0,0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.setters = {"motorOutput":self.motorOutput,
"discreteMotorOutput":self.discreteMotorOutput,
"say":self.cmdSay,
"analogOutput":self.analogOutput,
"setMaxEffectiveSonarDistance":self.setMaxEffectiveSonarDistance,
"enableAccelerationCap":self.enableAccelerationCap,
"setMaxVelocities":self.setMaxVelocities}
self.getters = {"pose":self.odpose.get,
"sonarDistances":self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs":self.analogInputs.get}
debug("Pioneer variables set up", 2)
self.serialready = False
try:
self.open(Serial)
debug("Serial Connection started", 2)
except:
debug("Could not open the serial port", 0)
raise CancelGUIAction("Error opening serial port")
self.cmdEnable(1)
debug("Robot cmdEnabled", 2)
app.soar.addFlowTriplet((self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if (self.serialready):
self.cmdEnable(0)
self.stopmoving()
self.sendPacket([ArcosCmd.CLOSE])
# self.flushSerialPort()
self.port.close()
app.soar.removeFlowTriplet((self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
if self.serialready:
self.cmdEnable(0)
self.stopmoving()
self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0,0,0,0,0,0,0,0]
from soar.serial import Serial
self.open(Serial)
self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
# Open connection to robot
def open(self, Serial):
#baudRates = [9600,38400,19200,115200,57600]
#baudRates = [115200,9600]#,38400,19200,57600]
baudRates = [9600,19200,38400,57600,115200]
#baudRates = [9600]
curBaudRate = 0
self.port = Serial(self.portName,baudRates[0])
self.port._timeout = 1.0
self.port._writeTimout = 1.0
self.port.flushInput()
self.port.flushOutput()
numAttempts = 3
timeout = 5.0
startt = time()
while (self.connectionState != STATE_READY):
if (self.connectionState == STATE_NO_SYNC):
self.sendPacket([CMD_SYNC0])
elif (self.connectionState == STATE_FIRST_SYNC):
self.sendPacket([CMD_SYNC1])
elif (self.connectionState == STATE_SECOND_SYNC):
self.sendPacket([CMD_SYNC2])
elif (self.connectionState == STATE_READY):
pass
try:
pkt = self.recvPacket()
except:
if (self.connectionState == STATE_NO_SYNC and numAttempts >= 0):
numAttempts -= 1
else:
curBaudRate += 1
if (curBaudRate<len(baudRates)):
self.port.close()
self.port = Serial(self.portName,baudRates[curBaudRate])
debug("Changing to baud rate: "+`baudRates[curBaudRate]`, 1)
self.port.flushInput()
self.port.flushOutput()
else:
self.port.close()
sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
continue
if (pkt[3]==CMD_SYNC0):
self.connectionState = STATE_FIRST_SYNC
elif (pkt[3]==CMD_SYNC1):
self.connectionState = STATE_SECOND_SYNC
elif (pkt[3]==CMD_SYNC2):
self.connectionState = STATE_READY
if (time()-startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
botName = ""
botType = ""
botSubType = ""
i=4
while (pkt[i]):
botName += chr(pkt[i])
i += 1
i+=1
while (pkt[i]):
botType += chr(pkt[i])
i += 1
i+=1
while (pkt[i]):
botSubType += chr(pkt[i])
i += 1
self.sendPacket([ArcosCmd.OPEN])
debug("P2OS Interface Ready - connected to "+`botName`+" "+`botType`+" "+`botSubType`,1)
print "P2OS Interface Ready - connected to "+`botType`+`botName`
changed = [0,0,0,0,0,0,0,0]
while 0 in changed:
self.cmdPulse()
self.sipRecv()
self.storedsonars.get()
changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))]
self.serialready = True
# turn on io packets
self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0])
def getPose(self):
return self.odpose.get()
#def initGlobals(self):
# self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write("Trying to set maximum translational velocity too high for real robot\n")
if maxRotVel > 1.75:
sys.stderr.write("Trying to set maximum rotational velocity too high for real robot\n")
def enableTeleportation(self, prob, pose):
sys.stderr.write("Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if (not self.asynchronous):
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v,w)
sleep(dt)
self.motorOutput(0,0)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get()*1000.0))
self.cmdRvel(int(self.rot.get()*180/pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [ArcosCmd.DIGOUT,ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get()*25.5)))&0xFF),0xFF]
self.sipSend(data)
# update sonars, odometry and stalled
def update(self, dummyparameter=0):
if (self.serialready):
self.sendMotorCmd()
self.sendDigitalOutCmd()
# if we receive no packets for a few cycles, we've lost touch with the robot
if (self.sipRecv() > 0):
self.zero_recv_cnt = 0
else:
self.zero_recv_cnt += 1
if (self.zero_recv_cnt > 40):
self.serialready = False
app.soar.closePioneer()
sys.stderr.write("Robot turned off or no longer connected.\n Dead reckoning is no longer valid.\n")
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self,data):
c = 0
i = 3
n = data[2]-2
while (n>1):
c += (data[i]<<8) | (data[i+1])
c = c & 0xFFFF
n -= 2
i += 2
if (n>0):
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if (self.asynchronous):
pass
# self.stopSerialThread()
else:
self.sendMotorCmd()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self,data):
pkt = [0xFA,0xFB,len(data)+2]
for d in data:
pkt.append(d)
pkt.append(0)
pkt.append(0)
chk = self.calcChecksum(pkt)
pkt[len(pkt)-2] = (chk>>8)
pkt[len(pkt)-1] = (chk&0xFF)
s = reduce(lambda x,y: x+chr(y),pkt,"")
try:
self.port.write(s)
except:
sys.stderr.write("Could not write to serial port. Is robot turned on and connected?\n")
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0,0,0]
while (1):
cnt = 0
tstart = time()
while (time()-tstart)<timeout:
if (self.port.inWaiting()>0):
data[2] = ord(self.port.read())
break
if (time()-tstart)>timeout:
raise Exception("Read timeout")
if (data[0] == 0xFA and data[1] == 0xFB):
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2]):
data.append(ord(self.port.read()))
crc = self.calcChecksum(data)
if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8):
self.port.flushInput()
raise Exception("Checksum failure")
# if self.port.inWaiting() > 0:
# self.port.flushInput()
return data
# Send a packet
def sipSend(self,data):
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
while(self.port.inWaiting() > 0):
try:
recv = self.recvPacket()
except:
print 'no recv packet'
#continue
break
iters += 1
# 0x3s means sip packet
if (recv[3]&0xF0)==0x30:
self.parseSip(recv)
# 0xF0 means io packet
elif recv[3]==0xF0:
self.parseIO(recv)
return iters
def parseIO(self, recv):
analogInputs = [0,0,0,0,0,0,0,0]
bufferidx = 12
for aninputidx in range(len(analogInputs)):
analogInputs[aninputidx] = (recv[bufferidx] | recv[bufferidx+1]<<8)
bufferidx += 2
analogInputs = [a*10.0/1023.0 for a in analogInputs]
self.analogInputs.set(analogInputs[4:len(analogInputs)])
def parseSip(self, recv):
# parse all data
xpos = recv[ 4] | (recv[ 5]<<8)
ypos = recv[ 6] | (recv[ 7]<<8)
thpos = recv[ 8] | (recv[ 9]<<8)
#lvel = recv[10] | (recv[11]<<8)
#rvel = recv[12] | (recv[13]<<8)
#battery = recv[14]
stallbump = recv[15] | (recv[16]<<8)
#control = recv[17] | (recv[18]<<8)
#flags = recv[19] | (recv[20]<<8)
#compass = recv[21]
sonarcount = recv[22]
sonars = [-1,-1,-1,-1,-1,-1,-1,-1]
for i in range(sonarcount):
num = recv[23+3*i]
sonars[num] = recv[24+3*i] | (recv[25+3*i]<<8)
#indx = 23 + 3*sonarcount
#gripstate = recv[indx]
#anport = recv[indx+1]
#analog = recv[indx+2]
#digin = recv[indx+3]
#digout = recv[indx+4]
#batteryx10 = recv[indx+5] | (recv[indx+6]<<8)
#chargestate = recv[indx+7]
# deal with the fact that x and y odometry roll over
dx, dy = xpos-self.lastxpos, ypos-self.lastypos
if dx > 60000:
dx-=65536
elif dx < -60000:
dx+=65536
if dy > 60000:
dy-=65536
elif dy < -60000:
dy+=65536
lastpose = self.odpose.get()
self.odpose.set((lastpose[0]+dx*METER_SCALE,
lastpose[1]+dy*METER_SCALE,
thpos*RADIAN_SCALE))
self.lastxpos, self.lastypos = xpos, ypos
#self.battery = battery/10.0
stall = [( (stallbump&0x0001) == 0x0001 ), ( (stallbump&0x0100) == 0x0100 )]
bump = [( (stallbump&0x00FE) >> 1 ), ( (stallbump&0xFE00) >> 9 )]
self.stalled.set(stall[0] or stall[1] or bump[0] or bump[1])
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i]!=-1:
storedsonars[i] = sonars[i]*METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self,data):
self.sipSend(data)
self.sipRecv()
def cmdPulse(self):
self.sipSendReceive([ArcosCmd.PULSE])
def cmdPulseSend(self):
self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self,v):
self.sendPacket([ArcosCmd.ENABLE,ArgType.ARGINT,v,0])
def cmdVel(self,v):
absv = int(abs(v))
if (v>=0):
data = [ArcosCmd.VEL,ArgType.ARGINT,absv&0xFF,absv>>8]
else:
data = [ArcosCmd.VEL,ArgType.ARGNINT,absv&0xFF,absv>>8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdVel2(self,l,r):
self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)])
def cmdRvel(self,rv):
absrv = abs(rv)
if (rv>=0):
data = [ArcosCmd.RVEL,ArgType.ARGINT,absrv&0xFF,absrv>>8]
else:
data = [ArcosCmd.RVEL,ArgType.ARGNINT,absrv&0xFF,absrv>>8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self,note,duration):
data = [ArcosCmd.SAY,ArgType.ARGSTR]
for d in ("%s,%s" % (note,duration)):
data.append(ord(d))
data.append(0)
self.sendPacket(data)
class eBot:
# Initialise connection to pioneer on specified port
def __init__(self):
try:
from soar.serial import Serial
except ImportError:
print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details."
raise CancelGUIAction
self.sonarValues = [0, 0, 0, 0, 0, 0]
self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.port = None
self.serialReady = False
self.ldrvalue = [0, 0]
self.p_value = [0, 0]
self.acc_values = [0, 0, 0, 0, 0, 0]
self.pos_values = [0, 0, 0]
self.EKF = Locator_EKF([0.0, 0.0], 0.0)
self.offset_counter = 0
self.thread_flag = 0
self.heading = 0.0
self.offset_counter_iteration = 200
self.Ultrasonic_rear_right = 0
self.Ultrasonic_right = 0
self.Ultrasonic_front = 0
self.Ultrasonic_left = 0
self.Ultrasonic_rear_left = 0
self.Ultrasonic_back = 0
self.portName = -1
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] # Change to 6 after
self.from_update = 0
self.connectionState = STATE_NO_SYNC
self.port = None
self.lastxpos, self.lastypos = 0, 0
self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0]) # Change to 6 after
self.trans, self.rot = SharedVar(0), SharedVar(0)
self.odpose = SharedVar((0, 0, 0))
self.stalled = SharedVar(False)
self.analogInputs = SharedVar([0, 0, 0, 0])
self.analogOut = SharedVar(0)
self.asynchronous = True
self.zero_recv_cnt = 0 # Added -> Not in pioneer
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"say": self.cmdSay,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities,
}
self.getters = {
"pose": self.odpose.get,
"sonarDistances": self.storedsonars.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get,
}
debug("eBot variables set up", 2)
self.serialReady = False
self.prev_sonar = [0, 0, 0, 0, 0, 0]
try:
self.connect(Serial)
self.open()
debug("Serial Connection started", 2)
print "eBot connection successful"
except:
sys.stderr.write("Couldn't connect to eBot.\n")
sys.stderr.write("- Check if eBot is on, paired and connected. If not, power up and try again. \n")
# sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n")
debug("Could not open the serial port", 0)
print 5
raise CancelGUIAction("Error opening serial port")
app.soar.addFlowTriplet((self.startmoving, self.update, self.stopmoving))
self.currentthread = None
self.acceptMotorCmds = False
self.startSerialThread()
sleep(7)
def destroy(self):
self.stopSerialThread()
sleep(0.2)
if self.serialReady:
# self.cmdEnable(0)
self.stopmoving()
self.sendOutputs()
self.sendPacket([CMD.CLOSE])
# self.flushSerialPort()
try:
self.port.close()
except:
pass
app.soar.removeFlowTriplet((self.startmoving, self.update, self.stopmoving))
# yes, this is repeated code, and should be neater. you fix it.
def initGlobals(self, dummy):
self.stopSerialThread()
sleep(0.2)
# if self.serialReady:
# self.cmdEnable(0)
# self.stopmoving()
# self.sendPacket([ArcosCmd.CLOSE])
self.connectionState = STATE_NO_SYNC
self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0]
from soar.serial import Serial
self.connect(Serial)
self.open()
# self.cmdEnable(1)
self.startSerialThread()
self.odpose.set((0.0, 0.0, 0.0))
def getOpenPorts(self):
"""
This Function Returns a list of tuples with the port number and its description. Used for Windows only
"""
path = "HARDWARE\\DEVICEMAP\\SERIALCOMM"
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
ports = []
# maximum 256 entries, will break anyways
for i in range(256):
try:
val = winreg.EnumValue(key, i)
port = (str(val[1]), str(val[0]))
ports.append(port)
except Exception:
winreg.CloseKey(key)
break
# return ports
# ports = getOpenPorts()
devicePorts = []
for port in ports:
# Just because it is formatted that way...
if "BthModem" in port[1][8:] or "VCP" in port[1][8:] or "ProlificSerial" in port[1][8:]:
devicePorts.append((int(port[0][3:]) - 1))
return devicePorts
def connect(self, Serial):
baudRate = 115200
ports = []
if os.name == "posix":
if sys.platform == "linux2":
# usbSerial = glob.glob('/dev/ttyUSB*')
print "Support for this OS is under development."
elif sys.platform == "darwin":
ports = glob.glob("/dev/tty.eBot*")
# usbSerial = glob.glob('/dev/tty.usbserial*')
else:
print "Unknown posix OS."
sys.exit()
elif os.name == "nt":
ports = self.getOpenPorts()
# ports = ['COM' + str(i + 1) for i in range(256)]
# EBOT_PORTS = getEBotPorts()
connect = 0
ebot_ports = []
ebot_names = []
for port in ports:
try:
s = Serial(port, baudRate, timeout=1, writeTimeout=1)
s._timeout = 1.0
s._writeTimeout = 1.0
# try:
# s.open()
# except:
# continue
line = "aaaa"
while line[:2] != "eB":
if s.inWaiting() > 0:
line = s.readline()
s.write("<<1?")
sleep(0.5)
line = s.readline()
if line[:2] == "eB":
ebot_ports.append(port)
ebot_names.append(line)
connect = 1
self.port = s
self.portName = port
self.port._timeout = 1.0
self.port._writeTimeout = 1.0
self.port.flushInput()
self.port.flushOutput()
if line[:2] == "eB":
break
# s.close()
# self.
except:
try:
if s.isOpen():
s.close()
except:
pass
if connect == 0:
try:
self.port.close()
except:
pass
# sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
# Open connection to robot
def open(self):
numAttempts = 3
timeout = 5.0
startt = time()
while self.connectionState != STATE_READY:
if self.connectionState == STATE_NO_SYNC:
self.sendPacket([CMD_SYNC_SEND.CHECK1])
elif self.connectionState == STATE_SYNC:
self.sendPacket([CMD_SYNC_SEND.CHECK2])
self.connectionState = STATE_READY
break
# elif (self.connectionState == STATE_READY):
# pass
try:
pkt = self.recvPacket()
except:
if self.connectionState == STATE_NO_SYNC and numAttempts >= 0:
numAttempts -= 1
else:
self.port.close()
sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n")
raise Exception("No Robot Found")
continue
if pkt[3] == CMD_SYNC_RECV.CHECK1:
self.connectionState = STATE_SYNC
# elif (pkt[3]==CMD_SYNC_RECV.CHECK2):
# self.connectionState = STATE_READY
if (time() - startt) > timeout:
self.port.close()
sys.stderr.write("Robot needs to be reset.\n")
raise Exception("Bad Robot State")
sleep(0.01)
try:
self.port.write("A")
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# changed = [0, 0, 0, 0, 0, 0]
# while 0 in changed:
# #self.cmdPulse()
# self.sipRecv()
# self.storedsonars.get()
# changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))]
self.serialReady = True
# turn on io packets
# self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0])
def getPose(self):
return self.odpose.get()
def initGlobals(self):
self.odpose.set(0.0, 0.0, 0.0)
def flushSerial(self):
self.port.flushInput()
self.port.flushOutput()
def setMaxEffectiveSonarDistance(self, d):
pass
def enableAccelerationCap(self, enable):
if not enable:
sys.stderr.write("Can't disable accleration cap on real robot.\n")
def setMaxVelocities(self, maxTransVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = maxTransVel
MAX_ROT = maxRotVel
if maxTransVel > 0.75:
sys.stderr.write("Trying to set maximum translational velocity too high for real robot\n")
if maxRotVel > 1.75:
sys.stderr.write("Trying to set maximum rotational velocity too high for real robot\n")
def enableTeleportation(self, prob, pose):
sys.stderr.write("Enabling teleportation on real robot has no effect.\n")
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (rad/sec)
def motorOutput(self, v, w):
# traceback.print_stack()
if self.acceptMotorCmds:
self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.rot.set(clip(w, -MAX_ROT, MAX_ROT))
if not self.asynchronous:
self.update()
def analogOutput(self, v):
self.analogOut.set(clip(v, 0.0, 10.0))
# Move by translating with velocity v (m/sec),
# and rotating with velocity r (deg/sec)
# for dt seconds
def discreteMotorOutput(self, v, w, dt):
if self.acceptMotorCmds:
self.motorOutput(v, w)
sleep(dt)
self.motorOutput(0, 0)
def sendOutputs(self):
data = [CMD.RECV]
v = int(self.trans.get() * 1000.0)
if v >= 0:
# data = [abs(v) & 0xFF, v >> 8, 0x01]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x01)
else:
# data = [abs(v) & 0xFF, v >> 8, 0x10]
data.append(abs(v) / 256)
data.append(abs(v) % 256)
data.append(0x10)
rv = int(self.rot.get() * 220 / pi) # 180*1.5
if rv >= 0:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x01)
else:
data.append(abs(rv) / 256)
data.append(abs(rv) % 256)
data.append(0x10)
self.sipSend(data)
def sendMotorCmd(self):
self.cmdVel(int(self.trans.get() * 1000.0))
self.cmdRvel(int(self.rot.get() * 180 / pi))
def sendDigitalOutCmd(self):
# high order byte selects output bit for change
# low order byte sets/resets bit
data = [
ArcosCmd.DIGOUT,
ArgType.ARGINT,
# convert 0-10 voltage to 0-255 value
(int(math.floor((self.analogOut.get() * 25.5))) & 0xFF),
0xFF,
]
self.sipSend(data)
# update sonars, odometry and stalled
def read(self):
self.incoming = None
if self.offset_counter > 1:
self.prev_time_stamp = self.time_stamp
if self.port.inWaiting() != 0:
self.incoming = self.port.readline().rstrip("\n")
self.offset_counter += 1
try:
self.time_stamp, self.Ax, self.Ay, self.Az, self.Gx, self.Gy, self.Gz, self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front, self.Ultrasonic_left, self.Ultrasonic_rear_left, self.Ultrasonic_back, self.encoder_right, self.encoder_left, self.LDR_top, self.LDR_front, self.tempreture_sensor, self.voltage, self.current = self.incoming.split(
";"
)
self.time_stamp = float(self.time_stamp)
self.Ax = float(self.Ax)
self.Ay = float(self.Ay)
self.Az = float(self.Az)
self.Gx = float(self.Gx)
self.Gy = float(self.Gy)
self.Gz = float(self.Gz)
self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right)
self.Ultrasonic_right = float(self.Ultrasonic_right)
self.Ultrasonic_front = float(self.Ultrasonic_front)
self.Ultrasonic_left = float(self.Ultrasonic_left)
self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left)
self.Ultrasonic_back = float(self.Ultrasonic_back)
self.encoder_right = float(self.encoder_right)
self.encoder_left = float(self.encoder_left)
self.LDR_top = float(self.LDR_top)
self.LDR_front = float(self.LDR_front)
self.tempreture_sensor = float(self.tempreture_sensor)
self.voltage = float(self.voltage)
self.current = float(self.current)
except:
print "eBot.read(): bad formatted data received"
print self.incoming
if self.offset_counter == 2:
self.Ax_offset = self.Ax
self.Ay_offset = self.Ay
self.Az_offset = self.Az
self.Gx_offset = self.Gx
self.Gy_offset = self.Gy
self.Gz_offset = self.Gz
if (
self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration
): # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax + self.Ax_offset
self.Ay_offset = self.Ay + self.Ay_offset
self.Az_offset = self.Az + self.Az_offset
self.Gx_offset = self.Gx + self.Gx_offset
self.Gy_offset = self.Gy + self.Gy_offset
self.Gz_offset = self.Gz + self.Gz_offset
if self.offset_counter == self.offset_counter_iteration: # we just average the first 50
# readings of the accelerometer and Gyro sensor and find the offset value by averaging them
self.Ax_offset = self.Ax_offset / self.offset_counter_iteration
self.Ay_offset = self.Ay_offset / self.offset_counter_iteration
self.Az_offset = self.Az_offset / self.offset_counter_iteration
self.Gx_offset = self.Gx_offset / self.offset_counter_iteration
self.Gy_offset = self.Gy_offset / self.offset_counter_iteration
self.Gz_offset = self.Gz_offset / self.offset_counter_iteration
if self.offset_counter > self.offset_counter_iteration:
sampling_time = (self.time_stamp - self.prev_time_stamp) / 1000.0
if sampling_time > 0:
if abs(self.Gz - self.Gz_offset) > 50: # to remove the noise
self.heading = self.heading + sampling_time * (
self.Gz - self.Gz_offset
) # the integration to get the heading
self.heading_scaled = self.heading / 130.5 # 128.7
self.heading_scaled = self.heading_scaled
self.pos_values[0], self.pos_values[1], self.pos_values[2] = self.EKF.update_state(
[self.heading_scaled * pi / 180.0, self.encoder_right / 1000.0, self.encoder_left / 1000.0],
sampling_time,
)
return self.incoming
def update(self, dummyparameter=0):
if self.serialReady:
# self.sendPacket([CMD.RECV])
# self.sendMotorCmd()
# self.sendDigitalOutCmd()
# temp = self.port.inWaiting()
self.sendOutputs()
# sleep(.005)
# self.sendPacket([CMD.SEND])
self.read()
sleep(0.005)
self.parseData()
self.from_update = 1
# if (self.sipRecv() > 0):
# self.zero_recv_cnt = 0
# else:
# self.zero_recv_cnt += 1
# if (self.zero_recv_cnt > 400):
# self.serialReady = False
# app.soar.closeEBot()
# sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
# Calculate checksum on P2OS packet (see Pioneer manual)
def calcChecksum(self, data):
c = 0
i = 3
n = data[2] - ord("0") - 2
while n > 1:
c += (data[i] << 8) | (data[i + 1])
c = c & 0xFFFF
n -= 2
i += 2
if n > 0:
c = c ^ data[i]
return c
def startmoving(self):
self.acceptMotorCmds = True
# if (self.asynchronous):
# self.stopmoving()
# self.startSerialThread()
def stopmoving(self):
self.motorOutput(0.0, 0.0)
self.acceptMotorCmds = False
if self.asynchronous:
pass
# self.stopSerialThread()
else:
# self.sendMotorCmd()
self.sendOutputs()
def startSerialThread(self):
self.flushSerial()
self.currentthread = Stepper(self.update, 50)
self.currentthread.start()
def stopSerialThread(self):
try:
self.currentthread.stop()
except AttributeError:
pass
# Send a packet to robot
def sendPacket(self, data):
# pkt = [0x3C,0x3C,len(data)+2]
pkt = [0x3C, 0x3C, len(data)]
for d in data:
pkt.append(d)
# pkt.append(0)
# pkt.append(0)
# chk = self.calcChecksum(pkt)
# pkt[len(pkt)-2] = (chk>>8)
# pkt[len(pkt)-1] = (chk&0xFF)
s = reduce(lambda x, y: x + chr(y), pkt, "")
try:
self.port.write(s)
except:
sys.stderr.write("Could not write to serial port.\n")
self.serialReady = False
app.soar.closeEBot()
sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n")
sleep(0.008)
# Block until packet recieved
def recvPacket(self):
timeout = 1.0
data = [0, 0, 0]
while 1:
cnt = 0
tstart = time()
while (time() - tstart) < timeout:
if self.port.inWaiting() > 0:
data[2] = ord(self.port.read())
break
if (time() - tstart) > timeout:
raise Exception("Read timeout")
if data[0] == 0x3E and data[1] == 0x3E:
break
data[0] = data[1]
data[1] = data[2]
for d in range(data[2] - ord("0")):
data.append(ord(self.port.read()))
# if self.from_update:
# crc = self.calcChecksum(data)
# if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8):
# self.port.flushInput()
# raise Exception("Checksum failure")
# if self.port.inWaiting() > 0:
# self.port.flushInput()
# self.from_update=0
return data
# Send a packet
def sipSend(self, data):
if self.serialReady:
self.sendPacket(data)
# Receive all waiting packets.
# returns the number of packets read
def sipRecv(self):
iters = 0
if self.serialReady:
while self.port.inWaiting() > 0:
try:
recv = self.recvPacket()
except:
print "no recv packet"
# continue
break
iters += 1
self.parseData(recv)
# 0x3s means sip packet
# if (recv[3]&0xF0)==0x30:
# self.parseSip(recv)
# 0xF0 means io packet
# elif recv[3]==0xF0:
# self.parseIO(recv)
return iters
def parseData(self):
# parse all data
stall = 0
bump = 0
sonars = [-1, -1, -1, -1, -1, -1]
# Right Sonar
sonars[4] = float(self.Ultrasonic_rear_right)
# if -0.5<self.prev_sonar[4]- sonars[4]<0.5:
# sonars[4] = self.prev_sonar4
# else:
# self.prev_sonar4=sonars[4]
# print 'Sonar 1: ', sonars[0]
# Right Front Sonar
sonars[3] = float(self.Ultrasonic_right)
# print 'Sonar 2: ', sonars[1]
# Front Sonar
sonars[2] = float(self.Ultrasonic_front)
# print 'Sonar 3: ', sonars[2]
# Left Front Sonar
sonars[1] = float(self.Ultrasonic_left)
# print 'Sonar 4: ', sonars[3
# Left Sonar
sonars[0] = float(self.Ultrasonic_rear_left)
# print 'Sonar 5: ', sonars[4]
# Back Sonar
sonars[5] = float(self.Ultrasonic_back)
# print 'Sonar 6: ', sonars[5]
self.pos_values[0], self.pos_values[1] = self.EKF.get_position()
self.pos_values[2] = self.EKF.get_heading()
self.pos_values[2] = self.pos_values[2] % (2 * pi)
if self.pos_values[2] > 2 * pi:
self.pos_values[2] -= 2 * pi
elif self.pos_values[2] < 0:
self.pos_values[2] += 2 * pi
self.odpose.set((self.pos_values[0], self.pos_values[1], self.pos_values[2]))
self.stalled.set((stall != 0x00) or (bump != 0x00))
storedsonars = self.storedsonars.get()
for i in range(len(sonars)):
if sonars[i] != -1:
storedsonars[i] = sonars[i] * METER_SCALE
self.sonarsChanged[i] = 1
else:
self.sonarsChanged[i] = 0
self.storedsonars.set(storedsonars)
# Send a packet and receive a SIP response from the robot
def sipSendReceive(self, data):
self.sipSend(data)
self.sipRecv()
# def cmdPulse(self):
# self.sipSendReceive([ArcosCmd.PULSE])
# def cmdPulseSend(self):
# self.sipSend([ArcosCmd.PULSE])
def cmdEnable(self, v):
# self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0])
pass
def cmdVel(self, v):
absv = int(abs(v))
if v >= 0:
data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8]
else:
data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
# def cmdVel2(self,l,r):
# self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)])
def cmdRvel(self, rv):
absrv = abs(rv)
if rv >= 0:
data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8]
else:
data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8]
# self.sipSendReceive(data)
self.sipSend(data)
def cmdSay(self, note, duration):
data = [ArcosCmd.SAY, ArgType.ARGSTR]
for d in "%s,%s" % (note, duration):
data.append(ord(d))
data.append(0)
class Simulator(object):
#DON'T change the dimensions on the fly, just make a new world
class World(object):
def wall(self, start, end):
self.walls.append((start, end))
def movingObstacle(self, vertices, com, d):
self.moving_obstacles.append((vertices, com, d))
def dimensions(self, width, height):
self.width = width
self.height = height
# borders to make collision detection/sonars easier:
self.wall((0, 0), (0, self.height))
self.wall((0, 0), (self.width, 0))
self.wall((self.width, self.height), (0, self.height))
self.wall((self.width, self.height), (self.width, 0))
def initialRobotLocation(self, x, y, theta=0):
self.init = x, y, theta
self.initset = True
def __init__(self, worldfile):
self.walls = []
self.moving_obstacles = []
self.initset = False
envin = {}
envin["dimensions"] = self.dimensions
envin["wall"] = self.wall
envin["initialRobotLoc"] = self.initialRobotLocation
envin["initialRobotLocation"] = self.initialRobotLocation
envin["movingObstacle"] = self.movingObstacle
parseFile(worldfile, envin)
if not self.initset:
self.init = self.width / 2.0, self.height / 2.0, 0.0
def __init__(self, worldfile, geom=None):
if len(worldfile) == 0:
raise CancelGUIAction
self.worldfile = worldfile
self.world = self.World(self.worldfile)
self.win = Toplevel(form.main.tk)
self.win.wm_title("Simulator")
self.win.protocol("WM_DELETE_WINDOW", skip)
# only pay attention to the position part, since the size is determined
# by the world file
self.geom = None
if geom:
self.geom = geom[geom.find('+'):]
self.win.geometry(self.geom)
debug("world loaded", 2)
self.initGlobals()
debug("simulator globals initialized", 2)
self.setters = {
"motorOutput": self.motorOutput,
"discreteMotorOutput": self.discreteMotorOutput,
"analogOutput": self.analogOutput,
"setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance,
"enableAccelerationCap": self.enableAccelerationCap,
"setMaxVelocities": self.setMaxVelocities
}
self.getters = {
"sonarDistances": self.reportSonars,
"pose": self.odpose.get,
"stalled": self.stalled.get,
"analogInputs": self.analogInputs.get
}
debug("module data set up", 2)
#self.resetbutton = Button(self.win, text="Reset", command=lambda: self.initGlobals(True))
#self.resetbutton.pack(side="top")
self.initCanvas()
debug("drawing area initialized", 2)
self.drawWorld()
self.draw_robot()
debug("updating sonars...", 2)
self.updateSonars()
debug("...updated!", 2)
app.soar.addFlowTriplet(
(self.startmoving, self.onestep, self.stopmoving))
debug("finished settup up simulator", 2)
# Gets called by the Start button
def startmoving(self):
self.stopmoving()
self.currentthread = Stepper(self.onestep, SIMULATOR_PERIOD)
self.currentthread.start()
# Gets called by the Stop button
def stopmoving(self):
self.motorOutput(0, 0)
try:
self.currentthread.stop()
except AttributeError:
pass
#Clean up before closing`
def destroy(self):
# tell the app what the current window geometry is so that we can
# restore it the next time soar is run
app.soar.simulator_geom = self.win.geometry()
app.soar.removeFlowTriplet(
(self.startmoving, self.onestep, self.stopmoving))
self.stopmoving()
self.win.destroy()
def initGlobals(self, reset=False):
self.ldr = SharedVar([0, 0])
self.temperature = SharedVar(0)
self.storedsonars = SharedVar()
self.oldsonars = SharedVar()
self.reallyoldsonars = SharedVar()
self.v = SharedVar(0.0)
self.w = SharedVar(0.0)
self.abspose = SharedVar(
(self.world.init[0], self.world.init[1], self.world.init[2]))
self.odpose = SharedVar((0.0, 0.0, 0.0))
self.analogInputs = SharedVar((0.0, 0.0, 0.0, 0.0))
if not reset:
self.obstacles = []
if self.world.width > self.world.height:
self.simheight = MAX_SIM_WIDTH * self.world.height / self.world.width
self.simwidth = MAX_SIM_WIDTH
else:
self.simwidth = MAX_SIM_HEIGHT * self.world.width / self.world.height
self.simheight = MAX_SIM_HEIGHT
self.pxmax = float(self.world.width)
self.pxmin = 0.0
self.pymax = float(self.world.height)
self.pymin = 0.0
self.cxmax = float(self.simwidth - 5)
self.cxmin = 5.0
self.cymax = 5.0
self.cymin = float(self.simheight - 5)
self.pxtocx = (self.cxmax - self.cxmin) / (self.pxmax - self.pxmin)
self.pytocy = (self.cymax - self.cymin) / (self.pymax - self.pymin)
self.cxtopx = (self.pxmax - self.pxmin) / (self.cxmax - self.cxmin)
self.cytopy = (self.pymax - self.pymin) / (self.cymax - self.cymin)
self.pxcxconstant = self.cxmin - self.pxmin * self.pxtocx
self.pycyoffset = self.cymin - self.pymin * self.pytocy
self.cxpxoffset = self.pxmin - self.cxmin * self.cxtopx
self.cypyoffset = self.pymin - self.cymin * self.cytopy
else:
self.draw_robot()
self.updateSonars()
self.inrobot = False
self.stalled = SharedVar(False)
# Get the drawable space ready
def initCanvas(self):
self.canvas = Canvas(self.win,
width=self.simwidth,
height=self.simheight,
background="white")
self.canvas.bind("<Button-1>", self.left_click_down)
self.canvas.bind("<B1-Motion>", self.left_click_moved)
self.canvas.bind("<ButtonRelease-1>", self.left_click_up)
#Thanks for making the right-button different on Macs, but not Linux.
#Thanks for making os.uname not exist on windows... ugh!
try:
if os.uname()[0] == 'Darwin':
self.canvas.bind("<Button-2>", self.right_click_down)
self.canvas.bind("<B2-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-2>", self.right_click_up)
else:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
except AttributeError:
self.canvas.bind("<Button-3>", self.right_click_down)
self.canvas.bind("<B3-Motion>", self.right_click_moved)
self.canvas.bind("<ButtonRelease-3>", self.right_click_up)
self.canvas.pack(side="top")
self.odometrytext = StringVar()
self.odometrylabel = Label(self.win, textvariable=self.odometrytext)
self.odometrylabel.pack(side="top")
# The next two functions are inverses of each other
# A mapping from the space the robot lives in to the space that is displayed onto the computer screen
def PtoC(self, coordinates):
(px, py) = coordinates
cx = px * self.pxtocx + self.pxcxconstant
cy = py * self.pytocy + self.cymin - self.pymin * self.pytocy
return (cx, cy)
# A mapping from the space that is displayed onto the computer screen to the space the robot lives in
def CtoP(self, coordinates):
(cx, cy) = coordinates
px = cx * self.cxtopx + self.pxmin - self.cxmin * self.cxtopx
py = cy * self.cytopy + self.pymin - self.cymin * self.cytopy
return (px, py)
# Draw the world when a world is loaded
def drawWorld(self):
self.canvas.create_rectangle(0,
0,
5,
self.simheight - 5,
fill="grey",
outline="grey")
self.canvas.create_rectangle(0,
self.simheight - 5,
self.simwidth - 5,
self.simheight,
fill="grey",
outline="grey")
self.canvas.create_rectangle(5,
0,
self.simwidth,
5,
fill="grey",
outline="grey")
self.canvas.create_rectangle(self.simwidth - 5,
5,
self.simwidth,
self.simheight,
fill="grey",
outline="grey")
self.lines = []
for wall in self.world.walls:
start, end = wall
icx, icy = self.PtoC(start)
fcx, fcy = self.PtoC(end)
self.lines.append(
self.canvas.create_line(icx,
icy,
fcx,
fcy,
fill="black",
width=2))
for obstacle in self.world.moving_obstacles:
obstacle = Obstacle(obstacle)
self.obstacles.append(obstacle)
self.drawObstacles(0)
def drawObstacles(self, dt):
newlines = []
for obstacle in self.obstacles:
obstacle.step(dt)
for wall in obstacle.current_walls():
newlines.append(list(map(self.PtoC, wall)))
def tk_update_obstacles():
for item in self.canvas.find_withtag("obstacle"):
self.canvas.delete(item)
for line in newlines:
self.canvas.create_line(
*line[0] + line[1], **{
'fill': 'brown',
'width': 2,
'tags': 'obstacle'
})
form.main.tk_enqueue(tk_update_obstacles)
def map_robot_point(self, robot_point):
absx, absy, absth = self.abspose.get()
cx, cy = self.PtoC((absx, absy))
x, y = robot_point
r = (x**2 + y**2)**(0.5)
th = atan2(x, y) + absth
return (cx + self.pxtocx * r * cos(th), cy + self.pytocy * r * sin(th))
# Draw the Pioneer robot at its current position
def draw_robot(self):
points = list(map(self.map_robot_point, ROBOT_POINTS))
def tk_update_robot():
for item in self.canvas.find_withtag("robot"):
self.canvas.delete(item)
if self.stalled.get():
colorstr = "red"
else:
colorstr = "black"
self.canvas.create_polygon( \
points[0][0], points[0][1], \
points[1][0], points[1][1], \
points[2][0], points[2][1], \
points[3][0], points[3][1], \
points[4][0], points[4][1], \
points[5][0], points[5][1], \
# points[6][0], points[6][1], \
# points[7][0], points[7][1], \
# points[8][0], points[8][1], \
# points[9][0], points[9][1], \
# points[10][0], points[10][1],\
# points[11][0], points[11][1],\
# points[12][0], points[12][1],\
tags="robot", fill=colorstr)
self.canvas.tag_raise("robot")
form.main.tk_enqueue(tk_update_robot)
#if os.name == "posix": form.main.tk_enqueue(self.canvas.update_idletasks)
absx, absy, absth = self.abspose.get()
sonarlines = []
for entry in SONAR_INFO:
r = (entry[0]**2 + entry[1]**2)**(0.5)
th = atan2(entry[1], entry[0]) + absth - pi / 2
xa, ya = (absx + r * cos(th), absy + r * sin(th))
xb, yb = xa + MAX_SONAR_DIST * cos(
absth + entry[2]), ya + MAX_SONAR_DIST * sin(absth + entry[2])
sonarlines.append(((xa, ya), (xb, yb)))
self.fullsonarlines = sonarlines
def reportSonars(self):
# return self.reallyoldsonars.get()
return self.storedsonars.get()
def setMaxEffectiveSonarDistance(self, d):
global MAX_EFFECTIVE_SONAR_DIST
MAX_EFFECTIVE_SONAR_DIST = d
def enableAccelerationCap(self, enable):
global CAP_ACC
CAP_ACC = enable
def setMaxVelocities(self, max_transVel, maxRotVel):
global MAX_TRANS, MAX_ROT
MAX_TRANS = max_transVel
MAX_ROT = maxRotVel
def enableTeleportation(self, perStepProbability, poseDist):
global TELEPORT_PROB, TELEPORT_DIST
TELEPORT_PROB = perStepProbability
TELEPORT_DIST = poseDist
print('Enabling teleportation with probability', TELEPORT_PROB)
# Set translational and rotational velocities
def motorOutput(self, v, w):
if TELEPORT_PROB > 0:
if random() < TELEPORT_PROB:
newPose = TELEPORT_DIST.draw()
print('Teleporting to', newPose)
self.abspose.set(newPose)
if CAP_ACC:
(oldTrans, oldRot) = (self.v.get(), self.w.get())
transDiff = clip(v - oldTrans, -TRANS_ACC, TRANS_ACC)
rotDiff = clip(w - oldRot, -ROT_ACC, ROT_ACC)
self.v.set(clip(oldTrans + transDiff, -MAX_TRANS, MAX_TRANS))
self.w.set(clip(oldRot + rotDiff, -MAX_ROT, MAX_ROT))
else:
self.v.set(clip(v, -MAX_TRANS, MAX_TRANS))
self.w.set(clip(w, -MAX_ROT, MAX_ROT))
def analogOutput(self, val):
pass
# Support for accurate prediction of motion even when the brain is slow
def discreteMotorOutput(self, v, w, dt):
self.motorOutput(v, w)
self.onestep(dt)
self.motorOutput(0, 0)
def cmdSay(self, freq, dur):
pass
def updateTemperature(self):
return self.temperature.set(0) # dummy value for simulator
def updateLDR(self):
return self.ldr.set([0, 0]) # dummy value for simulator
# Calculate new sonar values
def updateSonars(self):
sonars = []
sonars_to_draw = []
for sonar in self.fullsonarlines:
mindist = MAX_SONAR_DIST
lastint = False
anglegood = False
walls = []
walls += self.world.walls
for obstacle in self.obstacles:
walls += obstacle.current_walls()
for wall in walls:
i = intersection(sonar, wall)
if i:
ix, iy = i
sx, sy = sonar[0]
dist = ((ix - sx)**2 + (iy - sy)**2)**(0.5)
if dist < mindist:
anglegood = sonarangleok(
abs(pi / 2 - angle(sonar, wall)))
mindist = dist
lastint = i
if DRAW_SONARS:
icx, icy = self.PtoC(sonar[0])
if lastint:
fcx, fcy = self.PtoC(lastint)
else:
fcx, fcy = self.PtoC(sonar[1])
icx, icy, fcx, fcy = int(icx), int(icy), int(fcx), int(fcy)
if anglegood and mindist < MAX_EFFECTIVE_SONAR_DIST:
sonars_to_draw.append(
(icx, icy, fcx, fcy, SONAR_GOT_RESPONSE_COLOR))
else:
sonars_to_draw.append(
(icx, icy, fcx, fcy, SONAR_NO_RESPONSE_COLOR))
if anglegood and mindist < MAX_EFFECTIVE_SONAR_DIST:
val = gauss(mindist, SONAR_VARIANCE(mindist))
sonars.append(val)
else:
val = MAX_SONAR_DIST
sonars.append(val)
if DRAW_SONARS:
def tk_update_sonars():
for item in self.canvas.find_withtag("sonarlines"):
self.canvas.delete(item)
for sonar_to_draw in sonars_to_draw:
icx, icy, fcx, fcy, sonar_color = sonar_to_draw
self.canvas.create_line(icx,
icy,
fcx,
fcy,
fill=sonar_color,
tag="sonarlines")
form.main.tk_enqueue(tk_update_sonars)
self.reallyoldsonars.set(self.oldsonars.get())
self.oldsonars.set(self.storedsonars.get())
self.storedsonars.set(sonars)
# First part of dealing with collisions cleanishly
def cache(self):
try:
self.lastlastpos = self.lastpos
except AttributeError:
pass
self.lastpos = self.odpose.get() + self.abspose.get()
# Second part of dealing with collisions cleanishly
def uncache(self):
odx, ody, odth, absx, absy, absth = self.lastpos
self.odpose.set((odx, ody, odth))
self.abspose.set((absx, absy, absth))
try:
if self.collision():
odx, ody, odth, absx, absy, absth = self.lastlastpos
self.odpose.set((odx, ody, odth))
self.abspose.set((absx, absy, absth))
except AttributeError:
pass
# One step of the whole running simulation
def onestep(self, dt):
self.cache()
self.move(dt)
self.stalled.set(False)
if self.collision():
self.uncache()
self.stalled.set(True)
self.drawObstacles(dt)
self.draw_robot()
self.updateSonars()
self.updateTemperature()
self.updateLDR()
def perp(self, coordinates):
((x0, y0), (x1, y1)) = coordinates
x, y = self.abspose.get()[:2]
try:
a = tan(-(x1 - x0) / (y1 - y0))
except ZeroDivisionError:
a = pi / 2
dx = ROBOT_RADIUS * cos(a)
dy = ROBOT_RADIUS * sin(a)
return ((x - dx, y - dy), (x + dx, y + dy))
def collision(self):
for wall in self.world.walls:
if intersection(wall, self.perp(wall)):
return True
return False
# Move the robot properly based on how time passed
def move(self, dt):
v = self.v.get()
w = self.w.get()
odx, ody, odth = self.odpose.get()
absx, absy, absth = self.abspose.get()
thstep = dt * w
try:
#integrate the path
absxstep = v * (sin(absth + thstep) - sin(absth)) / w
odxstep = v * (sin(odth + thstep) - sin(odth)) / w
absystep = v * (cos(absth) - cos(absth + thstep)) / w
odystep = v * (cos(odth) - cos(odth + thstep)) / w
except ZeroDivisionError:
#deal with a zero case that I dropped deriving the above expression
absxstep = dt * cos(absth + thstep / 2) * v
odxstep = dt * cos(odth + thstep / 2) * v
absystep = dt * sin(absth + thstep / 2) * v
odystep = dt * sin(odth + thstep / 2) * v
absx += absxstep
absy += absystep
absth += thstep
absth %= 2 * pi
odx += odxstep
ody += odystep
odth += thstep
odth %= 2 * pi
form.main.tk_enqueue(lambda: self.odometrytext.set("POSE - X: " + repr(
odx)[:7] + ", Y: " + repr(ody)[:7] + ", TH: " + repr(odth)[:7]))
self.odpose.set((odx, ody, odth))
self.abspose.set((absx, absy, absth))
# Respond to a left click down
def left_click_down(self, event):
for item in self.canvas.find_withtag("robot"):
xn, yn, xx, yx = self.canvas.bbox(item)
if event.x <= xx and event.x >= xn and event.y <= yx and event.y >= yn:
self.inrobot = True
break
# Respond to a left click drag
def left_click_moved(self, event):
if self.inrobot:
x, y = self.CtoP((event.x, event.y))
self.abspose.set((x, y, self.abspose.get()[2]))
if DRAG_UPDATES_ROBOT_ODOMETRY:
self.odpose.set((x, y, self.odpose.get()[2]))
self.draw_robot()
self.updateSonars()
self.updateTemperature()
self.updateLDR()
# Respond to releasing left mouse button
def left_click_up(self, event):
self.left_click_moved(event)
self.inrobot = False
# Respond to a right click down
def right_click_down(self, event):
for item in self.canvas.find_withtag("robot"):
xn, yn, xx, yx = self.canvas.bbox(item)
if event.x <= xx and event.x >= xn and event.y <= yx and event.y >= yn:
self.inrobot = True
self.lasteventx, self.lasteventy = event.x, event.y
# Respond to releasing right mouse button
def right_click_up(self, event):
self.inrobot = False
# Respond to a right click drag
def right_click_moved(self, event):
if self.inrobot:
dx, dy = event.x - self.lasteventx, event.y - self.lasteventy
absx, absy, absth = self.abspose.get()
self.abspose.set((absx, absy, absth + (dy + dx) * DRAG_ROT_SPEED))
if DRAG_UPDATES_ROBOT_ODOMETRY:
odx, ody, odth = self.odpose.get()
self.odpose.set((odx, ody, odth + (dy + dx) * DRAG_ROT_SPEED))
self.draw_robot()
self.updateSonars()
self.updateTemperature()
self.updateLDR()
self.lasteventx, self.lasteventy = event.x, event.y