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
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271
  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. Sensors.readSensors(sensorData)
  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. Sensors.readSensors(sensordata)
  92. except Exception as e:
  93. logging.error(str(e))
  94. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  95. sensordata["PlantID"] = plant
  96. allPlantData["SensorData"].append(sensordata)
  97. # TODO How to Handle Pictures and PlantID
  98. logging.info("Taking Picture")
  99. sendPicture(client)
  100. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
  101. if errorCode != 0:
  102. logging.error(f"Robot Error {errorCode} occurred")
  103. logging.info("Drive Plant aborted, Robot at starting position")
  104. client.publish(Topics["ROBOT_DATA_ERROR"], f"Rotate arm aborted, Robot at plant {plant}", qos=1)
  105. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  106. return
  107. logging.info("All Plants measured, sending data")
  108. client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1)
  109. logging.info("Robot driving home")
  110. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
  111. if errorCode != 0:
  112. logging.error(f"Robot Error {errorCode} occurred")
  113. logging.info(f"Drive Home aborted, Robot at plant {plant}")
  114. client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plant}", qos=1)
  115. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  116. return
  117. logging.info("Robot home")
  118. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  119. #region MQTT callbacks
  120. def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  121. """
  122. Function to drive to plant according to request
  123. Starting Drive in Thread to not block main programm
  124. Args:
  125. client (mqtt.Client): current mqtt client
  126. userdata (_type_): _description_
  127. message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
  128. """
  129. dictMessage = json.loads(str(message.payload.decode("UTF-8")))
  130. plantID = dictMessage["PlantID"]
  131. actionID = dictMessage["ActionID"]
  132. logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
  133. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
  134. thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
  135. thread.start()
  136. # TODO Test Drive all Plants
  137. # FIXME Only possible with color codes
  138. def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  139. """
  140. Function to drive to plants according to request
  141. Starting Drive in Thread to not block main programm
  142. Args:
  143. client (mqtt.Client): current MQTT client
  144. userdata (_type_): _description_
  145. message (mqtt.MQTTMessage): received DRIVEALL message with PlantIDs and ActionID
  146. """
  147. dictMessage = json.loads(str(message.payload.decode("UTF-8")))
  148. plantIDs = dictMessage["PlantID"] # List of numbers
  149. actionID = dictMessage["ActionID"]
  150. logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}")
  151. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
  152. thread = threading.Thread(target= drive_plant_all_thread, args=(plantIDs, actionID, client), daemon=True)
  153. thread.start()
  154. def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  155. """
  156. Callback function for GPS position request
  157. Function to send actual GPS position via MQTT in form of POSITION
  158. Args:
  159. client (mqtt.Client): current mqtt client
  160. userdata (_type_): _description_
  161. message (mqtt.MQTTMessage): received message
  162. """
  163. logging.info("Received position request")
  164. # TODO Write GPS Sensor Function
  165. position = {
  166. "Position": ""
  167. }
  168. clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
  169. def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  170. """
  171. Callback function for battery status request
  172. Function to read battery status from ev3 and send via MQTT in form of BATTERY
  173. 8,5V -> 100%
  174. 5V -> 0%
  175. Args:
  176. client (mqtt.Client): current mqtt client
  177. userdata (_type_): _description_
  178. message (mqtt.MQTTMessage): received message
  179. """
  180. logging.info("Received battery status request")
  181. try:
  182. batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
  183. except:
  184. logging.error("Robot not connected")
  185. client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
  186. return
  187. batteryLevel = round(batteryLevel / 1000000, 2) # Voltage
  188. batteryLevel = batteryLevel - 5
  189. batteryLevel = round(batteryLevel / 3.5, 3) *100 # Percentage
  190. battery = {
  191. "Battery": batteryLevel
  192. }
  193. client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
  194. logging.info(f"Battery done {batteryLevel}")
  195. def sendPicture(client: mqtt.Client):
  196. """
  197. Takes picture and publish via MQTT
  198. Args:
  199. client (mqtt.Client): current mqtt client
  200. """
  201. try:
  202. Sensors.takePicture()
  203. except Exception as e:
  204. logging.error(str(e))
  205. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  206. return
  207. with open("picture.png", "rb") as f:
  208. file = f.read()
  209. byteArr = bytearray(file)
  210. client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
  211. logging.info("Picture Published")
  212. #endregion