added tracking based on encoder

Author: david-s-martinez

cv_pick_place/cv_pick_place_main.py

@@ -438,8 +438,8 @@ def main_pick_place_conveyor_w_point_cloud(server_in):
         ret, depth_frame, rgb_frame, colorized_depth = dc.get_frame()
 
         rgb_frame = rgb_frame[:,240:1680]
+        # 1080x1440x3
         height, width, depth = rgb_frame.shape
-        
         try:
             rgb_frame = apriltag.detect_tags(rgb_frame)
             if frame_count == 1:
@@ -464,15 +464,17 @@ def main_pick_place_conveyor_w_point_cloud(server_in):
         heatmap = colorized_depth
         heatmap = heatmap[90:400,97:507,:]
         heatmap = cv2.resize(heatmap, (width,height))
+        encoder_vel = robot_server_dict['encoder_vel']
+        encoder_pos = robot_server_dict['encoder_pos']
 
         img_detect, detected = pack_detect.deep_pack_obj_detector(
                                                             rgb_frame, 
-                                                            depth_frame, 
+                                                            depth_frame,
+                                                            encoder_pos, 
                                                             bnd_box = bbox)
         objects, deregistered_packets = pt.update(detected, depth_frame)
         print(objects, rob_stopped, stop_active, prog_done)
         is_detect = len(detected) != 0
-        encoder_vel = robot_server_dict['encoder_vel']
         is_conv_mov = encoder_vel < - 100.0
 
         if is_detect:
@@ -485,10 +487,11 @@ def main_pick_place_conveyor_w_point_cloud(server_in):
             track_result, packet = rc.pack_obj_tracking_update(objects, 
                                                     img_detect, 
                                                     homography, 
-                                                    is_detect, 
-                                                    x_fixed = x_fixed, 
-                                                    track_frame = track_frame,
-                                                    frames_lim = frames_lim)
+                                                    is_detect,
+                                                    x_fixed, 
+                                                    track_frame,
+                                                    frames_lim,
+                                                    encoder_pos) 
             if track_result is not None:
                 dist_to_pack = track_result[2]
                 delay = dist_to_pack/(abs(encoder_vel)/10)

cv_pick_place/packet_detection/__pycache__/packet_detector.cpython-39.pyc

@@ -323,7 +323,7 @@ def deep_detector_v2(self, color_frame, depth_frame, bnd_box = True, segment = F
         else:
             return img_np_detect, detected
 
-    def deep_pack_obj_detector(self, color_frame, depth_frame, bnd_box = True, segment = False):
+    def deep_pack_obj_detector(self, color_frame, depth_frame, encoder_pos, bnd_box = True, segment = False):
         """
         Main packet detector function.
     
@@ -393,7 +393,8 @@ def deep_pack_obj_detector(self, color_frame, depth_frame, bnd_box = True, segme
                             pack_type = detections['detection_classes'][i],
                             centroid = centroid, 
                             angle = angle,
-                            width = w, height= h)
+                            width = w, height = h, 
+                            encoder_position = encoder_pos)
                 if centroid[0] - w/2 > guard  and centroid[0] + w/2 < (width - guard ):
                     detected.append(packet)
         img_np_detect, box_mask = self.compute_mask(img_np_detect,box_mask, box_array)

cv_pick_place/packet_detection/packet_detector.py

@@ -2,7 +2,7 @@
 
 # Class containing relevant packet data for convenience
 class Packet:
-    def __init__(self, box = np.empty(()), pack_type = None,centroid=(0, 0), angle=0, width=0, height=0):
+    def __init__(self, box = np.empty(()), pack_type = None,centroid=(0, 0), angle=0, width=0, height=0, encoder_position=0):
         # Tuple of 2 numbers describing center of the packet
         self.centroid = centroid
 
@@ -21,6 +21,31 @@ def __init__(self, box = np.empty(()), pack_type = None,centroid=(0, 0), angle=0
         # Number of frames the packet has disappeared for
         self.disappeared = 0
 
+        self.time_of_disappearance = None
+
         self.box = box
 
-        self.pack_type = pack_type
+        self.pack_type = pack_type
+
+        # Encoder data
+        self.starting_encoder_position = encoder_position
+
+        # Used for offsetting centroid position calculated using encoder
+        self.first_centroid_position = centroid
+
+
+        self.marked_for_picking = False
+    def getCentroidFromEncoder(self, encoder_position):
+        # k = 0.8299  # 640 x 480
+        # k = 1.2365  # 1280 x 720
+        k = 1.8672  # 1440 x 1080
+        # k = 1.2365  # 1080 x 720
+        return (int(k * (encoder_position - self.starting_encoder_position) + self.first_centroid_position[0]), self.centroid[1])
+
+    # Get centroid in world coordinates
+    def getCentroidInWorldFrame(self, homography):
+        centroid_robot_frame = np.matmul(homography, np.array([self.centroid[0], self.centroid[1], 1]))
+
+        packet_x = centroid_robot_frame[0] * 10
+        packet_y = centroid_robot_frame[1] * 10
+        return packet_x, packet_y

cv_pick_place/packet_object/__pycache__/packet.cpython-39.pyc

@@ -48,7 +48,7 @@ def show_point_cloud(self):
         vis.create_window()
         vis.add_geometry(source)
         # vis.add_geometry(target)
-        icp_iteration = 500
+        icp_iteration = 250
 
         for i in range(icp_iteration):
             # source.transform(Rot_z)

cv_pick_place/packet_object/packet.py

@@ -451,7 +451,7 @@ def packet_tracking_update(self, objects, img, homog, enable, x_fixed,
                         mean_y = 0
                         return x_fixed, world_y, dist_to_pack
     def pack_obj_tracking_update(self, objects, img, homog, enable, x_fixed, 
-                                track_frame, frames_lim, track_list = []):
+                                track_frame, frames_lim, encoder_pos, track_list = []):
         """
         Computes distance to packet and updated x, mean y packet positions of tracked moving packets.
     
@@ -479,6 +479,7 @@ def pack_obj_tracking_update(self, objects, img, homog, enable, x_fixed,
             cv2.putText(img, text, (centroid[0] , centroid[1] - 40), 
                         cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)
             cv2.circle(img, (centroid[0], centroid[1]), 4, (255, 255, 0), -1)
+            cv2.circle(img, packet.getCentroidFromEncoder(encoder_pos), 4, (0, 0, 255), -1)
             if homog is not None:
                 new_centroid = np.append(centroid,1)
                 world_centroid = homog.dot(new_centroid)