From e2226605fef802a51279f4e4cfd696e2144b8862 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 14 Apr 2024 15:12:05 -0400 Subject: [PATCH 1/5] Start working on camera stuff --- CMakeLists.txt | 8 ++- cmake/deps.cmake | 2 + .../gst_websocket_streamer.cpp | 64 +++++++++++++++++-- .../gst_websocket_streamer.hpp | 3 +- src/esw/gst_websocket_streamer/pch.hpp | 12 ++-- 5 files changed, 75 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c23c58118..9552eada5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -198,9 +198,11 @@ if (CUDA_FOUND) # target_include_directories(nv_vid_codec_h265_enc SYSTEM PUBLIC deps/nvenc) endif () -mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) -mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES}) -mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS}) +if (Gst_FOUND AND GstApp_FOUND AND LibUsb_FOUND AND LibUdev_FOUND) + mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) + mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES}) + mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS} ${LibUsb_INCLUDE_DIRS} ${LibUdev_INCLUDE_DIRS}) +endif () if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) diff --git a/cmake/deps.cmake b/cmake/deps.cmake index b73637ce8..b3bb4a520 100644 --- a/cmake/deps.cmake +++ b/cmake/deps.cmake @@ -69,3 +69,5 @@ pkg_search_module(NetLink libnl-3.0 QUIET) pkg_search_module(NetLinkRoute libnl-route-3.0 QUIET) pkg_search_module(Gst gstreamer-1.0 QUIET) pkg_search_module(GstApp gstreamer-app-1.0 QUIET) +pkg_search_module(LibUsb libusb-1.0 QUIET) +pkg_search_module(LibUdev libudev QUIET) diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index a1430549b..8510c39e6 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -39,7 +39,7 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); std::string launch; - if (mCaptureDevice.empty()) { + if (mDevice.empty()) { if constexpr (IS_JETSON) { // ReSharper disable once CppDFAUnreachableCode launch = std::format( @@ -85,7 +85,7 @@ namespace mrover { "! video/x-raw(memory:NVMM),format=NV12 " "! 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); + mDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); } else { // ReSharper disable once CppDFAUnreachableCode launch = std::format( @@ -94,14 +94,14 @@ namespace mrover { "! videoconvert " "! nvh265enc name=encoder " "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight); + mDevice, mImageWidth, mImageHeight); } } ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); - if (mCaptureDevice.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); + if (mDevice.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); mStreamSink = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "streamSink")); if (gst_element_set_state(mPipeline, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) @@ -150,7 +150,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCaptureDevice = mPnh.param("device", ""); + mDevice = mPnh.param("device", ""); + mUsbIdentifier = mPnh.param("usb_id", "1"); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -161,6 +162,57 @@ namespace mrover { gst_init(nullptr, nullptr); + libusb_context* context{}; + if (libusb_init(&context) != 0) throw std::runtime_error{"Failed to initialize libusb"}; + + libusb_device** list{}; + ssize_t count = libusb_get_device_list(context, &list); + if (count < 0) throw std::runtime_error{"Failed to get device list"}; + + for (ssize_t i = 0; i < count; ++i) { + libusb_device* device = list[i]; + libusb_device_descriptor descriptor{}; + if (libusb_get_device_descriptor(device, &descriptor) != 0) continue; + + std::uint8_t bus = libusb_get_bus_number(device); + std::uint8_t address = libusb_get_device_address(device); + std::uint8_t port = libusb_get_port_number(device); + + // get associated video port + // libusb_get_port_numbers(device, port_numbers, sizeof(port_numbers)); + + ROS_INFO_STREAM(std::format("USB device: bus={}, address={}, port={}, vendor={}, product={}", + bus, address, port, descriptor.idVendor, descriptor.idProduct)); + } + + if (!mUsbIdentifier.empty()) { + udev* udevContext = udev_new(); + if (!udevContext) throw std::runtime_error{"Failed to initialize udev"}; + + udev_enumerate* enumerate = udev_enumerate_new(udevContext); + if (!enumerate) throw std::runtime_error{"Failed to create udev enumeration"}; + + udev_enumerate_add_match_subsystem(enumerate, "video4linux"); + udev_enumerate_scan_devices(enumerate); + + udev_list_entry* devices = udev_enumerate_get_list_entry(enumerate); + if (!devices) throw std::runtime_error{"Failed to get udev device list"}; + + udev_list_entry* entry; + udev_list_entry_foreach(entry, devices) { + std::string deviceSysPath = udev_list_entry_get_name(entry); + udev_device* device = udev_device_new_from_syspath(udevContext, deviceSysPath.c_str()); + if (!device) continue; + + std::string devicePath = udev_device_get_devnode(device); + if (devicePath.empty()) continue; + + mDevice = devicePath; + ROS_INFO_STREAM(std::format("Found USB device: {}", mDevice)); + break; + } + } + initPipeline(); mStreamServer.emplace( @@ -193,7 +245,7 @@ namespace mrover { ROS_INFO("Stopped GStreamer pipeline"); }); - if (mCaptureDevice.empty()) { + if (mDevice.empty()) { mImageSubscriber = mNh.subscribe(mImageTopic, 1, &GstWebsocketStreamerNodelet::imageCallback, this); } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index c4950f62c..55180d333 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -8,8 +8,9 @@ namespace mrover { ros::NodeHandle mNh, mPnh; - std::string mCaptureDevice; + std::string mDevice; std::string mImageTopic; + std::string mUsbIdentifier; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; diff --git a/src/esw/gst_websocket_streamer/pch.hpp b/src/esw/gst_websocket_streamer/pch.hpp index 2c224a3ec..cbac989d5 100644 --- a/src/esw/gst_websocket_streamer/pch.hpp +++ b/src/esw/gst_websocket_streamer/pch.hpp @@ -1,20 +1,24 @@ #pragma once -#include #include #include +#include #include -#include -#include #include -#include #include #include #include +#include +#include +#include #include #include #include +#include + +#include + #include From e102663ed82658d43e8a365eb0332cea6e1c98c8 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 15 Apr 2024 16:15:57 -0400 Subject: [PATCH 2/5] Add device resizing, rename some things --- CMakeLists.txt | 2 +- ansible/roles/esw/tasks/main.yml | 2 +- .../gst_websocket_streamer.cpp | 97 +++++++++++-------- .../gst_websocket_streamer.hpp | 13 ++- src/esw/gst_websocket_streamer/pch.hpp | 2 + 5 files changed, 72 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9552eada5..832fb2d6b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,7 +200,7 @@ endif () if (Gst_FOUND AND GstApp_FOUND AND LibUsb_FOUND AND LibUdev_FOUND) mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) - mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES}) + mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES} opencv_core opencv_imgproc) mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS} ${LibUsb_INCLUDE_DIRS} ${LibUdev_INCLUDE_DIRS}) endif () diff --git a/ansible/roles/esw/tasks/main.yml b/ansible/roles/esw/tasks/main.yml index f9a81d7a5..9ae41142b 100644 --- a/ansible/roles/esw/tasks/main.yml +++ b/ansible/roles/esw/tasks/main.yml @@ -22,4 +22,4 @@ - name: Install Moteus GUI pip: - name: moteus_gui + name: moteus-gui diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index 8510c39e6..9787ae241 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -8,6 +8,8 @@ constexpr bool IS_JETSON = false; namespace mrover { + using namespace std::string_view_literals; + template auto gstCheck(T* t) -> T* { if (!t) throw std::runtime_error{"Failed to create"}; @@ -39,7 +41,7 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); std::string launch; - if (mDevice.empty()) { + if (mDeviceNode.empty()) { if constexpr (IS_JETSON) { // ReSharper disable once CppDFAUnreachableCode launch = std::format( @@ -85,7 +87,7 @@ namespace mrover { "! video/x-raw(memory:NVMM),format=NV12 " "! 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", - mDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); + mDeviceNode, mImageWidth, mImageHeight, mImageFramerate, mBitrate); } else { // ReSharper disable once CppDFAUnreachableCode launch = std::format( @@ -94,14 +96,14 @@ namespace mrover { "! videoconvert " "! nvh265enc name=encoder " "! appsink name=streamSink sync=false", - mDevice, mImageWidth, mImageHeight); + mDeviceNode, mImageWidth, mImageHeight); } } ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); - if (mDevice.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); + if (mDeviceNode.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); mStreamSink = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "streamSink")); if (gst_element_set_state(mPipeline, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) @@ -127,14 +129,22 @@ namespace mrover { if (mStreamServer->clientCount() == 0) return; if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; - if (msg->width != mImageWidth || msg->height != mImageHeight) throw std::runtime_error{"Unsupported resolution"}; + + cv::Size receivedSize{static_cast(msg->width), static_cast(msg->height)}; + cv::Mat bgraFrame{receivedSize, CV_8UC4, const_cast(msg->data.data()), msg->step}; + + if (cv::Size targetSize{static_cast(mImageWidth), static_cast(mImageHeight)}; + receivedSize != targetSize) { + ROS_WARN_ONCE("Image size does not match pipeline app source size, will resize"); + resize(bgraFrame, bgraFrame, targetSize); + } // "step" is the number of bytes (NOT pixels) in an image row - std::size_t size = msg->step * msg->height; + std::size_t size = bgraFrame.step * bgraFrame.rows; GstBuffer* buffer = gstCheck(gst_buffer_new_allocate(nullptr, size, nullptr)); GstMapInfo info; gst_buffer_map(buffer, &info, GST_MAP_WRITE); - std::memcpy(info.data, msg->data.data(), size); + std::memcpy(info.data, bgraFrame.data, size); gst_buffer_unmap(buffer, &info); gst_app_src_push_buffer(GST_APP_SRC(mImageSource), buffer); @@ -150,8 +160,8 @@ namespace mrover { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mDevice = mPnh.param("device", ""); - mUsbIdentifier = mPnh.param("usb_id", "1"); + mDeviceNode = mPnh.param("dev_node", ""); + mDevicePath = mPnh.param("dev_path", "1"); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -162,30 +172,24 @@ namespace mrover { gst_init(nullptr, nullptr); - libusb_context* context{}; - if (libusb_init(&context) != 0) throw std::runtime_error{"Failed to initialize libusb"}; - - libusb_device** list{}; - ssize_t count = libusb_get_device_list(context, &list); - if (count < 0) throw std::runtime_error{"Failed to get device list"}; - - for (ssize_t i = 0; i < count; ++i) { - libusb_device* device = list[i]; - libusb_device_descriptor descriptor{}; - if (libusb_get_device_descriptor(device, &descriptor) != 0) continue; - - std::uint8_t bus = libusb_get_bus_number(device); - std::uint8_t address = libusb_get_device_address(device); - std::uint8_t port = libusb_get_port_number(device); + if (!mDevicePath.empty()) { + // libusb_context* context{}; + // if (libusb_init(&context) != 0) throw std::runtime_error{"Failed to initialize libusb"}; + // + // libusb_device** list{}; + // ssize_t count = libusb_get_device_list(context, &list); + // if (count < 0) throw std::runtime_error{"Failed to get device list"}; + // + // // Find the libusb device associated with our camera + // + // auto it = std::ranges::find_if(list, list + count, [this](libusb_device* device) { + // libusb_device_descriptor descriptor{}; + // if (libusb_get_device_descriptor(device, &descriptor) != 0) return false; + // return std::format("{}-{}", libusb_get_bus_number(device), libusb_get_port_number(device)) == mUsbIdentifier; + // }); + // if (it == list + count) throw std::runtime_error{"Failed to find USB device"}; + // libusb_device* targetUsbDevice = *it; - // get associated video port - // libusb_get_port_numbers(device, port_numbers, sizeof(port_numbers)); - - ROS_INFO_STREAM(std::format("USB device: bus={}, address={}, port={}, vendor={}, product={}", - bus, address, port, descriptor.idVendor, descriptor.idProduct)); - } - - if (!mUsbIdentifier.empty()) { udev* udevContext = udev_new(); if (!udevContext) throw std::runtime_error{"Failed to initialize udev"}; @@ -198,19 +202,32 @@ namespace mrover { udev_list_entry* devices = udev_enumerate_get_list_entry(enumerate); if (!devices) throw std::runtime_error{"Failed to get udev device list"}; + udev_device* device{}; udev_list_entry* entry; udev_list_entry_foreach(entry, devices) { - std::string deviceSysPath = udev_list_entry_get_name(entry); - udev_device* device = udev_device_new_from_syspath(udevContext, deviceSysPath.c_str()); - if (!device) continue; + device = udev_device_new_from_syspath(udevContext, udev_list_entry_get_name(entry)); - std::string devicePath = udev_device_get_devnode(device); - if (devicePath.empty()) continue; + if (udev_device_get_devpath(device) != mDevicePath) continue; + + if (!device) throw std::runtime_error{"Failed to get udev device"}; - mDevice = devicePath; - ROS_INFO_STREAM(std::format("Found USB device: {}", mDevice)); break; + + // for (udev_list_entry* listEntry = udev_device_get_properties_list_entry(device); listEntry != nullptr; listEntry = udev_list_entry_get_next(listEntry)) { + // char const* name = udev_list_entry_get_name(listEntry); + // char const* value = udev_list_entry_get_value(listEntry); + // ROS_INFO_STREAM(std::format("udev: {} = {}", name, value)); + // } + + // if (std::stoi(udev_device_get_sysattr_value(device, "busnum")) == libusb_get_bus_number(targetUsbDevice) && + // std::stoi(udev_device_get_sysattr_value(device, "devnum")) == libusb_get_device_address(targetUsbDevice)) { + // targetUdevDevice = device; + // break; + // } } + if (!device) throw std::runtime_error{"Failed to find udev device"}; + + mDeviceNode = udev_device_get_devnode(device); } initPipeline(); @@ -245,7 +262,7 @@ namespace mrover { ROS_INFO("Stopped GStreamer pipeline"); }); - if (mDevice.empty()) { + if (mDeviceNode.empty()) { mImageSubscriber = mNh.subscribe(mImageTopic, 1, &GstWebsocketStreamerNodelet::imageCallback, this); } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index 55180d333..f72f01253 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -8,9 +8,18 @@ namespace mrover { ros::NodeHandle mNh, mPnh; - std::string mDevice; + // For example, /dev/video0 + // These device paths are not garunteed to stay the same between reboots + // Prefer sys path for non-debugging purposes + std::string mDeviceNode; std::string mImageTopic; - std::string mUsbIdentifier; + // To find the sys path: + // 1) Disconnect all cameras + // 2) Confirm there are no /dev/video* devices + // 2) Connect the camera you want to use + // 3) Run "ls /dev/video*" to verify the device is connected + // 4) Run "udevadm info -q path -n /dev/video0" to get the sys path + std::string mDevicePath; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; diff --git a/src/esw/gst_websocket_streamer/pch.hpp b/src/esw/gst_websocket_streamer/pch.hpp index cbac989d5..2769438ad 100644 --- a/src/esw/gst_websocket_streamer/pch.hpp +++ b/src/esw/gst_websocket_streamer/pch.hpp @@ -21,4 +21,6 @@ #include +#include + #include From c1ae562f089bae32247bcf33529abc114e801c0d Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 14:16:53 -0400 Subject: [PATCH 3/5] Reorder --- src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index 5fc6644a8..477839ae3 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -213,10 +213,10 @@ namespace mrover { mChunkHeader.resolution = widthAndHeightToResolution(mImageWidth, mImageHeight); - gst_init(nullptr, nullptr); - if (!mDevicePath.empty()) mDeviceNode = findDeviceNode(mDevicePath); + gst_init(nullptr, nullptr); + initPipeline(); mStreamServer.emplace( From 2cfb2b854190286813894813eed499cf9ca376a1 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 14:21:36 -0400 Subject: [PATCH 4/5] Update launch file --- launch/esw_base.launch | 42 +++++++++++++++++++----------------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/launch/esw_base.launch b/launch/esw_base.launch index 0bcbf81c8..f213614bf 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -21,47 +21,43 @@ respawn="true" respawn_delay="2" output="screen"> - + streamer_nodelet - - + - - + - - + - + - - - - - - - - + + + + + + From 620179f104543323a06084765a81bb323514c5f3 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 14:22:25 -0400 Subject: [PATCH 5/5] FOrmat --- launch/esw_base.launch | 126 ++++++++++++++++++++--------------------- 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/launch/esw_base.launch b/launch/esw_base.launch index f213614bf..3ac09785b 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -1,73 +1,73 @@ - + - - - - - - - - - - - streamer_nodelet + + + + + + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - + - - - - - + + + + + - +