166 lines
4.3 KiB
Python
166 lines
4.3 KiB
Python
import paho.mqtt.client as mqtt
|
|
import json
|
|
import threading
|
|
import os
|
|
import signal
|
|
import time
|
|
from raspy_sensors import RaspySensors
|
|
|
|
#region global Varaibles
|
|
#sensors:RaspySensors()
|
|
leftRight = 50
|
|
plantNumber = 0
|
|
|
|
sensorData = {
|
|
"Air Temperature [°C]" : 0,
|
|
"Air Humidity [%]" : 0,
|
|
"Earth Humidity [%]" : 0,
|
|
"Brightness [Lux]" : 0,
|
|
"Plant ID": 0,
|
|
"Action ID": 0
|
|
}
|
|
|
|
gpsPosition = {
|
|
"Position": 0,
|
|
"Action ID": 0
|
|
}
|
|
|
|
batteryStatus = {
|
|
"Battery": 0,
|
|
"Action ID": 0
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
#region global functions
|
|
|
|
def measure_send_data(plantID, actionID):
|
|
'''Measure data for one plant via sensor class and send via MQTT'''
|
|
sensorData = sensors.readSensors()
|
|
sensorData["Plant_ID"] = plantID
|
|
sensorData["Action_ID"] = actionID
|
|
client.publish("ROBOT/DATA/SENSORDATA", json.dumps(sensorData, indent=4))
|
|
|
|
|
|
def drive_home():
|
|
'''Function to drive robot back to starting position in thread'''
|
|
print("Robot driving home")
|
|
os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}')
|
|
print("Robot home")
|
|
#TODO decide about robot occupied message
|
|
#client.publish("ROBOT/DATA/OCCUPIED", "false")
|
|
|
|
|
|
def drive_plant_thread():
|
|
'''Function to drive to plant according to number from MQTT message in thread'''
|
|
os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantNumber}.py')
|
|
print("Raising Signal to meassure")
|
|
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.connect(mqttBroker) #Has to be before subscribing
|
|
|
|
#Subscribe to topics
|
|
client.subscribe("ROBOT/DATA") #Testing
|
|
client.subscribe("ROBOT/ACTION/DRIVE")
|
|
client.subscribe("ROBOT/Action/GETPOSITION")
|
|
client.subscribe("ROBOT/ACTION/GETBATTERY")
|
|
|
|
|
|
|
|
|
|
def signal_measure(signum, frame):
|
|
'''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
|
|
|
|
#Testing
|
|
def send_data_json(client, userdata, message):
|
|
strIn = str(message.payload.decode("UTF-8"))
|
|
dataDict = json.loads(strIn)
|
|
print("Received data: ", json.dumps(dataDict))
|
|
|
|
|
|
def drive_plant(clients, userdata, message):
|
|
'''Function to drive to plant according to request
|
|
Starting Drive in Thread'''
|
|
global plantNumber, leftRight
|
|
#TODO define MQTT message
|
|
plantNumber = int(message.payload.decode("UTF-8"))
|
|
|
|
print(f"received drive to plant {plantNumber}")
|
|
|
|
if plantNumber % 2 == 0:
|
|
leftRight = -50 #rotating left
|
|
else:
|
|
leftRight = 50 #rotating right
|
|
|
|
thread = threading.Thread(target= drive_plant_thread, daemon=True)
|
|
thread.start()
|
|
|
|
|
|
def get_position(clients, userdata, message):
|
|
'''Callback function for GPS position request
|
|
Function to send actual GPS position via MQTT'''
|
|
|
|
#[ ]TODO handle MQTT message
|
|
gpsPosition["Position"] = sensors.readPosition()
|
|
gpsPosition["Action_ID"] = message
|
|
client.publish("ROBOT/DATA/POSITION", json.dumps(gpsPosition, indent=4))
|
|
|
|
|
|
def get_BatteryStatus(clients, userdata, message):
|
|
'''Callback function for battery status request
|
|
Function to read battery status from ev3 and send via MQTT'''
|
|
|
|
#[ ]TODO handle MQTT message
|
|
batteryStatus["Battery"] = sensors.readBattery()
|
|
batteryStatus["Action_ID"] = message
|
|
client.publish("ROBOT/DATA/BATTERY", json.dumps(batteryStatus, indent=4))
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
|
|
def main():
|
|
'''Initialises MQTT and Sensors
|
|
Runs forever and controlls all robot functions'''
|
|
|
|
init_mqtt()
|
|
print("MQTT initialized")
|
|
signal.signal(signal.SIGUSR1, signal_measure)
|
|
# dataDict = {} #Testing
|
|
|
|
client.loop_start()
|
|
|
|
print("Starting Loop")
|
|
while True:
|
|
# print("Looping")
|
|
# time.sleep(1)
|
|
pass
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |