From 6adc5a3426c5fcea3a589990d420ca67dfba8c4d Mon Sep 17 00:00:00 2001
From: waldhauserlu78556 <waldhauserlu78556@th-nuernberg.de>
Date: Sat, 22 Apr 2023 12:32:27 +0200
Subject: [PATCH] SSH start for robot scripts

---
 software/roboter/raspy/mainProg.py            | 75 +++++++++++++------
 .../{raspySensors.py => raspy_sensors.py}     |  0
 2 files changed, 52 insertions(+), 23 deletions(-)
 rename software/roboter/raspy/{raspySensors.py => raspy_sensors.py} (100%)

diff --git a/software/roboter/raspy/mainProg.py b/software/roboter/raspy/mainProg.py
index fe63ed2..7c0c238 100644
--- a/software/roboter/raspy/mainProg.py
+++ b/software/roboter/raspy/mainProg.py
@@ -1,10 +1,15 @@
 import paho.mqtt.client as mqtt
 import json
-from raspySensors import RaspySensors
+import threading
+import os
+import signal
+import time
+from raspy_sensors import RaspySensors
 
 #region global Varaibles 
-sensors = RaspySensors()
-client = mqtt.Client()
+#sensors:RaspySensors()
+leftRight = 50
+plantNumber = 0
 
 sensorData = {
     "Air Temperature [°C]" : 0,
@@ -38,26 +43,48 @@ def measure_send_data(plantID, actionID):
     client.publish("Robot/Data/SensorData", json.dumps(sensorData, indent=4))
 
 
+def drive_home():
+    '''Function to drive robot back to starting position in thread'''
+    thread = threading.Thread(target= os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}'), daemon=True)
+    thread.start()
+
+
+def drive_plant():
+    '''Function to drive to plant according to number from MQTT message in thread'''
+    print(f"Driving to Plant {plantNumber}")
+    os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantNumber}.py')
+    print("Raising Signal")
+    signal.raise_signal(signal.SIGUSR1)
+
+
 def init_mqtt():
     '''Initialise MQTT client'''
     mqttBroker = "mqtt.eclipseprojects.io"
+    global client
     client = mqtt.Client("Robot")
 
     #Add callbacks
-    client.message_callback_add("Robot/Data", send_data_json)       #Testing
-    client.message_callback_add("Robot/Action/Drive", drive_plant)
-    client.message_callback_add("Robot/Action/GetPosition", get_position)
-    client.message_callback_add("Robot/Action/GetBattery", get_BatteryStatus)
+    client.message_callback_add("ROBOT/DATA", send_data_json)       #Testing
+    client.message_callback_add("ROBOT/ACTION/DRIVE", drive_plant)
+    client.message_callback_add("ROBOT/ACTION/GETPOSITION", get_position)
+    client.message_callback_add("ROBOT/ACTION/GETBATTERY", get_BatteryStatus)
 
     #Subscribe to topics
-    client.subscribe("Robot/Data")      #Testing
-    client.subscribe("Robot/Action/Drive")
-    client.subscribe("Robot/Action/GetPosition")
-    client.subscribe("Robot/Action/GetBattery")
+    client.subscribe("ROBOT/DATA")      #Testing
+    client.subscribe("ROBOT/ACTION/DRIVE")
+    client.subscribe("ROBOT/Action/GETPOSITION")
+    client.subscribe("ROBOT/ACTION/GETBATTERY")
 
     client.connect(mqttBroker)
 
 
+def signal_measure():
+    '''Signal function to measure one plant and start driving home'''
+    print("Measuring Sensors")
+    #measure_send_data()
+    print("Driving Home")
+    drive_home()
+
 #endregion
 
 #region MQTT callbacks
@@ -71,19 +98,18 @@ def send_data_json(client, userdata, message):
 
 def drive_plant(clients, userdata, message):
     '''Function to drive to plant according to request'''
-    
-    #[ ]TODO handle MQTT message
-    print(f"Driving to plant {message}")
-    #[ ]TODO Start drive forward -> Thread
 
-    print(f"Measuring data at Plant {message}")
-    measure_send_data()       # With Signal not here
+    print("received drive to plant")
 
-    print("Driving back to start position")
-    #[ ]TODO Start Drive Back Function in Thread
+    plantNumber = int(message.payload.decode("UTF-8"))
+    if plantNumber % 2 == 0:
+        leftRight = -50         #rotating left
+    else:
+        leftRight = 50          #rotating right
+
+    thread = threading.Thread(target= drive_plant, daemon=True)
+    thread.start()
 
-    print("Back at starting Position") #Signal
-    
 
 def get_position(clients, userdata, message):
     '''Callback function for GPS position request
@@ -115,10 +141,13 @@ def get_BatteryStatus(clients, userdata, message):
 
 def main():
     init_mqtt()
-    dataDict = {}       #Testing
+    print("MQTT initialized")
+    signal.signal(signal.SIGUSR1, signal_measure)
+    # dataDict = {}       #Testing
 
-    client.loop_start()
+    client.loop_forever()
 
+    print("Starting Loop")
     while True:
         pass
 
diff --git a/software/roboter/raspy/raspySensors.py b/software/roboter/raspy/raspy_sensors.py
similarity index 100%
rename from software/roboter/raspy/raspySensors.py
rename to software/roboter/raspy/raspy_sensors.py