added svo conversions and tested after reinstalling cuda

rb_ws/src/buggy/CMakeLists.txt

@@ -36,6 +36,7 @@ install(PROGRAMS
   scripts/serial/ros_to_bnyahaj.py
   scripts/telemetry/telematics.py
   scripts/debug/debug_steer.py
+  scripts/vision/detector_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

rb_ws/src/buggy/launch/sc-main-vision.xml

@@ -0,0 +1,10 @@
+<launch>
+    <arg name="config_file" default="src/buggy/config/sc-mockroll.yaml"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
+            <param from="$(var config_file)"/>
+    </node>
+    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
+        <param from="$(var config_file)"/>
+    </node>
+    <node name="detector" pkg="buggy" exec="detector_node.py" output="screen" namespace="SC"/>
+</launch>

vision/convert.py

@@ -0,0 +1,199 @@
+########################################################################
+#
+# Copyright (c) 2022, STEREOLABS.
+#
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+########################################################################
+
+import sys
+import pyzed.sl as sl
+import numpy as np
+import cv2
+from pathlib import Path
+import enum
+import argparse
+import os
+
+class AppType(enum.Enum):
+    LEFT_AND_RIGHT = 1
+    LEFT_AND_DEPTH = 2
+    LEFT_AND_DEPTH_16 = 3
+
+
+def progress_bar(percent_done, bar_length=50):
+    #Display a progress bar
+    done_length = int(bar_length * percent_done / 100)
+    bar = '=' * done_length + '-' * (bar_length - done_length)
+    sys.stdout.write('[%s] %i%s\r' % (bar, percent_done, '%'))
+    sys.stdout.flush()
+
+
+def main():
+    # Get input parameters
+    svo_input_path = opt.input_svo_file
+    output_dir = opt.output_path_dir
+    avi_output_path = opt.output_avi_file
+    output_as_video = True
+    app_type = AppType.LEFT_AND_RIGHT
+    if opt.mode == 1 or opt.mode == 3:
+        app_type = AppType.LEFT_AND_DEPTH
+    if opt.mode == 4:
+        app_type = AppType.LEFT_AND_DEPTH_16
+
+    # Check if exporting to AVI or SEQUENCE
+    if opt.mode !=0 and opt.mode !=1:
+        output_as_video = False
+
+    if not output_as_video and not os.path.isdir(output_dir):
+        sys.stdout.write("Input directory doesn't exist. Check permissions or create it.\n",
+                         output_dir, "\n")
+        exit()
+
+    # Specify SVO path parameter
+    init_params = sl.InitParameters()
+    init_params.set_from_svo_file(svo_input_path)
+    init_params.svo_real_time_mode = False  # Don't convert in realtime
+    init_params.coordinate_units = sl.UNIT.MILLIMETER  # Use milliliter units (for depth measurements)
+
+    # Create ZED objects
+    zed = sl.Camera()
+
+    # Open the SVO file specified as a parameter
+    err = zed.open(init_params)
+    if err != sl.ERROR_CODE.SUCCESS:
+        sys.stdout.write(repr(err))
+        zed.close()
+        exit()
+
+    # Get image size
+    image_size = zed.get_camera_information().camera_configuration.resolution
+    width = image_size.width
+    height = image_size.height
+    width_sbs = width * 2
+
+    # Prepare side by side image container equivalent to CV_8UC4
+    svo_image_sbs_rgba = np.zeros((height, width_sbs, 4), dtype=np.uint8)
+
+    # Prepare single image containers
+    left_image = sl.Mat()
+    right_image = sl.Mat()
+    depth_image = sl.Mat()
+
+    video_writer = None
+    if output_as_video:
+        # Create video writer with MPEG-4 part 2 codec
+        video_writer = cv2.VideoWriter(avi_output_path,
+                                       cv2.VideoWriter_fourcc('M', 'P', '4', 'V'),
+                                       max(zed.get_camera_information().camera_configuration.fps, 25),
+                                       (width_sbs, height))
+        if not video_writer.isOpened():
+            sys.stdout.write("OpenCV video writer cannot be opened. Please check the .avi file path and write "
+                             "permissions.\n")
+            zed.close()
+            exit()
+
+    rt_param = sl.RuntimeParameters()
+
+    # Start SVO conversion to AVI/SEQUENCE
+    sys.stdout.write("Converting SVO... Use Ctrl-C to interrupt conversion.\n")
+
+    nb_frames = zed.get_svo_number_of_frames()
+
+    while True:
+        err = zed.grab(rt_param)
+        if err == sl.ERROR_CODE.SUCCESS:
+            svo_position = zed.get_svo_position()
+
+            # Retrieve SVO images
+            zed.retrieve_image(left_image, sl.VIEW.LEFT)
+
+            if app_type == AppType.LEFT_AND_RIGHT:
+                zed.retrieve_image(right_image, sl.VIEW.RIGHT)
+            elif app_type == AppType.LEFT_AND_DEPTH:
+                zed.retrieve_image(right_image, sl.VIEW.DEPTH)
+            elif app_type == AppType.LEFT_AND_DEPTH_16:
+                zed.retrieve_measure(depth_image, sl.MEASURE.DEPTH)
+
+            if output_as_video:
+                # Copy the left image to the left side of SBS image
+                svo_image_sbs_rgba[0:height, 0:width, :] = left_image.get_data()
+
+                # Copy the right image to the right side of SBS image
+                svo_image_sbs_rgba[0:, width:, :] = right_image.get_data()
+
+                # Convert SVO image from RGBA to RGB
+                ocv_image_sbs_rgb = cv2.cvtColor(svo_image_sbs_rgba, cv2.COLOR_RGBA2RGB)
+
+                # Write the RGB image in the video
+                video_writer.write(ocv_image_sbs_rgb)
+            else:
+                # Generate file names
+                filename1 = output_dir +"/"+ ("left%s.png" % str(svo_position).zfill(6))
+                filename2 = output_dir +"/"+ (("right%s.png" if app_type == AppType.LEFT_AND_RIGHT
+                                           else "depth%s.png") % str(svo_position).zfill(6))
+                # Save Left images
+                cv2.imwrite(str(filename1), left_image.get_data())
+
+                if app_type != AppType.LEFT_AND_DEPTH_16:
+                    # Save right images
+                    cv2.imwrite(str(filename2), right_image.get_data())
+                else:
+                    # Save depth images (convert to uint16)
+                    cv2.imwrite(str(filename2), depth_image.get_data().astype(np.uint16))
+
+            # Display progress
+            progress_bar((svo_position + 1) / nb_frames * 100, 30)
+        if err == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
+            progress_bar(100 , 30)
+            sys.stdout.write("\nSVO end has been reached. Exiting now.\n")
+            break
+    if output_as_video:
+        # Close the video writer
+        video_writer.release()
+
+    zed.close()
+    return 0
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter)
+    parser.add_argument('--mode', type = int, required=True, help= " Mode 0 is to export LEFT+RIGHT AVI. \n Mode 1 is to export LEFT+DEPTH_VIEW Avi. \n Mode 2 is to export LEFT+RIGHT image sequence. \n Mode 3 is to export LEFT+DEPTH_View image sequence. \n Mode 4 is to export LEFT+DEPTH_16BIT image sequence.")
+    parser.add_argument('--input_svo_file', type=str, required=True, help='Path to the .svo file')
+    parser.add_argument('--output_avi_file', type=str, help='Path to the output .avi file, if mode includes a .avi export', default = '')
+    parser.add_argument('--output_path_dir', type = str, help = 'Path to a directory, where .png will be written, if mode includes image sequence export', default = '')
+    opt = parser.parse_args()
+    if opt.mode > 4 or opt.mode < 0 :
+        print("Mode shoud be between 0 and 4 included. \n Mode 0 is to export LEFT+RIGHT AVI. \n Mode 1 is to export LEFT+DEPTH_VIEW Avi. \n Mode 2 is to export LEFT+RIGHT image sequence. \n Mode 3 is to export LEFT+DEPTH_View image sequence. \n Mode 4 is to export LEFT+DEPTH_16BIT image sequence.")
+        exit()
+    if not opt.input_svo_file.endswith(".svo") and not opt.input_svo_file.endswith(".svo2"):
+        print("--input_svo_file parameter should be a .svo file but is not : ",opt.input_svo_file,"Exit program.")
+        exit()
+    if not os.path.isfile(opt.input_svo_file):
+        print("--input_svo_file parameter should be an existing file but is not : ",opt.input_svo_file,"Exit program.")
+        exit()
+    if opt.mode < 2 and len(opt.output_avi_file)==0:
+        print("In mode ",opt.mode,", output_avi_file parameter needs to be specified.")
+        exit()
+    # if opt.mode < 2 and not opt.output_avi_file.endswith(".avi"):
+    #     print("--output_avi_file parameter should be a .avi file but is not : ",opt.output_avi_file,"Exit program.")
+    #     exit()
+    if opt.mode >=2  and len(opt.output_path_dir)==0 :
+        print("In mode ",opt.mode,", output_path_dir parameter needs to be specified.")
+        exit()
+    if opt.mode >=2 and not os.path.isdir(opt.output_path_dir):
+        print("--output_path_dir parameter should be an existing folder but is not : ",opt.output_path_dir,"Exit program.")
+        exit()
+    main()

vision/display_svo2.py

@@ -0,0 +1,54 @@
+# Release resources
+cap.release()
+out.release()
+cv2.destroyAllWindows()
+# Create a ZED camera object
+zed = sl.Camera()
+
+# Set SVO path for playback
+input_path = sys.argv[1]
+init_parameters = sl.InitParameters()
+init_parameters.set_from_svo_file(input_path)
+
+# Open the ZED
+err = zed.open(init_parameters)
+init_parameters.set_from_svo_file(input_path)
+
+# Open the ZED
+zed = sl.Camera()
+err = zed.open(init_parameters)
+
+width = 1280
+height = 480
+fourcc = cv2.VideoWriter_fourcc(*'mp4v')  # Or other codec
+out = cv2.VideoWriter('test_output_video.mp4', fourcc, 30, (width, height))
+
+cap = cv2.VideoCapture(0)
+
+if not cap.isOpened():
+    print("Error: Could not open camera.")
+    exit()
+
+svo_image = sl.Mat()
+while True:
+  if zed.grab() == sl.ERROR_CODE.SUCCESS:
+    # Read side by side frames stored in the SVO
+    zed.retrieve_image(svo_image, sl.VIEW.SIDE_BY_SIDE)
+
+    # Use get_data() to get the numpy array
+    image_ocv = svo_image.get_data()
+    # Display the left image from the numpy array
+    # cv2.imshow("Image", image_ocv)
+    out.write(image_ocv)
+
+    # Press Q on keyboard to exit
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+      break
+  elif zed.grab() == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
+    print("SVO end has been reached. Looping back to first frame")
+    cap.release()
+    out.release()
+    cv2.destroyAllWindows()
+    break
+  else:
+    break

vision/workflow-test/detector.py

@@ -13,7 +13,7 @@
 from threading import Lock, Thread
 from time import sleep
 
-import ogl_viewer.viewer as gl
+# import ogl_viewer.viewer as gl
 import cv_viewer.tracking_viewer as cv_viewer
 
 lock = Lock()
@@ -149,12 +149,12 @@ def main():
     camera_res = camera_infos.camera_configuration.resolution
 
     # Create OpenGL viewer
-    viewer = gl.GLViewer()
-    point_cloud_res = sl.Resolution(min(camera_res.width, 720), min(camera_res.height, 404))
-    point_cloud_render = sl.Mat()
-    viewer.init(camera_infos.camera_model, point_cloud_res, obj_param.enable_tracking)
+    # viewer = gl.GLViewer()
+    # point_cloud_res = sl.Resolution(min(camera_res.width, 720), min(camera_res.height, 404))
+    # point_cloud_render = sl.Mat()
+    # viewer.init(camera_infos.camera_model, point_cloud_res, obj_param.enable_tracking)
 
-    point_cloud = sl.Mat(point_cloud_res.width, point_cloud_res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU)
+    # point_cloud = sl.Mat(point_cloud_res.width, point_cloud_res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU)
     image_left = sl.Mat()
 
     # Utilities for 2D display
@@ -174,7 +174,7 @@ def main():
     print("Initialized display settings")
 
     i = 0
-    while viewer.is_available() and not exit_signal:
+    while not exit_signal: # viewer.is_available() and
         if (i % 10 == 0):
             print(i)
         if zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS:
@@ -198,22 +198,22 @@ def main():
 
             # -- Display
             # Retrieve display data
-            zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, point_cloud_res)
-            point_cloud.copy_to(point_cloud_render)
-            zed.retrieve_image(image_left, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution)
-            zed.get_position(cam_w_pose, sl.REFERENCE_FRAME.WORLD)
+            # zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, point_cloud_res)
+            # point_cloud.copy_to(point_cloud_render)
+            # zed.retrieve_image(image_left, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution)
+            # zed.get_position(cam_w_pose, sl.REFERENCE_FRAME.WORLD)
 
             # 3D rendering
-            viewer.updateData(point_cloud_render, objects)
+            # viewer.updateData(point_cloud_render, objects)
             # 2D rendering
-            np.copyto(image_left_ocv, image_left.get_data())
-            cv_viewer.render_2D(image_left_ocv, image_scale, objects, obj_param.enable_tracking)
-            global_image = cv2.hconcat([image_left_ocv, image_track_ocv])
+            # np.copyto(image_left_ocv, image_left.get_data())
+            # cv_viewer.render_2D(image_left_ocv, image_scale, objects, obj_param.enable_tracking)
+            # global_image = cv2.hconcat([image_left_ocv, image_track_ocv])
             # Tracking view
-            track_view_generator.generate_view(objects, cam_w_pose, image_track_ocv, objects.is_tracked)
+            # track_view_generator.generate_view(objects, cam_w_pose, image_track_ocv, objects.is_tracked)
 
-            cv2.imwrite("output/ZED__" + str(i) + ".jpg", global_image)
-            cv2.imshow("2D View w/ tracking", global_image)
+            # cv2.imwrite("output/ZED__" + str(i) + ".jpg", global_image)
+            # cv2.imshow("2D View w/ tracking", global_image)
 
             key = cv2.waitKey(10)
             if key == 27 or key == ord('q') or key == ord('Q'):
@@ -227,15 +227,15 @@ def main():
         else:
             exit_signal = True
 
-    viewer.exit()
+    # viewer.exit()
     exit_signal = True
     zed.close()
 
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser()
     parser.add_argument('--weights', type=str, default='../trained-models/01-15-25_no_pushbar_yolov11n.pt', help='model.pt path(s)')
-    parser.add_argument('--svo', type=str, default  =None, help='optional svo file, if not passed, use the plugged camera instead')
+    parser.add_argument('--svo', type=str, default=None, help='optional svo file, if not passed, use the plugged camera instead')
     parser.add_argument('--img_size', type=int, default=640, help='inference size (pixels)')
     parser.add_argument('--conf_thres', type=float, default=0.3, help='object confidence threshold')
     opt = parser.parse_args()