Video poor quality fixes

src/Cameras/video_streaming/config/cameras.yaml

@@ -10,6 +10,10 @@ input_node:
     Drive:
       path: "/dev/v4l/by-id/drive_camera"
       type: 0
+      encoded: false
+      height: 1200
+      width: 1920
+      framerate: 30
     EndEffector:
       path: "/dev/v4l/by-id/usb-MACROSILICON_USB_Video_20200909-video-index0"
       type: 0

src/Cameras/video_streaming/include/video_streaming/base_video_node.hpp

@@ -3,6 +3,7 @@
 #include <atomic>
 #include <gst/app/gstappsink.h>
 #include <gst/gst.h>
+#include <interfaces/srv/graph_dot.hpp>
 #include <mutex>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
@@ -46,4 +47,5 @@ class BaseVideoNode : public rclcpp::Node {
 
 private:
   rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr restart_sub_;
+  rclcpp::Service<interfaces::srv::GraphDot>::SharedPtr graph_dot_srv_;
 };

src/Cameras/video_streaming/launch/video_streaming.launch.py

@@ -45,7 +45,7 @@ def generate_launch_description():
         plugin="video_streaming::RtpNode",
         name="rtp_node",
         namespace="",
-        parameters=[{"dest_ip": "192.168.1.100", "dest_port": 5004}],
+        parameters=[{"dest_ip": "192.168.0.20", "dest_port": 5004}],
     )
 
     # Create a container for all 3 components

src/Cameras/video_streaming/src/base_video_node.cpp

@@ -28,6 +28,31 @@ BaseVideoNode::BaseVideoNode(const std::string name,
                        "Exception during pipeline restart: %s", e.what());
         }
       });
+  graph_dot_srv_ = create_service<interfaces::srv::GraphDot>(
+      "~/graph_dot",
+      [this](const std::shared_ptr<interfaces::srv::GraphDot::Request> request,
+             std::shared_ptr<interfaces::srv::GraphDot::Response> response) {
+        std::lock_guard<std::mutex> lock(pipeline_mutex_);
+        if (!pipeline_) {
+          RCLCPP_ERROR(this->get_logger(),
+                       "Cannot generate graph dot: pipeline is null.");
+          response->success = false;
+          return;
+        }
+        gchar *dot_data = gst_debug_bin_to_dot_data(GST_BIN(pipeline_),
+                                                    GST_DEBUG_GRAPH_SHOW_ALL);
+        if (!dot_data) {
+          RCLCPP_ERROR(this->get_logger(),
+                       "Cannot generate graph dot: gst_debug_bin_to_dot_data "
+                       "returned null.");
+          response->graph = "";
+          response->success = false;
+          return;
+        }
+        response->graph = dot_data;
+        g_free(dot_data);
+        response->success = true;
+      });
 }
 
 BaseVideoNode::~BaseVideoNode() {

src/Cameras/video_streaming/src/input_node.cpp

@@ -13,8 +13,8 @@ InputNode::InputNode(const rclcpp::NodeOptions &options)
 }
 
 void InputNode::declare_parameters() {
-  this->declare_parameter("out_width", 1280);
-  this->declare_parameter("out_height", 720);
+  this->declare_parameter("out_width", 1920);
+  this->declare_parameter("out_height", 1080);
   this->declare_parameter("out_framerate", 30);
   this->declare_parameter("camera_name", std::vector<std::string>());
   std::vector<std::string> camera_name;
@@ -24,12 +24,22 @@ void InputNode::declare_parameters() {
     this->declare_parameter(name + ".type",
                             static_cast<int>(CameraType::V4l2Src));
     this->declare_parameter(name + ".encoded", false);
+    this->declare_parameter(name + ".width", 0);
+    this->declare_parameter(name + ".height", 0);
+    this->declare_parameter(name + ".framerate", 0);
   }
 }
 
 bool InputNode::create_pipeline() {
   std::stringstream desc_stream;
-  size_t index = 0;
+  // Hack: Use a constant videotestsrc for the sink_0 to ensure the compositor
+  // always has a source pad of the correct size
+  desc_stream << "videotestsrc is-live=true pattern=black ! video/x-raw,width="
+              << this->get_parameter("out_width").as_int()
+              << ",height=" << this->get_parameter("out_height").as_int()
+              << ",framerate=" << this->get_parameter("out_framerate").as_int()
+              << "/1 ! nvvidconv ! compositor.sink_0 ";
+  size_t index = 1;
   std::vector<std::string> camera_name;
   this->get_parameter("camera_name", camera_name);
   for (const auto &name : camera_name) {
@@ -58,14 +68,35 @@ bool InputNode::create_pipeline() {
                   name.c_str());
       continue;
     }
+    int cam_width = this->get_parameter(name + ".width").as_int();
+    int cam_height = this->get_parameter(name + ".height").as_int();
+    int cam_framerate = this->get_parameter(name + ".framerate").as_int();
+    if (cam_width > 0 || cam_height > 0 || cam_framerate > 0) {
+      desc_stream << "video/x-raw,";
+      if (cam_width > 0) {
+        desc_stream << "width=" << cam_width << ",";
+      }
+      if (cam_height > 0) {
+        desc_stream << "height=" << cam_height << ",";
+      }
+      if (cam_framerate > 0) {
+        desc_stream << "framerate=" << cam_framerate << "/1,";
+      }
+      // Remove trailing comma
+      std::string current_desc = desc_stream.str();
+      if (current_desc.back() == ',') {
+        current_desc.pop_back();
+        desc_stream.str("");
+        desc_stream << current_desc << " ! ";
+      }
+    }
+
     desc_stream << "nvvidconv ! compositor.sink_" << index << " ";
     source_map_.emplace(name, index);
     ++index;
   }
-  desc_stream << "nvcompositor name=compositor sink_0::width="
-              << this->get_parameter("out_width").as_int() << " sink_0::height="
-              << this->get_parameter("out_height").as_int();
-  desc_stream << " ! nvvidconv ! videorate ! video/x-raw,width="
+  desc_stream << "nvcompositor name=compositor sink_0::alpha=0.0 ! ";
+  desc_stream << "nvvidconv ! videorate ! video/x-raw,width="
               << this->get_parameter("out_width").as_int()
               << ",height=" << this->get_parameter("out_height").as_int()
               << ",framerate=" << this->get_parameter("out_framerate").as_int()
@@ -121,7 +152,7 @@ void InputNode::video_cb(
     return;
   }
   for (size_t i = 0; i < source_map_.size(); ++i) {
-    std::string pad_name = "sink_" + std::to_string(i);
+    std::string pad_name = "sink_" + std::to_string(i + 1);
     GstPad *pad = gst_element_get_static_pad(compositor, pad_name.c_str());
     if (!pad) {
       RCLCPP_ERROR(this->get_logger(), "Failed to get compositor pad: %s",

src/Cameras/video_streaming/src/rtp_client_node.cpp

@@ -37,10 +37,15 @@ std::string RtpClientNode::get_pipeline_description() {
   constexpr guint storage_tolerance_ms = 20;
   guint64 storage_sz_ns = (latency_ms_ + storage_tolerance_ms) * 1000000ULL;
   desc << "udpsrc port=" << dest_port_ << " caps="
-       << "\"application/x-rtp,media=video,encoding-name=H265,payload=96, "
-       << "clock-rate=90000\" ! rtpstorage size-time=" << storage_sz_ns
-       << " ! rtpjitterbuffer name=rtp_buf mode=4 latency=" << latency_ms_
-       << " drop-on-latency=true do-lost=1 ! rtpulpfecdec name=fec_dec ! "
+       << "\"application/x-rtp, payload=96, clock-rate=90000\" ! "
+       << "rtpstorage size-time=" << storage_sz_ns
+       << " ! rtpssrcdemux ! capsfilter "
+          "caps=\"application/"
+          "x-rtp,media=video,encoding-name=H265,payload=96,clock-rate=90000\" "
+          "! rtpjitterbuffer name=rtp_buf mode=4 latency="
+       << latency_ms_
+       << " drop-on-latency=true do-lost=true post-drop-messages=true ! "
+          "rtpulpfecdec name=fec_dec pt=122 ! "
           "rtph265depay ! h265parse ! nvv4l2decoder ! nvvidconv ! "
        << "capsfilter caps=\"video/x-raw(memory:NVMM),width=1920,height=1080\""
           " ! nvvidconv ! nveglglessink sync=false";

src/Cameras/video_streaming/src/rtp_node.cpp

@@ -66,9 +66,10 @@ std::string RtpNode::get_pipeline_description() {
        << "/1\" ! nvvidconv ! "
        << "capsfilter caps=\"video/x-raw(memory:NVMM),height=" << height_
        << ",width=" << width_ << "\" ! nvv4l2h265enc bitrate=" << video_bitrate
-       << " name=h265_enc EnableTwopassCBR=true control-rate=1 "
-          "maxperf-enable=true ! queue ! rtph265pay  config-interval=-1 ! "
-          "rtpulpfecenc percentage="
+       << " name=h265_enc bit-packetization=true EnableTwopassCBR=true "
+          "slice-header-spacing=900 control-rate=1 "
+          "maxperf-enable=true ! queue ! rtph265pay pt=96 config-interval=-1 ! "
+          "rtpulpfecenc pt=122 percentage="
        << fec_ << " ! udpsink host=" << dest_ip_ << " port=" << dest_port_;
   return desc.str();
 }
@@ -193,7 +194,7 @@ void RtpNode::set_bitrate(const int32_t bitrate) {
     set_resolution(640, 360);
   } else if (bits_per_frame < 100000) {
     set_resolution(854, 480);
-  } else if (bits_per_frame < 200000) {
+  } else if (bits_per_frame < 150000) {
     set_resolution(1280, 720);
   } else {
     set_resolution(1920, 1080);

src/URDF/rover_urdf/config/ros2_controllers.yaml

@@ -65,6 +65,6 @@ chassis_controller:
     track_width: 0.54
     wheel_radius: 0.12
     odom_publish_rate: 50.0
-    cmd_timeout: 0.0
+    cmd_timeout: 0.5
     min_angle: -1.6
     max_angle: 1.6

src/interfaces/CMakeLists.txt

@@ -33,6 +33,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/VideoCapture.srv"
   "srv/GetCameras.srv"
   "srv/Raman.srv"
+  "srv/GraphDot.srv"
   DEPENDENCIES 
   builtin_interfaces
   geometry_msgs 

src/interfaces/srv/GraphDot.srv

@@ -0,0 +1,5 @@
+# GraphDot.srv
+
+---
+bool success
+string graph