Hardware H265 streaming (#651)

* read messages from base station * sending RTCMs over ros * launch file with both GPSs, RTCMs successfully transmitted * port change * output screen * added gps driver for rover gps and merged master into rtk * edited rtk launch and esw.yaml * used pyubx reader instead of serial getline() * added preliminary dual gnss file * add notes * graph vectors for heading calculations * calc heading and bearing * driver testing; not fully working 👽 * plots for heading calculations * added get_heading_vector function * add tests for vector heading method * working as intended furing testing * add tests to determine optimal amt of sig figs for gps coordinates * added more message parsing * started adding gps to gazebo * finished adding new gps and links * edited links * added links * try to debug plugins and make test subscriber file * added left and right gps using groups * gps creation and subscribers * add tests for moving gps and subscribed * started editing gps linearization * gps now put in sim * edited test subscriber * added pose calculation using heading * tests for both gps distance * changes for testing with convert lat long to cartesian to see offset better * tests for offset * offset is 10? * chanegd offset to 1 and modified testing code * Add requirements * updated linearlization * debug linearization function and try to debug sim * fixed reference points and removed z component for better rviz experience * made a temporary GPS sim * fixed pose publishing offset temporarily * Add starter work for gps * cartesian to geodetic conversion * Finish off gps and add imu * Yo workingin the new sim * Submodule update for dawn * added rviz to launch file, misc cleanup * refactored utils and tag detector, crashes when it sees a tag * debugging * Init * Work on encoders * Update * Breh * Use bgra instead of i420 * partially fixed script * fixed macos std::for_each error * more factoring test * refactored SE3 util functions back into lie library * refactored most of sim * more debugging * Add manif as submodule, find out some disgustig things about transforms, normalize some vars before so3d ctor * refactored camera controls using tangent elements * pr cleanup * more cleanup * more cleanup * Remove unrelated stuff, clean up some stuff, remove old lie stuff * Rename to lie.hpp * Refactor look controls * Comments * Manif refactor (#648) * Fix collider rendering with new se3 * slight refactoring of simulated sensors, added untested noise * changed some type names, added rviz to launch file, separate roll pitch yaw noises * fixed type names and ctors * slightly reduced noise and moved waypoint * Restream * Update * Fix format * Stream that can be reused * Naming --------- Co-authored-by: Riley Bridges <[email protected]> Co-authored-by: nehakankanala <[email protected]> Co-authored-by: Pearl Lin <[email protected]> Co-authored-by: dllliu <[email protected]> Co-authored-by: Rahul <[email protected]> Co-authored-by: umroverPerception <[email protected]> Co-authored-by: Riley Bridges <[email protected]>
umrover · Feb 18, 2024 · 5e91527 · 5e91527
1 parent edddf01
commit 5e91527
Show file tree

Hide file tree

Showing 45 changed files with 633 additions and 560 deletions.
diff --git a/.gitmodules b/.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
diff --git a/CMakeLists.txt b/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)

diff --git a/config/simulator/simulator.yaml b/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

diff --git a/deps/emsdk b/deps/emsdk
diff --git a/deps/manif b/deps/manif
diff --git a/launch/simulator.launch b/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>
diff --git a/scripts/build_teleop_stream_client.sh b/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
diff --git a/scripts/toggle_basestation_networking b/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
diff --git a/src/esw/cameras/cameras.cpp b/src/esw/cameras/cameras.cpp
diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/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;
+    }
+}