Work on dashboard

Unity/LineRobot/Library/CurrentLayout-default.dwlt

@@ -14,10 +14,10 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   m_PixelRect:
     serializedVersion: 2
-    x: 0
-    y: 43.2
-    width: 1536
-    height: 780.8
+    x: -1920
+    y: 43
+    width: 1920
+    height: 997
   m_ShowMode: 4
   m_Title: Game
   m_RootView: {fileID: 4}
@@ -41,8 +41,8 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 0
-    width: 218.4
-    height: 730.8
+    width: 271
+    height: 947
   m_MinSize: {x: 101, y: 121}
   m_MaxSize: {x: 4001, y: 4021}
   m_ActualView: {fileID: 13}
@@ -65,10 +65,10 @@ MonoBehaviour:
   m_Children: []
   m_Position:
     serializedVersion: 2
-    x: 246.4
+    x: 308
     y: 0
-    width: 758.4
-    height: 459.2
+    width: 949
+    height: 823
   m_MinSize: {x: 202, y: 221}
   m_MaxSize: {x: 4002, y: 4021}
   m_ActualView: {fileID: 17}
@@ -96,8 +96,8 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 0
-    width: 1536
-    height: 780.8
+    width: 1920
+    height: 997
   m_MinSize: {x: 875, y: 300}
   m_MaxSize: {x: 10000, y: 10000}
   m_UseTopView: 1
@@ -121,7 +121,7 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 0
-    width: 1536
+    width: 1920
     height: 30
   m_MinSize: {x: 0, y: 0}
   m_MaxSize: {x: 0, y: 0}
@@ -145,8 +145,8 @@ MonoBehaviour:
   m_Position:
     serializedVersion: 2
     x: 0
-    y: 760.8
-    width: 1536
+    y: 977
+    width: 1920
     height: 20
   m_MinSize: {x: 0, y: 0}
   m_MaxSize: {x: 0, y: 0}
@@ -170,12 +170,12 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 30
-    width: 1536
-    height: 730.8
+    width: 1920
+    height: 947
   m_MinSize: {x: 680, y: 342}
   m_MaxSize: {x: 16005, y: 8042}
   vertical: 0
-  controlID: 65
+  controlID: 85
 --- !u!114 &8
 MonoBehaviour:
   m_ObjectHideFlags: 52
@@ -193,14 +193,14 @@ MonoBehaviour:
   - {fileID: 11}
   m_Position:
     serializedVersion: 2
-    x: 218.4
+    x: 271
     y: 0
-    width: 1004.8
-    height: 730.8
+    width: 1257
+    height: 947
   m_MinSize: {x: 304, y: 342}
   m_MaxSize: {x: 8004, y: 8042}
   vertical: 1
-  controlID: 66
+  controlID: 86
 --- !u!114 &9
 MonoBehaviour:
   m_ObjectHideFlags: 52
@@ -220,12 +220,12 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 0
-    width: 1004.8
-    height: 459.2
+    width: 1257
+    height: 823
   m_MinSize: {x: 304, y: 221}
   m_MaxSize: {x: 8004, y: 4021}
   vertical: 0
-  controlID: 67
+  controlID: 87
 --- !u!114 &10
 MonoBehaviour:
   m_ObjectHideFlags: 52
@@ -243,8 +243,8 @@ MonoBehaviour:
     serializedVersion: 2
     x: 0
     y: 0
-    width: 246.4
-    height: 459.2
+    width: 308
+    height: 823
   m_MinSize: {x: 100, y: 100}
   m_MaxSize: {x: 4000, y: 4000}
   m_ActualView: {fileID: 18}
@@ -270,9 +270,9 @@ MonoBehaviour:
   m_Position:
     serializedVersion: 2
     x: 0
-    y: 459.2
-    width: 1004.8
-    height: 271.59998
+    y: 823
+    width: 1257
+    height: 124
   m_MinSize: {x: 102, y: 121}
   m_MaxSize: {x: 4002, y: 4021}
   m_ActualView: {fileID: 21}
@@ -298,10 +298,10 @@ MonoBehaviour:
   m_Children: []
   m_Position:
     serializedVersion: 2
-    x: 1223.2
+    x: 1528
     y: 0
-    width: 312.80005
-    height: 730.8
+    width: 392
+    height: 947
   m_MinSize: {x: 275, y: 50}
   m_MaxSize: {x: 4000, y: 4000}
   m_ActualView: {fileID: 22}
@@ -325,15 +325,15 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Hierarchy
-    m_Image: {fileID: -3734745235275155857, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: 7966133145522015247, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
     serializedVersion: 2
-    x: 0
-    y: 73.6
-    width: 217.4
-    height: 709.8
+    x: -1920
+    y: 73
+    width: 270
+    height: 926
   m_ViewDataDictionary: {fileID: 0}
   m_SceneHierarchy:
     m_TreeViewState:
@@ -380,7 +380,7 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Preferences
-    m_Image: {fileID: 866346219090771560, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -5712115415447495865, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
@@ -410,7 +410,7 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Project Settings
-    m_Image: {fileID: 866346219090771560, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -5712115415447495865, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
@@ -465,15 +465,15 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Game
-    m_Image: {fileID: 4621777727084837110, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -6423792434712278376, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
     serializedVersion: 2
-    x: 464.80002
-    y: 73.6
-    width: 756.4
-    height: 438.2
+    x: -1341
+    y: 73
+    width: 947
+    height: 802
   m_ViewDataDictionary: {fileID: 0}
   m_SerializedViewNames: []
   m_SerializedViewValues: []
@@ -481,7 +481,7 @@ MonoBehaviour:
   m_ShowGizmos: 0
   m_TargetDisplay: 0
   m_ClearColor: {r: 0, g: 0, b: 0, a: 0}
-  m_TargetSize: {x: 756.4, y: 417.2}
+  m_TargetSize: {x: 947, y: 781}
   m_TextureFilterMode: 0
   m_TextureHideFlags: 61
   m_RenderIMGUI: 1
@@ -496,10 +496,10 @@ MonoBehaviour:
     m_VRangeLocked: 0
     hZoomLockedByDefault: 0
     vZoomLockedByDefault: 0
-    m_HBaseRangeMin: -302.56003
-    m_HBaseRangeMax: 302.56003
-    m_VBaseRangeMin: -166.88
-    m_VBaseRangeMax: 166.88
+    m_HBaseRangeMin: -473.5
+    m_HBaseRangeMax: 473.5
+    m_VBaseRangeMin: -390.5
+    m_VBaseRangeMax: 390.5
     m_HAllowExceedBaseRangeMin: 1
     m_HAllowExceedBaseRangeMax: 1
     m_VAllowExceedBaseRangeMin: 1
@@ -517,23 +517,23 @@ MonoBehaviour:
       serializedVersion: 2
       x: 0
       y: 21
-      width: 756.4
-      height: 417.2
+      width: 947
+      height: 781
     m_Scale: {x: 1, y: 1}
-    m_Translation: {x: 378.2, y: 208.6}
+    m_Translation: {x: 473.5, y: 390.5}
     m_MarginLeft: 0
     m_MarginRight: 0
     m_MarginTop: 0
     m_MarginBottom: 0
     m_LastShownAreaInsideMargins:
       serializedVersion: 2
-      x: -378.2
-      y: -208.6
-      width: 756.4
-      height: 417.2
+      x: -473.5
+      y: -390.5
+      width: 947
+      height: 781
     m_MinimalGUI: 1
   m_defaultScale: 1
-  m_LastWindowPixelSize: {x: 945.5, y: 547.75}
+  m_LastWindowPixelSize: {x: 947, y: 802}
   m_ClearInEditMode: 1
   m_NoCameraWarning: 1
   m_LowResolutionForAspectRatios: 01000000000000000001
@@ -555,15 +555,15 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Scene
-    m_Image: {fileID: 8634526014445323508, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: 2593428753322112591, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
     serializedVersion: 2
-    x: 218.40001
-    y: 73.6
-    width: 244.4
-    height: 438.2
+    x: -1649
+    y: 73
+    width: 306
+    height: 802
   m_ViewDataDictionary: {fileID: 0}
   m_ShowContextualTools: 0
   m_WindowGUID: c2bad47ba94a2bf41ad53eca9bdf5f39
@@ -672,7 +672,7 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Animator
-    m_Image: {fileID: 1711060831702674872, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -1673928668082335149, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
@@ -735,7 +735,7 @@ MonoBehaviour:
   m_MaxSize: {x: 10000, y: 10000}
   m_TitleContent:
     m_Text: Project
-    m_Image: {fileID: -5179483145760003458, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -5467254957812901981, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
@@ -873,15 +873,15 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Console
-    m_Image: {fileID: -4950941429401207979, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -4327648978806127646, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
     serializedVersion: 2
-    x: 218.40001
-    y: 532.8
-    width: 1002.8
-    height: 250.59998
+    x: -1649
+    y: 896
+    width: 1255
+    height: 103
   m_ViewDataDictionary: {fileID: 0}
 --- !u!114 &22
 MonoBehaviour:
@@ -899,15 +899,15 @@ MonoBehaviour:
   m_MaxSize: {x: 4000, y: 4000}
   m_TitleContent:
     m_Text: Inspector
-    m_Image: {fileID: -440750813802333266, guid: 0000000000000000d000000000000000,
+    m_Image: {fileID: -2667387946076563598, guid: 0000000000000000d000000000000000,
       type: 0}
     m_Tooltip: 
   m_Pos:
     serializedVersion: 2
-    x: 1223.2001
-    y: 73.6
-    width: 311.80005
-    height: 709.8
+    x: -392
+    y: 73
+    width: 391
+    height: 926
   m_ViewDataDictionary: {fileID: 0}
   m_ObjectsLockedBeforeSerialization: []
   m_InstanceIDsLockedBeforeSerialization: 

dashboard/main.py

@@ -1,17 +1,26 @@
 import cv2, sys
 import numpy as np
 import tkinter as tk
+from tkinter import ttk
 from PIL import ImageTk, Image
 
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
 from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
 
-import rclpy
-from rclpy.node import Node
-import std_msgs.msg
-import sensor_msgs.msg
-import threading
+from vision import process, obtain_depth_view
+
+try:
+	import rclpy
+	from rclpy.node import Node
+	import std_msgs.msg
+	import sensor_msgs.msg
+	import threading
+
+	_ROS2_FOUND = True
+except ImportError:
+	print("Could not import ROS2; please activate!")
+	_ROS2_FOUND = False
 
 class Dashboard:
 	def __init__(self):
@@ -23,27 +32,43 @@ class Dashboard:
 
 		self.DEPTH_SIZE = 300
 		self.PLOT_SIZE = 600
+		self.REDSENSOR_HISTORY = 100
 
 		self.data = {
 			"redsensor": []
 		}
 
 		# Show the depth image
-		self.depth_image = tk.Canvas(self.right, width=self.DEPTH_SIZE, height=self.DEPTH_SIZE)
-		self.depth_image.pack(side="top")
-		self.image = None
-		self.dit = self.depth_image.create_text(self.DEPTH_SIZE // 2, self.DEPTH_SIZE // 2, text="Waiting for Signal...", anchor=tk.CENTER)
-		self.di = self.depth_image.create_image(0, 0, anchor=tk.NW)
-
-		# Show the color sensor value history plot
+		self.image_notebook = ttk.Notebook(self.right)
+		self.depth_image_canvas = tk.Canvas(self.image_notebook, width=self.DEPTH_SIZE, height=self.DEPTH_SIZE)
+		self.depth_image_canvas.pack(side="top")
+		self.depth_image = None
+		self.dit = self.depth_image_canvas.create_text(self.DEPTH_SIZE // 2, self.DEPTH_SIZE // 2,
+		                                               text="Waiting for Signal...", anchor=tk.CENTER)
+		self.di = self.depth_image_canvas.create_image(0, 0, anchor=tk.NW)
+
+		self.processed_image_canvas = tk.Canvas(self.image_notebook, width=self.DEPTH_SIZE, height=self.DEPTH_SIZE)
+		self.processed_image_canvas.pack(side="top")
+		self.processed_image = None
+		self.pit = self.processed_image_canvas.create_text(self.DEPTH_SIZE // 2, self.DEPTH_SIZE // 2,
+		                                                   text="Waiting for Signal...", anchor=tk.CENTER)
+		self.pi = self.processed_image_canvas.create_image(0, 0, anchor=tk.NW)
+
+		self.image_notebook.add(self.depth_image_canvas, text="Depth Vision Image")
+		self.image_notebook.add(self.processed_image_canvas, text="OpenCV Processed Image")
+		self.image_notebook.pack(side="top", padx=5, pady=2)
+
+		# Show the color sensor value (+ history plot)
+		self.colvalue_label = tk.Label(self.right, text="Sensor Value: ???")
+		self.colvalue_label.pack(side="top", padx=5, pady=2)
 		self.colfig = plt.figure(figsize=(3, 1), dpi=100)
 		self.colfig_ax = self.colfig.add_subplot(111)
-		self.colfig_ax.set_xlim((0, 3))
+		self.colfig_ax.set_xlim((0, self.REDSENSOR_HISTORY))
 		self.colfig_ax.set_ylim((0, 1))
 		self.colfig_line, = self.colfig_ax.plot([], [], c="red")
 		self.colfig_canvas = FigureCanvasTkAgg(self.colfig, master=self.right)
 		self.colfig_canvas.draw()
-		self.colfig_canvas.get_tk_widget().pack(side="top")
+		self.colfig_canvas.get_tk_widget().pack(side="top", padx=5, pady=2)
 
 		# Create the drawing canvas
 		self.canvas = tk.Canvas(self.viewpanel, width=self.PLOT_SIZE, height=self.PLOT_SIZE)
@@ -58,63 +83,75 @@ class Dashboard:
 		self.root.wm_protocol("WM_DELETE_WINDOW", self.on_close)
 
 		# ROS2
-		self.node = Node("dashboardLFR")
-		self.create_listeners()
-		self.spinner = threading.Thread(target=lambda: rclpy.spin(self.node), daemon=True)
-		self.spinner.start()
+		if _ROS2_FOUND:
+			rclpy.init()
+
+			self.node = Node("dashboardLFR")
+			self.create_listeners()
+			self.spinner = threading.Thread(target=lambda: rclpy.spin(self.node), daemon=True)
+			self.spinner.start()
 
 
 	def on_close(self):
-		rclpy.shutdown()
-		self.spinner.join(1)
-		self.root.quit()
+		if _ROS2_FOUND:
+			rclpy.shutdown()
+			self.root.quit()
+			self.spinner.join(1)
+		else:
+			self.root.quit()
 
 	def create_listeners(self):
 		self.node.create_subscription(sensor_msgs.msg.Image, "rt/depth_processor", self.update_depthimage, 10)
+		self.node.create_subscription(sensor_msgs.msg.Image, "rt/depth_processor", self.update_processimage, 10)
 		self.node.create_subscription(std_msgs.msg.Float32, "rt/redsensor", self.update_redsensor, 10)
 		self.colfig_ani = animation.FuncAnimation(self.colfig, lambda _: self.update_redsensor_plot(), interval=100)
 
 	def update_redsensor(self, msg):
 		self.data["redsensor"].append(msg.data)
+		self.colvalue_label.config(text="Sensor Value: %.5f" % msg.data)
 
 	def update_redsensor_plot(self):
-		LENG = 50
-		while len(self.data["redsensor"]) > LENG:
+		while len(self.data["redsensor"]) > self.REDSENSOR_HISTORY:
 			self.data["redsensor"].pop(0)
-		# TODO: "objects cannot be broadcast to a single shape"
-		self.colfig_line.set_data(range(LENG), self.data["redsensor"])
+		data = self.data["redsensor"][:]
+		self.colfig_line.set_data(list(range(len(data))), data)
 
 	def update_depthimage(self, msg):
 		if self.dit is not None:
-			self.depth_image.delete(self.dit)
+			self.depth_image_canvas.delete(self.dit)
 			self.dit = None
 		dt = np.dtype('float32')
 		dt = dt.newbyteorder('<')
 		current_frame = np.ndarray(shape=(msg.height, msg.width, 1), dtype=dt, buffer=bytearray(msg.data))
 
-		clipmin = 0.3
-		clipmax = 2.0
-		skewmin = np.amin(current_frame)
-		skewmax = np.amax(current_frame)
+		im_color = obtain_depth_view(current_frame, self.DEPTH_SIZE)
+
+		self.depth_image = Image.fromarray(im_color)
+		self.depth_image = ImageTk.PhotoImage(self.depth_image)
 
-		new_frame = current_frame.copy()
+		self.depth_image_canvas.itemconfig(self.di, image=self.depth_image)
 
-		np.clip(new_frame, clipmin, clipmax, out=new_frame)
-		new_frame -= skewmin
-		new_frame /= (skewmax - skewmin) / 255.0
-		new_frame = new_frame.astype(dtype=np.uint8, copy=False)
+	def update_processimage(self, msg):
+		if self.pit is not None:
+			self.processed_image_canvas.delete(self.pit)
+			self.pit = None
+		dt = np.dtype('float32')
+		dt = dt.newbyteorder('<')
+		current_frame = np.ndarray(shape=(msg.height, msg.width, 1), dtype=dt, buffer=bytearray(msg.data))
 
-		im_color = cv2.applyColorMap(new_frame, cv2.COLORMAP_BONE)
-		im_color = cv2.resize(im_color, (self.DEPTH_SIZE, self.DEPTH_SIZE))
+		new_frame, state = process(current_frame.copy(), self.DEPTH_SIZE)
 
-		self.image = Image.fromarray(im_color)
-		self.image = ImageTk.PhotoImage(self.image)
+		# show processed image
+		self.processed_image = Image.fromarray(new_frame)
+		self.processed_image = ImageTk.PhotoImage(self.processed_image)
 
-		self.depth_image.itemconfig(self.di, image=self.image)
+		self.processed_image_canvas.itemconfig(self.pi, image=self.processed_image)
 
+		# TODO: send identified state via ROS2
+		if _ROS2_FOUND:
+			pass
 
-if __name__ == '__main__':
-	rclpy.init()
 
+if __name__ == '__main__':
 	app = Dashboard()
 	app.root.mainloop()

dashboard/vision.py

@@ -0,0 +1,105 @@
+import cv2
+import numpy as np
+
+CLIPPING_MASK_MIN = 0.3
+CLIPPING_MASK_MAX = 2.0
+CLIPPING_MASK_NEAR = 0.9
+CLIPPING_MASK_FAR = 1.0
+
+EPSILON = 1e-10
+ARROWLENGTH = 20
+
+# State variables
+_prev_pos = None
+_prev_heading = None
+
+def obtain_depth_view(image, size):
+	frame = image.copy()
+	frame = cv2.resize(frame, (size, size))
+	skewmin = np.amin(frame)
+	skewmax = np.amax(frame)
+
+	np.clip(frame, CLIPPING_MASK_MIN, CLIPPING_MASK_MAX, out=frame)
+	frame -= skewmin
+	frame /= (skewmax - skewmin) / 255.0
+	frame = frame.astype(dtype=np.uint8, copy=False)
+
+	im_color = cv2.applyColorMap(frame, cv2.COLORMAP_BONE)
+	return im_color
+
+
+def process(image, size):
+	repr_image = obtain_depth_view(image, size)
+
+	depth_image_3d = image.copy()
+	depth_image_3d = cv2.resize(depth_image_3d, (size, size))
+
+	white = 255 * np.ones(depth_image_3d.shape, np.uint8)
+	bg_removed = np.where((depth_image_3d > CLIPPING_MASK_FAR) | (depth_image_3d < CLIPPING_MASK_NEAR), 0, white)
+
+	blurred = cv2.blur(bg_removed, (5, 5))
+	kernel = np.ones((4, 4), np.uint8)
+	dilation = cv2.dilate(blurred, kernel, iterations=5)
+	kernel = np.ones((5, 5), np.uint8)
+	erosion = cv2.erode(dilation, kernel, iterations=3)
+	# edged = cv2.Canny(erosion, 100, 200, 5)
+
+	contours, _ = cv2.findContours(erosion, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
+	csize = len(contours)
+	if csize == 0:
+		print("No Contours Detected!")
+		return repr_image, (0, 0, 0)
+	else:
+		for c in contours:
+			x, y, w, h = cv2.boundingRect(c)
+			cv2.rectangle(repr_image, (x, y), (x + w, y + h), (255, 0, 90), 2)
+
+		ctr, ccenter, tl, br = _findLargestContour(contours)
+		cv2.circle(repr_image, ccenter, 4, (255, 0, 0), -1)
+
+		heading = 0
+
+		global _prev_pos, _prev_heading
+		if _prev_pos is not None:
+			# You have moved if the distance is large enough
+			#   small distances can occur due to the object detection
+			pdist = _tuple_distance(ccenter, _prev_pos)
+			if pdist < EPSILON:
+				# You have actually moved
+				ccenter = _prev_pos
+
+			# pv is the normalized direction identified in the robot
+			pv = (ccenter[0] - _prev_pos[0]) / pdist, (ccenter[1] - _prev_pos[1]) / pdist
+			heading = np.arctan2(pv[1], pv[0])
+
+			cv2.arrowedLine(repr_image, ccenter, (ccenter[0] + int(pv[0] * ARROWLENGTH), ccenter[1] + int(pv[1] * ARROWLENGTH)),
+			                (255, 0, 0), 2, tipLength=0.3)
+
+			if _prev_heading is not None:
+				# inconsistencies in the object detection may result in an opposite angle
+				angle_diff = abs(_prev_heading - heading)
+				while angle_diff > np.pi / 2:
+					if heading > _prev_heading:
+						heading -= np.pi
+					else:
+						heading += np.pi
+					angle_diff = abs(_prev_heading - heading)
+
+		_prev_heading = heading
+		_prev_pos = ccenter
+
+	# TODO: from image coords to world coords (wrt center of camera?)
+	# TODO: field of view transformation!
+	return repr_image, (ccenter[0], ccenter[1], heading)
+
+
+def _findLargestContour(contours):
+	cnt = sorted(contours, key=cv2.contourArea)[-1]
+	M = cv2.moments(cnt)
+	pos = int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])
+	x, y, w, h = cv2.boundingRect(cnt)
+	return cnt, pos, (x, y), (x + w, y + h)
+
+
+def _tuple_distance(p1, p2):
+	return np.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)