diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index 77f9d38de..3763e582a 100644 --- 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 index 08dbe5ae1..e83d6cac2 100644 --- 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(imageTopicName, 1); mCamInfoPub = mNh.advertise(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(); - 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);