Skip to content

Commit 49bd907

Browse files
gardeggardeg
committed
image to gstreamer finished
1 parent 4fbeb11 commit 49bd907

4 files changed

Lines changed: 219 additions & 0 deletions

File tree

‎image_to_gstreamer/CMakeLists.txt‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(image_to_gstreamer)
3+
4+
find_package(ament_cmake REQUIRED)
5+
find_package(rclcpp REQUIRED)
6+
find_package(sensor_msgs REQUIRED)
7+
8+
find_package(PkgConfig REQUIRED)
9+
pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0)
10+
11+
include_directories(${GST_INCLUDE_DIRS})
12+
13+
add_executable(image_to_gstreamer_node src/image_to_gstreamer_node.cpp)
14+
15+
ament_target_dependencies(image_to_gstreamer_node
16+
rclcpp
17+
sensor_msgs
18+
)
19+
20+
target_link_libraries(image_to_gstreamer_node ${GST_LIBRARIES})
21+
22+
install(
23+
TARGETS image_to_gstreamer_node
24+
DESTINATION lib/${PROJECT_NAME}
25+
)
26+
27+
install(
28+
DIRECTORY launch
29+
DESTINATION share/${PROJECT_NAME}
30+
)
31+
32+
33+
ament_package()

‎image_to_gstreamer/launch/image_to_gstreamer.launch.py‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
4+
5+
def generate_launch_description():
6+
return LaunchDescription([
7+
Node(
8+
package='image_to_gstreamer',
9+
executable='image_to_gstreamer_node',
10+
name='image_to_gstreamer_node',
11+
parameters=[
12+
# Topics
13+
{'input_topic': '/cam/image_color'},
14+
15+
],
16+
)
17+
])

‎image_to_gstreamer/package.xml‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<package format="3">
3+
<name>image_to_gstreamer</name>
4+
<version>0.1.0</version>
5+
<description>ROS 2 node that fits a line over a white segmented mask using IRLS (Huber/Tukey) and OpenCV.</description>
6+
7+
<maintainer email="you@example.com">You</maintainer>
8+
<license>MIT</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>sensor_msgs</depend>
14+
15+
<export>
16+
<build_type>ament_cmake</build_type>
17+
</export>
18+
</package>

‎image_to_gstreamer/src/image_to_gstreamer_node.cpp‎

Lines changed: 151 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <sensor_msgs/msg/image.hpp>
3+
4+
#include <gst/gst.h>
5+
#include <gst/app/gstappsrc.h>
6+
7+
class ImageToGStreamer : public rclcpp::Node
8+
{
9+
public:
10+
ImageToGStreamer()
11+
: Node("image_to_gstreamer_node")
12+
{
13+
gst_init(nullptr, nullptr);
14+
15+
input_topic_ = this->declare_parameter<std::string>("input_topic", "/cam/image_color");
16+
17+
sub_ = create_subscription<sensor_msgs::msg::Image>(
18+
input_topic_, rclcpp::SensorDataQoS(),
19+
std::bind(&ImageToGStreamer::imageCb, this, std::placeholders::_1));
20+
21+
create_pipeline();
22+
}
23+
24+
~ImageToGStreamer()
25+
{
26+
gst_element_set_state(pipeline_, GST_STATE_NULL);
27+
gst_object_unref(pipeline_);
28+
}
29+
30+
private:
31+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
32+
33+
GstElement *pipeline_;
34+
GstElement *appsrc_;
35+
36+
bool pipeline_started_ = false;
37+
38+
void create_pipeline()
39+
{
40+
pipeline_ = gst_pipeline_new("ros2-h265-pipeline");
41+
42+
appsrc_ = gst_element_factory_make("appsrc", "source");
43+
GstElement *convert = gst_element_factory_make("videoconvert", "convert");
44+
GstElement *encoder = gst_element_factory_make("nvh265enc", "encoder");
45+
GstElement *parser = gst_element_factory_make("h265parse", "parser");
46+
GstElement *pay = gst_element_factory_make("rtph265pay", "pay");
47+
GstElement *sink = gst_element_factory_make("udpsink", "sink");
48+
49+
if (!appsrc_ || !convert ||
50+
!encoder || !parser || !pay || !sink)
51+
{
52+
if (!appsrc_)
53+
RCLCPP_FATAL(get_logger(), "Failed to create appsrc");
54+
if (!convert)
55+
RCLCPP_FATAL(get_logger(), "Failed to create convert");
56+
if (!encoder)
57+
RCLCPP_FATAL(get_logger(), "Failed to create encoder");
58+
if (!parser)
59+
RCLCPP_FATAL(get_logger(), "Failed to create parser");
60+
if (!pay)
61+
RCLCPP_FATAL(get_logger(), "Failed to create pay");
62+
if (!sink)
63+
RCLCPP_FATAL(get_logger(), "Failed to create sink");
64+
65+
RCLCPP_FATAL(get_logger(), "Failed to create GStreamer elements");
66+
return;
67+
}
68+
if (!pipeline_) {
69+
RCLCPP_FATAL(get_logger(), "Pipeline creation failed");
70+
return;
71+
}
72+
73+
g_object_set(sink,
74+
"host", "127.0.0.1",
75+
"port", 5000,
76+
"sync", FALSE,
77+
NULL);
78+
79+
g_object_set(encoder,
80+
"bitrate", 50000, // 4 Mbps
81+
"preset", 3, // low-latency
82+
"rc-mode", 1, // CBR
83+
"gop-size", 30,
84+
NULL);
85+
86+
g_object_set(pay,
87+
"config-interval", 1, // send SPS/PPS every 1 second
88+
"pt", 96, // payload type
89+
NULL);
90+
91+
gst_bin_add_many(GST_BIN(pipeline_),
92+
appsrc_, convert, encoder, parser, pay, sink, NULL);
93+
94+
if (!gst_element_link_many(
95+
appsrc_, convert, encoder, parser, pay, sink, NULL))
96+
{
97+
RCLCPP_FATAL(get_logger(), "Pipeline linking failed");
98+
return;
99+
}
100+
}
101+
102+
void imageCb(const sensor_msgs::msg::Image::SharedPtr msg)
103+
{
104+
if (!pipeline_started_)
105+
{
106+
GstCaps *caps = gst_caps_new_simple(
107+
"video/x-raw",
108+
"format", G_TYPE_STRING, "RGB",
109+
"width", G_TYPE_INT, msg->width,
110+
"height", G_TYPE_INT, msg->height,
111+
"framerate", GST_TYPE_FRACTION, 30, 1,
112+
NULL);
113+
g_object_set(appsrc_,
114+
"caps", caps,
115+
"format", GST_FORMAT_TIME,
116+
"is-live", TRUE,
117+
"do-timestamp", TRUE,
118+
NULL);
119+
gst_caps_unref(caps);
120+
121+
gst_element_set_state(pipeline_, GST_STATE_PLAYING);
122+
pipeline_started_ = true;
123+
124+
RCLCPP_INFO(get_logger(), "H.265 GPU pipeline started");
125+
}
126+
127+
GstBuffer *buffer = gst_buffer_new_allocate(
128+
nullptr, msg->data.size(), nullptr);
129+
130+
gst_buffer_fill(buffer, 0, msg->data.data(), msg->data.size());
131+
132+
GstFlowReturn ret;
133+
g_signal_emit_by_name(appsrc_, "push-buffer", buffer, &ret);
134+
gst_buffer_unref(buffer);
135+
136+
if (ret != GST_FLOW_OK)
137+
RCLCPP_WARN(get_logger(), "Failed to push buffer");
138+
}
139+
140+
// ----------------------------- Params & ROS --------------------------------
141+
// Topics
142+
std::string input_topic_{"/cam/image_color"};
143+
};
144+
145+
int main(int argc, char **argv)
146+
{
147+
rclcpp::init(argc, argv);
148+
rclcpp::spin(std::make_shared<ImageToGStreamer>());
149+
rclcpp::shutdown();
150+
return 0;
151+
}

0 commit comments

Comments
 (0)