diff --git a/src/Cameras/video_streaming/config/cameras.yaml b/src/Cameras/video_streaming/config/cameras.yaml index 7ebaeac9..2f06eca9 100644 --- a/src/Cameras/video_streaming/config/cameras.yaml +++ b/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 diff --git a/src/Cameras/video_streaming/include/video_streaming/base_video_node.hpp b/src/Cameras/video_streaming/include/video_streaming/base_video_node.hpp index 11d296f5..e14fd001 100644 --- a/src/Cameras/video_streaming/include/video_streaming/base_video_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/base_video_node.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -46,4 +47,5 @@ class BaseVideoNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr restart_sub_; + rclcpp::Service::SharedPtr graph_dot_srv_; }; diff --git a/src/Cameras/video_streaming/launch/video_streaming.launch.py b/src/Cameras/video_streaming/launch/video_streaming.launch.py index 237cfabb..1bb92925 100644 --- a/src/Cameras/video_streaming/launch/video_streaming.launch.py +++ b/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 diff --git a/src/Cameras/video_streaming/src/base_video_node.cpp b/src/Cameras/video_streaming/src/base_video_node.cpp index 01e9a1b5..94bad9e7 100644 --- a/src/Cameras/video_streaming/src/base_video_node.cpp +++ b/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( + "~/graph_dot", + [this](const std::shared_ptr request, + std::shared_ptr response) { + std::lock_guard 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() { diff --git a/src/Cameras/video_streaming/src/input_node.cpp b/src/Cameras/video_streaming/src/input_node.cpp index d3715f89..26bb5aa9 100644 --- a/src/Cameras/video_streaming/src/input_node.cpp +++ b/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::vector camera_name; @@ -24,12 +24,22 @@ void InputNode::declare_parameters() { this->declare_parameter(name + ".type", static_cast(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 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", diff --git a/src/Cameras/video_streaming/src/rtp_client_node.cpp b/src/Cameras/video_streaming/src/rtp_client_node.cpp index fd132146..dd147508 100644 --- a/src/Cameras/video_streaming/src/rtp_client_node.cpp +++ b/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"; diff --git a/src/Cameras/video_streaming/src/rtp_node.cpp b/src/Cameras/video_streaming/src/rtp_node.cpp index be7d7c9c..d4d3ac0f 100644 --- a/src/Cameras/video_streaming/src/rtp_node.cpp +++ b/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); diff --git a/src/URDF/rover_urdf/config/ros2_controllers.yaml b/src/URDF/rover_urdf/config/ros2_controllers.yaml index 387d6c46..9da651c8 100644 --- a/src/URDF/rover_urdf/config/ros2_controllers.yaml +++ b/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 \ No newline at end of file diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index 9880c640..4f117aed 100644 --- a/src/interfaces/CMakeLists.txt +++ b/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 diff --git a/src/interfaces/srv/GraphDot.srv b/src/interfaces/srv/GraphDot.srv new file mode 100644 index 00000000..028d8233 --- /dev/null +++ b/src/interfaces/srv/GraphDot.srv @@ -0,0 +1,5 @@ +# GraphDot.srv + +--- +bool success +string graph \ No newline at end of file