RobotReady and SendError added

software/roboter/raspy/defines.py

@@ -19,6 +19,8 @@ Topics = {
     "ROBOT_DATA_BATTERY": "ROBOT/DATA/BATTERY",
     "ROBOT_DATA_POSITION": "ROBOT/DATA/POSITION",
     "ROBOT_DATA_PICTURE": "ROBOT/DATA/PICTURE",
+    "ROBOT_DATA_ERROR": "ROBOT/DATA/ERROR",
+    "ROBOT_DATA_ROBOTREADY": "ROBOT/DATA/ROBOTREADY",
 
     "BACKEND_ACTION_DRIVE": "BACKEND/ACTION/DRIVE",
     "BACKEND_ACTION_DRIVEPALL": "BACKEND/ACTION/DRIVEALL",
@@ -36,6 +38,8 @@ Topics = {
     "BACKEND_DATA_BATTERY": "BACKEND/DATA/BATTERY",
     "BACKEND_DATA_PICTURE": "BACKEND/DATA/PICTURE",
     "BACKEND_DATA_PLANTCOUNT": "BACKEND/DATA/PLANTCOUNT",
+    "BACKEND_DATA_ERROR": "BACKEND/DATA/ERROR",
+    "BACKEND_DATA_ROBOTREADY": "BACKEND/DATA/ROBOTREADY"
 
 
 }
@@ -49,7 +53,7 @@ SENSORDATA = {
     "SoilMoisture": 0.0,
     "Brightness": 0,
     "PlantID": 0,
-    "ActionID": 0
+    "ActionID": ""
 }
 
 # TODO When sensor is available
@@ -67,7 +71,7 @@ BATTERY = {
 
 DRIVE = {
     "PlantID": 0,
-    "ActionID": 0
+    "ActionID": ""
 }
 
 # GETPOSITION -> no message needed

software/roboter/raspy/functions.py

@@ -20,12 +20,13 @@ def measure_send_data(plantID, actionID, client: mqtt.Client):
     try:
         sensorData = Sensors.readSensors()
     except Exception as e:
-        print(str(e))
+        logging.error(str(e))
         # TODO Error message MQTT
+        client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
 
     sensorData["PlantID"] = plantID
     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):
@@ -43,10 +44,13 @@ 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/drive_plant.py {plantID}')
 
-    # TODO Error Message
     if errorCode != 0:
         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
    
     logging.info("Measuring Sensors")
@@ -55,21 +59,23 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
     logging.info("Robot driving home")
     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:
         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
 
     logging.info("Robot home")
 
     #TODO decide about robot occupied message
-    #client.publish("ROBOT/DATA/OCCUPIED", "false")
+    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
 
 
 #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
     Starting Drive in Thread to not block main programm
@@ -80,12 +86,18 @@ def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
         message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
     """
     dictMessage = json.loads(str(message.payload.decode("UTF-8")))
+
+    print(dictMessage)
+
     plantID = dictMessage["PlantID"]
     actionID = dictMessage["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()
 
 
@@ -99,15 +111,17 @@ def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
         userdata (_type_): _description_
         message (mqtt.MQTTMessage): received message
     """
+    logging.info("Received position request")
+
     # TODO Write GPS Sensor Function
     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
     Function to read battery status from ev3 and send via MQTT in form of BATTERY
@@ -125,7 +139,7 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
         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:
         logging.error("Robot not connected")
-        #TODO Error Message
+        client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
         return
 
     batteryLevel = round(batteryLevel / 1000000, 2)       # Voltage
@@ -135,6 +149,6 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
     battery = {
         "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

software/roboter/raspy/mainProg.py

@@ -57,7 +57,7 @@ def main():
     # CHECK forever or start
     client.loop_start()
 
-    logging.info("Starting Loop")
+    logging.info("Robot initialised")
     while True:
         pass