diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml index 7b9b6db..84d601f 100644 --- a/.github/workflows/code-coverage.yml +++ b/.github/workflows/code-coverage.yml @@ -8,5 +8,6 @@ on: jobs: code-coverage: uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main + continue-on-error: true secrets: CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} # Set in the repository secrets diff --git a/README.md b/README.md index e672527..82b602e 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,9 @@ Repository to move image to gstreamer ``` -# Setup +# Dependencies ``` +sudo apt install libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev ``` ## launch variables @@ -14,8 +15,7 @@ ros2 launch image_to_gstreamer image_to_gstreamer.launch.py host:=127.0.0.1 port ``` ## terminal launch for gstreamer ``` -gst-launch-1.0 udpsrc port=5001 caps="application/x-rtp,media=video,encoding-name=H265,payload=96" -! rtph265depay ! avdec_h265 ! videoconvert ! fpsdisplaysink +gst-launch-1.0 udpsrc port=5001 caps="application/x-rtp,media=video,encoding-name=H265,payload=96" ! rtph265depay ! avdec_h265 ! videoconvert ! fpsdisplaysink ``` ## Using pre-commit @@ -47,11 +47,3 @@ pre-commit autoupdate - This project uses GitHub Actions for CI. - Workflows are located in [`.github/workflows`](.github/workflows/). - For more information, see [vortex-ci](https://github.com/vortexntnu/vortex-ci). - - - - - - - - diff --git a/gstreamer_from_ros/CMakeLists.txt b/gstreamer_from_ros/CMakeLists.txt new file mode 100644 index 0000000..85f76a0 --- /dev/null +++ b/gstreamer_from_ros/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(gstreamer_from_ros) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(GST REQUIRED + gstreamer-1.0 + gstreamer-app-1.0 +) + +add_library(gstreamer_from_ros_component SHARED + src/gstreamer_from_ros_node.cpp +) + +target_include_directories(gstreamer_from_ros_component PUBLIC + include + ${GST_INCLUDE_DIRS} +) + +target_link_libraries(gstreamer_from_ros_component + ${GST_LIBRARIES} +) + +ament_target_dependencies(gstreamer_from_ros_component + rclcpp + rclcpp_components + sensor_msgs +) + +rclcpp_components_register_node(gstreamer_from_ros_component + PLUGIN "gstreamer_from_ros::GStreamerFromRos" + EXECUTABLE gstreamer_from_ros_node +) + +install(TARGETS gstreamer_from_ros_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/gstreamer_from_ros/config/gstreamer_from_ros.yaml b/gstreamer_from_ros/config/gstreamer_from_ros.yaml new file mode 100644 index 0000000..88a8847 --- /dev/null +++ b/gstreamer_from_ros/config/gstreamer_from_ros.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + input_topic: "/blackfly_s/image_raw" + destination_ip: "127.0.0.1" + destination_port: 5001 + + expected_input_fps: 15 + bitrate: 500000 + preset_level: 1 + iframe_interval: 15 + control_rate: 1 + pt: 96 + config_interval: 1 + input_format: "RGB" diff --git a/gstreamer_from_ros/include/gstreamer_from_ros/gstreamer_from_ros.hpp b/gstreamer_from_ros/include/gstreamer_from_ros/gstreamer_from_ros.hpp new file mode 100644 index 0000000..51eeb91 --- /dev/null +++ b/gstreamer_from_ros/include/gstreamer_from_ros/gstreamer_from_ros.hpp @@ -0,0 +1,47 @@ +#ifndef GSTREAMER_FROM_ROS__GSTREAMER_FROM_ROS_HPP_ +#define GSTREAMER_FROM_ROS__GSTREAMER_FROM_ROS_HPP_ + +#include + +#include +#include + +#include +#include + +namespace gstreamer_from_ros { + +class GStreamerFromRos : public rclcpp::Node { + public: + explicit GStreamerFromRos(const rclcpp::NodeOptions& options); + ~GStreamerFromRos(); + + private: + void create_pipeline(); + void imageCb(const sensor_msgs::msg::Image::SharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; + int bitrate_; + int preset_level_; + int iframe_interval_; + int control_rate_; + int pt_; + int config_interval_; + int expected_input_fps_; + std::string input_format_; + bool hw_encoder_; + + GstElement* pipeline_; + GstElement* appsrc_; + + bool pipeline_started_; + + std::string input_topic_; + std::string destination_ip_; + int destination_port_; +}; + +} // namespace gstreamer_from_ros + +#endif // GSTREAMER_FROM_ROS__GSTREAMER_FROM_ROS_HPP_ diff --git a/gstreamer_from_ros/launch/gstreamer_from_ros.launch.py b/gstreamer_from_ros/launch/gstreamer_from_ros.launch.py new file mode 100644 index 0000000..120ef95 --- /dev/null +++ b/gstreamer_from_ros/launch/gstreamer_from_ros.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _launch_setup(context, *args, **kwargs): + use_nvidia = LaunchConfiguration('gst_nvidia_encoder').perform(context).lower() == 'true' + + container = ComposableNodeContainer( + name='gstreamer_from_ros_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='gstreamer_from_ros', + plugin='gstreamer_from_ros::GStreamerFromRos', + name='gstreamer_from_ros_node', + parameters=[{ + 'input_topic': '/zed_node/left/image_rect_color', + 'destination_ip': '10.0.0.154', + 'destination_port': 5001, + 'expected_input_fps': 15, + 'bitrate': 500000, + 'preset_level': 1, + 'iframe_interval': 15, + 'control_rate': 1, + 'pt': 96, + 'config_interval': 1, + 'input_format': 'RGB', + 'hw_encoder': use_nvidia, + }], + ), + ], + output='screen', + ) + + return [container] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'gst_nvidia_encoder', + default_value='true', + description='Use NVIDIA hardware H.265 encoder (nvv4l2h265enc). ' + 'Set false to use software x265enc.', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/gstreamer_from_ros/package.xml b/gstreamer_from_ros/package.xml new file mode 100644 index 0000000..9c1c035 --- /dev/null +++ b/gstreamer_from_ros/package.xml @@ -0,0 +1,28 @@ + + + gstreamer_from_ros + 0.1.0 + takes image form ROS node and sends it via gstreamer + + Gard + MIT + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + + + pkg-config + pkg-config + + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + + + ament_cmake + + diff --git a/gstreamer_from_ros/src/gstreamer_from_ros_node.cpp b/gstreamer_from_ros/src/gstreamer_from_ros_node.cpp new file mode 100644 index 0000000..f86901b --- /dev/null +++ b/gstreamer_from_ros/src/gstreamer_from_ros_node.cpp @@ -0,0 +1,145 @@ +#include "gstreamer_from_ros/gstreamer_from_ros.hpp" + +#include + +namespace gstreamer_from_ros { + +GStreamerFromRos::GStreamerFromRos(const rclcpp::NodeOptions& options) + : Node("gstreamer_from_ros_node", options), + pipeline_(nullptr), + appsrc_(nullptr), + pipeline_started_(false) { + gst_init(nullptr, nullptr); + + input_topic_ = declare_parameter("input_topic", ""); + destination_ip_ = declare_parameter("destination_ip", ""); + destination_port_ = declare_parameter("destination_port", 5000); + bitrate_ = declare_parameter("bitrate", 500000); + preset_level_ = declare_parameter("preset_level", 1); + iframe_interval_ = declare_parameter("iframe_interval", 15); + control_rate_ = declare_parameter("control_rate", 1); + pt_ = declare_parameter("pt", 96); + config_interval_ = declare_parameter("config_interval", 1); + expected_input_fps_ = declare_parameter("expected_input_fps", 15); + input_format_ = declare_parameter("input_format", "RGB"); + hw_encoder_ = declare_parameter("hw_encoder", true); + + auto qos = rclcpp::QoS(rclcpp::KeepLast(3)).best_effort().durability_volatile(); + + sub_ = create_subscription( + input_topic_, qos, + std::bind(&GStreamerFromRos::imageCb, this, std::placeholders::_1)); + + timer_ = create_wall_timer(std::chrono::seconds(5), [this]() { + RCLCPP_INFO(get_logger(), "Waiting for images on topic: '%s'", + input_topic_.c_str()); + }); + + create_pipeline(); +} + +GStreamerFromRos::~GStreamerFromRos() { + if (pipeline_) { + gst_element_set_state(pipeline_, GST_STATE_NULL); + gst_object_unref(pipeline_); + } +} + +void GStreamerFromRos::create_pipeline() { + pipeline_ = gst_pipeline_new("ros2-h265-pipeline"); + appsrc_ = gst_element_factory_make("appsrc", "source"); + GstElement* convert = gst_element_factory_make("videoconvert", "convert"); + GstElement* parser = gst_element_factory_make("h265parse", "parser"); + GstElement* pay = gst_element_factory_make("rtph265pay", "pay"); + GstElement* sink = gst_element_factory_make("udpsink", "sink"); + + GstElement* encoder = nullptr; + GstElement* nvconv = nullptr; + + if (hw_encoder_) { + nvconv = gst_element_factory_make("nvvidconv", "nvconv"); + encoder = gst_element_factory_make("nvv4l2h265enc", "encoder"); + } else { + encoder = gst_element_factory_make("x265enc", "encoder"); + } + + bool elements_ok = appsrc_ && convert && encoder && parser && pay && sink && pipeline_; + if (hw_encoder_) elements_ok = elements_ok && nvconv; + + if (!elements_ok) { + RCLCPP_FATAL(get_logger(), "Failed to create GStreamer elements"); + return; + } + + if (hw_encoder_) { + g_object_set(encoder, "bitrate", bitrate_, "preset-level", preset_level_, + "iframeinterval", iframe_interval_, "control-rate", control_rate_, NULL); + } else { + // x265enc bitrate is in kbits/sec; key-int-max is the I-frame interval + g_object_set(encoder, "bitrate", bitrate_ / 1000, + "key-int-max", iframe_interval_, + "speed-preset", 0, // ultrafast — minimise latency + NULL); + } + + g_object_set(pay, "config-interval", config_interval_, "pt", pt_, NULL); + g_object_set(sink, "host", destination_ip_.c_str(), "port", destination_port_, "sync", FALSE, NULL); + + if (hw_encoder_) { + gst_bin_add_many(GST_BIN(pipeline_), appsrc_, convert, nvconv, encoder, + parser, pay, sink, NULL); + if (!gst_element_link_many(appsrc_, convert, nvconv, encoder, parser, pay, sink, NULL)) { + RCLCPP_FATAL(get_logger(), "Pipeline linking failed"); + return; + } + } else { + gst_bin_add_many(GST_BIN(pipeline_), appsrc_, convert, encoder, parser, pay, sink, NULL); + if (!gst_element_link_many(appsrc_, convert, encoder, parser, pay, sink, NULL)) { + RCLCPP_FATAL(get_logger(), "Pipeline linking failed"); + return; + } + } + + RCLCPP_INFO(get_logger(), "GStreamer H.265 pipeline created (%s encoder)", + hw_encoder_ ? "NVIDIA hw" : "x265 sw"); +} + +void GStreamerFromRos::imageCb(const sensor_msgs::msg::Image::SharedPtr msg) { + static size_t frame_count = 0; + frame_count++; + + if (!pipeline_started_) { + GstCaps* caps = gst_caps_new_simple( + "video/x-raw", "format", G_TYPE_STRING, input_format_.c_str(), + "width", G_TYPE_INT, msg->width, + "height", G_TYPE_INT, msg->height, + "framerate", GST_TYPE_FRACTION, expected_input_fps_, 1, NULL); + + g_object_set(appsrc_, "caps", caps, "format", GST_FORMAT_TIME, + "is-live", TRUE, "do-timestamp", TRUE, NULL); + gst_caps_unref(caps); + + gst_element_set_state(pipeline_, GST_STATE_PLAYING); + pipeline_started_ = true; + timer_->cancel(); + + RCLCPP_INFO(get_logger(), "H.265 pipeline started (%s)", + hw_encoder_ ? "NVIDIA hw encoder" : "x265 sw encoder"); + } + + GstBuffer* buffer = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr); + gst_buffer_fill(buffer, 0, msg->data.data(), msg->data.size()); + + GstFlowReturn ret; + g_signal_emit_by_name(appsrc_, "push-buffer", buffer, &ret); + gst_buffer_unref(buffer); + + if (ret != GST_FLOW_OK) + RCLCPP_WARN(get_logger(), "Failed to push buffer"); + else + RCLCPP_DEBUG(get_logger(), "Pushed frame #%zu into GStreamer", frame_count); +} + +} // namespace gstreamer_from_ros + +RCLCPP_COMPONENTS_REGISTER_NODE(gstreamer_from_ros::GStreamerFromRos) diff --git a/gstreamer_to_ros/CMakeLists.txt b/gstreamer_to_ros/CMakeLists.txt new file mode 100644 index 0000000..e93453b --- /dev/null +++ b/gstreamer_to_ros/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(gstreamer_to_ros) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(GST REQUIRED + gstreamer-1.0 + gstreamer-app-1.0 +) + +add_library(gstreamer_to_ros_component SHARED + src/gstreamer_to_ros_node.cpp +) + +target_include_directories(gstreamer_to_ros_component PUBLIC + include + ${GST_INCLUDE_DIRS} +) + +target_link_libraries(gstreamer_to_ros_component + ${GST_LIBRARIES} +) + +ament_target_dependencies(gstreamer_to_ros_component + rclcpp + rclcpp_components + sensor_msgs +) + +rclcpp_components_register_node(gstreamer_to_ros_component + PLUGIN "gstreamer_to_ros::GStreamerToROS" + EXECUTABLE gstreamer_to_ros_node +) + +install(TARGETS gstreamer_to_ros_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/gstreamer_to_ros/config/gstreamer_to_ros.yaml b/gstreamer_to_ros/config/gstreamer_to_ros.yaml new file mode 100644 index 0000000..be09a4f --- /dev/null +++ b/gstreamer_to_ros/config/gstreamer_to_ros.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + host: "0.0.0.0" # Listen on all interfaces + port: 5001 # Must match sender + output_topic: "/camera/image_raw" diff --git a/gstreamer_to_ros/include/gstreamer_to_ros/gstreamer_to_ros.hpp b/gstreamer_to_ros/include/gstreamer_to_ros/gstreamer_to_ros.hpp new file mode 100644 index 0000000..6cddd21 --- /dev/null +++ b/gstreamer_to_ros/include/gstreamer_to_ros/gstreamer_to_ros.hpp @@ -0,0 +1,36 @@ +#ifndef GSTREAMER_TO_ROS__GSTREAMER_TO_ROS_HPP_ +#define GSTREAMER_TO_ROS__GSTREAMER_TO_ROS_HPP_ + +#include + +#include +#include +#include +#include + +namespace gstreamer_to_ros { + +class GStreamerToROS : public rclcpp::Node { + public: + explicit GStreamerToROS(const rclcpp::NodeOptions& options); + ~GStreamerToROS(); + + private: + void create_pipeline(); + + static GstFlowReturn on_new_sample(GstAppSink* sink, gpointer user_data); + + rclcpp::Publisher::SharedPtr pub_; + + GstElement* pipeline_; + GstElement* appsink_; + + std::string host_; + int port_; + std::string output_topic_; + bool hw_decoder_; +}; + +} // namespace gstreamer_to_ros + +#endif // GSTREAMER_TO_ROS__GSTREAMER_TO_ROS_HPP_ diff --git a/gstreamer_to_ros/launch/gstreamer_to_ros.launch.py b/gstreamer_to_ros/launch/gstreamer_to_ros.launch.py new file mode 100644 index 0000000..b8d66ac --- /dev/null +++ b/gstreamer_to_ros/launch/gstreamer_to_ros.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _launch_setup(context, *args, **kwargs): + use_nvidia = LaunchConfiguration('gst_nvidia_encoder').perform(context).lower() == 'true' + + container = ComposableNodeContainer( + name='gstreamer_to_ros_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='gstreamer_to_ros', + plugin='gstreamer_to_ros::GStreamerToROS', + name='gstreamer_to_ros_node', + parameters=[{ + 'host': '0.0.0.0', + 'port': 5001, + 'output_topic': '/camera/image_raw', + 'hw_decoder': use_nvidia, + }], + ), + ], + output='screen', + ) + + return [container] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'gst_nvidia_encoder', + default_value='true', + description='Use NVIDIA hardware H.265 decoder (nvh265dec). ' + 'Set false to use software avdec_h265.', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/image_to_gstreamer/package.xml b/gstreamer_to_ros/package.xml similarity index 54% rename from image_to_gstreamer/package.xml rename to gstreamer_to_ros/package.xml index 90da01e..2f2ff45 100644 --- a/image_to_gstreamer/package.xml +++ b/gstreamer_to_ros/package.xml @@ -1,6 +1,6 @@ - image_to_gstreamer + gstreamer_to_ros 0.1.0 ROS 2 node that fits a line over a white segmented mask using IRLS (Huber/Tukey) and OpenCV. @@ -10,8 +10,17 @@ ament_cmake rclcpp + rclcpp_components sensor_msgs + pkg-config + pkg-config + + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + ament_cmake diff --git a/gstreamer_to_ros/src/gstreamer_to_ros_node.cpp b/gstreamer_to_ros/src/gstreamer_to_ros_node.cpp new file mode 100644 index 0000000..89bfda9 --- /dev/null +++ b/gstreamer_to_ros/src/gstreamer_to_ros_node.cpp @@ -0,0 +1,111 @@ +#include "gstreamer_to_ros/gstreamer_to_ros.hpp" + +#include + +namespace gstreamer_to_ros { + +GStreamerToROS::GStreamerToROS(const rclcpp::NodeOptions& options) + : Node("gstreamer_to_ros_node", options), pipeline_(nullptr), appsink_(nullptr) { + gst_init(nullptr, nullptr); + + host_ = declare_parameter("host", "0.0.0.0"); + port_ = declare_parameter("port", 5001); + output_topic_ = declare_parameter("output_topic", "/camera/image_raw"); + hw_decoder_ = declare_parameter("hw_decoder", true); + + pub_ = create_publisher( + output_topic_, rclcpp::SensorDataQoS().keep_last(1)); + + create_pipeline(); +} + +GStreamerToROS::~GStreamerToROS() { + if (pipeline_) { + gst_element_set_state(pipeline_, GST_STATE_NULL); + gst_object_unref(pipeline_); + } +} + +void GStreamerToROS::create_pipeline() { + pipeline_ = gst_pipeline_new("h265-receive-pipeline"); + + GstElement* src = gst_element_factory_make("udpsrc", "src"); + GstElement* depay = gst_element_factory_make("rtph265depay", "depay"); + GstElement* parse = gst_element_factory_make("h265parse", "parse"); + GstElement* convert = gst_element_factory_make("videoconvert", "convert"); + appsink_ = gst_element_factory_make("appsink", "sink"); + + GstElement* decoder = hw_decoder_ + ? gst_element_factory_make("nvh265dec", "decoder") + : gst_element_factory_make("avdec_h265", "decoder"); + + if (!pipeline_ || !src || !depay || !parse || !decoder || !convert || !appsink_) { + RCLCPP_FATAL(get_logger(), "Failed to create GStreamer elements"); + return; + } + + GstCaps* caps = gst_caps_new_simple( + "application/x-rtp", "media", G_TYPE_STRING, "video", + "encoding-name", G_TYPE_STRING, "H265", + "payload", G_TYPE_INT, 96, NULL); + + g_object_set(src, "port", port_, "caps", caps, NULL); + gst_caps_unref(caps); + + g_object_set(appsink_, "emit-signals", TRUE, "sync", FALSE, NULL); + g_signal_connect(appsink_, "new-sample", G_CALLBACK(GStreamerToROS::on_new_sample), this); + + gst_bin_add_many(GST_BIN(pipeline_), src, depay, parse, decoder, convert, appsink_, NULL); + + if (!gst_element_link_many(src, depay, parse, decoder, convert, appsink_, NULL)) { + RCLCPP_FATAL(get_logger(), "Pipeline linking failed"); + return; + } + + gst_element_set_state(pipeline_, GST_STATE_PLAYING); + + RCLCPP_INFO(get_logger(), "H.265 UDP receiver pipeline started on port %d (%s decoder)", + port_, hw_decoder_ ? "NVIDIA hw" : "avdec sw"); +} + +GstFlowReturn GStreamerToROS::on_new_sample(GstAppSink* sink, gpointer user_data) { + auto* node = static_cast(user_data); + + GstSample* sample = gst_app_sink_pull_sample(sink); + if (!sample) + return GST_FLOW_ERROR; + + GstBuffer* buffer = gst_sample_get_buffer(sample); + GstCaps* caps = gst_sample_get_caps(sample); + GstStructure* structure = gst_caps_get_structure(caps, 0); + + int width = 0, height = 0; + gst_structure_get_int(structure, "width", &width); + gst_structure_get_int(structure, "height", &height); + + GstMapInfo map; + if (!gst_buffer_map(buffer, &map, GST_MAP_READ)) { + gst_sample_unref(sample); + return GST_FLOW_ERROR; + } + + sensor_msgs::msg::Image msg; + msg.header.stamp = node->now(); + msg.header.frame_id = "camera"; + msg.width = width; + msg.height = height; + msg.encoding = "bgr8"; + msg.step = width * 3; + msg.data.assign(map.data, map.data + map.size); + + node->pub_->publish(msg); + + gst_buffer_unmap(buffer, &map); + gst_sample_unref(sample); + + return GST_FLOW_OK; +} + +} // namespace gstreamer_to_ros + +RCLCPP_COMPONENTS_REGISTER_NODE(gstreamer_to_ros::GStreamerToROS) diff --git a/image_to_gstreamer/CMakeLists.txt b/image_to_gstreamer/CMakeLists.txt deleted file mode 100644 index 7bd5df1..0000000 --- a/image_to_gstreamer/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(image_to_gstreamer) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0) - -include_directories(${GST_INCLUDE_DIRS}) - -add_executable(image_to_gstreamer_node src/image_to_gstreamer_node.cpp) - -ament_target_dependencies(image_to_gstreamer_node - rclcpp - sensor_msgs -) - -target_link_libraries(image_to_gstreamer_node ${GST_LIBRARIES}) - -install( - TARGETS image_to_gstreamer_node - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - - -ament_package() \ No newline at end of file diff --git a/image_to_gstreamer/launch/image_to_gstreamer.launch.py b/image_to_gstreamer/launch/image_to_gstreamer.launch.py deleted file mode 100644 index 5c73ff2..0000000 --- a/image_to_gstreamer/launch/image_to_gstreamer.launch.py +++ /dev/null @@ -1,33 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - - host_arg = DeclareLaunchArgument( - 'host', - default_value='127.0.0.1', - description='Destination host for UDP stream' - ) - - port_arg = DeclareLaunchArgument( - 'port', - default_value='5000', - description='Destination UDP port' - ) - - return LaunchDescription([ - host_arg, - port_arg, - Node( - package='image_to_gstreamer', - executable='image_to_gstreamer_node', - name='image_to_gstreamer_node', - parameters=[ - {'input_topic': '/cam/image_color'}, - {'host': LaunchConfiguration('host')}, - {'port': LaunchConfiguration('port')}, - ], - ) - ]) \ No newline at end of file diff --git a/image_to_gstreamer/src/image_to_gstreamer_node.cpp b/image_to_gstreamer/src/image_to_gstreamer_node.cpp deleted file mode 100644 index 15f8b97..0000000 --- a/image_to_gstreamer/src/image_to_gstreamer_node.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include -#include - -#include -#include - -class ImageToGStreamer : public rclcpp::Node -{ -public: - ImageToGStreamer() - : Node("image_to_gstreamer_node") - { - gst_init(nullptr, nullptr); - - input_topic_ = this->declare_parameter("input_topic", "/cam/image_color"); - - host_ = this->declare_parameter("host", "127.0.0.1"); - port_ = this->declare_parameter("port", 5000); - - sub_ = create_subscription( - input_topic_, rclcpp::SensorDataQoS(), - std::bind(&ImageToGStreamer::imageCb, this, std::placeholders::_1)); - - create_pipeline(); - } - - ~ImageToGStreamer() - { - gst_element_set_state(pipeline_, GST_STATE_NULL); - gst_object_unref(pipeline_); - } - -private: - rclcpp::Subscription::SharedPtr sub_; - - GstElement *pipeline_; - GstElement *appsrc_; - - bool pipeline_started_ = false; - - void create_pipeline() - { - pipeline_ = gst_pipeline_new("ros2-h265-pipeline"); - - appsrc_ = gst_element_factory_make("appsrc", "source"); - GstElement *convert = gst_element_factory_make("videoconvert", "convert"); - GstElement *encoder = gst_element_factory_make("nvh265enc", "encoder"); - GstElement *parser = gst_element_factory_make("h265parse", "parser"); - GstElement *pay = gst_element_factory_make("rtph265pay", "pay"); - GstElement *sink = gst_element_factory_make("udpsink", "sink"); - - if (!appsrc_ || !convert || - !encoder || !parser || !pay || !sink) - { - if (!appsrc_) - RCLCPP_FATAL(get_logger(), "Failed to create appsrc"); - if (!convert) - RCLCPP_FATAL(get_logger(), "Failed to create convert"); - if (!encoder) - RCLCPP_FATAL(get_logger(), "Failed to create encoder"); - if (!parser) - RCLCPP_FATAL(get_logger(), "Failed to create parser"); - if (!pay) - RCLCPP_FATAL(get_logger(), "Failed to create pay"); - if (!sink) - RCLCPP_FATAL(get_logger(), "Failed to create sink"); - - RCLCPP_FATAL(get_logger(), "Failed to create GStreamer elements"); - return; - } - if (!pipeline_) { - RCLCPP_FATAL(get_logger(), "Pipeline creation failed"); - return; - } - - g_object_set(sink, - "host", host_.c_str(), - "port", port_, - "sync", FALSE, - NULL); - - g_object_set(encoder, - "bitrate", 50000, // 4 Mbps - "preset", 3, // low-latency - "rc-mode", 1, // CBR - "gop-size", 30, - NULL); - - g_object_set(pay, - "config-interval", 1, // send SPS/PPS every 1 second - "pt", 96, // payload type - NULL); - - gst_bin_add_many(GST_BIN(pipeline_), - appsrc_, convert, encoder, parser, pay, sink, NULL); - - if (!gst_element_link_many( - appsrc_, convert, encoder, parser, pay, sink, NULL)) - { - RCLCPP_FATAL(get_logger(), "Pipeline linking failed"); - return; - } - } - - void imageCb(const sensor_msgs::msg::Image::SharedPtr msg) - { - if (!pipeline_started_) - { - GstCaps *caps = gst_caps_new_simple( - "video/x-raw", - "format", G_TYPE_STRING, "RGB", - "width", G_TYPE_INT, msg->width, - "height", G_TYPE_INT, msg->height, - "framerate", GST_TYPE_FRACTION, 30, 1, - NULL); - g_object_set(appsrc_, - "caps", caps, - "format", GST_FORMAT_TIME, - "is-live", TRUE, - "do-timestamp", TRUE, - NULL); - gst_caps_unref(caps); - - gst_element_set_state(pipeline_, GST_STATE_PLAYING); - pipeline_started_ = true; - - RCLCPP_INFO(get_logger(), "H.265 GPU pipeline started"); - } - - GstBuffer *buffer = gst_buffer_new_allocate( - nullptr, msg->data.size(), nullptr); - - gst_buffer_fill(buffer, 0, msg->data.data(), msg->data.size()); - - GstFlowReturn ret; - g_signal_emit_by_name(appsrc_, "push-buffer", buffer, &ret); - gst_buffer_unref(buffer); - - if (ret != GST_FLOW_OK) - RCLCPP_WARN(get_logger(), "Failed to push buffer"); - } - - // ----------------------------- Params & ROS -------------------------------- - // Topics - std::string input_topic_{"/cam/image_color"}; - std::string host_{"127.0.0.1"}; - int port_{5000}; -}; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file