gscam.cpp
Go to the documentation of this file.
1 
2 #include <stdlib.h>
3 #include <unistd.h>
4 #include <sys/ipc.h>
5 #include <sys/shm.h>
6 
7 
8 #include <iostream>
9 extern "C"{
10 #include <gst/gst.h>
11 #include <gst/app/gstappsink.h>
12 }
13 
14 #include <ros/ros.h>
15 
18 
19 
20 #include <sensor_msgs/Image.h>
21 #include <sensor_msgs/CompressedImage.h>
22 #include <sensor_msgs/CameraInfo.h>
23 #include <sensor_msgs/SetCameraInfo.h>
25 
27 
28 #include <gscam/gscam.h>
29 
30 namespace gscam {
31 
32  GSCam::GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private) :
33  gsconfig_(""),
34  pipeline_(NULL),
35  sink_(NULL),
36  nh_(nh_camera),
37  nh_private_(nh_private),
38  image_transport_(nh_camera),
39  camera_info_manager_(nh_camera)
40  {
41  }
42 
44  {
45  }
46 
48  {
49  // Get gstreamer configuration
50  // (either from environment variable or ROS param)
51  std::string gsconfig_rosparam = "";
52  bool gsconfig_rosparam_defined = false;
53  char *gsconfig_env = NULL;
54 
55  gsconfig_rosparam_defined = nh_private_.getParam("gscam_config",gsconfig_rosparam);
56  gsconfig_env = getenv("GSCAM_CONFIG");
57 
58  if (!gsconfig_env && !gsconfig_rosparam_defined) {
59  ROS_FATAL( "Problem getting GSCAM_CONFIG environment variable and 'gscam_config' rosparam is not set. This is needed to set up a gstreamer pipeline." );
60  return false;
61  } else if(gsconfig_env && gsconfig_rosparam_defined) {
62  ROS_FATAL( "Both GSCAM_CONFIG environment variable and 'gscam_config' rosparam are set. Please only define one." );
63  return false;
64  } else if(gsconfig_env) {
65  gsconfig_ = gsconfig_env;
66  ROS_INFO_STREAM("Using gstreamer config from env: \""<<gsconfig_env<<"\"");
67  } else if(gsconfig_rosparam_defined) {
68  gsconfig_ = gsconfig_rosparam;
69  ROS_INFO_STREAM("Using gstreamer config from rosparam: \""<<gsconfig_rosparam<<"\"");
70  }
71 
72  // Get additional gscam configuration
73  nh_private_.param("sync_sink", sync_sink_, true);
74  nh_private_.param("preroll", preroll_, false);
75  nh_private_.param("use_gst_timestamps", use_gst_timestamps_, false);
76 
77  nh_private_.param("reopen_on_eof", reopen_on_eof_, false);
78 
79  // Get the camera parameters file
80  nh_private_.getParam("camera_info_url", camera_info_url_);
81  nh_private_.getParam("camera_name", camera_name_);
82 
83  // Get the image encoding
87  image_encoding_ != "jpeg") {
88  ROS_FATAL_STREAM("Unsupported image encoding: " + image_encoding_);
89  }
90 
92 
95  ROS_INFO_STREAM("Loaded camera calibration from "<<camera_info_url_);
96  } else {
97  ROS_WARN_STREAM("Camera info at: "<<camera_info_url_<<" not found. Using an uncalibrated config.");
98  }
99 
100  // Get TF Frame
101  if(!nh_private_.getParam("frame_id",frame_id_)){
102  frame_id_ = "/camera_frame";
103  ROS_WARN_STREAM("No camera frame_id set, using frame \""<<frame_id_<<"\".");
104  nh_private_.setParam("frame_id",frame_id_);
105  }
106 
107  return true;
108  }
109 
111  {
112  if(!gst_is_initialized()) {
113  // Initialize gstreamer pipeline
114  ROS_DEBUG_STREAM( "Initializing gstreamer..." );
115  gst_init(0,0);
116  }
117 
118  ROS_DEBUG_STREAM( "Gstreamer Version: " << gst_version_string() );
119 
120  GError *error = 0; // Assignment to zero is a gst requirement
121 
122  pipeline_ = gst_parse_launch(gsconfig_.c_str(), &error);
123  if (pipeline_ == NULL) {
124  ROS_FATAL_STREAM( error->message );
125  return false;
126  }
127 
128  // Create RGB sink
129  sink_ = gst_element_factory_make("appsink",NULL);
130  GstCaps * caps = gst_app_sink_get_caps(GST_APP_SINK(sink_));
131 
132 #if (GST_VERSION_MAJOR == 1)
133  // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html
135  caps = gst_caps_new_simple( "video/x-raw",
136  "format", G_TYPE_STRING, "RGB",
137  NULL);
139  caps = gst_caps_new_simple( "video/x-raw",
140  "format", G_TYPE_STRING, "GRAY8",
141  NULL);
142  } else if (image_encoding_ == "jpeg") {
143  caps = gst_caps_new_simple("image/jpeg", NULL, NULL);
144  }
145 #else
147  caps = gst_caps_new_simple( "video/x-raw-rgb", NULL,NULL);
149  caps = gst_caps_new_simple("video/x-raw-gray", NULL, NULL);
150  } else if (image_encoding_ == "jpeg") {
151  caps = gst_caps_new_simple("image/jpeg", NULL, NULL);
152  }
153 #endif
154 
155  gst_app_sink_set_caps(GST_APP_SINK(sink_), caps);
156  gst_caps_unref(caps);
157 
158  // Set whether the sink should sync
159  // Sometimes setting this to true can cause a large number of frames to be
160  // dropped
161  gst_base_sink_set_sync(
162  GST_BASE_SINK(sink_),
163  (sync_sink_) ? TRUE : FALSE);
164 
165  if(GST_IS_PIPELINE(pipeline_)) {
166  GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC);
167  g_assert(outpad);
168 
169  GstElement *outelement = gst_pad_get_parent_element(outpad);
170  g_assert(outelement);
171  gst_object_unref(outpad);
172 
173  if(!gst_bin_add(GST_BIN(pipeline_), sink_)) {
174  ROS_FATAL("gst_bin_add() failed");
175  gst_object_unref(outelement);
176  gst_object_unref(pipeline_);
177  return false;
178  }
179 
180  if(!gst_element_link(outelement, sink_)) {
181  ROS_FATAL("GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement));
182  gst_object_unref(outelement);
183  gst_object_unref(pipeline_);
184  return false;
185  }
186 
187  gst_object_unref(outelement);
188  } else {
189  GstElement* launchpipe = pipeline_;
190  pipeline_ = gst_pipeline_new(NULL);
191  g_assert(pipeline_);
192 
193  gst_object_unparent(GST_OBJECT(launchpipe));
194 
195  gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, NULL);
196 
197  if(!gst_element_link(launchpipe, sink_)) {
198  ROS_FATAL("GStreamer: cannot link launchpipe -> sink");
199  gst_object_unref(pipeline_);
200  return false;
201  }
202  }
203 
204  // Calibration between ros::Time and gst timestamps
205  GstClock * clock = gst_system_clock_obtain();
206  ros::Time now = ros::Time::now();
207  GstClockTime ct = gst_clock_get_time(clock);
208  gst_object_unref(clock);
209  time_offset_ = now.toSec() - GST_TIME_AS_USECONDS(ct)/1e6;
210  ROS_INFO("Time offset: %.3f",time_offset_);
211 
212  gst_element_set_state(pipeline_, GST_STATE_PAUSED);
213 
214  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
215  ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration.");
216  return false;
217  } else {
218  ROS_DEBUG_STREAM("Stream is PAUSED.");
219  }
220 
221  // Create ROS camera interface
222  if (image_encoding_ == "jpeg") {
223  jpeg_pub_ = nh_.advertise<sensor_msgs::CompressedImage>("camera/image_raw/compressed",1);
224  cinfo_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera/camera_info",1);
225  } else {
226  camera_pub_ = image_transport_.advertiseCamera("camera/image_raw", 1);
227  }
228 
229  return true;
230  }
231 
233  {
234  ROS_INFO_STREAM("Publishing stream...");
235 
236  // Pre-roll camera if needed
237  if (preroll_) {
238  ROS_DEBUG("Performing preroll...");
239 
240  //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
241  //I am told this is needed and am erring on the side of caution.
242  gst_element_set_state(pipeline_, GST_STATE_PLAYING);
243  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
244  ROS_ERROR("Failed to PLAY during preroll.");
245  return;
246  } else {
247  ROS_DEBUG("Stream is PLAYING in preroll.");
248  }
249 
250  gst_element_set_state(pipeline_, GST_STATE_PAUSED);
251  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
252  ROS_ERROR("Failed to PAUSE.");
253  return;
254  } else {
255  ROS_INFO("Stream is PAUSED in preroll.");
256  }
257  }
258 
259  if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
260  ROS_ERROR("Could not start stream!");
261  return;
262  }
263  ROS_INFO("Started stream.");
264 
265  // Poll the data as fast a spossible
266  while(ros::ok())
267  {
268  // This should block until a new frame is awake, this way, we'll run at the
269  // actual capture framerate of the device.
270  // ROS_DEBUG("Getting data...");
271 #if (GST_VERSION_MAJOR == 1)
272  GstSample* sample = gst_app_sink_pull_sample(GST_APP_SINK(sink_));
273  if(!sample) {
274  ROS_ERROR("Could not get gstreamer sample.");
275  break;
276  }
277  GstBuffer* buf = gst_sample_get_buffer(sample);
278  GstMemory *memory = gst_buffer_get_memory(buf, 0);
279  GstMapInfo info;
280 
281  gst_memory_map(memory, &info, GST_MAP_READ);
282  gsize &buf_size = info.size;
283  guint8* &buf_data = info.data;
284 #else
285  GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));
286  guint &buf_size = buf->size;
287  guint8* &buf_data = buf->data;
288 #endif
289  GstClockTime bt = gst_element_get_base_time(pipeline_);
290  // ROS_INFO("New buffer: timestamp %.6f %lu %lu %.3f",
291  // GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_);
292 
293 
294 #if 0
295  GstFormat fmt = GST_FORMAT_TIME;
296  gint64 current = -1;
297 
298  Query the current position of the stream
299  if (gst_element_query_position(pipeline_, &fmt, &current)) {
300  ROS_INFO_STREAM("Position "<<current);
301  }
302 #endif
303 
304  // Stop on end of stream
305  if (!buf) {
306  ROS_INFO("Stream ended.");
307  break;
308  }
309 
310  // ROS_DEBUG("Got data.");
311 
312  // Get the image width and height
313  GstPad* pad = gst_element_get_static_pad(sink_, "sink");
314 #if (GST_VERSION_MAJOR == 1)
315  const GstCaps *caps = gst_pad_get_current_caps(pad);
316 #else
317  const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
318 #endif
319  GstStructure *structure = gst_caps_get_structure(caps,0);
320  gst_structure_get_int(structure,"width",&width_);
321  gst_structure_get_int(structure,"height",&height_);
322 
323  // Update header information
324  sensor_msgs::CameraInfo cur_cinfo = camera_info_manager_.getCameraInfo();
325  sensor_msgs::CameraInfoPtr cinfo;
326  cinfo.reset(new sensor_msgs::CameraInfo(cur_cinfo));
327  if (use_gst_timestamps_) {
328 #if (GST_VERSION_MAJOR == 1)
329  cinfo->header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->pts+bt)/1e6+time_offset_);
330 #else
331  cinfo->header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_);
332 #endif
333  } else {
334  cinfo->header.stamp = ros::Time::now();
335  }
336  // ROS_INFO("Image time stamp: %.3f",cinfo->header.stamp.toSec());
337  cinfo->header.frame_id = frame_id_;
338  if (image_encoding_ == "jpeg") {
339  sensor_msgs::CompressedImagePtr img(new sensor_msgs::CompressedImage());
340  img->header = cinfo->header;
341  img->format = "jpeg";
342  img->data.resize(buf_size);
343  std::copy(buf_data, (buf_data)+(buf_size),
344  img->data.begin());
345  jpeg_pub_.publish(img);
346  cinfo_pub_.publish(cinfo);
347  } else {
348  // Complain if the returned buffer is smaller than we expect
349  const unsigned int expected_frame_size =
351  ? width_ * height_ * 3
352  : width_ * height_;
353 
354  if (buf_size < expected_frame_size) {
355  ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
356  << expected_frame_size << " bytes but got only "
357  << (buf_size) << " bytes. (make sure frames are correctly encoded)");
358  }
359 
360  // Construct Image message
361  sensor_msgs::ImagePtr img(new sensor_msgs::Image());
362 
363  img->header = cinfo->header;
364 
365  // Image data and metadata
366  img->width = width_;
367  img->height = height_;
368  img->encoding = image_encoding_;
369  img->is_bigendian = false;
370  img->data.resize(expected_frame_size);
371 
372  // Copy only the data we received
373  // Since we're publishing shared pointers, we need to copy the image so
374  // we can free the buffer allocated by gstreamer
376  img->step = width_ * 3;
377  } else {
378  img->step = width_;
379  }
380  std::copy(
381  buf_data,
382  (buf_data)+(buf_size),
383  img->data.begin());
384 
385  // Publish the image/info
386  camera_pub_.publish(img, cinfo);
387  }
388 
389  // Release the buffer
390  if(buf) {
391 #if (GST_VERSION_MAJOR == 1)
392  gst_memory_unmap(memory, &info);
393  gst_memory_unref(memory);
394 #endif
395  gst_buffer_unref(buf);
396  }
397 
398  ros::spinOnce();
399  }
400  }
401 
403  {
404  // Clean up
405  ROS_INFO("Stopping gstreamer pipeline...");
406  if(pipeline_) {
407  gst_element_set_state(pipeline_, GST_STATE_NULL);
408  gst_object_unref(pipeline_);
409  pipeline_ = NULL;
410  }
411  }
412 
413  void GSCam::run() {
414  while(ros::ok()) {
415  if(!this->configure()) {
416  ROS_FATAL("Failed to configure gscam!");
417  break;
418  }
419 
420  if(!this->init_stream()) {
421  ROS_FATAL("Failed to initialize gscam stream!");
422  break;
423  }
424 
425  // Block while publishing
426  this->publish_stream();
427 
428  this->cleanup_stream();
429 
430  ROS_INFO("GStreamer stream stopped!");
431 
432  if(reopen_on_eof_) {
433  ROS_INFO("Reopening stream...");
434  } else {
435  ROS_INFO("Cleaning up stream and exiting...");
436  break;
437  }
438  }
439 
440  }
441 
442  // Example callbacks for appsink
443  // TODO: enable callback-based capture
444  void gst_eos_cb(GstAppSink *appsink, gpointer user_data ) {
445  }
446  GstFlowReturn gst_new_preroll_cb(GstAppSink *appsink, gpointer user_data ) {
447  return GST_FLOW_OK;
448  }
449  GstFlowReturn gst_new_asample_cb(GstAppSink *appsink, gpointer user_data ) {
450  return GST_FLOW_OK;
451  }
452 
453 }
void gst_eos_cb(GstAppSink *appsink, gpointer user_data)
Definition: gscam.cpp:444
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
#define ROS_FATAL(...)
bool sync_sink_
Definition: gscam.h:43
sensor_msgs::CameraInfo getCameraInfo(void)
void publish(const boost::shared_ptr< M > &message) const
std::string gsconfig_
Definition: gscam.h:36
GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private)
Definition: gscam.cpp:32
GstElement * pipeline_
Definition: gscam.h:39
int height_
Definition: gscam.h:50
bool loadCameraInfo(const std::string &url)
ros::NodeHandle nh_
Definition: gscam.h:58
bool use_gst_timestamps_
Definition: gscam.h:46
bool configure()
Definition: gscam.cpp:47
GstFlowReturn gst_new_preroll_cb(GstAppSink *appsink, gpointer user_data)
Definition: gscam.cpp:446
ros::Publisher jpeg_pub_
Definition: gscam.h:63
ros::Publisher cinfo_pub_
Definition: gscam.h:64
bool validateURL(const std::string &url)
bool preroll_
Definition: gscam.h:44
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
double time_offset_
Definition: gscam.h:57
bool init_stream()
Definition: gscam.cpp:110
image_transport::ImageTransport image_transport_
Definition: gscam.h:59
#define ROS_FATAL_STREAM(args)
GstElement * sink_
Definition: gscam.h:40
bool reopen_on_eof_
Definition: gscam.h:45
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
camera_info_manager::CameraInfoManager camera_info_manager_
Definition: gscam.h:60
void cleanup_stream()
Definition: gscam.cpp:402
std::string image_encoding_
Definition: gscam.h:51
Definition: gscam.h:20
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
std::string camera_info_url_
Definition: gscam.h:53
int width_
Definition: gscam.h:50
void run()
Definition: gscam.cpp:413
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
static Time now()
image_transport::CameraPublisher camera_pub_
Definition: gscam.h:61
void publish_stream()
Definition: gscam.cpp:232
std::string frame_id_
Definition: gscam.h:49
GstFlowReturn gst_new_asample_cb(GstAppSink *appsink, gpointer user_data)
Definition: gscam.cpp:449
ROSCPP_DECL void spinOnce()
ros::NodeHandle nh_private_
Definition: gscam.h:58
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
bool setCameraName(const std::string &cname)
#define ROS_DEBUG(...)
std::string camera_name_
Definition: gscam.h:52


gscam
Author(s): Jonathan Bohren , Graylin Trevor Jay , Christopher Crick
autogenerated on Mon Jun 10 2019 13:26:59