Drive all Plants added

software/roboter/raspy/defines.py

@@ -76,6 +76,11 @@ DRIVE = {
     "ActionID": ""
 }
 
+DRIVEALL = {
+    "PlantIDs": [],
+    "ActionID": ""
+}
+
 # GETPOSITION -> no message needed
 
 # GETBATTERY -> no message needed

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):

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")