118 lines
3.6 KiB
Python
118 lines
3.6 KiB
Python
import paho.mqtt.client as mqtt
|
|
import json
|
|
import threading
|
|
import os
|
|
import raspy_sensors as Sensors
|
|
from defines import Topics
|
|
|
|
def measure_send_data(plantID, actionID, client: mqtt.Client):
|
|
"""
|
|
Reads all sensors and publishes data via MQTT
|
|
|
|
Args:
|
|
plantID (_type_): plant to measure
|
|
actionID (_type_): current ID of driving action
|
|
client (mqtt.Client): current mqtt client for publishing
|
|
"""
|
|
sensor = Sensors.RaspySensors()
|
|
sensorData = sensor.readSensors()
|
|
sensorData["PlantID"] = plantID
|
|
sensorData["ActionID"] = actionID
|
|
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4))
|
|
|
|
|
|
def drive_plant_thread(plantID, actionID, client: mqtt.Client):
|
|
"""
|
|
Function to drive to plant according to number from MQTT message in thread
|
|
Meassure and publish data via MQTT
|
|
Drive home to starting point
|
|
|
|
Args:
|
|
plantID (_type_): plant to measure
|
|
actionID (_type_): current ID of driving action
|
|
client (mqtt.Client): current mqtt client for publishing
|
|
"""
|
|
# FIXME Change to color code driving
|
|
os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
|
|
# os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
|
|
|
|
print("Measuring Sensors")
|
|
|
|
#FIXME Sensor not working
|
|
# measure_send_data(plantID, actionID, client)
|
|
|
|
print("Robot driving home")
|
|
os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
|
|
print("Robot home")
|
|
|
|
#TODO decide about robot occupied message
|
|
#client.publish("ROBOT/DATA/OCCUPIED", "false")
|
|
|
|
|
|
#region MQTT callbacks
|
|
|
|
#Testing
|
|
def send_data_json(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
|
strIn = str(message.payload.decode("UTF-8"))
|
|
dataDict = json.loads(strIn)
|
|
print("Received data: ", json.dumps(dataDict))
|
|
|
|
|
|
def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
|
"""
|
|
Function to drive to plant according to request
|
|
Starting Drive in Thread
|
|
|
|
Args:
|
|
clients (mqtt.Client): current mqtt client
|
|
userdata (_type_): _description_
|
|
message (mqtt.MQTTMessage): received message
|
|
"""
|
|
dictMessage = json.loads(str(message.payload.decode("UTF-8")))
|
|
plantID = dictMessage["PlantID"]
|
|
actionID = dictMessage["ActionID"]
|
|
|
|
print(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
|
|
|
|
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True)
|
|
thread.start()
|
|
|
|
|
|
def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
|
"""
|
|
Callback function for GPS position request
|
|
Function to send actual GPS position via MQTT
|
|
|
|
Args:
|
|
clients (mqtt.Client): current mqtt client
|
|
userdata (_type_): _description_
|
|
message (mqtt.MQTTMessage): received message
|
|
"""
|
|
# TODO Write GPS Sensor Function
|
|
position = {
|
|
"Position": ""
|
|
}
|
|
|
|
clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4))
|
|
|
|
|
|
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
|
|
|
|
Args:
|
|
clients (mqtt.Client): current mqtt client
|
|
userdata (_type_): _description_
|
|
message (mqtt.MQTTMessage): received message
|
|
"""
|
|
|
|
battery = {
|
|
"Battery": 0.0
|
|
}
|
|
|
|
#TODO read Battery
|
|
clients.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4))
|
|
|
|
|
|
#endregion |