Hardware H265 streaming

.gitmodules

@@ -8,3 +8,12 @@
 	url = https://dawn.googlesource.com/dawn
 	shallow = true
 	branch = chromium/6108
+[submodule "deps/emsdk"]
+	path = deps/emsdk
+	url = [email protected]:emscripten-core/emsdk.git
+	shallow = true
+	branch = 3.1.53
+[submodule "deps/manif"]
+	path = deps/manif
+	url = https://github.com/artivis/manif.git
+	shallow = true

CMakeLists.txt

@@ -86,18 +86,15 @@ if (MROVER_BUILD_SIM)
     if (NOT Assimp_FOUND AND NOT assimp_FOUND)
         message(FATAL_ERROR "Assimp not found")
     endif ()
-
     find_package(Bullet REQUIRED)
-
     find_package(glfw3 REQUIRED)
-
     include(cmake/webgpu.cmake)
-
-    add_subdirectory(deps/glfw3webgpu)
+    add_subdirectory(deps/glfw3webgpu SYSTEM)
 endif ()
 find_package(OpenCV REQUIRED)
 find_package(ZED QUIET)
 find_package(Eigen3 REQUIRED)
+add_subdirectory(deps/manif SYSTEM)
 find_package(PkgConfig REQUIRED)
 pkg_search_module(NetLink libnl-3.0)
 pkg_search_module(NetLinkRoute libnl-route-3.0)
@@ -147,6 +144,7 @@ catkin_package()
 ## Libraries
 
 mrover_add_library(lie src/util/lie/*.cpp src/util/lie)
+target_link_libraries(lie PUBLIC manif)
 
 ## ESW
 
@@ -188,6 +186,9 @@ endif ()
 
 ## Perception
 
+mrover_add_library(streaming src/esw/streaming/*.cpp src/esw/streaming)
+target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED)
+
 mrover_add_nodelet(zed_tag_detector src/perception/tag_detector/zed/*.cpp src/perception/tag_detector/zed src/perception/tag_detector/zed/pch.hpp)
 mrover_nodelet_link_libraries(zed_tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie)
 
@@ -197,13 +198,18 @@ mrover_nodelet_link_libraries(long_range_tag_detector opencv_core opencv_objdete
 mrover_add_nodelet(usb_camera src/perception/long_range_cam/*.cpp src/perception/long_range_cam src/perception/long_range_cam/pch.hpp)
 mrover_nodelet_link_libraries(usb_camera opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui tbb lie)
 if (CUDA_FOUND)
-    mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming)
-    #    target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec)
-    target_link_libraries(streaming PUBLIC cuda nvidia-encode opencv_core)
-    target_include_directories(streaming SYSTEM PUBLIC deps/nvenc)
-    target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED)
+    mrover_add_node(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c*)
+    target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc opencv_imgcodecs opencv_videoio streaming)
+    target_include_directories(hardware_h265_encoder SYSTEM PUBLIC deps/nvenc)
 endif ()
 
+add_subdirectory(deps/libde265)
+include_directories(deps/libde265/extra deps/libde265 SYSTEM)
+
+mrover_add_node(software_h265_encoder src/esw/software_h265_encoder/*.c*)
+target_link_libraries(software_h265_encoder PUBLIC opencv_core opencv_imgproc opencv_imgcodecs de265 streaming)
+target_include_directories(software_h265_encoder SYSTEM PUBLIC deps/nvenc)
+
 if (ZED_FOUND)
     mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp)
     mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb)

config/simulator/simulator.yaml

@@ -9,7 +9,7 @@ objects:
   - type: urdf
     name: tag_0
     uri: package://mrover/urdf/world/tag_0.urdf.xacro
-    translation: [ -3, -3, 0.7 ]
+    translation: [ -2, -2, 0.7 ]
   - type: urdf
     name: tag_1
     uri: package://mrover/urdf/world/tag_1.urdf.xacro

launch/simulator.launch

@@ -1,4 +1,5 @@
 <launch>
+    <arg name="run_rviz" default="true" />
     <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml" />
 
     <include file="$(find mrover)/launch/autonomy.launch">
@@ -13,4 +14,9 @@
           args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen" />
 
     <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen" />
+
+    <group if="$(arg run_rviz)">
+        <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz" />
+        <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" />
+    </group>
 </launch>

scripts/build_teleop_stream_client.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+source deps/emsdk/emsdk_env.sh
+emcmake cmake -B src/teleoperation/streaming/embuild -G Ninja -DCMAKE_BUILD_TYPE=Release src/teleoperation/streaming/
+cmake --build src/teleoperation/streaming/embuild

scripts/toggle_basestation_networking

@@ -1,12 +1,20 @@
-# !/usr/bin/env bash
+# !/usr/bin/env zsh
 
 # Script to add or remove ROS basestation networking script from bashrc
 # TODO: make this work for bash and other catkin workspace locations
 
-if grep -q "source ~/catkin_ws/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation.sh" ~/.zshrc; then
+CATKIN_WORKSPACE_PATH=~/catkin_ws
+network_script_path="${CATKIN_WORKSPACE_PATH}/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation"
+enable_message="Basestation network setup enabled, disable with scripts/toggle_basestation_networking"
+
+if grep -q "source ${network_script_path}" ~/.zshrc; then
     echo "Removing basestation networking script from .zshrc"
     sed -i '/source ~\/catkin_ws\/src\/mrover\/ansible\/roles\/basestation_networks\/files\/networking_setup_basestation.sh/d' ~/.zshrc
 else
     echo "Adding basestation networking script to .zshrc"
-    echo "source ~/catkin_ws/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation.sh" >> ~/.zshrc
+    if [[ $(tail -c1 ~/.zshrc | wc -l) -eq 0 ]]; then
+        echo "\n" >> ~/.zshrc
+    fi
+    echo "echo '${enable_message}'" >> ~/.zshrc
+    echo "source ${network_script_path}" >> ~/.zshrc
 fi

src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp

@@ -0,0 +1,84 @@
+#include "hardware_h265_encoder.hpp"
+
+#include <ros/init.h>
+#include <ros/node_handle.h>
+#include <ros/subscriber.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <streaming.hpp>
+
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/videoio.hpp>
+
+std::optional<StreamServer> streamServer;
+
+std::optional<Encoder> encoder;
+
+auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void {
+    try {
+        if (msg->encoding != sensor_msgs::image_encodings::BGR8) throw std::runtime_error{"Unsupported encoding"};
+
+        cv::Size size{static_cast<int>(msg->width), static_cast<int>(msg->height)};
+
+        if (!encoder) encoder.emplace(size);
+
+        cv::Mat bgrFrame{size, CV_8UC3, const_cast<std::uint8_t*>(msg->data.data()), msg->step};
+        cv::Mat bgraFrame;
+        cv::cvtColor(bgrFrame, bgraFrame, cv::COLOR_BGR2BGRA);
+
+        bool feedSuccessful = false;
+        {
+            Encoder::BitstreamView view = encoder->feed(bgraFrame);
+            std::span span{static_cast<std::byte*>(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes};
+            feedSuccessful = streamServer->feed(span);
+        }
+        if (!feedSuccessful) encoder.reset();
+
+    } catch (std::exception const& e) {
+        ROS_ERROR_STREAM(std::format("Exception encoding frame: {}", e.what()));
+        ros::requestShutdown();
+    }
+}
+
+// auto capture() -> void {
+//     cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=30/1 ! appsink", 640, 480), cv::CAP_GSTREAMER};
+//     if (!cap.isOpened()) throw std::runtime_error{"Failed to open capture"};
+//
+//     while (cap.isOpened()) {
+//         cv::Mat i420Frame;
+//         if (!cap.read(i420Frame)) throw std::runtime_error{"Failed to read frame"};
+//
+//         cv::Mat bgraFrame;
+//         cvtColor(i420Frame, bgraFrame, cv::COLOR_YUV2BGRA_I420);
+//
+//         Encoder::BitstreamView view = encoder->feed(bgraFrame);
+//         std::span span{static_cast<std::byte*>(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes};
+//         streamServer->feed(span);
+//
+//         ros::spinOnce();
+//     }
+// }
+
+auto main(int argc, char** argv) -> int {
+    try {
+        ros::init(argc, argv, "software_h265_encoder");
+        ros::NodeHandle nh, pnh{"~"};
+
+        std::string imageTopic = pnh.param("image_topic", std::string{"/camera/left/image"});
+
+        streamServer.emplace("0.0.0.0", 8080);
+
+        ros::Subscriber imageSubscriber = nh.subscribe(imageTopic, 1, imageCallback);
+
+        // capture();
+
+        ros::spin();
+        return EXIT_SUCCESS;
+
+    } catch (std::exception const& e) {
+        ROS_ERROR_STREAM(std::format("Exception initializing: {}", e.what()));
+        return EXIT_FAILURE;
+    }
+}