RobotReady and SendError added
This commit is contained in:
parent
0385aa7792
commit
43fbbfcbbb
@ -19,6 +19,8 @@ Topics = {
|
||||
"ROBOT_DATA_BATTERY": "ROBOT/DATA/BATTERY",
|
||||
"ROBOT_DATA_POSITION": "ROBOT/DATA/POSITION",
|
||||
"ROBOT_DATA_PICTURE": "ROBOT/DATA/PICTURE",
|
||||
"ROBOT_DATA_ERROR": "ROBOT/DATA/ERROR",
|
||||
"ROBOT_DATA_ROBOTREADY": "ROBOT/DATA/ROBOTREADY",
|
||||
|
||||
"BACKEND_ACTION_DRIVE": "BACKEND/ACTION/DRIVE",
|
||||
"BACKEND_ACTION_DRIVEPALL": "BACKEND/ACTION/DRIVEALL",
|
||||
@ -36,6 +38,8 @@ Topics = {
|
||||
"BACKEND_DATA_BATTERY": "BACKEND/DATA/BATTERY",
|
||||
"BACKEND_DATA_PICTURE": "BACKEND/DATA/PICTURE",
|
||||
"BACKEND_DATA_PLANTCOUNT": "BACKEND/DATA/PLANTCOUNT",
|
||||
"BACKEND_DATA_ERROR": "BACKEND/DATA/ERROR",
|
||||
"BACKEND_DATA_ROBOTREADY": "BACKEND/DATA/ROBOTREADY"
|
||||
|
||||
|
||||
}
|
||||
@ -49,7 +53,7 @@ SENSORDATA = {
|
||||
"SoilMoisture": 0.0,
|
||||
"Brightness": 0,
|
||||
"PlantID": 0,
|
||||
"ActionID": 0
|
||||
"ActionID": ""
|
||||
}
|
||||
|
||||
# TODO When sensor is available
|
||||
@ -67,7 +71,7 @@ BATTERY = {
|
||||
|
||||
DRIVE = {
|
||||
"PlantID": 0,
|
||||
"ActionID": 0
|
||||
"ActionID": ""
|
||||
}
|
||||
|
||||
# GETPOSITION -> no message needed
|
||||
|
||||
@ -20,12 +20,13 @@ def measure_send_data(plantID, actionID, client: mqtt.Client):
|
||||
try:
|
||||
sensorData = Sensors.readSensors()
|
||||
except Exception as e:
|
||||
print(str(e))
|
||||
logging.error(str(e))
|
||||
# TODO Error message MQTT
|
||||
client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
|
||||
|
||||
sensorData["PlantID"] = plantID
|
||||
sensorData["ActionID"] = actionID
|
||||
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4))
|
||||
client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1)
|
||||
|
||||
|
||||
def drive_plant_thread(plantID, actionID, client: mqtt.Client):
|
||||
@ -43,10 +44,13 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
|
||||
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
|
||||
# errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
|
||||
|
||||
# TODO Error Message
|
||||
if errorCode != 0:
|
||||
logging.error(f"Robot Error {errorCode} occurred")
|
||||
logging.error("Drive Plant aborted, Robot at starting position")
|
||||
logging.info("Drive Plant aborted, Robot at starting position")
|
||||
client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
|
||||
#TODO decide about robot occupied message
|
||||
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
|
||||
|
||||
return
|
||||
|
||||
logging.info("Measuring Sensors")
|
||||
@ -55,21 +59,23 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
|
||||
logging.info("Robot driving home")
|
||||
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
|
||||
|
||||
# TODO Error Message
|
||||
if errorCode != 0:
|
||||
logging.error(f"Robot Error {errorCode} occurred")
|
||||
logging.error(f"Drive Home aborted, Robot at plant {plantID}")
|
||||
logging.info(f"Drive Home aborted, Robot at plant {plantID}")
|
||||
client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1)
|
||||
#TODO decide about robot occupied message
|
||||
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
|
||||
return
|
||||
|
||||
logging.info("Robot home")
|
||||
|
||||
#TODO decide about robot occupied message
|
||||
#client.publish("ROBOT/DATA/OCCUPIED", "false")
|
||||
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
|
||||
|
||||
|
||||
#region MQTT callbacks
|
||||
|
||||
def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
"""
|
||||
Function to drive to plant according to request
|
||||
Starting Drive in Thread to not block main programm
|
||||
@ -80,12 +86,18 @@ def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
|
||||
"""
|
||||
dictMessage = json.loads(str(message.payload.decode("UTF-8")))
|
||||
|
||||
print(dictMessage)
|
||||
|
||||
plantID = dictMessage["PlantID"]
|
||||
actionID = dictMessage["ActionID"]
|
||||
|
||||
logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
|
||||
|
||||
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True)
|
||||
#TODO decide about robot occupied message
|
||||
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
|
||||
|
||||
thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
|
||||
thread.start()
|
||||
|
||||
|
||||
@ -99,15 +111,17 @@ def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
userdata (_type_): _description_
|
||||
message (mqtt.MQTTMessage): received message
|
||||
"""
|
||||
logging.info("Received position request")
|
||||
|
||||
# TODO Write GPS Sensor Function
|
||||
position = {
|
||||
"Position": ""
|
||||
}
|
||||
|
||||
clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4))
|
||||
clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
|
||||
|
||||
|
||||
def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
|
||||
"""
|
||||
Callback function for battery status request
|
||||
Function to read battery status from ev3 and send via MQTT in form of BATTERY
|
||||
@ -125,7 +139,7 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
|
||||
batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
|
||||
except:
|
||||
logging.error("Robot not connected")
|
||||
#TODO Error Message
|
||||
client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
|
||||
return
|
||||
|
||||
batteryLevel = round(batteryLevel / 1000000, 2) # Voltage
|
||||
@ -135,6 +149,6 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
|
||||
battery = {
|
||||
"Battery": batteryLevel
|
||||
}
|
||||
clients.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4))
|
||||
client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
|
||||
|
||||
#endregion
|
||||
@ -57,7 +57,7 @@ def main():
|
||||
# CHECK forever or start
|
||||
client.loop_start()
|
||||
|
||||
logging.info("Starting Loop")
|
||||
logging.info("Robot initialised")
|
||||
while True:
|
||||
pass
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user