def sendStopCommand(reason):
# command 4
msg = b'4' + b'\n'
cartGlobal.log(f"Send stop, reason: {reason}")
ser.write(msg)
cartGlobal.setCartMoving(False)
def moveCart(self):
cartSpeed=int(self.sbSpeed.get())
distance=int(self.sbDist.get())
cartGlobal.log(f"moveCart, speed: {cartSpeed}, distance: {distance}")
arduino.sendMoveCommand(self.direction, cartSpeed, distance)
self.lblCommandValue.configure(text="Move")
#self.lblMove.configure(text=str(speed))
self.w.update_idletasks()
def estimateUntrackedMoveInPix(untrackedMoveTime):
calculatedDxPix = _lastDxPixPerSec * untrackedMoveTime
calculatedDyPix = _lastDyPixPerSec * untrackedMoveTime
#log(f"calculated dx,dy, _lastDxPixPerSec: {_lastDxPixPerSec:.3f}, _lastDyPixPerSec: {_lastDyPixPerSec:.3f}, _lastTrackedMoveDuration: {_lastTrackedMoveDuration:.3f}")
distancePix = np.hypot(calculatedDxPix, calculatedDyPix)
speed = distancePix / untrackedMoveTime
cartGlobal.log(f"untrackedMove duration: {untrackedMoveTime:.4f}, distancePix: {distancePix:.2f}, speed [pix/s]: {speed:.2f}")
return calculatedDxPix, calculatedDyPix
def startArduino(self):
cartGlobal.log("try to open serial connection to arduino on cart (COM5)")
arduino.initSerial("COM5")
self.lblInfo.configure(text = "arduino connected, waiting for cart ready message", bg="white smoke", fg="orange")
self.btnArduino.configure(state = "disabled")
self.w.update_idletasks()
self.w.after(2000, self.checkArduinoReady)
def trackCartMovements():
global cap, camConnected, orb, bf
cartGlobal.log("trackCartMovements started")
if not camConnected:
cap = cv2.VideoCapture(1)
cap.set(cv2.CAP_PROP_FPS, 120)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
cartGlobal.log(f"cam params fps: {cap.get(cv2.CAP_PROP_FPS)}, w: {cap.get(cv2.CAP_PROP_FRAME_WIDTH)}, h: {cap.get(cv2.CAP_PROP_FRAME_HEIGHT)}")
time.sleep(0.5) # allow cam to adjust to light
# remove tracking jpg's from previous run
for imgFile in glob.glob("C:/cartControl/floorImages/*.jpg"):
os.remove(imgFile)
for _ in range(2): # first frame is black
ret, img = cap.read()
if ret:
cartGlobal.log("cam connected")
cv2.imshow("initial frame", img)
cv2.waitKey(500)
cartGlobal.saveImg(img, 0)
cv2.destroyAllWindows()
camConnected = True
# Initiate ORB detector (fastThreshold has highest impact on number of candidate points seen)
orb = cv2.ORB_create(nfeatures=250, edgeThreshold=8, patchSize=18, fastThreshold=6, scoreType=cv2.ORB_FAST_SCORE) # 2.5.2018
# create BFMatcher object
bf = cv2.BFMatcher()
else:
cartGlobal.log("no cam found")
raise SystemExit()
while True:
if cartGlobal.isCartMoving():
doOdometry()
cartGlobal.log(f"odometry, cart stopped, moveDistance: {cartGlobal.currMoveDistance():.0f}")
def exposed_heartBeat(self):
global lastMessage
# watchdog for incoming commands from navManager
# stop cart if we do not get regular messages
currTime = int(round(time.time() * 1000))
cartGlobal.log(currTime - lastMessage)
if currTime - lastMessage > TIMEOUT:
if cartGlobal.isCartMoving:
arduino.sendStopCommand("missing heartbeat from navManager")
else:
arduino.sendHeartbeat()
cartGlobal.log("arduino Heartbeat sent")
lastMessage = int(round(time.time() * 1000))
def exposed_startListening(self, logIP, logPort):
'''
a first call from navManager
'''
if not cartGlobal.standAloneMode:
try:
cartGlobal.navManager = rpyc.connect(logIP, logPort)
cartGlobal.navManager.root.connectionStatus("cart", True)
cartGlobal.log("logger connection established")
except:
print("cart - could not establish connection to logger")
raise SystemExit()
if not cartGlobal.standAloneMode:
cartGlobal.navManager.root.processStatus("cart", True)
def sendRotateCommand(relAngle, speed):
ROTATION_SPEED = 200
cartGlobal.setMovementBlocked(False)
cartGlobal.setTargetOrientation(relAngle)
cartGlobal.setRequestedCartSpeed(speed)
# command 2
# msg = f"b,{cartGlobal.SPEED_FACTOR:07.4f},{cartGlobal.SPEED_EXPONENT:07.4f},{cartGlobal.SPEED_FACTOR_SIDEWAY:05.2f}"
# ser.write(bytes(msg + '\n', 'ascii'))
if relAngle < 0: # rotate counterclock
msg = f"2,{abs(relAngle):03.0f},{speed:03.0f}"
cartGlobal.log(f"Send rotate counterclock {msg}")
ser.write(bytes(msg + '\n', 'ascii'))
# command 3
if relAngle > 0:
msg = f"3,{abs(relAngle):03.0f},{speed:.03f}"
cartGlobal.log(f"Send rotate clockwise {msg}")
ser.write(bytes(msg + '\n', 'ascii'))
def checkDecelerate():
global _lastCheckDecelerate
_lastCheckDecelerate = time.time()
if cartGlobal.isCartMoving():
cartSpeed = cartGlobal.getRequestedCartSpeed()
remainingDist = cartGlobal.getRemainingDistance()
#cartGlobal.log(f"check decelerate move, remainingDist: {int(remainingDist)}, currSpeed: {cartSpeed}")
if cartSpeed * remainingDist / 80 < cartSpeed:
cartSpeed -= 30
cartGlobal.log(f"decelerate, remainingDist: {remainingDist:.0f}, new speed: {cartSpeed:.0f}")
arduino.sendSpeedCommand(cartSpeed)
if cartGlobal.isCartRotating():
#cartGlobal.log(f"check decelerate rotation, restRot: {restRot}, currSpeed: {cartSpeed}")
if cartSpeed * cartGlobal.getRemainingRotation() / 5 < cartGlobal.getRequestedCartSpeed():
cartSpeed -= 20
arduino.sendSpeedCommand(cartSpeed)
def initSerial(comPort):
global ser
cartGlobal.arduinoStatus = 0
while True:
try:
ser = serial.Serial(comPort)
# try to reset the arduino
ser.setDTR(False)
time.sleep(0.1)
ser.setDTR(True)
ser.baudrate = 115200
ser.writeTimeout = 0
cartGlobal.log("Serial comm to arduino established")
return
except:
cartGlobal.log(
f"exception on serial connect with {comPort} {sys.exc_info()[0]}"
)
time.sleep(3)
def sendMoveCommand(direction, speed, distanceMm):
'''
send move command to cart
distance calculation is based on time travelled and requested speed
if final orientation <> initial orientation send a rotate command at the end to end up in same orientation
'''
# conmmand 1,<dir>,<speed>,<distance>,<max duration>
# e.g. forward, speed=180, distance=100 mm, max duration=5 s: 1,0,180,0100,0005
distanceMmLimited = min(distanceMm,
2500) # limit distance for single command
cartGlobal.setMoveDirection(direction)
cartGlobal.setCartMoveDistance(distanceMmLimited)
cartGlobal.setRequestedCartSpeed(speed)
moveDuration = ((distanceMmLimited / speed) * 800) + 1500
if direction in [
cartGlobal.Direction.LEFT.value, cartGlobal.Direction.RIGHT.value
]:
moveDuration = int(moveDuration / cartGlobal.SPEED_FACTOR_SIDEWAY)
if direction in [
cartGlobal.Direction.BACK_DIAG_LEFT,
cartGlobal.Direction.BACK_DIAG_RIGHT,
cartGlobal.Direction.FOR_DIAG_LEFT,
cartGlobal.Direction.FOR_DIAG_RIGHT
]:
moveDuration = int(moveDuration / cartGlobal.SPEED_FACTOR_DIAGONAL)
durationLimited = min(moveDuration,
9999) # do not move more than 10 seconds
cartGlobal.setMovementBlocked(False)
cartGlobal.setCartMoving(True)
moveMsg = f"1,{direction},{speed:03.0f},{distanceMmLimited:04.0f},{durationLimited:04.0f}"
ser.write(bytes(moveMsg + "\n", 'ascii'))
cartGlobal.log(
f"Send move {moveMsg}, speed: {speed:.0f}, distance: {distanceMmLimited:.0f}, duration: {durationLimited:.0f}"
)
def doOdometry():
global matchFailures, _lastDxPixPerSec, _lastDyPixPerSec
saveFloorImages = True
goodFrames = 0
badFrames = 0
frameNr = 0
failures = np.zeros(10)
cartGlobal.log("cart starts moving")
# check for working cam and take a start image
while True:
# take a start image
img1Timestamp = time.time()
img2Timestamp = time.time()
for _ in range(2): # first frame is outdated
ret, img2 = cap.read()
if ret:
if saveFloorImages:
cartGlobal.saveImg(img2, frameNr)
# use bw image for comparison
img2bw = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# find relevant points in image
kps2, des2 = orb.detectAndCompute(img2bw, None)
_lastDxPixPerSec = 0
_lastDyPixPerSec = 0
break
else:
cartGlobal.log("cam problems, no start frame")
frameSkipped = False
# while cart is moving
# take a ground pic and compare it with the previous one
# find distance travelled in mm and update current position
########################################################
while cartGlobal.isCartMoving():
# check for problems with last frame comparison, compensate with last good move
if frameSkipped:
badFrames +=1
# based on last good move and time since last eval update cart position
duration = img2Timestamp - img1Timestamp
dxPix, dyPix = estimateUntrackedMoveInPix(duration)
# cartGlobal.log(f" frame comparison failed with frame {frameNr}!, using last good dx/dy {dxPix:.3f},{dyPix:.3f}")
cartGlobal.updateCartPositionInMm(dxPix, dyPix, duration)
else:
goodFrames += 1
# check for slowing down
checkDecelerate()
# check for requested distance travelled
if cartGlobal.checkMoveDistanceReached():
arduino.sendStopCommand("distance reached")
cartGlobal.log(f"move distance reached: target distance: {cartGlobal._moveDistanceRequested}, travelled distance: {cartGlobal.currMoveDistance()}, move time: {time.time() - cartGlobal._moveStartTime:.2f}")
posX, posY = cartGlobal.getCartPosition()
cartGlobal.log(f"current position {posX}/{posY}")
frameSkipped = True # assume we have a problem detecting movement
frameNr += 1
cartGlobal.log("")
cartGlobal.log(f"cart is moving, check progress, current frame: {frameNr}")
#cartGlobal.setDistEvalTimestamp()
# use last taken image as previous image
img1bw = img2bw[:]
kps1 = kps2[:]
des1 = des2[:]
img1Timestamp = img2Timestamp
# get new image
img2Timestamp = time.time()
for _ in range(2):
ret, img2 = cap.read()
if ret:
if saveFloorImages:
cartGlobal.saveImg(img2, frameNr)
img2bw = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
kps2, des2 = orb.detectAndCompute(img2bw, None)
else:
#cartGlobal.log("cam problems, frame skipped")
failures[0] += 1
continue # retry
try:
matches = bf.knnMatch(des1, des2, k=2)
except:
#cartGlobal.log("knnMatch failure")
failures[1] += 1
continue
if not matches:
#cartGlobal.log(f"No matches from bf.knnMach!")
failures[2] += 1
continue
# get good matches (g_matches) as per Lowe's ratio
g_match = []
if np.shape(matches)[1] != 2:
#cartGlobal.log(f"no valid matches pairs found, frame: {frameNr}")
failures[3] += 1
continue
for m, n in matches:
if m.distance < 0.8 * n.distance:
g_match.append(m)
if len(g_match) < MIN_CANDIDATES:
#cartGlobal.log(f"not enough matches pairs found, frame: {frameNr}")
failures[4] += 1
continue
else:
# get source and target points
src_pts = np.float32([ kps1[m.queryIdx].pt for m in g_match ]).reshape(-1,1,2)
dst_pts = np.float32([ kps2[m.trainIdx].pt for m in g_match ]).reshape(-1,1,2)
#cartGlobal.log(f"src/dst points {len(src_pts)}/{len(dst_pts)}")
#_, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
_, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
matchesMask = mask.ravel().tolist()
# try to find x/y distance moved
src = [src_pts for (src_pts, matchesMask) in zip(src_pts, matchesMask) if matchesMask > 0]
dst = [dst_pts for (dst_pts, matchesMask) in zip(dst_pts, matchesMask) if matchesMask > 0]
#if len(src) < MIN_CANDIDATES + 5:
if len(src) < MIN_CANDIDATES:
#cartGlobal.log(f"not enough matches after findHomography, frame: {frameNr}")
failures[5] += 1
continue
# use only similar angle/distance values
if not similarAngleDistance(src, dst):
#cartGlobal.log(f"not enough similar angles/distancies found, frame: {frameNr}")
failures[6] += 1
continue
#try:
xSum, ySum = 0, 0
numCompare = min(len(src), MIN_CANDIDATES)
for i in range(numCompare):
xSum += src[i][0][0]-dst[i][0][0]
ySum += src[i][0][1]-dst[i][0][1]
if numCompare:
dxPix = xSum / numCompare
dyPix = ySum / numCompare
duration = img2Timestamp - img1Timestamp
distance = np.hypot(dxPix, dyPix)
_lastDxPixPerSec = dxPix / (duration)
_lastDyPixPerSec = dyPix / (duration)
#cartGlobal.log(f"dxPix {dxPix:.4f}, dyPix {dyPix:.4f}, duration: {img2Timestamp-img1Timestamp:.4f}, speed: {distance/duration:.4f}")
frameSkipped = False
cartGlobal.updateCartPositionInMm(dxPix, dyPix, duration)
else:
#cartGlobal.log(f"no src points")
failures[7] += 1
#cartGlobal.log(f"time used for frame comparison {time.time()-start:.4f}")
# move done
cartGlobal.log(f"frames: {goodFrames+badFrames}, bad frames: {badFrames}")
if failures[0] > 0:
cartGlobal.log(f"cam problems, frame skipped: {failures[0]:.0f}")
if failures[1] > 0:
cartGlobal.log(f"knnMatch failure: {failures[1]:.0f}")
if failures[2] > 0:
cartGlobal.log(f"No matches from bf.knnMach: {failures[2]:.0f}")
if failures[3] > 0:
cartGlobal.log(f"no valid matches pairs found, {failures[3]:.0f}")
if failures[4] > 0:
cartGlobal.log(f"not enough matches pairs found: {failures[4]:.0f}")
if failures[5] > 0:
cartGlobal.log(f"not enough matches after findHomography: {failures[5]:.0f}")
if failures[6] > 0:
cartGlobal.log(f"not enough similar angles/distancies found: {failures[6]:.0f}")
if failures[7] > 0:
cartGlobal.log(f"no src points: {failures[7]:.0f}")
def exposed_rotateRelative(self, angle, speed):
if abs(angle) > 0:
cartGlobal.log(f"exposed_rotateRelative, angle: {angle:.1f}")
arduino.sendRotateCommand(int(angle), int(speed))
gui.controller.updateTargetRotation(cartGlobal.getOrientation() +
int(angle))
def exposed_move(self, direction, speed, distance=10):
cartGlobal.log(
f"exposed_move, direction: {direction}, speed: {speed}, distance: {distance}"
)
arduino.sendMoveCommand(direction, speed, distance)
def navigateTo(self):
start=time.time()
#PathFinder.analyze((int(self.sbX.get()),int(self.sbY.get())))
cartGlobal.log(f"analyzed in: {time.time() - start} seconds")
self.w.update_idletasks()
def exposed_stop(self):
cartGlobal.log("exposed_stop")
gui.controller.stopCart()
def showNewDistances(self):
#cartGlobal.log("showNewDistancies")
cartControl.obstacleInfo=[]
r = cartControl.NUM_DISTANCE_SENSORS
for i in range(r):
s=cartControl.distanceSensors[i]
d=cartControl.distanceList[i]
sensor=sensors[i]
# check for new data
if s['newValuesShown']:
continue
cartControl.setSensorDataShown(i, True)
#cartGlobal.log(f"show new sensor data {i}")
# clear area
col="white"
if sensor.id < 4:
self.canvas.create_rectangle(sensor.x1 - 20, sensor.y1 - 10, sensor.x2 + 20, sensor.y, fill=col, outline=col)
if sensor.id > 5:
self.canvas.create_rectangle(sensor.x1 - 20, sensor.y, sensor.x2 + 20, sensor.y + 40, fill=col, outline=col)
if sensor.id == 4:
self.canvas.create_rectangle(sensor.x1 - 20, sensor.y - 40, sensor.x, sensor.y + 40, fill=col, outline=col)
if sensor.id == 5:
self.canvas.create_rectangle(sensor.x, sensor.y - 40, sensor.x + 20, sensor.y + 40, fill=col, outline=col)
# show valid range of distance measures
self.canvas.create_arc(sensor.x1, sensor.y1, sensor.x2, sensor.y2,
start=sensor.arcFrom, extent=sensor.arcLength, style=tk.ARC, width=sensor.arcWidth, outline="snow3")
if s.get('range') == 'long':
lengthFactor=LINE_SCALE_LONG
minRange=cartGlobal.LONG_RANGE_MIN
maxRange=cartGlobal.LONG_RANGE_MAX
else:
lengthFactor=LINE_SCALE_SHORT
minRange=cartGlobal.SHORT_RANGE_MIN
maxRange=cartGlobal.SHORT_RANGE_MAX
for k in range(cartControl.NUM_MEASUREMENTS_PER_SCAN):
# line color depends on distance value
age=time.time() - s.get('timestamp')
# limit line length
if d[k] < 40:
lineLength=d[k]
else:
lineLength=40
if lineLength < minRange or lineLength > maxRange:
cartControl.obstacleInfo.append({'direction': s.get('direction'), 'position': s.get('position')})
if age > 2:
col="coral1"
lineWidth = 2
else:
col="red"
lineWidth = 2
else:
if age > 2:
col="paleGreen"
lineWidth = 1
else:
col="green"
lineWidth = 1
# Angle = value index * 15 + const + start rotation of arc
xOffset=np.cos(np.radians(k * 15 + 15 + s.get('rotation'))) * lineLength * lengthFactor
yOffset=np.sin(np.radians(k * 15 + 15 + s.get('rotation'))) * lineLength * lengthFactor
try:
self.canvas.create_line(sensor.x, sensor.y, sensor.x + xOffset, sensor.y + yOffset, fill=col, width=lineWidth)
except:
cartGlobal.log("ERROR: sensor.x,.y: " + str(sensor.x) + ", " + str(sensor.y))
def readMessages():
import cartControl
global ser
while ser is None:
time.sleep(0.1)
while ser.is_open:
if ser.inWaiting() > 0:
#cartGlobal.log(f"inWaiting > 0")
recvB = ser.readline(120)
try:
recv = recvB.decode()
except:
cartGlobal.log(f"problem with decoding cart msg '{recvB}'")
#cartGlobal.log(f"line read {recv}")
msgID = recvB[0:3].decode()
#cartGlobal.log("in read message: ", msgID)
if msgID == "!A0": #"cart ready"
cartGlobal.arduinoStatus = 1
cartGlobal.log("cart ready")
sendConfigValues()
elif msgID == "!A1": # distance values from obstacle sensors
#!A1,<sensorID>,<ANZ_MESSUNGEN_PRO_SCAN>,[ANZ_MESSUNGEN_PRO_SCAN<value>,]
messageItems = [
int(e) if e.isdigit() else e for e in recv.split(',')
]
cartControl.updateDistances(messageItems[1:])
#gui.controller.showDistances(messageItems[1]) test, could be responsible for delays in message read?
elif msgID == "!A2": #"obstacle:":
#!A2,<shortest distance>,<farthest distance>,<first sensorId with exceeding range>
try:
distanceShort = float(recv.split(",")[1])
distanceLong = float(recv.split(",")[2])
sensorID = int(recv.split(",")[3])
except:
cartGlobal.log(
f"exception with message: '{recv[:-2]}', error in message structure"
)
distanceShort = -1
distanceLong = -1
cartGlobal.setMovementBlocked(True)
if distanceShort < distanceLong:
distance = distanceShort
else:
distance = distanceLong
gui.controller.updateDistanceSensorObstacle(distance, sensorID)
gui.controller.updateDistanceSensorAbyss("", -1)
elif msgID == "!A3": #"abyss:":
#!A2,<farthest distance>,<farthest distance>,<first sensorId with exceeding range>
try:
distanceShort = float(recv.split(",")[1])
distanceLong = float(recv.split(",")[2])
sensorID = int(recv.split(",")[3])
except:
cartGlobal.log(
f"exception with message: '{recv[:-2]}', error in message structure"
)
distanceShort = -1
distanceLong = -1
if distanceShort > distanceLong:
distance = distanceShort
else:
distance = distanceLong
cartGlobal.setMovementBlocked(True)
gui.controller.updateDistanceSensorAbyss(distance, sensorID)
gui.controller.updateDistanceSensorObstacle("", -1)
elif msgID == "!A4": # free path after move blocked
cartGlobal.setMovementBlocked(False)
gui.controller.updateDistanceSensorAbyss("", -1)
gui.controller.updateDistanceSensorObstacle("", -1)
elif msgID == "!A5": #"cart stopped":
# !A5,<orientation>,<distance moved>
orientation = round(float(recv.split(",")[1]))
distance = round(float(recv.split(",")[2]))
direction = int(recv.split(",")[3])
cartGlobal.setOrientation(
orientation) # delays with A9 occurred
cartGlobal.calculateNewCartPosition(orientation, distance,
direction)
cartGlobal.setMovementBlocked(False)
cartGlobal.setCartRotating(False)
cartGlobal.setCartMoving(False)
cartGlobal.log("<-A " + recv[:-2]) # stop and reason
cartGlobal.log("!A5 cart stopped received from Arduino")
# persist position
# do this on the cart as cart might be moved locally not through navManager
cartGlobal.saveCartLocation()
elif msgID == "!Aa": # cart position update:
# !Aa,<orientation>,<distance moved>
orientation = round(float(recv.split(",")[1]))
distance = round(float(recv.split(",")[2]))
direction = int(recv.split(",")[3])
cartGlobal.setOrientation(orientation)
if distance > 0:
cartGlobal.calculateNewCartPosition(
orientation, distance, direction)
cartGlobal.log(f"!Aa, distance: {distance}")
elif msgID == "!A6": # cart docked
# Arduino triggers the relais for loading batteries through the docking contacts
# gui update by heartbeat logic
cartGlobal.log("cart docked")
if cartGlobal.navManager is not None:
cartGlobal.navManager.root.cartDocked(True)
elif msgID == "!A7": # cart docked
# Arduino releases the relais for loading batteries through the docking contacts
cartGlobal.log("cart undocked")
if cartGlobal.navManager is not None:
cartGlobal.navManager.root.cartDocked(False)
elif msgID == "!A8": #12 V supply, measured value
cartGlobal.log(f"{recv[:-2]} received")
try:
newVoltage12V = round(float(recv.split(",")[1]))
except:
cartGlobal.log(
"!A8 not able to retrieve voltage value {recv[:-2]}")
newVoltage12V = cartGlobal.getVoltage12V()
# test for change in Voltage
if abs(newVoltage12V - cartGlobal.getVoltage12V()) > 500:
cartGlobal.setVoltage12V(newVoltage12V)
cartGlobal.log(
f"new 12V voltage read [mV]: {newVoltage12V}")
elif msgID == "!A9": # orientation x:":
#!A9,<orientation>
try:
newOrientation = round(float(recv.split(",")[1]))
except:
cartGlobal.log(
"!A9 not able to retrieve orientation {recv[:-2]}")
newOrientation = cartGlobal.getOrientation()
# test for change of orientation
if cartGlobal.getOrientation() != newOrientation:
cartGlobal.setOrientation(newOrientation)
#cartGlobal.log(f"new cart orientation: {newOrientation}")
# if cart is in rotation no blocking exists
if cartGlobal.getMovementBlocked()[0]:
cartGlobal.setMovementBlocked(False)
# no new orientation, check for blocked movement and stop cart
else:
blocked, startTime = cartGlobal.getMovementBlocked()
if blocked:
if time.time() - startTime > 3:
if cartGlobal.isCartMoving():
#cartGlobal.log("cart stopped by MovementBlocked")
sendStopCommand("blocked movement")
cartGlobal.setMovementBlocked(False)
else:
try:
cartGlobal.log("<-A " + recv[:-2])
except:
cartGlobal.log(
f"Unexpected error on reading messages: {sys.exc_info()[0]}"
)
#cartGlobal.log("msg processed")
time.sleep(0.001) # give other threads a chance
def sendReadSensorCommand(sensorID):
msg = b'7' + bytes(str(sensorID), 'ascii') + b'\n'
cartGlobal.log("Send readSensor " + str(msg))
ser.write(msg)
def exposed_getCartInfo(self):
posX, posY = cartGlobal.getCartPosition()
return cartGlobal.getOrientation(
), posX, posY, cartGlobal.isCartMoving(), cartGlobal.isCartRotating()
def exposed_getBatteryStatus(self):
return cartGlobal.getBatteryStatus()
def exposed_isCartMoving(self):
return cartGlobal.isCartMoving()
def exposed_isCartRotating(self):
return cartGlobal.isCartRotating()
if __name__ == '__main__':
windowName = "marvin//cartControl"
os.system("title " + windowName)
hwnd = win32gui.FindWindow(None, windowName)
win32gui.MoveWindow(hwnd, 280, 0, 1200, 400, True)
print("startup cart")
cartInit()
if not cartGlobal.standAloneMode:
cartGlobal.log("wait for messages from navManager ...")
t = ThreadedServer(cartCommands, port=CART_PORT)
t.start()