From 409c38a96d70437aea96d53b22a035e1f0bb8943 Mon Sep 17 00:00:00 2001 From: waldluis Date: Wed, 17 May 2023 21:35:58 +0200 Subject: [PATCH] Drive all Plants added --- software/roboter/raspy/defines.py | 5 ++ software/roboter/raspy/functions.py | 87 ++++++++++++++++++++++++++--- software/roboter/raspy/mainProg.py | 2 + 3 files changed, 85 insertions(+), 9 deletions(-) diff --git a/software/roboter/raspy/defines.py b/software/roboter/raspy/defines.py index 59c6989..1a69b4e 100644 --- a/software/roboter/raspy/defines.py +++ b/software/roboter/raspy/defines.py @@ -76,6 +76,11 @@ DRIVE = { "ActionID": "" } +DRIVEALL = { + "PlantIDs": [], + "ActionID": "" +} + # GETPOSITION -> no message needed # GETBATTERY -> no message needed diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py index a94f86a..14b1468 100644 --- a/software/roboter/raspy/functions.py +++ b/software/roboter/raspy/functions.py @@ -4,7 +4,7 @@ import threading import os import logging import raspy_sensors as Sensors -from defines import Topics, SENSORDATA +from defines import Topics, SENSORDATA, ALLPLANTDATA def measure_send_data(plantID, actionID, client: mqtt.Client): """ @@ -54,6 +54,7 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client): logging.info("Measuring Sensors") measure_send_data(plantID, actionID, client) + # TODO How to Handle Pictures and PlantID logging.info("Taking Picture") sendPicture(client) @@ -72,15 +73,75 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client): # TODO thread all plants -def drive_plant_all(plantID, actionID, client: mqtt.Client): +def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client): + """ + Function to drive to plants according to numbers from MQTT message in thread + Meassure and publish data for all plants via MQTT + Drive home to starting point + + Args: + plantIDs (list): Plants to drive to + actionID (_type_): ActionID from Backend for whole drive action + client (mqtt.Client): current MQTT client + """ + + allPlantData = ALLPLANTDATA + allPlantData["ActionID"] = actionID # TODO loop through plantID array + for plant in plantIDs: + # FIXME Only possible with color codes + errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}') + + if errorCode != 0: + logging.error(f"Robot Error {errorCode} occurred") + logging.info("Drive Plant aborted, Robot at starting position") + client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1) + client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) + return + + + # TODO Test error handling + logging.info("Measuring Sensors") + try: + allPlantData["PlantData"].append(Sensors.readSensors()) + except Exception as e: + logging.error(str(e)) + client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1) + + + # TODO How to Handle Pictures and PlantID + logging.info("Taking Picture") + sendPicture(client) + + # TODO Test Drive arm back to middle + errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}') + + if errorCode != 0: + logging.error(f"Robot Error {errorCode} occurred") + logging.info("Drive Plant aborted, Robot at starting position") + client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1) + client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) + return + + logging.info("All Plants measured, sending data") + client.publish(Topics["ROBOT_DATA_DRIVEALL"], json.dumps(allPlantData, indent=4), qos=1) + + # TODO Test Drive back straight without arm + logging.info("Robot driving home") + errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py') + + if errorCode != 0: + logging.error(f"Robot Error {errorCode} occurred") + logging.info(f"Drive Home aborted, Robot at plant {plant}") + client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plant}", qos=1) + client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) + return logging.info("Robot home") client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) - #region MQTT callbacks def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): @@ -105,19 +166,27 @@ def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): thread.start() -# TODO Drive all Plants -# TODO plantID array -# TODO only possible with color codes +# TODO Test Drive all Plants +# FIXME Only possible with color codes def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): + """ + Function to drive to plants according to request + Starting Drive in Thread to not block main programm + + Args: + client (mqtt.Client): current MQTT client + userdata (_type_): _description_ + message (mqtt.MQTTMessage): received DRIVEALL message with PlantIDs and ActionID + """ dictMessage = json.loads(str(message.payload.decode("UTF-8"))) - plantID = dictMessage["PlantID"] + plantIDs = dictMessage["PlantID"] # List of numbers actionID = dictMessage["ActionID"] - logging.info(f"Received Drive-request to plants {plantID}, ActionID: {actionID}") + logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}") client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1) - thread = threading.Thread(target= drive_plant_all, args=(plantID, actionID, client), daemon=True) + thread = threading.Thread(target= drive_plant_all, args=(plantIDs, actionID, client), daemon=True) thread.start() def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): diff --git a/software/roboter/raspy/mainProg.py b/software/roboter/raspy/mainProg.py index 4edc26a..262eb0a 100644 --- a/software/roboter/raspy/mainProg.py +++ b/software/roboter/raspy/mainProg.py @@ -31,11 +31,13 @@ def on_connect(client: mqtt.Client, userdata, flags, rc): client.message_callback_add(Topics["ROBOT_ACTION_DRIVE"], functions.drive_plant) client.message_callback_add(Topics["ROBOT_ACTION_GETPOSITION"], functions.get_position) client.message_callback_add(Topics["ROBOT_ACTION_GETBATTERY"], functions.get_BatteryStatus) + client.message_callback_add(Topics["ROBOT_ACTION_DRIVEALL"], functions.drive_plant_all) #Subscribe to topics client.subscribe(Topics["ROBOT_ACTION_DRIVE"]) client.subscribe(Topics["ROBOT_ACTION_GETPOSITION"]) client.subscribe(Topics["ROBOT_ACTION_GETBATTERY"]) + client.subscribe(Topics["ROBOT_ACTION_DRIVEALL"]) logging.info("MQTT initialized")