|
|
|
|
|
|
|
|
import paho.mqtt.client as mqtt |
|
|
import paho.mqtt.client as mqtt |
|
|
import json |
|
|
import json |
|
|
from raspySensors import RaspySensors |
|
|
|
|
|
|
|
|
import threading |
|
|
|
|
|
import os |
|
|
|
|
|
import signal |
|
|
|
|
|
import time |
|
|
|
|
|
from raspy_sensors import RaspySensors |
|
|
|
|
|
|
|
|
#region global Varaibles |
|
|
#region global Varaibles |
|
|
sensors = RaspySensors() |
|
|
|
|
|
client = mqtt.Client() |
|
|
|
|
|
|
|
|
#sensors:RaspySensors() |
|
|
|
|
|
leftRight = 50 |
|
|
|
|
|
plantNumber = 0 |
|
|
|
|
|
|
|
|
sensorData = { |
|
|
sensorData = { |
|
|
"Air Temperature [°C]" : 0, |
|
|
"Air Temperature [°C]" : 0, |
|
|
|
|
|
|
|
|
client.publish("Robot/Data/SensorData", json.dumps(sensorData, indent=4)) |
|
|
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(): |
|
|
def init_mqtt(): |
|
|
'''Initialise MQTT client''' |
|
|
'''Initialise MQTT client''' |
|
|
mqttBroker = "mqtt.eclipseprojects.io" |
|
|
mqttBroker = "mqtt.eclipseprojects.io" |
|
|
|
|
|
global client |
|
|
client = mqtt.Client("Robot") |
|
|
client = mqtt.Client("Robot") |
|
|
|
|
|
|
|
|
#Add callbacks |
|
|
#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 |
|
|
#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) |
|
|
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 |
|
|
#endregion |
|
|
|
|
|
|
|
|
#region MQTT callbacks |
|
|
#region MQTT callbacks |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def drive_plant(clients, userdata, message): |
|
|
def drive_plant(clients, userdata, message): |
|
|
'''Function to drive to plant according to request''' |
|
|
'''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): |
|
|
def get_position(clients, userdata, message): |
|
|
'''Callback function for GPS position request |
|
|
'''Callback function for GPS position request |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main(): |
|
|
def main(): |
|
|
init_mqtt() |
|
|
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: |
|
|
while True: |
|
|
pass |
|
|
pass |
|
|
|
|
|
|