$search
00001 #include <stdlib.h> 00002 #include <unistd.h> 00003 00004 #include <iostream> 00005 extern "C"{ 00006 #include <gst/gst.h> 00007 #include <gst/app/gstappsink.h> 00008 } 00009 00010 #include <ros/ros.h> 00011 #include <image_transport/image_transport.h> 00012 #include <sensor_msgs/Image.h> 00013 #include <sensor_msgs/CameraInfo.h> 00014 #include <sensor_msgs/SetCameraInfo.h> 00015 #include <camera_calibration_parsers/parse_ini.h> 00016 00017 #include <unistd.h> 00018 #include <sys/ipc.h> 00019 #include <sys/shm.h> 00020 00021 //forward declarations 00022 static gboolean processData(GstPad *pad, GstBuffer *buffer, gpointer u_data); 00023 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp); 00024 00025 //globals 00026 bool gstreamerPad, rosPad; 00027 int width, height; 00028 sensor_msgs::CameraInfo camera_info; 00029 00030 int main(int argc, char** argv) { 00031 char *config = getenv("GSCAM_CONFIG"); 00032 if (config == NULL) { 00033 std::cout << "Problem getting GSCAM_CONFIG variable." << std::endl; 00034 exit(-1); 00035 } 00036 00037 gst_init(0,0); 00038 std::cout << "Gstreamer Version: " << gst_version_string() << std::endl; 00039 00040 GError *error = 0; //assignment to zero is a gst requirement 00041 GstElement *pipeline = gst_parse_launch(config,&error); 00042 if (pipeline == NULL) { 00043 std::cout << error->message << std::endl; 00044 exit(-1); 00045 } 00046 GstElement * sink = gst_element_factory_make("appsink",NULL); 00047 GstCaps * caps = gst_caps_new_simple("video/x-raw-rgb", NULL); 00048 gst_app_sink_set_caps(GST_APP_SINK(sink), caps); 00049 gst_caps_unref(caps); 00050 00051 gst_base_sink_set_sync(GST_BASE_SINK(sink), TRUE); 00052 00053 if(GST_IS_PIPELINE(pipeline)) { 00054 GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline), GST_PAD_SRC); 00055 g_assert(outpad); 00056 GstElement *outelement = gst_pad_get_parent_element(outpad); 00057 g_assert(outelement); 00058 gst_object_unref(outpad); 00059 00060 00061 if(!gst_bin_add(GST_BIN(pipeline), sink)) { 00062 fprintf(stderr, "gst_bin_add() failed\n"); // TODO: do some unref 00063 gst_object_unref(outelement); 00064 gst_object_unref(pipeline); 00065 return -1; 00066 } 00067 00068 if(!gst_element_link(outelement, sink)) { 00069 fprintf(stderr, "GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement)); 00070 gst_object_unref(outelement); 00071 gst_object_unref(pipeline); 00072 return -1; 00073 } 00074 00075 gst_object_unref(outelement); 00076 } else { 00077 GstElement* launchpipe = pipeline; 00078 pipeline = gst_pipeline_new(NULL); 00079 g_assert(pipeline); 00080 00081 gst_object_unparent(GST_OBJECT(launchpipe)); 00082 00083 gst_bin_add_many(GST_BIN(pipeline), launchpipe, sink, NULL); 00084 00085 if(!gst_element_link(launchpipe, sink)) { 00086 fprintf(stderr, "GStreamer: cannot link launchpipe -> sink\n"); 00087 gst_object_unref(pipeline); 00088 return -1; 00089 } 00090 } 00091 00092 gst_element_set_state(pipeline, GST_STATE_PAUSED); 00093 00094 if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 00095 std::cout << "Failed to PAUSE." << std::endl; 00096 exit(-1); 00097 } else { 00098 std::cout << "stream is PAUSED." << std::endl; 00099 } 00100 00101 // We could probably do something with the camera name, check 00102 // errors or something, but at the moment, we don't care. 00103 std::string camera_name; 00104 if (camera_calibration_parsers::readCalibrationIni("../camera_parameters.txt", camera_name, camera_info)) { 00105 ROS_INFO("Successfully read camera calibration. Rerun camera calibrator if it is incorrect."); 00106 } 00107 else { 00108 ROS_ERROR("No camera_parameters.txt file found. Use default file if no other is available."); 00109 } 00110 00111 ros::init(argc, argv, "gscam_publisher"); 00112 ros::NodeHandle nh; 00113 00114 int preroll; 00115 nh.param("brown/gscam/preroll", preroll, 0); 00116 if (preroll) { 00117 //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll 00118 //I am told this is needed and am erring on the side of caution. 00119 gst_element_set_state(pipeline, GST_STATE_PLAYING); 00120 00121 if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 00122 std::cout << "Failed to PLAY." << std::endl; 00123 exit(-1); 00124 } else { 00125 std::cout << "stream is PLAYING." << std::endl; 00126 } 00127 00128 gst_element_set_state(pipeline, GST_STATE_PAUSED); 00129 00130 if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 00131 std::cout << "Failed to PAUSE." << std::endl; 00132 exit(-1); 00133 } else { 00134 std::cout << "stream is PAUSED." << std::endl; 00135 } 00136 } 00137 00138 image_transport::ImageTransport it(nh); 00139 image_transport::CameraPublisher pub = it.advertiseCamera("gscam/image_raw", 1); 00140 00141 ros::ServiceServer set_camera_info = nh.advertiseService("gscam/set_camera_info", setCameraInfo); 00142 00143 std::cout << "Processing..." << std::endl; 00144 00145 //processVideo 00146 rosPad = false; 00147 gstreamerPad = true; 00148 gst_element_set_state(pipeline, GST_STATE_PLAYING); 00149 while(nh.ok()) { 00150 // This should block until a new frame is awake, this way, we'll run at the 00151 // actual capture framerate of the device. 00152 GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink)); 00153 if (!buf) break; 00154 00155 GstPad* pad = gst_element_get_static_pad(sink, "sink"); 00156 const GstCaps *caps = gst_pad_get_negotiated_caps(pad); 00157 GstStructure *structure = gst_caps_get_structure(caps,0); 00158 gst_structure_get_int(structure,"width",&width); 00159 gst_structure_get_int(structure,"height",&height); 00160 00161 sensor_msgs::Image msg; 00162 msg.width = width; 00163 msg.height = height; 00164 msg.encoding = "rgb8"; 00165 msg.is_bigendian = false; 00166 msg.step = width*3; 00167 msg.data.resize(width*height*3); 00168 std::copy(buf->data, buf->data+(width*height*3), msg.data.begin()); 00169 00170 pub.publish(msg, camera_info); 00171 00172 gst_buffer_unref(buf); 00173 00174 ros::spinOnce(); 00175 00176 } 00177 00178 //close out 00179 std::cout << "\nquitting..." << std::endl; 00180 gst_element_set_state(pipeline, GST_STATE_NULL); 00181 gst_object_unref(pipeline); 00182 00183 return 0; 00184 } 00185 00186 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp) { 00187 00188 ROS_INFO("New camera info received"); 00189 camera_info = req.camera_info; 00190 00191 if (camera_calibration_parsers::writeCalibrationIni("../camera_parameters.txt", "gscam", camera_info)) { 00192 ROS_INFO("Camera information written to camera_parameters.txt"); 00193 return true; 00194 } 00195 else { 00196 ROS_ERROR("Could not write camera_parameters.txt"); 00197 return false; 00198 } 00199 }