23 #include "gazebo/sensors/DepthCameraSensor.hh" 38 static
void cb_need_data(GstElement *appsrc, guint unused_size, gpointer user_data) {
43 void GstCameraPlugin::gstCallback(GstElement *appsrc) {
45 frameBufferMutex.lock();
47 while (!frameBuffer) {
49 frameBufferMutex.unlock();
51 frameBufferMutex.lock();
54 GST_BUFFER_PTS(frameBuffer) = gstTimestamp;
55 GST_BUFFER_DURATION(frameBuffer) = gst_util_uint64_scale_int (1, GST_SECOND, (
int)rate);
56 gstTimestamp += GST_BUFFER_DURATION(frameBuffer);
59 g_signal_emit_by_name(appsrc,
"push-buffer", frameBuffer, &ret);
61 frameBufferMutex.unlock();
63 if (ret != GST_FLOW_OK) {
65 gzerr <<
"g_signal_emit_by_name failed" << endl;
66 g_main_loop_quit(mainLoop);
76 void GstCameraPlugin::startGstThread() {
80 mainLoop = g_main_loop_new(NULL,
FALSE);
82 gzerr <<
"Create loop failed. \n";
86 GstElement* pipeline = gst_pipeline_new(
"sender");
88 gzerr <<
"ERR: Create pipeline failed. \n";
92 GstElement* dataSrc = gst_element_factory_make(
"appsrc",
"AppSrc");
93 GstElement* testSrc = gst_element_factory_make(
"videotestsrc",
"FileSrc");
94 GstElement* conv = gst_element_factory_make(
"videoconvert",
"Convert");
95 GstElement* encoder = gst_element_factory_make(
"x264enc",
"AvcEncoder");
96 GstElement*
parser = gst_element_factory_make(
"h264parse",
"Parser");
97 GstElement* payload = gst_element_factory_make(
"rtph264pay",
"PayLoad");
98 GstElement* sink = gst_element_factory_make(
"udpsink",
"UdpSink");
99 if (!dataSrc || !testSrc || !conv || !encoder || !parser || !payload || !sink) {
100 gzerr <<
"ERR: Create elements failed. \n";
109 g_object_set(G_OBJECT(dataSrc),
"caps",
110 gst_caps_new_simple (
"video/x-raw",
111 "format", G_TYPE_STRING,
"RGB",
112 "width", G_TYPE_INT, this->width,
113 "height", G_TYPE_INT, this->height,
114 "framerate", GST_TYPE_FRACTION, (
unsigned int)this->rate, 1,
120 g_object_set(G_OBJECT(encoder),
"bitrate", 800, NULL);
121 g_object_set(G_OBJECT(encoder),
"speed-preset", 2, NULL);
127 g_object_set(G_OBJECT(payload),
"config-interval", 1, NULL);
130 g_object_set(G_OBJECT(sink),
"host",
"127.0.0.1", NULL);
131 g_object_set(G_OBJECT(sink),
"port", this->udpPort, NULL);
136 gst_bin_add_many(GST_BIN(pipeline), dataSrc, conv, encoder, parser, payload, sink, NULL);
139 if (gst_element_link_many(dataSrc, conv, encoder, parser, payload, sink, NULL) !=
TRUE) {
140 gzerr <<
"ERR: Link all the elements failed. \n";
145 g_object_set(G_OBJECT(dataSrc),
"stream-type", 0,
"format", GST_FORMAT_TIME, NULL);
146 g_signal_connect(dataSrc,
"need-data", G_CALLBACK(
cb_need_data),
this);
149 gst_element_set_state(pipeline, GST_STATE_PLAYING);
150 g_main_loop_run(mainLoop);
153 gst_element_set_state(pipeline, GST_STATE_NULL);
154 gst_object_unref(GST_OBJECT(pipeline));
155 g_main_loop_unref(mainLoop);
161 GstCameraPlugin::GstCameraPlugin()
162 : SensorPlugin(), width(0), height(0), depth(0), frameBuffer(nullptr), mainLoop(nullptr),
186 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
190 gzerr <<
"Invalid sensor pointer.\n";
193 std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
197 gzerr <<
"GstCameraPlugin requires a CameraSensor.\n";
199 if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(sensor))
200 gzmsg <<
"It is a depth camera sensor\n";
207 gzerr <<
"GstCameraPlugin not attached to a camera sensor\n";
217 if (!isfinite(this->
rate)) {
221 if (sdf->HasElement(
"robotNamespace"))
222 namespace_ = sdf->GetElement(
"robotNamespace")->Get<std::string>();
224 gzwarn <<
"[gazebo_gst_camera_plugin] Please specify a robotNamespace.\n";
227 if (sdf->HasElement(
"udpPort")) {
228 this->
udpPort = sdf->GetElement(
"udpPort")->Get<
int>();
231 node_handle_ = transport::NodePtr(
new transport::Node());
250 const std::string &
format)
253 image = this->
camera->ImageData(0);
262 guint
size = width * height * 3;
263 frameBuffer = gst_buffer_new_allocate(NULL, size, NULL);
266 if (gst_buffer_map(
frameBuffer, &mapInfo, GST_MAP_WRITE)) {
267 memcpy(mapInfo.data, image, size);
270 gzerr <<
"gst_buffer_map failed"<<endl;
A Gazebo plugin that can be attached to a camera and then streams the video data using gstreamer...
GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin)
sensors::CameraSensorPtr parentSensor
virtual void Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf)
static void cb_need_data(GstElement *appsrc, guint unused_size, gpointer user_data)
void gstCallback(GstElement *appsrc)
event::ConnectionPtr newFrameConnection
static const bool kPrintOnPluginLoad
rendering::CameraPtr camera
static void * start_thread(void *param)
transport::NodePtr node_handle_
virtual ~GstCameraPlugin()
virtual void OnNewFrame(const unsigned char *image, unsigned int width, unsigned int height, unsigned int depth, const std::string &format)
std::mutex frameBufferMutex