Merge pull request #21 from ut-ras/feature/start-up

Restart on failure service

.gitignore

@@ -1 +1,2 @@
-__pycache__/
+__pycache__/
+logging/

communication/communication.py

@@ -88,7 +88,6 @@ def send_turret_data(pos, vel, acc,
     xAcc = acc['x_acc']
     yAcc = acc['y_acc']
     zAcc = acc['z_acc']
-    # logging.debug("Sending UART data at: ", str(time.localtime(time.time())))
     send_message(TurretDataFormat.pack(xPos, yPos, zPos, xVel, yVel, zVel, xAcc, yAcc, zAcc, hasTarget), TurretDataFormat, MessageType.CMD_Turret_Aim)
 
 def send_no_data():

depth_calculation/depth_calculation.py

@@ -14,6 +14,7 @@ class RealSense:
     def __init__(self):
         # Set up pipeline
         self.__pipeline__ = rs.pipeline()
+        self.__color_sensor__ = None
         self.__config__ = rs.config()
         self.__color_frame__ = None
         self.__depth_frame__ = None
@@ -37,6 +38,8 @@ def initialize_real_sense(self):
         pipeline_wrapper = rs.pipeline_wrapper(self.__pipeline__)
         self.__pipeline_profile__ = self.__config__.resolve(pipeline_wrapper)
         self.__device__ = self.__pipeline_profile__.get_device()
+        self.__color_sensor__ = self.__device__.query_sensors()[1]
+        self.__color_sensor__.set_option(rs.option.enable_auto_exposure, True)
         device_product_line = str(self.__device__.get_info(rs.camera_info.product_line))
 
         found_rgb = False
@@ -86,7 +89,7 @@ def set_all_bounding_box_depth_values(self, box_list):
 
                 #check for invalid depth values
                 if(np.isnan(depth_value)):
-                    # logging.warning("Invalid depth value")
+
                     invalid_depth_value = True
                     return invalid_depth_value
 

error.txt

@@ -0,0 +1,75 @@
+confidence  0.36909086
+class_idx_float  0.0
+Time to send message:  0.003603219985961914
+confidence  0.29401532
+class_idx_float  0.0
+Time to send message:  0.004235029220581055
+confidence  0.31385234
+class_idx_float  1.0
+Time to send message:  0.0033359527587890625
+confidence  0.34983367
+class_idx_float  1.0
+confidence  0.29517502
+class_idx_float  1.0
+Time to send message:  0.0036661624908447266
+confidence  0.36796167
+class_idx_float  1.0
+confidence  0.32374036
+class_idx_float  1.0
+Traceback (most recent call last):
+  File "/home/debian/robomaster_CV/prediction/prediction.py", line 52, in sqrt_func
+    result = np.linalg.cholesky(x)
+  File "<__array_function__ internals>", line 5, in cholesky
+  File "/usr/lib/python3/dist-packages/numpy/linalg/linalg.py", line 764, in cholesky
+    r = gufunc(a, signature=signature, extobj=extobj)
+  File "/usr/lib/python3/dist-packages/numpy/linalg/linalg.py", line 91, in _raise_linalgerror_nonposdef
+    raise LinAlgError("Matrix is not positive definite")
+numpy.linalg.LinAlgError: Matrix is not positive definite
+
+During handling of the above exception, another exception occurred:
+
+Traceback (most recent call last):
+  File "/home/debian/robomaster_CV/prediction/prediction.py", line 56, in sqrt_func
+    result = np.linalg.cholesky(x)
+  File "<__array_function__ internals>", line 5, in cholesky
+  File "/usr/lib/python3/dist-packages/numpy/linalg/linalg.py", line 764, in cholesky
+    r = gufunc(a, signature=signature, extobj=extobj)
+  File "/usr/lib/python3/dist-packages/numpy/linalg/linalg.py", line 91, in _raise_linalgerror_nonposdef
+    raise LinAlgError("Matrix is not positive definite")
+numpy.linalg.LinAlgError: Matrix is not positive definite
+
+During handling of the above exception, another exception occurred:
+
+Traceback (most recent call last):
+  File "/home/debian/robomaster_CV/main.py", line 67, in <module>
+    run_forever()
+  File "/home/debian/robomaster_CV/main.py", line 51, in run_forever
+    pos, vel, acc = object.select_target(boundingbox_list)
+  File "/home/debian/robomaster_CV/object_log/objectlog.py", line 51, in select_target
+    self.plates.update_box(boundingbox_list[lowest_distance_idx])
+  File "/home/debian/robomaster_CV/object_log/armorplate.py", line 165, in update_box
+    self.kf.kinematicPredict(delta_t)
+  File "/home/debian/robomaster_CV/prediction/prediction.py", line 65, in kinematicPredict
+    self.filter.predict(dt=del_t)
+  File "/usr/local/lib/python3.9/dist-packages/filterpy/kalman/UKF.py", line 388, in predict
+    self.compute_process_sigmas(dt, fx, **fx_args)
+  File "/usr/local/lib/python3.9/dist-packages/filterpy/kalman/UKF.py", line 500, in compute_process_sigmas
+    sigmas = self.points_fn.sigma_points(self.x, self.P)
+  File "/usr/local/lib/python3.9/dist-packages/filterpy/kalman/sigma_points.py", line 168, in sigma_points
+    U = self.sqrt((lambda_ + n)*P)
+  File "/home/debian/robomaster_CV/prediction/prediction.py", line 58, in sqrt_func
+    return result
+UnboundLocalError: local variable 'result' referenced before assignment
+  8585.146595 s:  VX_ZONE_INIT:[tivxHostDeInitLocal:100] De-Initialization Done for HOST !!!
+  8585.150154 s:  VX_ZONE_INIT:[tivxDeInitLocal:193] De-Initialization Done !!!
+APP: Deinit ... !!!
+REMOTE_SERVICE: Deinit ... !!!
+REMOTE_SERVICE: Deinit ... Done !!!
+IPC: Deinit ... !!!
+IPC: DeInit ... Done !!!
+MEM: Deinit ... !!!
+MEM: Alloc's: 7 alloc's of 14532597 bytes 
+MEM: Free's : 7 free's  of 14532597 bytes 
+MEM: Open's : 0 allocs  of 0 bytes 
+MEM: Deinit ... Done !!!
+APP: Deinit ... Done !!!

main.py

@@ -11,7 +11,7 @@
 import time
 #begin initialization
 def run_forever():
-	# logging.basicConfig(filename='error_logs_'+str(time.localtime(time.time())), encoding='utf-8', level=logging.DEBUG)
+	logging.basicConfig(filename='/home/debian/robomaster_CV/logging/log'+str(time.time())+'.log',encoding='utf-8',level=logging.DEBUG)
 	RealSense = dp.RealSense()
 	RealSense.initialize_real_sense()
 	detector = od.object_detector()
@@ -22,22 +22,25 @@ def run_forever():
 	object = objectlog.objectlog()
 
 	while True:
+		start_time = time.time()
 		is_depth_invalid = False
 
 		RealSense.get_color_depth_image()
 
 		detector.run_object_detections(RealSense, boundingbox_list)
 
 		if(len(boundingbox_list)==0):
-			# logging.debug("No detections, skipping frame", str(time.localtime(time.time())))
+			end_time = time.time()
+			logging.warning('No detections, skipping frame: ' + str(end_time - start_time) + ' seconds')
 			com.send_no_data()
 			boundingbox_list.clear()
 			continue
 
 		is_depth_invalid = RealSense.set_all_bounding_box_depth_values(boundingbox_list)
 
 		if(is_depth_invalid):
-			# logging.warning("Invalid depth data", str(boundingbox_list))
+			end_time = time.time()
+			logging.warning('No valid depth data, skipping frame: ' + str(end_time - start_time) + ' seconds')
 			com.send_no_data()
 			boundingbox_list.clear()
 			continue
@@ -46,9 +49,11 @@ def run_forever():
 
 		pos, vel, acc = object.select_target(boundingbox_list)
 
-		# logging.debug("pos, vel, acc: ", str(pos), str(vel), str(acc))
 		com.send_turret_data(pos, vel, acc, hasTarget=True)
-
+
+		end_time = time.time()
+		logging.debug('System finished: '+str(end_time-start_time) + ' seconds')
+
 		boundingbox_list.clear()
 
 

object_detection/object_detection.py

@@ -3,6 +3,7 @@
 import cv2
 import time
 from bounding_box import bounding_box as bb
+import logging
 
 model_path = '/home/debian/robomaster_CV/object_detection/best_with_shapes.onnx'
 artifacts_dir = '/home/debian/robomaster_CV/object_detection/tidl_output'
@@ -70,9 +71,9 @@ def render_boxes(self, image, output, boundingbox_list):
 			# print("x1 ", x1)
 			# print("y1 ", y1)
 			# print("x2 ", x2)
-			# print("y2 ", y2)
-			print("confidence ", confidence)
-			print("class_idx_float ", class_idx_float)
+			# # print("y2 ", y2)
+			# logging.debug("Confidence: " + str(confidence))
+			# logging.debug("Class of plate: " + str(class_idx_float))
 
 			x1 = int(round(x1 / self.__width__ * image.shape[1]))
 			y1 = int(round(y1 / self.__height__ * image.shape[0]))
@@ -89,7 +90,7 @@ def render_boxes(self, image, output, boundingbox_list):
 			}[class_idx_float]
 
 			#print bounding box onto image, for debugging purposes
-			cv2.rectangle(image, (x1, y1), (x2, y2), class_draw_color, 3)
+			# cv2.rectangle(image, (x1, y1), (x2, y2), class_draw_color, 3)
 
 			bounding_box = bb.BoundingBox()
 			bounding_box.set_x_value(x1, x2)