sensor2D.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * Copyright (c) 2014
00003  * VoXel Interaction Design GmbH
00004  *
00005  * @author Angel Merino Sastre
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy
00008  * of this software and associated documentation files (the "Software"), to deal
00009  * in the Software without restriction, including without limitation the rights
00010  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011  * copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in
00015  * all copies or substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00020  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023  * THE SOFTWARE.
00024  *
00025  ******************************************************************************/
00026 
00042 #include <bta_ros/sensor2D.hpp>
00043 
00044 namespace bta_ros 
00045 {
00046         Sensor2D::Sensor2D(ros::NodeHandle nh_camera, 
00047                                                                                 ros::NodeHandle nh_private, 
00048                                                                                 std::string nodeName) : 
00049                 nh_(nh_camera),
00050                 nh_private_(nh_private),
00051                 it_(nh_camera),
00052                 cim_rgb_(nh_camera),
00053                 nodeName_(nodeName),
00054                 address_("192.168.0.10")
00055         {
00056         
00057                 if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 
00058                                                                         ros::console::levels::Debug) ) {
00059                         ros::console::notifyLoggerLevelsChanged();
00060                 }
00061                 
00062         }
00063         
00064         Sensor2D::~Sensor2D() 
00065         {
00066                 stop();
00067         }
00068         
00069         static void     cb_new_pad (GstElement *element,
00070                                                         GstPad     *pad,
00071                                                         gpointer    data)
00072         {
00073           GstElement* elem = ( GstElement* ) data;
00074           GstPad *sinkpad;
00075            GstPadLinkReturn lres;
00076 
00077           sinkpad = gst_element_get_static_pad ( elem , "sink" );
00078           g_assert (sinkpad);
00079           lres == gst_pad_link ( pad, sinkpad ); 
00080           g_assert (GST_PAD_LINK_SUCCESSFUL(lres));
00081           gst_object_unref( sinkpad );
00082         }
00083 
00084         void Sensor2D::init() {
00085                 GstElement *souphttpsrc, 
00086                         *sdpdemux, *videodepay, 
00087                         *videodecoder, *videoconvert, 
00088                         *filter;
00089                 //GstElement *pipeline_;
00090         
00091                 gboolean res;
00092                 GstPadLinkReturn lres;
00093                 GstPad *srcpad, *sinkpad;
00094         
00095                 //GstPadTemplate *teePad;
00096         
00097           if(!nh_private_.getParam(nodeName_+"/2dURL",address_))
00098                         ROS_INFO_STREAM(
00099                                 "No ip for download sdp file given. Trying with default: " << address_);
00100                 
00101                 // always init first 
00102                 gst_init (NULL,NULL);
00103         
00104                 // the pipeline_ to hold everything 
00105                 pipeline_ = gst_pipeline_new ("pipeline");
00106                 g_assert (pipeline_);
00107         
00108                 souphttpsrc = gst_element_factory_make ("souphttpsrc", "sdphttpsrc");
00109                 g_assert (souphttpsrc);
00110                 g_object_set (souphttpsrc, "location", address_.c_str(), NULL);
00111         
00112                 sdpdemux = gst_element_factory_make ("sdpdemux", NULL);
00113                 g_assert (sdpdemux);
00114                 //g_object_set (sdpdemux, "debug", TRUE, NULL);
00115                 //g_object_set (sdpdemux, "redirect", FALSE, NULL);
00116                 //g_object_set (sdpdemux, "latency", 0, NULL);
00117         
00118                 // the depayloading 
00119                 videodepay = gst_element_factory_make ("rtph264depay", NULL);
00120                 g_assert (videodepay);
00121         
00122                 /* the decoding */
00123                 videodecoder = gst_element_factory_make ("avdec_h264", NULL);
00124                 g_assert (videodecoder);
00125                 g_object_set (videodecoder, "skip-frame", 5, NULL);
00126 
00127                 /*tee = gst_element_factory_make ("tee", NULL);
00128                 g_assert (tee);*/
00129         
00130                 // queue for local visualization
00131                 /*queueLocal = gst_element_factory_make ("queue", NULL);
00132                 g_assert (queueLocal);*/
00133         
00134                 /*queueApp = gst_element_factory_make ("queue", NULL);
00135                 g_assert (queueApp);*/
00136         
00137                 /*videoflip = gst_element_factory_make ("videoflip", NULL);
00138                 g_assert (videoflip);
00139                 g_object_set (videoflip, "method", 1, NULL);
00140         
00141                 videocrop = gst_element_factory_make ("videocrop", NULL);
00142                 g_assert (videocrop);
00143                 g_object_set (videocrop, "top", 0, NULL);
00144                 g_object_set (videocrop, "left", 0, NULL);
00145                 g_object_set (videocrop, "right", 0, NULL);
00146                 g_object_set (videocrop, "bottom", 0, NULL);*/
00147         
00148                 // video playback
00149                 /*videosink = gst_element_factory_make ("autovideosink", NULL);
00150                 g_assert (videosink);
00151                 g_object_set (videosink, "sync", FALSE, NULL);*/
00152         
00153                 videoconvert =gst_element_factory_make("videoconvert", NULL);
00154                 g_assert (videoconvert);
00155         
00156                 filter = gst_element_factory_make ("capsfilter", "filter");
00157                         g_assert (filter);
00158                 GstCaps *filtercaps;
00159                 filtercaps = gst_caps_new_simple ("video/x-raw",
00160                                          "format", G_TYPE_STRING, "BGR",
00161                                          NULL);
00162                         g_object_set (G_OBJECT (filter), "caps", filtercaps, NULL);
00163                         gst_caps_unref (filtercaps);
00164         
00165                 appsink = gst_element_factory_make("appsink", NULL);
00166                 g_assert (appsink);
00167                 //g_object_set (appsink, "emit-signals", TRUE, NULL);
00168                 g_object_set (appsink, "drop", TRUE, NULL);
00169                 g_object_set (appsink, "max-buffers", 1, NULL);
00170         
00171                 // add depayloading and playback to the pipeline_ and link 
00172                 gst_bin_add_many (GST_BIN (pipeline_),
00173                                         souphttpsrc,
00174                                         sdpdemux,
00175                                         videodepay, 
00176                                         videodecoder,
00177                                         videoconvert,
00178                                         filter, 
00179                                         appsink,
00180                                         NULL);
00181         
00182                 res = gst_element_link (souphttpsrc, sdpdemux);
00183                 g_assert (res == TRUE);
00184                 //g_print ("link %d result \n", res);
00185         
00186                 g_signal_connect (sdpdemux, "pad-added", G_CALLBACK (cb_new_pad), videodepay);
00187         
00188                 res = gst_element_link_many (videodepay, videodecoder, videoconvert, filter, appsink, NULL);
00189                 g_assert (res == TRUE);
00190         
00191         
00192                 //g_signal_connect(appsink, "new-sample", G_CALLBACK(newFrame), NULL);
00193         
00194                 //GstElement *session = sdpdemux.session;
00195                 // the RTP pad that we have to connect to the depayloader will be created
00196                 // dynamically so we connect to the pad-added signal, pass the depayloader as
00197                 // user_data so that we can link to it.
00198                 //printf("rtpbin: %p\n",rtpbin);
00199                 //g_signal_connect (rtpbin, "pad-added", G_CALLBACK (pad_added_cb), videodepay);
00200 
00201                 // give some stats when we receive RTCP 
00202                 //g_signal_connect (sdpdemux, "on-ssrc-active", G_CALLBACK (on_ssrc_active_cb), videodepay);
00203         
00204                 // set the pipeline_ to playing 
00205                 ROS_INFO ("starting receiver pipeline");
00206                 
00207                 gst_element_set_state(pipeline_, GST_STATE_PAUSED);
00208                 if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
00209                         ROS_FATAL("Failed to PAUSE stream, check your configuration.");
00210                         ros::shutdown();
00211                         return;
00212                 } else {
00213                         ROS_DEBUG("Stream is PAUSED.");
00214                 }       
00215                 gst_element_set_state (pipeline_, GST_STATE_PLAYING);
00216                 if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
00217                         ROS_WARN("Failed to PLAY stream.");
00218                 }
00219                 //loop = g_main_loop_new (NULL, FALSE);
00220                 //g_main_loop_run (loop);
00221         
00222          {
00223                 // Advertise all published topics
00224                         cim_rgb_.setCameraName(nodeName_);
00225                         if(cim_rgb_.validateURL(
00226                                         /*camera_info_url_*/"package://bta_ros/calib.yml")) {
00227                                 cim_rgb_.loadCameraInfo("package://bta_ros/calib.yml");
00228                                 ROS_INFO_STREAM("Loaded camera calibration from " << 
00229                                         "package://bta_ros/calib.yml"   );
00230                         } else {
00231                                 ROS_WARN_STREAM("Camera info at: " <<
00232                                         "package://bta_ros/calib.yml" <<
00233                                         " not found. Using an uncalibrated config.");
00234                         }       
00235         
00236                         pub_rgb_ = it_.advertiseCamera(nodeName_ + "/sensor2d/image_raw", 1);
00237 
00238                         //sub_amp_ = nh_private_.subscribe("bta_node_amp", 1, &BtaRos::ampCb, this);
00239                         //sub_dis_ = nh_private_.subscribe("bta_node_dis", 1, &BtaRos::disCb, this);
00240                 }
00241                 GstState state;
00242                 while (nh_.ok() && !ros::isShuttingDown()) {
00243                         gst_element_get_state(pipeline_, &state, NULL, -1);
00244                         if (state == GST_STATE_PLAYING)
00245                                 getFrame();
00246                         else {
00247                                 ROS_INFO("Stream STOPPED. Reconnecting");
00248                                 gst_element_set_state (pipeline_, GST_STATE_NULL);
00249                                 if (gst_element_set_state (pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
00250                                         ROS_ERROR("Failed to PLAY stream.");
00251                                 }
00252                         }
00253                         ros::spinOnce ();       
00254                 }
00255                 
00256                 //stop();
00257         
00258                 return;
00259         }
00260 
00261         void Sensor2D::stop() {
00262                 g_print ("Returned, stopping playback\n");
00263                 gst_element_set_state (pipeline_, GST_STATE_NULL);
00264         
00265                 g_print ("Deleting pipeline_\n");
00266                 gst_object_unref (GST_OBJECT (pipeline_));
00267 
00268                 return;
00269         }
00270 
00271         void Sensor2D::getFrame() {
00272                 GstSample *sample;
00273         
00274                 sample = gst_app_sink_pull_sample((GstAppSink *)appsink);
00275         
00276                 if(sample != NULL) {
00277                         ROS_DEBUG("             frame 2D Arrived");
00278                         sensor_msgs::ImagePtr rgb (new sensor_msgs::Image);
00279                   GstBuffer* sampleBuffer = NULL;
00280                         GstStructure *s;
00281                         gint width, height;
00282                         GstMapInfo bufferInfo;
00283                         gboolean res;
00284                         GstCaps *caps;
00285                   
00286                 caps = gst_sample_get_caps (sample);
00287                 s = gst_caps_get_structure (caps, 0);
00288                 res = gst_structure_get_int (s, "width", &width);
00289                         res |= gst_structure_get_int (s, "height", &height);
00290                         if (!res) {
00291                                 ROS_DEBUG ("could not get snapshot dimension\n");
00292                                 return;
00293                         }
00294                         
00295       sampleBuffer = gst_sample_get_buffer(sample);
00296       if(sampleBuffer != NULL)
00297       {
00298           gst_buffer_map(sampleBuffer, &bufferInfo, GST_MAP_READ);
00299                                         //dis->header.seq = frame->frameCounter;
00300                 //dis->header.stamp.sec = frame->timeStamp;
00301                 rgb->height = height;
00302                 rgb->width = width;
00303                 rgb->encoding = sensor_msgs::image_encodings::BGR8;
00304                 rgb->step = width*(sizeof(unsigned char)*3);
00305                 rgb->data.resize(bufferInfo.size);
00306                 memcpy ( &rgb->data[0], bufferInfo.data, bufferInfo.size );
00307 
00308          sensor_msgs::CameraInfoPtr ci_rgb(
00309                         new sensor_msgs::CameraInfo(cim_rgb_.getCameraInfo()));
00310                                         ci_rgb->header.frame_id = nodeName_+"/sensor2d";
00311                                         rgb->header.frame_id = nodeName_+"/sensor2d";
00312 
00313                                         pub_rgb_.publish(rgb,ci_rgb);
00314       }
00315       gst_buffer_unmap (sampleBuffer, &bufferInfo);
00316                   gst_sample_unref(sample);
00317                  }
00318                   return;
00319         }
00320         
00321 }
00322 


bta_ros
Author(s): Angel Merino , Simon Vogl
autogenerated on Thu Jun 2 2016 04:07:04