gscam.cpp
Go to the documentation of this file.
00001 
00002 #include <stdlib.h>
00003 #include <unistd.h>
00004 #include <sys/ipc.h>
00005 #include <sys/shm.h>
00006 
00007 
00008 #include <iostream>
00009 extern "C"{
00010 #include <gst/gst.h>
00011 #include <gst/app/gstappsink.h>
00012 }
00013 
00014 #include <ros/ros.h>
00015 
00016 #include <image_transport/image_transport.h>
00017 #include <camera_info_manager/camera_info_manager.h>
00018 
00019 
00020 #include <sensor_msgs/Image.h>
00021 #include <sensor_msgs/CompressedImage.h>
00022 #include <sensor_msgs/CameraInfo.h>
00023 #include <sensor_msgs/SetCameraInfo.h>
00024 #include <sensor_msgs/image_encodings.h>
00025 
00026 #include <camera_calibration_parsers/parse_ini.h>
00027 
00028 #include <gscam/gscam.h>
00029 
00030 namespace gscam {
00031 
00032   GSCam::GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private) :
00033     gsconfig_(""),
00034     pipeline_(NULL),
00035     sink_(NULL),
00036     nh_(nh_camera),
00037     nh_private_(nh_private),
00038     image_transport_(nh_camera),
00039     camera_info_manager_(nh_camera)
00040   {
00041   }
00042 
00043   GSCam::~GSCam()
00044   {
00045   }
00046 
00047   bool GSCam::configure()
00048   {
00049     // Get gstreamer configuration
00050     // (either from environment variable or ROS param)
00051     std::string gsconfig_rosparam = "";
00052     bool gsconfig_rosparam_defined = false;
00053     char *gsconfig_env = NULL;
00054 
00055     gsconfig_rosparam_defined = nh_private_.getParam("gscam_config",gsconfig_rosparam);
00056     gsconfig_env = getenv("GSCAM_CONFIG");
00057 
00058     if (!gsconfig_env && !gsconfig_rosparam_defined) {
00059       ROS_FATAL( "Problem getting GSCAM_CONFIG environment variable and 'gscam_config' rosparam is not set. This is needed to set up a gstreamer pipeline." );
00060       return false;
00061     } else if(gsconfig_env && gsconfig_rosparam_defined) {
00062       ROS_FATAL( "Both GSCAM_CONFIG environment variable and 'gscam_config' rosparam are set. Please only define one." );
00063       return false;
00064     } else if(gsconfig_env) {
00065       gsconfig_ = gsconfig_env;
00066       ROS_INFO_STREAM("Using gstreamer config from env: \""<<gsconfig_env<<"\"");
00067     } else if(gsconfig_rosparam_defined) {
00068       gsconfig_ = gsconfig_rosparam;
00069       ROS_INFO_STREAM("Using gstreamer config from rosparam: \""<<gsconfig_rosparam<<"\"");
00070     }
00071 
00072     // Get additional gscam configuration
00073     nh_private_.param("sync_sink", sync_sink_, true);
00074     nh_private_.param("preroll", preroll_, false);
00075     nh_private_.param("use_gst_timestamps", use_gst_timestamps_, false);
00076 
00077     nh_private_.param("reopen_on_eof", reopen_on_eof_, false);
00078 
00079     // Get the camera parameters file
00080     nh_private_.getParam("camera_info_url", camera_info_url_);
00081     nh_private_.getParam("camera_name", camera_name_);
00082 
00083     // Get the image encoding
00084     nh_private_.param("image_encoding", image_encoding_, sensor_msgs::image_encodings::RGB8);
00085     if (image_encoding_ != sensor_msgs::image_encodings::RGB8 &&
00086         image_encoding_ != sensor_msgs::image_encodings::MONO8 && 
00087         image_encoding_ != "jpeg") {
00088       ROS_FATAL_STREAM("Unsupported image encoding: " + image_encoding_);
00089     }
00090 
00091     camera_info_manager_.setCameraName(camera_name_);
00092 
00093     if(camera_info_manager_.validateURL(camera_info_url_)) {
00094       camera_info_manager_.loadCameraInfo(camera_info_url_);
00095       ROS_INFO_STREAM("Loaded camera calibration from "<<camera_info_url_);
00096     } else {
00097       ROS_WARN_STREAM("Camera info at: "<<camera_info_url_<<" not found. Using an uncalibrated config.");
00098     }
00099 
00100     // Get TF Frame
00101     if(!nh_private_.getParam("frame_id",frame_id_)){
00102       frame_id_ = "/camera_frame";
00103       ROS_WARN_STREAM("No camera frame_id set, using frame \""<<frame_id_<<"\".");
00104       nh_private_.setParam("frame_id",frame_id_);
00105     }
00106 
00107     return true;
00108   }
00109 
00110   bool GSCam::init_stream()
00111   {
00112     if(!gst_is_initialized()) {
00113       // Initialize gstreamer pipeline
00114       ROS_DEBUG_STREAM( "Initializing gstreamer..." );
00115       gst_init(0,0);
00116     }
00117 
00118     ROS_DEBUG_STREAM( "Gstreamer Version: " << gst_version_string() );
00119 
00120     GError *error = 0; // Assignment to zero is a gst requirement
00121 
00122     pipeline_ = gst_parse_launch(gsconfig_.c_str(), &error);
00123     if (pipeline_ == NULL) {
00124       ROS_FATAL_STREAM( error->message );
00125       return false;
00126     }
00127 
00128     // Create RGB sink
00129     sink_ = gst_element_factory_make("appsink",NULL);
00130     GstCaps * caps = NULL;
00131     if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
00132         caps = gst_caps_new_simple("video/x-raw-rgb", NULL); 
00133     } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) {
00134         caps = gst_caps_new_simple("video/x-raw-gray", NULL);
00135     } else if (image_encoding_ == "jpeg") {
00136         caps = gst_caps_new_simple("image/jpeg", NULL);
00137     }
00138     gst_app_sink_set_caps(GST_APP_SINK(sink_), caps);
00139     gst_caps_unref(caps);
00140 
00141     // Set whether the sink should sync
00142     // Sometimes setting this to true can cause a large number of frames to be
00143     // dropped
00144     gst_base_sink_set_sync(
00145         GST_BASE_SINK(sink_),
00146         (sync_sink_) ? TRUE : FALSE);
00147 
00148     if(GST_IS_PIPELINE(pipeline_)) {
00149       GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC);
00150       g_assert(outpad);
00151 
00152       GstElement *outelement = gst_pad_get_parent_element(outpad);
00153       g_assert(outelement);
00154       gst_object_unref(outpad);
00155 
00156       if(!gst_bin_add(GST_BIN(pipeline_), sink_)) {
00157         ROS_FATAL("gst_bin_add() failed");
00158         gst_object_unref(outelement);
00159         gst_object_unref(pipeline_);
00160         return false;
00161       }
00162 
00163       if(!gst_element_link(outelement, sink_)) {
00164         ROS_FATAL("GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement));
00165         gst_object_unref(outelement);
00166         gst_object_unref(pipeline_);
00167         return false;
00168       }
00169 
00170       gst_object_unref(outelement);
00171     } else {
00172       GstElement* launchpipe = pipeline_;
00173       pipeline_ = gst_pipeline_new(NULL);
00174       g_assert(pipeline_);
00175 
00176       gst_object_unparent(GST_OBJECT(launchpipe));
00177 
00178       gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, NULL);
00179 
00180       if(!gst_element_link(launchpipe, sink_)) {
00181         ROS_FATAL("GStreamer: cannot link launchpipe -> sink");
00182         gst_object_unref(pipeline_);
00183         return false;
00184       }
00185     }
00186 
00187     // Calibration between ros::Time and gst timestamps
00188     GstClock * clock = gst_system_clock_obtain();
00189     ros::Time now = ros::Time::now();
00190     GstClockTime ct = gst_clock_get_time(clock);
00191     gst_object_unref(clock);
00192     time_offset_ = now.toSec() - GST_TIME_AS_USECONDS(ct)/1e6;
00193     ROS_INFO("Time offset: %.3f",time_offset_);
00194 
00195     gst_element_set_state(pipeline_, GST_STATE_PAUSED);
00196 
00197     if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
00198       ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration.");
00199       return false;
00200     } else {
00201       ROS_DEBUG_STREAM("Stream is PAUSED.");
00202     }
00203 
00204     // Create ROS camera interface
00205     if (image_encoding_ == "jpeg") {
00206         jpeg_pub_ = nh_.advertise<sensor_msgs::CompressedImage>("camera/image_raw/compressed",1);
00207         cinfo_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera/camera_info",1);
00208     } else {
00209         camera_pub_ = image_transport_.advertiseCamera("camera/image_raw", 1);
00210     }
00211 
00212     return true;
00213   }
00214 
00215   void GSCam::publish_stream()
00216   {
00217     ROS_INFO_STREAM("Publishing stream...");
00218 
00219     // Pre-roll camera if needed
00220     if (preroll_) {
00221       ROS_DEBUG("Performing preroll...");
00222 
00223       //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
00224       //I am told this is needed and am erring on the side of caution.
00225       gst_element_set_state(pipeline_, GST_STATE_PLAYING);
00226       if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
00227         ROS_ERROR("Failed to PLAY during preroll.");
00228         return;
00229       } else {
00230         ROS_DEBUG("Stream is PLAYING in preroll.");
00231       }
00232 
00233       gst_element_set_state(pipeline_, GST_STATE_PAUSED);
00234       if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
00235         ROS_ERROR("Failed to PAUSE.");
00236         return;
00237       } else {
00238         ROS_INFO("Stream is PAUSED in preroll.");
00239       }
00240     }
00241 
00242     if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
00243       ROS_ERROR("Could not start stream!");
00244       return;
00245     }
00246     ROS_INFO("Started stream.");
00247 
00248     // Poll the data as fast a spossible
00249     while(ros::ok()) {
00250       // This should block until a new frame is awake, this way, we'll run at the
00251       // actual capture framerate of the device.
00252       // ROS_DEBUG("Getting data...");
00253       GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));
00254       GstClockTime bt = gst_element_get_base_time(pipeline_);
00255       // ROS_INFO("New buffer: timestamp %.6f %lu %lu %.3f",
00256       //         GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_);
00257 
00258 
00259 #if 0
00260       GstFormat fmt = GST_FORMAT_TIME;
00261       gint64 current = -1;
00262 
00263        Query the current position of the stream
00264       if (gst_element_query_position(pipeline_, &fmt, &current)) {
00265           ROS_INFO_STREAM("Position "<<current);
00266       }
00267 #endif
00268 
00269       // Stop on end of stream
00270       if (!buf) {
00271         ROS_INFO("Stream ended.");
00272         break;
00273       }
00274 
00275       // ROS_DEBUG("Got data.");
00276 
00277       // Get the image width and height
00278       GstPad* pad = gst_element_get_static_pad(sink_, "sink");
00279       const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
00280       GstStructure *structure = gst_caps_get_structure(caps,0);
00281       gst_structure_get_int(structure,"width",&width_);
00282       gst_structure_get_int(structure,"height",&height_);
00283 
00284       // Update header information
00285       sensor_msgs::CameraInfoPtr cinfo;
00286       cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
00287       if (use_gst_timestamps_) {
00288           cinfo->header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_);
00289       } else {
00290           cinfo->header.stamp = ros::Time::now();
00291       }
00292       // ROS_INFO("Image time stamp: %.3f",cinfo->header.stamp.toSec());
00293       cinfo->header.frame_id = frame_id_;
00294       if (image_encoding_ == "jpeg") {
00295           sensor_msgs::CompressedImagePtr img(new sensor_msgs::CompressedImage());
00296           img->header = cinfo->header;
00297           img->format = "jpeg";
00298           img->data.resize(buf->size);
00299           std::copy(buf->data, (buf->data)+(buf->size),
00300                   img->data.begin());
00301           jpeg_pub_.publish(img);
00302           cinfo_pub_.publish(cinfo);
00303       } else {
00304           // Complain if the returned buffer is smaller than we expect
00305           const unsigned int expected_frame_size =
00306               image_encoding_ == sensor_msgs::image_encodings::RGB8
00307               ? width_ * height_ * 3
00308               : width_ * height_;
00309 
00310           if (buf->size < expected_frame_size) {
00311               ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
00312                       << expected_frame_size << " bytes but got only "
00313                       << (buf->size) << " bytes. (make sure frames are correctly encoded)");
00314           }
00315 
00316           // Construct Image message
00317           sensor_msgs::ImagePtr img(new sensor_msgs::Image());
00318 
00319           img->header = cinfo->header;
00320 
00321           // Image data and metadata
00322           img->width = width_;
00323           img->height = height_;
00324           img->encoding = image_encoding_;
00325           img->is_bigendian = false;
00326           img->data.resize(expected_frame_size);
00327 
00328           // Copy only the data we received
00329           // Since we're publishing shared pointers, we need to copy the image so
00330           // we can free the buffer allocated by gstreamer
00331           if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
00332               img->step = width_ * 3;
00333           } else {
00334               img->step = width_;
00335           }
00336           std::copy(
00337                   buf->data,
00338                   (buf->data)+(buf->size),
00339                   img->data.begin());
00340 
00341           // Publish the image/info
00342           camera_pub_.publish(img, cinfo);
00343       }
00344 
00345       // Release the buffer
00346       gst_buffer_unref(buf);
00347 
00348       ros::spinOnce();
00349     }
00350   }
00351 
00352   void GSCam::cleanup_stream()
00353   {
00354     // Clean up
00355     ROS_INFO("Stopping gstreamer pipeline...");
00356     if(pipeline_) {
00357       gst_element_set_state(pipeline_, GST_STATE_NULL);
00358       gst_object_unref(pipeline_);
00359       pipeline_ = NULL;
00360     }
00361   }
00362 
00363   void GSCam::run() {
00364     while(ros::ok()) {
00365       if(!this->configure()) {
00366         ROS_FATAL("Failed to configure gscam!");
00367         break;
00368       }
00369 
00370       if(!this->init_stream()) {
00371         ROS_FATAL("Failed to initialize gscam stream!");
00372         break;
00373       }
00374 
00375       // Block while publishing
00376       this->publish_stream();
00377 
00378       this->cleanup_stream();
00379 
00380       ROS_INFO("GStreamer stream stopped!");
00381 
00382       if(reopen_on_eof_) {
00383         ROS_INFO("Reopening stream...");
00384       } else {
00385         ROS_INFO("Cleaning up stream and exiting...");
00386         break;
00387       }
00388     }
00389 
00390   }
00391 
00392   // Example callbacks for appsink
00393   // TODO: enable callback-based capture
00394   void gst_eos_cb(GstAppSink *appsink, gpointer user_data ) {
00395   }
00396   GstFlowReturn gst_new_preroll_cb(GstAppSink *appsink, gpointer user_data ) {
00397     return GST_FLOW_OK;
00398   }
00399   GstFlowReturn gst_new_asample_cb(GstAppSink *appsink, gpointer user_data ) {
00400     return GST_FLOW_OK;
00401   }
00402 
00403 }


gscam
Author(s): Jonathan Bohren , Graylin Trevor Jay , Christopher Crick
autogenerated on Mon Oct 6 2014 00:17:38