Added playback scripts, realsense intrincis param code, fixes

Author: kahlelar

Scripts/camera_playback.py

@@ -0,0 +1,35 @@
+import numpy as np
+import cv2
+import time
+
+
+recording = []
+with open("2023_02_15_empty_belt_recording.npy", "rb") as f:
+    recording = np.load(f)
+
+frame_count = recording.shape[3]
+frame_index = 1
+fps = 9
+
+while True:
+    rgb_frame = recording[:, :, 0:3, frame_index].astype(np.uint8)
+    depth_frame = recording[:, :, 3, frame_index].astype(np.uint16)
+
+    clahe = cv2.createCLAHE(clipLimit=20.0, tileGridSize=(5, 5))
+    depth_frame_hist = clahe.apply(depth_frame.astype(np.uint8))
+    cv2_colorized_depth = cv2.applyColorMap(depth_frame_hist, cv2.COLORMAP_JET)
+
+    frame_height, frame_width, frame_channel_count = rgb_frame.shape
+    cv2.imshow("RGB Frame", rgb_frame)
+    cv2.imshow("Depth Frame", cv2_colorized_depth)
+
+    frame_index += 1
+    time.sleep(1 / fps)
+
+    if frame_index >= frame_count:
+        print("Recording ended, Starting over...")
+        frame_index = 1
+
+    key = cv2.waitKey(1)
+    if key == 27:  # 'Esc'
+        break

Scripts/camera_record.py

@@ -0,0 +1,45 @@
+from robot_cell.detection.realsense_depth import DepthCamera
+import numpy as np
+import cv2
+import time
+
+camera = DepthCamera(config_path="./config/D435_camera_config.json")
+just_started = True
+max_num_frames = 100
+_, depth_frame, rgb_frame, _ = camera.get_frames()
+frame_height, frame_width, frame_channel_count = rgb_frame.shape
+
+recording = np.empty(
+    (frame_height, frame_width, frame_channel_count + 1, max_num_frames), dtype=np.uint16
+)
+
+depth_frame = np.expand_dims(depth_frame, axis=2)
+recording[:, :, :, 0] = np.concatenate((rgb_frame.astype(np.uint16), depth_frame), axis=2)
+
+time.sleep(5)
+
+frm_num = 1
+while True:
+    start_time = time.time()
+    success, depth_frame, rgb_frame, _ = camera.get_frames()
+    if not success:
+        continue
+
+    cv2.imshow("RGB Frame", rgb_frame)
+
+    depth_frame = np.expand_dims(depth_frame, axis=2)
+    recording[:, :, :, frm_num] = np.concatenate((rgb_frame.astype(np.uint16), depth_frame), axis=2)
+
+    end_time = time.time()
+    duration = end_time - start_time
+    print("Time:", duration, "; Frame number:", frm_num)
+
+    key = cv2.waitKey(1)
+    frm_num += 1
+    if key == 27 or frm_num >= max_num_frames:  # 'Esc'
+        print("Saving to file...")
+        with open("recording.npy", "wb") as f:
+            np.save(f, recording)
+        camera.release()
+        print("Finished")
+        break

cv_pick_place/robot_cell/control/robot_communication.py

@@ -49,11 +49,11 @@ def get_nodes(self):
         self.Safe_Operational_Stop = self.client.get_node(
             'ns=3;s="Robot_Data"."Status"."Safe_Operational_Stop"')
         self.Conveyor_Left = self.client.get_node(
-            'ns=3;s="Program_Data"."Conveyor_Left_Toggle"')
+            'ns=3;s="Program_Data"."OPCUA"."Conveyor_Left"')
         self.Conveyor_Right = self.client.get_node(
-            'ns=3;s="Program_Data"."Conveyor_Right_Toggle"')
+            'ns=3;s="Program_Data"."OPCUA"."Conveyor_Right"')
         self.Gripper_State = self.client.get_node(
-            'ns=3;s="Program_Data"."Gripper_Toggle"')
+            'ns=3;s="Program_Data"."OPCUA"."Gripper"')
         self.Encoder_Vel = self.client.get_node(
             'ns=3;s="Encoder_1".ActualVelocity')
         self.Encoder_Pos = self.client.get_node(

cv_pick_place/robot_cell/control/robot_control.py

@@ -103,37 +103,35 @@ def close_program(self):
 
         self.client.disconnect()
 
-    def change_gripper_state(self, state: bool):
+    def change_gripper_state(self):
         """
         Switch gripper on/off.
-
-        Args:
-            state (bool): If the gripper should be turned on or off.
         """
 
-        self.Gripper_State.set_value(opcua.ua.DataValue(state))
+        self.Gripper_State.set_value(opcua.ua.DataValue(True))
+        self.Gripper_State.set_value(opcua.ua.DataValue(False))
         if self.verbose:
-            print("[INFO] Gripper state is {}.".format(state))
+            print("[INFO] Toggled gripper.")
 
-    def change_conveyor_right(self, conv_right: bool):
+    def change_conveyor_right(self):
         """
         Switch conveyor right direction on/off.
-
-        Args:
-            conv_right (bool): If the conveyor should be turned on or off in the right direction.
         """
 
-        self.Conveyor_Right.set_value(opcua.ua.DataValue(conv_right))
+        self.Conveyor_Right.set_value(opcua.ua.DataValue(True))
+        self.Conveyor_Right.set_value(opcua.ua.DataValue(False))
+        if self.verbose:
+            print("[INFO] Toggled conveyor belt.")
 
-    def change_conveyor_left(self, conv_left: bool):
+    def change_conveyor_left(self):
         """
         Switch conveyor left direction on/off.
-
-        Args:
-            conv_left (bool): If the conveyor should be turned on or off in the left direction.
         """
 
-        self.Conveyor_Left.set_value(opcua.ua.DataValue(conv_left))
+        self.Conveyor_Left.set_value(opcua.ua.DataValue(True))
+        self.Conveyor_Left.set_value(opcua.ua.DataValue(False))
+        if self.verbose:
+            print("[INFO] Toggled conveyor belt.")
 
     def set_trajectory(
         self,
@@ -329,13 +327,13 @@ def control_server(self, pipe: multiprocessing.connection.PipeConnection):
                 data = input.data
 
                 if command == RcCommand.GRIPPER:
-                    self.change_gripper_state(data)
+                    self.change_gripper_state()
 
                 elif command == RcCommand.CONVEYOR_LEFT:
-                    self.change_conveyor_left(data)
+                    self.change_conveyor_left()
 
                 elif command == RcCommand.CONVEYOR_RIGHT:
-                    self.change_conveyor_right(data)
+                    self.change_conveyor_right()
 
                 elif command == RcCommand.CONTINUE_PROGRAM:
                     self.continue_program()

cv_pick_place/robot_cell/detection/realsense_depth.py

@@ -135,6 +135,17 @@ def __init__(
         # Start video stream
         self.profile = self.pipeline.start(self.config)
 
+        # Code for getting camera intrinsic parameters
+        # self.stream_profile_color = self.profile.get_stream(rs.stream.color, 0)
+        # self.stream_profile_depth = self.profile.get_stream(rs.stream.depth, 0)
+        # self.video_stream_profile = self.stream_profile_color.as_video_stream_profile()
+        # ; = self.video_stream_profile.get_intrinsics()
+        # print("fx =", intrinsics.fx, "; fy =", intrinsics.fy)
+        # print("ppx =", intrinsics.ppx, "; ppy =", intrinsics.ppy)
+        # print("height =", intrinsics.height, "; width =", intrinsics.width)
+        # print("coeffs =", intrinsics.coeffs)
+        # print("model =", intrinsics.model)
+
     def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
         """
         Reads and processes frames from connected camera.
@@ -160,7 +171,9 @@ def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
         # Apply hole filling filter
         depth_frame = self.hole_filling.process(depth_frame)
 
-        colorized_depth_frame = np.asanyarray(self.colorizer.colorize(depth_frame).get_data())
+        colorized_depth_frame = np.asanyarray(
+            self.colorizer.colorize(depth_frame).get_data()
+        )
         depth_frame = np.asanyarray(depth_frame.get_data())
         color_frame = np.asanyarray(color_frame.get_data())