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.5KB

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