def __init__(self,initial=(0,0,0),port=0):
self.robot = Robot(port)
self.port = port
client.message(INITIAL_TOPIC(port),initial)
client.subscribe(SPEED_TOPIC(port),self.setForward)
client.subscribe(OMEGA_TOPIC(port),self.setRotational)
client.subscribe(VOLTAGE_TOPIC(port),self.setVoltage)
client.subscribe(BRAIN_MSG,self.brain_background)
client.subscribe(SIM_MSG,self.brain_background)
def __init__(self, initial=(0, 0, 0), port=0):
self.robot = Robot(port)
self.port = port
client.message(INITIAL_TOPIC(port), initial)
client.subscribe(SPEED_TOPIC(port), self.setForward)
client.subscribe(OMEGA_TOPIC(port), self.setRotational)
client.subscribe(VOLTAGE_TOPIC(port), self.setVoltage)
client.subscribe(BRAIN_MSG, self.brain_background)
client.subscribe(SIM_MSG, self.brain_background)
def step(self, dt, expect_paused=True):
client.message(INITIAL_TOPIC(self.port), self.initial)
if expect_paused and (not self.paused.is_set()):
return
x, y, theta = self.position
x += self.v * cos(theta) * dt
y += self.v * sin(theta) * dt
theta += self.omega * dt
ix, iy, itheta = self.initial
# Check if we are colliding.
# Build robot segments
robot_segments = []
px, py = transform((ix, iy, itheta), (x, y))
pos = (px, py, itheta + theta - pi / 2.)
robot_points = (transform(pos, point) for point in model.points)
prev = None
first = None
for p in robot_points:
if prev is not None:
robot_segments.append(
geom.Segment(geom.Point(*prev), geom.Point(*p)))
else:
first = p
prev = p
robot_segments.append(
geom.Segment(geom.Point(*prev), geom.Point(*first)))
for w in self.walls:
self.colliding = any(r.intersects(w) for r in robot_segments)
if self.colliding:
break
# If we are, then don't move.
client.message(COLLIDES_TOPIC(self.port), self.colliding)
if not self.colliding:
self.position = (x, y, theta)
client.message(POSITION_TOPIC(self.port), self.position)
# Calculate sonars, and send them.
sonars = {}
# Create sonar beams in [MIN_SONAR_RANGE,MAX_SONAR_RANGE]
# Then find the walls it collides with, and choose the nearest one.
x, y, theta = self.position
px, py = transform((ix, iy, itheta), (x, y))
pos = (px, py, itheta + theta)
for i, (sx, sy, stheta) in enumerate(model.sonar_poses):
(x, y) = transform(pos, (sx, sy))
origin = (x, y, itheta + theta + stheta)
p1 = geom.Point(*transform(origin, (MIN_SONAR_RANGE, 0)))
p2 = geom.Point(*transform(origin, (MAX_SONAR_RANGE, 0)))
beam = geom.Segment(p1, p2)
intersects = (beam.intersection_point(w) for w in self.walls)
intersects = (i for i in intersects if i is not None)
ranges = (p1.distance(i) for i in intersects)
try:
r = min(ranges)
r = int((r + MIN_SONAR_RANGE) *
1000) # This is what the robot sends
except ValueError: # No intersections
r = None
sonars[i] = r
client.message(SONARS_TOPIC(self.port), sonars)
def step(self,dt,expect_paused=True):
client.message(INITIAL_TOPIC(self.port),self.initial)
if expect_paused and (not self.paused.is_set()):
return
x,y,theta = self.position
x += self.v*cos(theta)*dt
y += self.v*sin(theta)*dt
theta += self.omega*dt
ix,iy,itheta = self.initial
# Check if we are colliding.
# Build robot segments
robot_segments = []
px,py = transform((ix,iy,itheta),(x,y))
pos = (px,py,itheta+theta-pi/2.)
robot_points = (transform(pos,point) for point in model.points)
prev = None
first = None
for p in robot_points:
if prev is not None:
robot_segments.append(geom.Segment(geom.Point(*prev),geom.Point(*p)))
else:
first = p
prev = p
robot_segments.append(geom.Segment(geom.Point(*prev),geom.Point(*first)))
for w in self.walls:
self.colliding = any(r.intersects(w) for r in robot_segments)
if self.colliding:
break
# If we are, then don't move.
client.message(COLLIDES_TOPIC(self.port),self.colliding)
if not self.colliding:
self.position = (x,y,theta)
client.message(POSITION_TOPIC(self.port),self.position)
# Calculate sonars, and send them.
sonars = {}
# Create sonar beams in [MIN_SONAR_RANGE,MAX_SONAR_RANGE]
# Then find the walls it collides with, and choose the nearest one.
x,y,theta = self.position
px,py = transform((ix,iy,itheta),(x,y))
pos = (px,py,itheta+theta)
for i,(sx,sy,stheta) in enumerate(model.sonar_poses):
(x,y) = transform(pos,(sx,sy))
origin = (x,y,itheta+theta+stheta)
p1 = geom.Point(*transform(origin,(MIN_SONAR_RANGE,0)))
p2 = geom.Point(*transform(origin,(MAX_SONAR_RANGE,0)))
beam = geom.Segment(p1,p2)
intersects = (beam.intersection_point(w) for w in self.walls)
intersects = (i for i in intersects if i is not None)
ranges = (p1.distance(i) for i in intersects)
try:
r = min(ranges)
r = int((r+MIN_SONAR_RANGE)*1000) # This is what the robot sends
except ValueError: # No intersections
r = None
sonars[i] = r
client.message(SONARS_TOPIC(self.port),sonars)
def brain_background(msg):
global paused,g_step,g_robot,g_timer,g_period
assert not stop.is_set(), "The brain is terminated"
assert msg in (PAUSE_MSG,CONTINUE_MSG,STEP_MSG,CLOSE_MSG), "Message not recognized"
if msg == PAUSE_MSG:
if g_timer is not None:
g_timer.cancel()
g_robot.stopAll()
paused.set()
elif msg == CONTINUE_MSG:
paused.clear()
step_thread()
elif msg == STEP_MSG:
g_step(g_robot)
client.message(SIM_STEP_MSG,g_period)
elif msg == CLOSE_MSG:
terminate()
client.terminate()
def reload_cmd(self):
assert self.brain is not None and (self.real_set or self.map is not None)
client.message(BRAIN_MSG,CLOSE_MSG)
client.message(OPEN_MSG,PIONEER_PROC if self.real_set else SIM_PROC(self.map))
client.message(OPEN_MSG,BRAIN_PROC(self.brain))
def step_cmd(self):
assert self.brain is not None and not self.real_set and self.map is not None
client.message(BRAIN_MSG,STEP_MSG)
def pause_cmd(self):
assert self.brain is not None and (self.real_set or self.map is not None)
client.message(BRAIN_MSG,PAUSE_MSG)
def start_cmd(self):
assert self.brain is not None and (self.real_set or self.map is not None)
client.message(BRAIN_MSG,CONTINUE_MSG)
def brain_cmd(self):
new_brain = filedialog.askopenfilename(**self.file_opt)
if new_brain != '':
self.brain = new_brain
client.message(BRAIN_MSG,CLOSE_MSG)
client.message(OPEN_MSG,BRAIN_PROC(self.brain))
def map_cmd(self):
self.map = filedialog.askopenfilename(**self.file_opt)
client.message(OPEN_MSG,GUI_PROC(self.map))
def sim_cmd(self):
assert self.map is not None
self.real_set = False
self.pause_cmd()
client.message(SIM_MSG,CLOSE_MSG)
client.message(OPEN_MSG,SIM_PROC(self.map))
def real_cmd(self):
self.real_set = True
self.pause_cmd()
client.message(SIM_MSG,CLOSE_MSG)
client.message(OPEN_MSG,PIONEER_PROC)
if __name__ == "__main__":
app = SoarUI(None)
app.title("Snakes On A Robot")
app.mainloop()
client.message(CLOSE_MSG)
def setForward(self,v):
assert isinstance(v,numbers), "Forward velocity should be number"
client.message(SPEED_TOPIC(self.port),v)
def sim_cmd(self):
assert self.map is not None
self.real_set = False
self.pause_cmd()
client.message(SIM_MSG,CLOSE_MSG)
client.message(OPEN_MSG,SIM_PROC(self.map))
def __update_io__(self, io_pac):
client.message(ANALOG_INPUTS_TOPIC(self.port),io_pac.analogInputs)
def __update_sip__(self, sip_pac):
position = (sip_pac.x/1000.,sip_pac.y/1000.,sip_pac.theta) # in meters
client.message(SONARS_TOPIC(self.port),sip_pac.sonars)
client.message(POSITION_TOPIC(self.port),position)
def setVoltage(self,v):
assert isinstance(v,numbers), "Voltage should be number"
client.message(VOLTAGE_TOPIC(self.port),v)
def setRotational(self,omega):
assert isinstance(omega,numbers), "Rotational velocity should be number"
client.message(OMEGA_TOPIC(self.port),omega)
def brain_cmd(self):
new_brain = filedialog.askopenfilename(**self.file_opt)
if new_brain != '':
self.brain = new_brain
client.message(BRAIN_MSG,CLOSE_MSG)
client.message(OPEN_MSG,BRAIN_PROC(self.brain))
def map_cmd(self):
self.map = filedialog.askopenfilename(**self.file_opt)
client.message(OPEN_MSG,GUI_PROC(self.map))
def __update_sip__(self, sip_pac):
position = (sip_pac.x / 1000., sip_pac.y / 1000., sip_pac.theta
) # in meters
client.message(SONARS_TOPIC(self.port), sip_pac.sonars)
client.message(POSITION_TOPIC(self.port), position)
def real_cmd(self):
self.real_set = True
self.pause_cmd()
client.message(SIM_MSG,CLOSE_MSG)
client.message(OPEN_MSG,PIONEER_PROC)
def __update_io__(self, io_pac):
client.message(ANALOG_INPUTS_TOPIC(self.port), io_pac.analogInputs)
