caliskanbi83990/projektarbeit_duelger_waldhauser_caliskan
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Drive all Plants added

Browse Source
This commit is contained in:
waldluis 2023-05-17 21:35:58 +02:00
parent 6964f9855e
commit 409c38a96d
3 changed files with 85 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
software/roboter/raspy/defines.py
View File

@ -76,6 +76,11 @@ DRIVE = {
"ActionID": ""
}
DRIVEALL = {
"PlantIDs": [],
"ActionID": ""
}
# GETPOSITION -> no message needed
# GETBATTERY -> no message needed

87
software/roboter/raspy/functions.py
View File

@ -4,7 +4,7 @@ import threading
import os
import logging
import raspy_sensors as Sensors
from defines import Topics, SENSORDATA
from defines import Topics, SENSORDATA, ALLPLANTDATA
def measure_send_data(plantID, actionID, client: mqtt.Client):
"""
@ -54,6 +54,7 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
logging.info("Measuring Sensors")
measure_send_data(plantID, actionID, client)
# TODO How to Handle Pictures and PlantID
logging.info("Taking Picture")
sendPicture(client)
@ -72,15 +73,75 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
# TODO thread all plants
def drive_plant_all(plantID, actionID, client: mqtt.Client):
def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client):
"""
Function to drive to plants according to numbers from MQTT message in thread
Meassure and publish data for all plants via MQTT
Drive home to starting point
Args:
plantIDs (list): Plants to drive to
actionID (_type_): ActionID from Backend for whole drive action
client (mqtt.Client): current MQTT client
"""
allPlantData = ALLPLANTDATA
allPlantData["ActionID"] = actionID
# TODO loop through plantID array
for plant in plantIDs:
# FIXME Only possible with color codes
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}')
if errorCode != 0:
logging.error(f"Robot Error {errorCode} occurred")
logging.info("Drive Plant aborted, Robot at starting position")
client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
return
# TODO Test error handling
logging.info("Measuring Sensors")
try:
allPlantData["PlantData"].append(Sensors.readSensors())
except Exception as e:
logging.error(str(e))
client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
# TODO How to Handle Pictures and PlantID
logging.info("Taking Picture")
sendPicture(client)
# TODO Test Drive arm back to middle
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
if errorCode != 0:
logging.error(f"Robot Error {errorCode} occurred")
logging.info("Drive Plant aborted, Robot at starting position")
client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
return
logging.info("All Plants measured, sending data")
client.publish(Topics["ROBOT_DATA_DRIVEALL"], json.dumps(allPlantData, indent=4), qos=1)
# TODO Test Drive back straight without arm
logging.info("Robot driving home")
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
if errorCode != 0:
logging.error(f"Robot Error {errorCode} occurred")
logging.info(f"Drive Home aborted, Robot at plant {plant}")
client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plant}", qos=1)
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
return
logging.info("Robot home")
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
#region MQTT callbacks
def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
@ -105,19 +166,27 @@ def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
thread.start()
# TODO Drive all Plants
# TODO plantID array
# TODO only possible with color codes
# TODO Test Drive all Plants
# FIXME Only possible with color codes
def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
"""
Function to drive to plants according to request
Starting Drive in Thread to not block main programm
Args:
client (mqtt.Client): current MQTT client
userdata (_type_): _description_
message (mqtt.MQTTMessage): received DRIVEALL message with PlantIDs and ActionID
"""
dictMessage = json.loads(str(message.payload.decode("UTF-8")))
plantID = dictMessage["PlantID"]
plantIDs = dictMessage["PlantID"] # List of numbers
actionID = dictMessage["ActionID"]
logging.info(f"Received Drive-request to plants {plantID}, ActionID: {actionID}")
logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}")
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
thread = threading.Thread(target= drive_plant_all, args=(plantID, actionID, client), daemon=True)
thread = threading.Thread(target= drive_plant_all, args=(plantIDs, actionID, client), daemon=True)
thread.start()
def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):

2
software/roboter/raspy/mainProg.py
View File

@ -31,11 +31,13 @@ def on_connect(client: mqtt.Client, userdata, flags, rc):
client.message_callback_add(Topics["ROBOT_ACTION_DRIVE"], functions.drive_plant)
client.message_callback_add(Topics["ROBOT_ACTION_GETPOSITION"], functions.get_position)
client.message_callback_add(Topics["ROBOT_ACTION_GETBATTERY"], functions.get_BatteryStatus)
client.message_callback_add(Topics["ROBOT_ACTION_DRIVEALL"], functions.drive_plant_all)
#Subscribe to topics
client.subscribe(Topics["ROBOT_ACTION_DRIVE"])
client.subscribe(Topics["ROBOT_ACTION_GETPOSITION"])
client.subscribe(Topics["ROBOT_ACTION_GETBATTERY"])
client.subscribe(Topics["ROBOT_ACTION_DRIVEALL"])
logging.info("MQTT initialized")