|
|
|
|
|
|
|
|
import os |
|
|
import os |
|
|
import logging |
|
|
import logging |
|
|
import raspy_sensors as Sensors |
|
|
import raspy_sensors as Sensors |
|
|
from defines import Topics, SENSORDATA, ALLPLANTDATA |
|
|
|
|
|
|
|
|
from defines import Topics, SENSORDATA, ALLSENSORDATA |
|
|
|
|
|
|
|
|
def measure_send_data(plantID, actionID, client: mqtt.Client): |
|
|
def measure_send_data(plantID, actionID, client: mqtt.Client): |
|
|
""" |
|
|
""" |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# TODO thread all plants |
|
|
|
|
|
def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client): |
|
|
|
|
|
|
|
|
def drive_plant_all_thread(plantIDs: list, actionID, client: mqtt.Client): |
|
|
""" |
|
|
""" |
|
|
Function to drive to plants according to numbers from MQTT message in thread |
|
|
Function to drive to plants according to numbers from MQTT message in thread |
|
|
Meassure and publish data for all plants via MQTT |
|
|
Meassure and publish data for all plants via MQTT |
|
|
|
|
|
|
|
|
client (mqtt.Client): current MQTT client |
|
|
client (mqtt.Client): current MQTT client |
|
|
""" |
|
|
""" |
|
|
|
|
|
|
|
|
allPlantData = ALLPLANTDATA |
|
|
|
|
|
|
|
|
allPlantData = ALLSENSORDATA |
|
|
allPlantData["ActionID"] = actionID |
|
|
allPlantData["ActionID"] = actionID |
|
|
|
|
|
logging.debug(f"plants {plantIDs}, type {type(plantIDs)}") |
|
|
|
|
|
|
|
|
# TODO loop through plantID array |
|
|
|
|
|
for plant in plantIDs: |
|
|
for plant in plantIDs: |
|
|
# FIXME Only possible with color codes |
|
|
# FIXME Only possible with color codes |
|
|
|
|
|
logging.info(f"Robot Driving to plant {plant}") |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}') |
|
|
|
|
|
|
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.info("Drive Plant aborted, Robot at starting position") |
|
|
logging.info("Drive Plant aborted, Robot at starting position") |
|
|
|
|
|
|
|
|
logging.info("Taking Picture") |
|
|
logging.info("Taking Picture") |
|
|
sendPicture(client) |
|
|
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}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}') |
|
|
|
|
|
|
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.info("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) |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], f"Rotate arm aborted, Robot at plant {plant}", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
logging.info("All Plants measured, sending data") |
|
|
logging.info("All Plants measured, sending data") |
|
|
client.publish(Topics["ROBOT_DATA_DRIVEALL"], json.dumps(allPlantData, indent=4), qos=1) |
|
|
|
|
|
|
|
|
client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1) |
|
|
|
|
|
|
|
|
# TODO Test Drive back straight without arm |
|
|
|
|
|
logging.info("Robot driving home") |
|
|
logging.info("Robot driving home") |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py') |
|
|
|
|
|
|
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.error(f"Robot Error {errorCode} occurred") |
|
|
logging.info(f"Drive Home aborted, Robot at plant {plant}") |
|
|
logging.info(f"Drive Home aborted, Robot at plant {plant}") |
|
|
|
|
|
|
|
|
logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}") |
|
|
logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}") |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1) |
|
|
|
|
|
|
|
|
thread = threading.Thread(target= drive_plant_all, args=(plantIDs, actionID, client), daemon=True) |
|
|
|
|
|
|
|
|
thread = threading.Thread(target= drive_plant_all_thread, args=(plantIDs, actionID, client), daemon=True) |
|
|
thread.start() |
|
|
thread.start() |
|
|
|
|
|
|
|
|
def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage): |