repository to manage all files related to the makeathon farm bot project (Software + Documentation).
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

functions.py 9.6KB

1 year ago
1 year ago
1 year ago
1 year ago
1 year ago
1 year ago
1 year ago
1 year ago
1 year ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272
  1. import paho.mqtt.client as mqtt
  2. import json
  3. import threading
  4. import os
  5. import logging
  6. import raspy_sensors as Sensors
  7. from defines import Topics, SENSORDATA, ALLSENSORDATA
  8. import time
  9. def measure_send_data(plantID, actionID, client: mqtt.Client):
  10. """
  11. Reads all sensors and publishes data via MQTT in form of SENSORDATA
  12. Args:
  13. plantID (_type_): plant to measure
  14. actionID (_type_): current ID of driving action
  15. client (mqtt.Client): current mqtt client for publishing
  16. """
  17. sensordata = {}
  18. sensordata |= SENSORDATA
  19. try:
  20. sensorData = Sensors.readSensors()
  21. except Exception as e:
  22. logging.error(str(e))
  23. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  24. sensorData["PlantID"] = plantID
  25. sensorData["ActionID"] = actionID
  26. client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1)
  27. def drive_plant_thread(plantID, actionID, client: mqtt.Client):
  28. """
  29. Function to drive to plant according to number from MQTT message in thread
  30. Meassure and publish data via MQTT
  31. Drive home to starting point
  32. Args:
  33. plantID (_type_): plant to measure
  34. actionID (_type_): current ID of driving action
  35. client (mqtt.Client): current mqtt client for publishing
  36. """
  37. # FIXME Change to color code driving
  38. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
  39. # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
  40. if errorCode != 0:
  41. logging.error(f"Robot Error {errorCode} occurred")
  42. logging.info("Drive Plant aborted, Robot at starting position")
  43. client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
  44. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  45. return
  46. logging.info("Measuring Sensors")
  47. measure_send_data(plantID, actionID, client)
  48. # TODO How to Handle Pictures and PlantID
  49. logging.info("Taking Picture")
  50. sendPicture(client)
  51. logging.info("Robot driving home")
  52. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
  53. if errorCode != 0:
  54. logging.error(f"Robot Error {errorCode} occurred")
  55. logging.info(f"Drive Home aborted, Robot at plant {plantID}")
  56. client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1)
  57. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  58. return
  59. logging.info("Robot home")
  60. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  61. def drive_plant_all_thread(plantIDs: list, actionID, client: mqtt.Client):
  62. """
  63. Function to drive to plants according to numbers from MQTT message in thread
  64. Meassure and publish data for all plants via MQTT
  65. Drive home to starting point
  66. Args:
  67. plantIDs (list): Plants to drive to
  68. actionID (_type_): ActionID from Backend for whole drive action
  69. client (mqtt.Client): current MQTT client
  70. """
  71. allPlantData = {
  72. "SensorData": [],
  73. "ActionID": ""
  74. }
  75. allPlantData["ActionID"] = actionID
  76. for plant in plantIDs:
  77. # FIXME Only possible with color codes
  78. logging.info(f"Robot Driving to plant {plant}")
  79. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}')
  80. if errorCode != 0:
  81. logging.error(f"Robot Error {errorCode} occurred")
  82. logging.info("Drive Plant aborted, Robot at starting position")
  83. client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
  84. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  85. return
  86. # TODO Test error handling
  87. logging.info("Measuring Sensors")
  88. try:
  89. sensordata = {}
  90. sensordata |= SENSORDATA
  91. sensordata = Sensors.readSensors()
  92. sensordata["PlantID"] = plant
  93. allPlantData["SensorData"].append(sensordata)
  94. except Exception as e:
  95. logging.error(str(e))
  96. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  97. time.sleep(3)
  98. # TODO How to Handle Pictures and PlantID
  99. logging.info("Taking Picture")
  100. sendPicture(client)
  101. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
  102. if errorCode != 0:
  103. logging.error(f"Robot Error {errorCode} occurred")
  104. logging.info("Drive Plant aborted, Robot at starting position")
  105. client.publish(Topics["ROBOT_DATA_ERROR"], f"Rotate arm aborted, Robot at plant {plant}", qos=1)
  106. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  107. return
  108. logging.info("All Plants measured, sending data")
  109. client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1)
  110. logging.info("Robot driving home")
  111. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
  112. if errorCode != 0:
  113. logging.error(f"Robot Error {errorCode} occurred")
  114. logging.info(f"Drive Home aborted, Robot at plant {plant}")
  115. client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plant}", qos=1)
  116. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  117. return
  118. logging.info("Robot home")
  119. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  120. #region MQTT callbacks
  121. def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  122. """
  123. Function to drive to plant according to request
  124. Starting Drive in Thread to not block main programm
  125. Args:
  126. client (mqtt.Client): current mqtt client
  127. userdata (_type_): _description_
  128. message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
  129. """
  130. dictMessage = json.loads(str(message.payload.decode("UTF-8")))
  131. plantID = dictMessage["PlantID"]
  132. actionID = dictMessage["ActionID"]
  133. logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
  134. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
  135. thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
  136. thread.start()
  137. # TODO Test Drive all Plants
  138. # FIXME Only possible with color codes
  139. def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  140. """
  141. Function to drive to plants according to request
  142. Starting Drive in Thread to not block main programm
  143. Args:
  144. client (mqtt.Client): current MQTT client
  145. userdata (_type_): _description_
  146. message (mqtt.MQTTMessage): received DRIVEALL message with PlantIDs and ActionID
  147. """
  148. dictMessage = json.loads(str(message.payload.decode("UTF-8")))
  149. plantIDs = dictMessage["PlantID"] # List of numbers
  150. actionID = dictMessage["ActionID"]
  151. logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}")
  152. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
  153. thread = threading.Thread(target= drive_plant_all_thread, args=(plantIDs, actionID, client), daemon=True)
  154. thread.start()
  155. def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  156. """
  157. Callback function for GPS position request
  158. Function to send actual GPS position via MQTT in form of POSITION
  159. Args:
  160. client (mqtt.Client): current mqtt client
  161. userdata (_type_): _description_
  162. message (mqtt.MQTTMessage): received message
  163. """
  164. logging.info("Received position request")
  165. # TODO Write GPS Sensor Function
  166. position = {
  167. "Position": ""
  168. }
  169. clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
  170. def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  171. """
  172. Callback function for battery status request
  173. Function to read battery status from ev3 and send via MQTT in form of BATTERY
  174. 8,5V -> 100%
  175. 5V -> 0%
  176. Args:
  177. client (mqtt.Client): current mqtt client
  178. userdata (_type_): _description_
  179. message (mqtt.MQTTMessage): received message
  180. """
  181. logging.info("Received battery status request")
  182. try:
  183. batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
  184. except:
  185. logging.error("Robot not connected")
  186. client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
  187. return
  188. batteryLevel = round(batteryLevel / 1000000, 2) # Voltage
  189. batteryLevel = batteryLevel - 5
  190. batteryLevel = round(batteryLevel / 3.5, 3) *100 # Percentage
  191. battery = {
  192. "Battery": batteryLevel
  193. }
  194. client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
  195. logging.info(f"Battery done {batteryLevel}")
  196. def sendPicture(client: mqtt.Client):
  197. """
  198. Takes picture and publish via MQTT
  199. Args:
  200. client (mqtt.Client): current mqtt client
  201. """
  202. try:
  203. Sensors.takePicture()
  204. except Exception as e:
  205. logging.error(str(e))
  206. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  207. return
  208. with open("picture.png", "rb") as f:
  209. file = f.read()
  210. byteArr = bytearray(file)
  211. client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
  212. logging.info("Picture Published")
  213. #endregion