|
|
|
|
|
|
|
|
import json |
|
|
import json |
|
|
import threading |
|
|
import threading |
|
|
import os |
|
|
import os |
|
|
|
|
|
import logging |
|
|
import raspy_sensors as Sensors |
|
|
import raspy_sensors as Sensors |
|
|
from defines import Topics, SENSORDATA |
|
|
from defines import Topics, SENSORDATA |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# TODO Error Message |
|
|
# TODO Error Message |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
print(f"Robot Error {errorCode} occurred") |
|
|
|
|
|
print("Drive Plant aborted, Robot at starting position") |
|
|
|
|
|
|
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
|
|
|
logging.error("Drive Plant aborted, Robot at starting position") |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
print("Measuring Sensors") |
|
|
|
|
|
|
|
|
logging.info("Measuring Sensors") |
|
|
measure_send_data(plantID, actionID, client) |
|
|
measure_send_data(plantID, actionID, client) |
|
|
|
|
|
|
|
|
print("Robot driving home") |
|
|
|
|
|
|
|
|
logging.info("Robot driving home") |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}') |
|
|
|
|
|
|
|
|
# TODO Error Message |
|
|
# TODO Error Message |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
print(f"Robot Error {errorCode} occurred") |
|
|
|
|
|
print(f"Drive Home aborted, Robot at plant {plantID}") |
|
|
|
|
|
|
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
|
|
|
logging.error(f"Drive Home aborted, Robot at plant {plantID}") |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
print("Robot home") |
|
|
|
|
|
|
|
|
logging.info("Robot home") |
|
|
|
|
|
|
|
|
#TODO decide about robot occupied message |
|
|
#TODO decide about robot occupied message |
|
|
#client.publish("ROBOT/DATA/OCCUPIED", "false") |
|
|
#client.publish("ROBOT/DATA/OCCUPIED", "false") |
|
|
|
|
|
|
|
|
plantID = dictMessage["PlantID"] |
|
|
plantID = dictMessage["PlantID"] |
|
|
actionID = dictMessage["ActionID"] |
|
|
actionID = dictMessage["ActionID"] |
|
|
|
|
|
|
|
|
print(f"Received Drive-request to plant {plantID}, ActionID: {actionID}") |
|
|
|
|
|
|
|
|
logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}") |
|
|
|
|
|
|
|
|
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True) |
|
|
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True) |
|
|
thread.start() |
|
|
thread.start() |
|
|
|
|
|
|
|
|
userdata (_type_): _description_ |
|
|
userdata (_type_): _description_ |
|
|
message (mqtt.MQTTMessage): received message |
|
|
message (mqtt.MQTTMessage): received message |
|
|
""" |
|
|
""" |
|
|
print("Received battery status request") |
|
|
|
|
|
|
|
|
logging.info("Received battery status request") |
|
|
|
|
|
|
|
|
try: |
|
|
try: |
|
|
batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read()) |
|
|
batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read()) |
|
|
except: |
|
|
except: |
|
|
print("Robot not connected") |
|
|
|
|
|
|
|
|
logging.error("Robot not connected") |
|
|
#TODO Error Message |
|
|
#TODO Error Message |
|
|
return |
|
|
return |
|
|
|
|
|
|