def moveAuto(self, callback):
if not self.on() or self.path is None:
callback(None, None, None)
return
if self.moveIndex < len(self.path):
time.sleep(3)
reset = False
dest = self.directions[self.moveIndex]
if self.moveIndex > 0:
start = self.directions[self.moveIndex - 1]
else:
start = dest
reset = True
motion = convertToMotion(self.topics.cmd, start, dest, self.dist)
self.currentDirection = dest
y, x = self.path[self.moveIndex]
self.current = (y, x)
self.moveIndex += 1
formatMeasurements = self.formatMeasurements
self.server.connection.sense(
lambda Z: callback(motion, formatMeasurements(dest, Z), reset), normalizeCmd(self.topics.cmd, motion[0])
)
def moveCmd(self, callback):
if not self.on() or self.cmd is None:
callback(None, None, None)
return
cmdTopic = self.topics.cmd
pathDelta = {cmdTopic.left: [0, -1], cmdTopic.down: [1, 0], cmdTopic.up: [-1, 0], cmdTopic.right: [0, 1]}
newDelta = pathDelta[self.cmd]
y = self.current[0] + newDelta[0]
x = self.current[1] + newDelta[1]
self.current = [y, x]
reset = False
if self.currentDirection is None:
self.currentDirection = self.cmd
reset = True
motion = convertToMotion(cmdTopic, self.currentDirection, self.cmd, self.dist)
self.currentDirection = self.cmd
cmd = self.cmd
self.cmd = None
formatMeasurements = self.formatMeasurements
self.server.connection.sense(
lambda Z: callback(motion, formatMeasurements(cmd, Z), reset), normalizeCmd(self.topics.cmd, motion[0])
)
