|
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330 |
- """
- created by waldhauser
-
- This file contains the functions for the mainProg.py.
- The functions are part of three groups, MQTT-callbacks, functions for external threads and one normal function.
- """
-
- import paho.mqtt.client as mqtt
- import json
- import threading
- import os
- import logging
- import raspy_sensors as Sensors
- from defines import Topics, SENSORDATA, ALLSENSORDATA
-
-
- def measure_send_data(plantID, actionID, client: mqtt.Client):
- """
- Reads all sensors and publishes data via MQTT in form of SENSORDATA
-
- ***Function is only neccessary for driving without color codes***
-
- Args:
- plantID (_type_): plant to measure
- actionID (_type_): current ID of driving action
- client (mqtt.Client): current mqtt client for publishing
- """
- sensorData = {}
- sensorData |= SENSORDATA
-
- try:
- Sensors.readSensors(sensorData)
- except Exception as e:
- logging.error(str(e))
- 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), qos=1)
-
- #region Thread
- # Functions to be executed in external Thread to not block the mainProg
- # Functions contain the drive and sensor measurement operations
-
- def drive_plant_thread(plantID, actionID, client: mqtt.Client):
- """
- Function to drive to plant according to number from MQTT message in thread
- Drive programm based on hardcoded drive programms specific for each plant
- Only possible to drive to one plant per execution
- Meassure and publish data via MQTT
- Drive home to starting point
-
- ***If color codes are working properly this function is not needed anymore***
-
- Args:
- plantID (_type_): plant to measure
- actionID (_type_): current ID of driving action
- client (mqtt.Client): current mqtt client for publishing
- """
-
- errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
-
- # Error handling for drive operation
- # Abort when drive operation at start when error occurs
- if errorCode != 0:
- if errorCode == 65280:
- errorMessage = "EV3 not connected"
- else:
- errorMessage = "Motor or Sensor unplugged"
-
- logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position")
- client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at starting position", qos=1)
- client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
- return
-
- logging.info("Measuring Sensors")
- measure_send_data(plantID, actionID, client)
-
- # Taking Picture not fully implemented
- # logging.info("Taking Picture")
- # sendPicture(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}')
-
- # Error handling for drive operation
- # Abort when drive operation at start when error occurs
- if errorCode != 0:
- if errorCode == 65280:
- errorMessage = "EV3 not connected"
- else:
- errorMessage = "Motor or Sensor unplugged"
-
- logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position")
- client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at plant {plantID}", 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)
-
-
- def drive_plant_all_thread(plantIDs: list, actionID, client: mqtt.Client):
- """
- Function to drive to plants according to numbers from MQTT message in thread, one or more plants possible
- PlantIDs have to be in ascending order
- Drive programm based on color codes in the flowerbed
- Meassure and publish data for all plants via MQTT
- Drive home to starting point
-
- ***If color codes are working properly, only this function is needed for drive operations***
-
- Args:
- plantIDs (list): Plants to drive to, plants have to be in ascending order
- actionID (_type_): ActionID from Backend for whole drive action
- client (mqtt.Client): current MQTT client
- """
-
- allPlantData = {
- "SensorData": [],
- "ActionID": ""
- }
-
- allPlantData["ActionID"] = actionID
-
- for plant in plantIDs:
- 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}')
-
- # Error handling for drive operation
- # Abort when drive operation at start when error occurs
- if errorCode != 0:
- if errorCode == 65280:
- errorMessage = "EV3 not connected"
- else:
- errorMessage = "Motor or Sensor unplugged"
-
- logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position")
- client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at starting position", qos=1)
- client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
- return
-
-
- # Read Sensors and store data, take picture
- logging.info("Measuring Sensors")
- try:
- sensordata = {}
- sensordata |= SENSORDATA
- Sensors.readSensors(sensordata)
- except Exception as e:
- logging.error(str(e))
- client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
-
- sensordata["PlantID"] = plant
- allPlantData["SensorData"].append(sensordata)
-
-
- # Taking Picture not fully implemented
- # logging.info("Taking Picture")
- # sendPicture(client)
-
- errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
- # Error handling for drive operation
- # Abort when drive operation at start when error occurs
- if errorCode != 0:
- if errorCode == 65280:
- errorMessage = "EV3 not connected"
- else:
- errorMessage = "Motor or Sensor unplugged"
-
- logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position")
- client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at plant {plant}", qos=1)
- client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
- return
-
- logging.info("All Plants measured, sending data")
- client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1)
-
-
- logging.info("Robot driving home")
- # Error handling for drive operation
- # Abort when drive operation at start when error occurs
- errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
- if errorCode != 0:
- if errorCode == 65280:
- errorMessage = "EV3 not connected"
- else:
- errorMessage = "Motor or Sensor unplugged"
-
- logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position")
- client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant 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)
-
- #endregion
-
- #region MQTT callbacks
- # Functions to be called by the MQTT callbacks
- # Starting the drive operations in threads or executing the small operations directly
-
- 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
-
- ***If color codes are working properly this function is not needed anymore***
-
- Args:
- client (mqtt.Client): current mqtt client
- userdata (_type_): _description_
- message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
- """
- dictMessage = json.loads(str(message.payload.decode("UTF-8")))
-
- plantID = dictMessage["PlantID"]
- actionID = dictMessage["ActionID"]
-
- logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
- client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
-
- thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
- thread.start()
-
-
- 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
-
- ***If color codes are working properly, only this function is needed for drive operations***
-
- 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")))
-
- plantIDs = dictMessage["PlantID"] # List of numbers
- actionID = dictMessage["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_thread, args=(plantIDs, actionID, client), 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 in form of POSITION
-
- ***Not implemented, available GPS-sensor was not working***
-
- Args:
- client (mqtt.Client): current mqtt client
- userdata (_type_): _description_
- message (mqtt.MQTTMessage): received message
- """
- logging.info("Received position request")
-
- position = {
- "Position": ""
- }
-
- clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
-
-
- 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
- Current battery level is stored in "voltage_now" file
- Reading battery level via SSH script execution
- Battery level shown in Volts:
- 8,5V -> 100%
- 5V -> 0%
-
- Args:
- client (mqtt.Client): current mqtt client
- userdata (_type_): _description_
- message (mqtt.MQTTMessage): received message
- """
- logging.info("Received battery status request")
-
- try:
- 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("EV3 not connected")
- client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
- return
-
- batteryLevel = round(batteryLevel / 1000000, 2) # Conversion to Volt
- batteryLevel = batteryLevel - 5
- batteryLevel = round(batteryLevel / 3.5, 3) *100 # Conversion to Percentage, 8,5V -> 100%, 5V -> 0%
-
- battery = {
- "Battery": batteryLevel
- }
- client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
- logging.info(f"Battery done {batteryLevel}")
-
-
- def sendPicture(client: mqtt.Client):
- """
- Takes picture and publish via MQTT
-
- ***Pictures can not be send until now -> assumption of reaching maximum payload limit***
-
- Args:
- client (mqtt.Client): current mqtt client
- """
- try:
- Sensors.takePicture()
- except Exception as e:
- logging.error(str(e))
- client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
- return
-
- with open("picture.png", "rb") as f:
- file = f.read()
- byteArr = bytearray(file)
- client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
- logging.info("Picture Published")
-
- #endregion
|