|
|
|
|
|
|
|
|
try: |
|
|
try: |
|
|
sensorData = Sensors.readSensors() |
|
|
sensorData = Sensors.readSensors() |
|
|
except Exception as e: |
|
|
except Exception as e: |
|
|
print(str(e)) |
|
|
|
|
|
|
|
|
logging.error(str(e)) |
|
|
# TODO Error message MQTT |
|
|
# TODO Error message MQTT |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1) |
|
|
|
|
|
|
|
|
sensorData["PlantID"] = plantID |
|
|
sensorData["PlantID"] = plantID |
|
|
sensorData["ActionID"] = actionID |
|
|
sensorData["ActionID"] = actionID |
|
|
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4)) |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def drive_plant_thread(plantID, actionID, client: mqtt.Client): |
|
|
def drive_plant_thread(plantID, actionID, client: mqtt.Client): |
|
|
|
|
|
|
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') |
|
|
# errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}') |
|
|
# errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}') |
|
|
|
|
|
|
|
|
# TODO Error Message |
|
|
|
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error("Drive Plant aborted, Robot at starting position") |
|
|
|
|
|
|
|
|
logging.info("Drive Plant aborted, Robot at starting position") |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1) |
|
|
|
|
|
#TODO decide about robot occupied message |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
|
|
|
|
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
logging.info("Measuring Sensors") |
|
|
logging.info("Measuring Sensors") |
|
|
|
|
|
|
|
|
logging.info("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 |
|
|
|
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Drive Home aborted, Robot at plant {plantID}") |
|
|
|
|
|
|
|
|
logging.info(f"Drive Home aborted, Robot at plant {plantID}") |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1) |
|
|
|
|
|
#TODO decide about robot occupied message |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
logging.info("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(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#region MQTT callbacks |
|
|
#region MQTT callbacks |
|
|
|
|
|
|
|
|
def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
|
|
|
|
|
|
def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
""" |
|
|
""" |
|
|
Function to drive to plant according to request |
|
|
Function to drive to plant according to request |
|
|
Starting Drive in Thread to not block main programm |
|
|
Starting Drive in Thread to not block main programm |
|
|
|
|
|
|
|
|
message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID |
|
|
message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID |
|
|
""" |
|
|
""" |
|
|
dictMessage = json.loads(str(message.payload.decode("UTF-8"))) |
|
|
dictMessage = json.loads(str(message.payload.decode("UTF-8"))) |
|
|
|
|
|
|
|
|
|
|
|
print(dictMessage) |
|
|
|
|
|
|
|
|
plantID = dictMessage["PlantID"] |
|
|
plantID = dictMessage["PlantID"] |
|
|
actionID = dictMessage["ActionID"] |
|
|
actionID = dictMessage["ActionID"] |
|
|
|
|
|
|
|
|
logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}") |
|
|
logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}") |
|
|
|
|
|
|
|
|
|
|
|
#TODO decide about robot occupied message |
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1) |
|
|
|
|
|
|
|
|
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True) |
|
|
|
|
|
|
|
|
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True) |
|
|
thread.start() |
|
|
thread.start() |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
userdata (_type_): _description_ |
|
|
userdata (_type_): _description_ |
|
|
message (mqtt.MQTTMessage): received message |
|
|
message (mqtt.MQTTMessage): received message |
|
|
""" |
|
|
""" |
|
|
|
|
|
logging.info("Received position request") |
|
|
|
|
|
|
|
|
# TODO Write GPS Sensor Function |
|
|
# TODO Write GPS Sensor Function |
|
|
position = { |
|
|
position = { |
|
|
"Position": "" |
|
|
"Position": "" |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4)) |
|
|
|
|
|
|
|
|
clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
|
|
|
|
|
|
def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
""" |
|
|
""" |
|
|
Callback function for battery status request |
|
|
Callback function for battery status request |
|
|
Function to read battery status from ev3 and send via MQTT in form of BATTERY |
|
|
Function to read battery status from ev3 and send via MQTT in form of BATTERY |
|
|
|
|
|
|
|
|
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: |
|
|
logging.error("Robot not connected") |
|
|
logging.error("Robot not connected") |
|
|
#TODO Error Message |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1) |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
batteryLevel = round(batteryLevel / 1000000, 2) # Voltage |
|
|
batteryLevel = round(batteryLevel / 1000000, 2) # Voltage |
|
|
|
|
|
|
|
|
battery = { |
|
|
battery = { |
|
|
"Battery": batteryLevel |
|
|
"Battery": batteryLevel |
|
|
} |
|
|
} |
|
|
clients.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4)) |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1) |
|
|
|
|
|
|
|
|
#endregion |
|
|
#endregion |