Be more explicit in USB camera

umrover · Mar 29, 2024 · a6d372f · a6d372f
1 parent e88d415
commit a6d372f
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 18 deletions.
diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp
@@ -67,19 +67,27 @@ namespace mrover {
                         mImageWidth, mImageHeight);
             }
         } else {
-            assert(IS_JETSON);
-            // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4
-            //                nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV)
-
-            // ReSharper disable once CppDFAUnreachableCode
-            launch = std::format(
-                    "nvv4l2camerasrc device={} "
-                    "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 "
-                    "! nvvidconv "
-                    "! video/x-raw(memory:NVMM),format=I420 "
-                    "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true "
-                    "! appsink name=streamSink sync=false",
-                    mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate);
+            if constexpr (IS_JETSON) {
+                // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4
+                //                nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV)
+                // ReSharper disable once CppDFAUnreachableCode
+                launch = std::format(
+                        "nvv4l2camerasrc device={} "
+                        "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 "
+                        "! nvvidconv "
+                        "! video/x-raw(memory:NVMM),format=I420 "
+                        "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true "
+                        "! appsink name=streamSink sync=false",
+                        mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate);
+            } else {
+                launch = std::format(
+                        "v4l2src device={} "
+                        "! videoconvert "
+                        "! video/x-raw,format=I420,width={},height={},framerate={}/1 "
+                        "! nvh265enc "
+                        "! appsink name=streamSink sync=false",
+                        mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate);
+            }
         }
 
         ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch));

diff --git a/src/perception/usb_camera/usb_camera.cpp b/src/perception/usb_camera/usb_camera.cpp
@@ -41,8 +41,9 @@ namespace mrover {
             mImgPub = mNh.advertise<sensor_msgs::Image>(imageTopicName, 1);
             mCamInfoPub = mNh.advertise<sensor_msgs::CameraInfo>(cameraInfoTopicName, 1);
 
-            std::string videoString = std::format("video/x-raw,format=I420,width={},height={},framerate={}/1", width, height, framerate);
-            std::string gstString = std::format("v4l2src device={} ! videoconvert ! {} ! appsink", device, videoString);
+            std::string captureFormat = std::format("video/x-raw,format=YUY2,width={},height={},framerate={}/1", width, height, framerate);
+            std::string convertFormat = "video/x-raw,format=BGRA";
+            std::string gstString = std::format("v4l2src device={} ! {} ! videoconvert ! {} ! appsink", device, captureFormat, convertFormat);
             NODELET_INFO_STREAM(std::format("GStreamer string: {}", gstString));
             cv::VideoCapture capture{gstString, cv::CAP_GSTREAMER};
             if (!capture.isOpened()) throw std::runtime_error{"USB camera failed to open"};
@@ -56,9 +57,7 @@ namespace mrover {
 
                 if (mImgPub.getNumSubscribers()) {
                     auto imageMessage = boost::make_shared<sensor_msgs::Image>();
-                    cv::Mat bgra;
-                    cv::cvtColor(frame, bgra, cv::COLOR_YUV2BGRA_I420);
-                    fillImageMessage(bgra, imageMessage);
+                    fillImageMessage(frame, imageMessage);
                     imageMessage->header.frame_id = "long_range_cam_frame";
                     imageMessage->header.stamp = ros::Time::now();
                     mImgPub.publish(imageMessage);