def __init__(self, root, robotx=0, roboty=0, robota=0, **kwargs):
self.robot = RobotView(
front=0.15,
rear=0.15,
left=0.15,
right=0.15,
wheeldistance=0.315,
wheeldiam=0.25,
wheelwidth=0.05,
x=robotx,
y=roboty,
a=robota,
)
self.llines = []
Viewer.__init__(self, root, **kwargs)
self.reset()
class Sick2016(Viewer):
def __init__(self, root, robotx=0, roboty=0, robota=0, **kwargs):
self.robot = RobotView(
front=0.15,
rear=0.15,
left=0.15,
right=0.15,
wheeldistance=0.315,
wheeldiam=0.25,
wheelwidth=0.05,
x=robotx,
y=roboty,
a=robota,
)
self.llines = []
Viewer.__init__(self, root, **kwargs)
self.reset()
def reset(self):
self.lasers = None
Viewer.reset(self)
def redraw(self):
self.canvas.delete("all")
self.draw_playfield()
self.robot.draw(self)
self.draw_lasers()
self.canvas.configure(scrollregion=self.canvas.bbox("all"))
def draw_playfield(self):
self.create_line([0, 0, 13.0, 0, 13.0, 7.0, 0, 7.0, 0, 0], activefill="red", fill="black", width=1),
self.create_polygon([(0.0, 2.0), (3.0, 2.0), (3.0, 5.0), (0.0, 5.0)], fill="cyan")
self.create_polygon([(4.0, 1.0), (9.0, 1.0), (9.0, 6.0), (4.0, 6.0)], fill="blue")
self.create_polygon([(10.0, 2.0), (13.0, 2.0), (13.0, 5.0), (10.0, 5.0)], fill="green")
self.create_polygon([(3.0, 1.0), (5.0, 1.0), (5.0, 3.0), (3.0, 3.0)], fill="yellow")
crosssize = 0.025
for x in range(14):
for y in range(8):
self.create_line([x - crosssize, y, x + crosssize, y], activefill="red", fill="black", width=2)
self.create_line([x, y - crosssize, x, y + crosssize], activefill="red", fill="black", width=2)
def redraw_lasers(self):
for i in self.llines:
self.canvas.delete(i)
self.draw_lasers()
def draw_lasers(self):
if self.lasers:
lx, ly = self.robot.transponse(0.24, -0.13)
la = self.robot.a
self.llines = []
for a, dist in self.lasers:
self.llines.append(
self.create_line(
[(lx, ly), (lx + cos(a + la) * dist, ly + sin(a + la) * dist)], fill="red", width=1
)
)
def MsgListener(self, time, pose, msgs):
if "RFU620LOG" in msgs and msgs["RFU620LOG"]:
_, scans = msgs["RFU620LOG"]
for scan in scans:
pass
# distance = getDistanceToTag(pose, scan.id)
# data=[time, pose[0], pose[1], pose[2], scan.id, scan.RSSI, distance ]
# self.output.write(';'.join(data)+'\n')
if "LASERLOG" in msgs:
# don draw old laser logs, read even the None
if msgs["LASERLOG"]:
self.lasers = [(radians(a), d / 1000.0) for a, d in zip(range(135, -136, -1), msgs["LASERLOG"])]
