|
|
|
|
|
|
|
|
import signal |
|
|
import signal |
|
|
import time |
|
|
import time |
|
|
from raspy_sensors import RaspySensors |
|
|
from raspy_sensors import RaspySensors |
|
|
|
|
|
from software.defines import Topics |
|
|
|
|
|
|
|
|
#region global Varaibles |
|
|
#region global Varaibles |
|
|
#sensors:RaspySensors() |
|
|
#sensors:RaspySensors() |
|
|
leftRight = 50 |
|
|
leftRight = 50 |
|
|
plantNumber = 0 |
|
|
|
|
|
|
|
|
plantID = 0 |
|
|
|
|
|
|
|
|
sensorData = { |
|
|
sensorData = { |
|
|
"Air Temperature [°C]" : 0, |
|
|
"Air Temperature [°C]" : 0, |
|
|
|
|
|
|
|
|
sensorData = sensors.readSensors() |
|
|
sensorData = sensors.readSensors() |
|
|
sensorData["Plant_ID"] = plantID |
|
|
sensorData["Plant_ID"] = plantID |
|
|
sensorData["Action_ID"] = actionID |
|
|
sensorData["Action_ID"] = actionID |
|
|
client.publish("ROBOT/DATA/SENSORDATA", json.dumps(sensorData, indent=4)) |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4)) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def drive_home(): |
|
|
def drive_home(): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def drive_plant_thread(): |
|
|
def drive_plant_thread(): |
|
|
'''Function to drive to plant according to number from MQTT message in 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') |
|
|
|
|
|
|
|
|
os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') |
|
|
print("Raising Signal to meassure") |
|
|
print("Raising Signal to meassure") |
|
|
signal.raise_signal(signal.SIGUSR1) |
|
|
signal.raise_signal(signal.SIGUSR1) |
|
|
|
|
|
#TODO alles hier ohne weitere Threads |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def init_mqtt(): |
|
|
def init_mqtt(): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#Add callbacks |
|
|
#Add callbacks |
|
|
client.message_callback_add("ROBOT/DATA", send_data_json) #Testing |
|
|
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(Topics["ROBOT_ACTION_DRIVE"], drive_plant) |
|
|
|
|
|
client.message_callback_add(Topics["ROBOT_ACTION_GETPOSITION"], get_position) |
|
|
|
|
|
client.message_callback_add(Topics["ROBOT_ACTION_GETBATTERY"], get_BatteryStatus) |
|
|
|
|
|
|
|
|
client.connect(mqttBroker) #Has to be before subscribing |
|
|
client.connect(mqttBroker) #Has to be before subscribing |
|
|
|
|
|
|
|
|
#Subscribe to topics |
|
|
#Subscribe to topics |
|
|
client.subscribe("ROBOT/DATA") #Testing |
|
|
client.subscribe("ROBOT/DATA") #Testing |
|
|
client.subscribe("ROBOT/ACTION/DRIVE") |
|
|
|
|
|
client.subscribe("ROBOT/Action/GETPOSITION") |
|
|
|
|
|
client.subscribe("ROBOT/ACTION/GETBATTERY") |
|
|
|
|
|
|
|
|
client.subscribe(Topics["ROBOT_ACTION_DRIVE"]) |
|
|
|
|
|
client.subscribe(Topics["ROBOT_ACTION_GETPOSITION"]) |
|
|
|
|
|
client.subscribe(Topics["ROBOT_ACTION_GETBATTERY"]) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#region MQTT callbacks |
|
|
#region MQTT callbacks |
|
|
|
|
|
|
|
|
#Testing |
|
|
#Testing |
|
|
def send_data_json(client, userdata, message): |
|
|
|
|
|
|
|
|
def send_data_json(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
strIn = str(message.payload.decode("UTF-8")) |
|
|
strIn = str(message.payload.decode("UTF-8")) |
|
|
dataDict = json.loads(strIn) |
|
|
dataDict = json.loads(strIn) |
|
|
print("Received data: ", json.dumps(dataDict)) |
|
|
print("Received data: ", json.dumps(dataDict)) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def drive_plant(clients, userdata, message): |
|
|
|
|
|
|
|
|
def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
'''Function to drive to plant according to request |
|
|
'''Function to drive to plant according to request |
|
|
Starting Drive in Thread''' |
|
|
Starting Drive in Thread''' |
|
|
global plantNumber, leftRight |
|
|
|
|
|
|
|
|
global plantID, leftRight |
|
|
#TODO define MQTT message |
|
|
#TODO define MQTT message |
|
|
plantNumber = int(message.payload.decode("UTF-8")) |
|
|
|
|
|
|
|
|
plantID = int(message.payload.decode("UTF-8")) |
|
|
|
|
|
|
|
|
print(f"received drive to plant {plantNumber}") |
|
|
|
|
|
|
|
|
print(f"received drive to plant {plantID}") |
|
|
|
|
|
|
|
|
if plantNumber % 2 == 0: |
|
|
|
|
|
|
|
|
if plantID % 2 == 0: |
|
|
leftRight = -50 #rotating left |
|
|
leftRight = -50 #rotating left |
|
|
else: |
|
|
else: |
|
|
leftRight = 50 #rotating right |
|
|
leftRight = 50 #rotating right |
|
|
|
|
|
|
|
|
thread.start() |
|
|
thread.start() |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_position(clients, userdata, message): |
|
|
|
|
|
|
|
|
def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
'''Callback function for GPS position request |
|
|
'''Callback function for GPS position request |
|
|
Function to send actual GPS position via MQTT''' |
|
|
Function to send actual GPS position via MQTT''' |
|
|
|
|
|
|
|
|
#[ ]TODO handle MQTT message |
|
|
|
|
|
gpsPosition["Position"] = sensors.readPosition() |
|
|
|
|
|
gpsPosition["Action_ID"] = message |
|
|
|
|
|
|
|
|
#[ ]TODO Write Sensor Function |
|
|
client.publish("ROBOT/DATA/POSITION", json.dumps(gpsPosition, indent=4)) |
|
|
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''' |
|
|
|
|
|
|
|
|
def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
|
|
|
""" |
|
|
|
|
|
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 |
|
|
|
|
|
|
|
|
Args: |
|
|
|
|
|
clients (mqtt.Client): _description_ |
|
|
|
|
|
userdata (_type_): _description_ |
|
|
|
|
|
message (mqtt.MQTTMessage): _description_ |
|
|
|
|
|
""" |
|
|
|
|
|
|
|
|
|
|
|
#[ ]TODO read Battery |
|
|
client.publish("ROBOT/DATA/BATTERY", json.dumps(batteryStatus, indent=4)) |
|
|
client.publish("ROBOT/DATA/BATTERY", json.dumps(batteryStatus, indent=4)) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main(): |
|
|
def main(): |
|
|
'''Initialises MQTT and Sensors |
|
|
|
|
|
Runs forever and controlls all robot functions''' |
|
|
|
|
|
|
|
|
""" |
|
|
|
|
|
Initialises MQTT and Sensors |
|
|
|
|
|
Runs forever and controlls all robot functions |
|
|
|
|
|
""" |
|
|
|
|
|
|
|
|
|
|
|
#TODO on_connect |
|
|
init_mqtt() |
|
|
init_mqtt() |
|
|
print("MQTT initialized") |
|
|
print("MQTT initialized") |
|
|
signal.signal(signal.SIGUSR1, signal_measure) |
|
|
signal.signal(signal.SIGUSR1, signal_measure) |