/* Augments the supplied pipeline with the source elements: the actual
* ShellRecorderSrc element where we inject frames then additional elements
* to convert the output into something palatable.
*/
static gboolean
recorder_pipeline_add_source (RecorderPipeline *pipeline)
{
GstPad *sink_pad = NULL, *src_pad = NULL;
gboolean result = FALSE;
GstElement *videoconvert;
sink_pad = gst_bin_find_unlinked_pad (GST_BIN (pipeline->pipeline), GST_PAD_SINK);
if (sink_pad == NULL)
{
g_warning("ShellRecorder: pipeline has no unlinked sink pad");
goto out;
}
pipeline->src = gst_element_factory_make ("shellrecordersrc", NULL);
if (pipeline->src == NULL)
{
g_warning ("Can't create recorder source element");
goto out;
}
gst_bin_add (GST_BIN (pipeline->pipeline), pipeline->src);
recorder_pipeline_set_caps (pipeline);
/* The videoconvert element is a generic converter; it will convert
* our supplied fixed format data into whatever the encoder wants
*/
videoconvert = gst_element_factory_make ("videoconvert", NULL);
if (!videoconvert)
{
g_warning("Can't create videoconvert element");
goto out;
}
gst_bin_add (GST_BIN (pipeline->pipeline), videoconvert);
gst_element_link_many (pipeline->src, videoconvert, NULL);
src_pad = gst_element_get_static_pad (videoconvert, "src");
if (!src_pad)
{
g_warning("ShellRecorder: can't get src pad to link into pipeline");
goto out;
}
if (gst_pad_link (src_pad, sink_pad) != GST_PAD_LINK_OK)
{
g_warning("ShellRecorder: can't link to sink pad");
goto out;
}
result = TRUE;
out:
if (sink_pad)
gst_object_unref (sink_pad);
if (src_pad)
gst_object_unref (src_pad);
return result;
}
/**
* gst_camerabin_try_add_element:
* @bin: tries adding an element to this bin
* @new_elem: new element to be added
*
* Adds given element to given @bin. Looks for an unconnected src pad
* from the @bin and links the element to it.
*
* Returns: %TRUE if adding and linking succeeded, %FALSE otherwise.
*/
gboolean
gst_camerabin_try_add_element (GstBin * bin, GstElement * new_elem)
{
GstPad *bin_pad;
GstElement *bin_elem;
gboolean ret = TRUE;
if (!bin || !new_elem) {
return FALSE;
}
/* Get pads for linking */
bin_pad = gst_bin_find_unlinked_pad (bin, GST_PAD_SRC);
/* Add to bin */
gst_bin_add (GST_BIN (bin), new_elem);
/* Link, if unconnected pad was found, otherwise just add it to bin */
if (bin_pad) {
GST_DEBUG_OBJECT (bin, "linking %s to %s:%s", GST_OBJECT_NAME (new_elem),
GST_DEBUG_PAD_NAME (bin_pad));
bin_elem = gst_pad_get_parent_element (bin_pad);
gst_object_unref (bin_pad);
if (!gst_element_link (bin_elem, new_elem)) {
gst_bin_remove (bin, new_elem);
ret = FALSE;
}
gst_object_unref (bin_elem);
} else {
GST_INFO_OBJECT (bin, "no unlinked source pad in bin");
}
return ret;
}
/**
* gst_camerabin_try_add_element:
* @bin: tries adding an element to this bin
* @srcpad: src pad name, or NULL for any
* @new_elem: new element to be added
* @dstpad: dst pad name, or NULL for any
*
* Adds given element to given @bin. Looks for an unconnected src pad
* (with name @srcpad, if specified) from the @bin and links the element to
* it.
*
* Returns: %TRUE if adding and linking succeeded, %FALSE otherwise.
*/
gboolean
gst_camerabin_try_add_element (GstBin * bin, const gchar * srcpad,
GstElement * new_elem, const gchar * dstpad)
{
GstPad *bin_pad;
GstElement *bin_elem;
gboolean ret = TRUE;
g_return_val_if_fail (bin, FALSE);
g_return_val_if_fail (new_elem, FALSE);
/* Get pads for linking */
bin_pad = gst_bin_find_unlinked_pad (bin, GST_PAD_SRC);
/* Add to bin */
gst_bin_add (GST_BIN (bin), new_elem);
/* Link, if unconnected pad was found, otherwise just add it to bin */
if (bin_pad) {
GST_DEBUG_OBJECT (bin, "linking %s to %s:%s", GST_OBJECT_NAME (new_elem),
GST_DEBUG_PAD_NAME (bin_pad));
bin_elem = gst_pad_get_parent_element (bin_pad);
gst_object_unref (bin_pad);
if (!gst_element_link_pads_full (bin_elem, srcpad, new_elem, dstpad,
GST_PAD_LINK_CHECK_CAPS)) {
gst_object_ref (new_elem);
gst_bin_remove (bin, new_elem);
ret = FALSE;
}
gst_object_unref (bin_elem);
} else {
GST_INFO_OBJECT (bin, "no unlinked source pad in bin");
}
return ret;
}
static void show_unlinked_pads(GstBin *bin)
{
GstPad *pad = gst_bin_find_unlinked_pad(bin, GST_PAD_SRC);
if (pad)
{
gchar *pad_name = gst_pad_get_name(pad);
g_print("Pad %s is not linked.\n", pad_name);
g_free(pad_name);
}
}
/* Augments the supplied pipeline with a sink element to write to the output
* file, if necessary.
*/
static gboolean
recorder_pipeline_add_sink (RecorderPipeline *pipeline)
{
GstPad *sink_pad = NULL, *src_pad = NULL;
GstElement *fdsink;
gboolean result = FALSE;
src_pad = gst_bin_find_unlinked_pad (GST_BIN (pipeline->pipeline), GST_PAD_SRC);
if (src_pad == NULL)
{
/* Nothing to do - assume that we were given a complete pipeline */
return TRUE;
}
pipeline->outfile = recorder_open_outfile (pipeline->recorder,
&pipeline->filename);
if (pipeline->outfile == -1)
goto out;
fdsink = gst_element_factory_make ("fdsink", NULL);
if (fdsink == NULL)
{
g_warning("Can't create fdsink element");
goto out;
}
gst_bin_add (GST_BIN (pipeline->pipeline), fdsink);
g_object_set (fdsink, "fd", pipeline->outfile, NULL);
sink_pad = gst_element_get_static_pad (fdsink, "sink");
if (!sink_pad)
{
g_warning("ShellRecorder: can't get sink pad to link pipeline output");
goto out;
}
if (gst_pad_link (src_pad, sink_pad) != GST_PAD_LINK_OK)
{
g_warning("ShellRecorder: can't link to sink pad");
goto out;
}
result = TRUE;
out:
if (src_pad)
gst_object_unref (src_pad);
if (sink_pad)
gst_object_unref (sink_pad);
return result;
}
static gboolean
link_unlinked_pads (GstElement *bin,
GstPadDirection dir,
const gchar *pad_name,
guint *pad_count,
GError **error)
{
GstPad *pad = NULL;
guint i = 0;
while ((pad = gst_bin_find_unlinked_pad (GST_BIN (bin), dir)))
{
GstPad *ghostpad;
gchar *tmp;
if (i)
tmp = g_strdup_printf ("%s%d", pad_name, i);
else
tmp = g_strdup (pad_name);
i++;
ghostpad = gst_ghost_pad_new (tmp, pad);
gst_object_unref (pad);
g_free (tmp);
if (!ghostpad)
{
g_set_error (error, FS_ERROR, FS_ERROR_CONSTRUCTION,
"Could not create ghostpad for pad %s:%s",
GST_DEBUG_PAD_NAME (pad));
return FALSE;
}
if (!gst_element_add_pad (bin, ghostpad))
{
g_set_error (error, FS_ERROR, FS_ERROR_CONSTRUCTION,
"Could not add pad %s to bin", GST_OBJECT_NAME (ghostpad));
return FALSE;
}
}
if (pad_count)
*pad_count = i;
return TRUE;
}
static void
test_pipeline (const char *pipeline)
{
GstElement *bin, *sink;
GstPad *pad, *sinkpad;
GstBus *bus;
GError *error = NULL;
GMainLoop *loop;
GstPadLinkReturn linkret;
bin = gst_parse_launch (pipeline, &error);
fail_unless (bin != NULL, "Error parsing pipeline: %s",
error ? error->message : "(invalid error)");
pad = gst_bin_find_unlinked_pad (GST_BIN (bin), GST_PAD_SRC);
fail_unless (pad != NULL, "Could not locate free src pad");
/* connect the fake sink */
sink = gst_element_factory_make ("fakesink", "fake_sink");
fail_unless (sink != NULL, "Could create fakesink");
fail_unless (gst_bin_add (GST_BIN (bin), sink), "Could not insert fakesink");
sinkpad = gst_element_get_static_pad (sink, "sink");
fail_unless (sinkpad != NULL, "Could not get fakesink src pad");
linkret = gst_pad_link (pad, sinkpad);
fail_unless (GST_PAD_LINK_SUCCESSFUL (linkret),
"Could not link to fake sink");
gst_object_unref (sinkpad);
/* run until we receive EOS */
loop = g_main_loop_new (NULL, FALSE);
bus = gst_element_get_bus (bin);
gst_bus_add_watch (bus, (GstBusFunc) eos_watch, loop);
gst_object_unref (bus);
start_pipeline (bin, pad);
g_main_loop_run (loop);
stop_pipeline (bin, pad);
/* clean up */
g_main_loop_unref (loop);
gst_object_unref (pad);
gst_object_unref (bin);
}
static void
test_pipeline (const char *pipeline)
{
GstElement *bin, *sink;
GstPad *pad, *sinkpad;
GstBus *bus;
GError *error = NULL;
GMainLoop *loop;
GstPadLinkReturn linkret;
guint bus_watch = 0;
bin = gst_parse_launch (pipeline, &error);
fail_unless (bin != NULL, "Error parsing pipeline: %s",
error ? error->message : "(invalid error)");
pad = gst_bin_find_unlinked_pad (GST_BIN (bin), GST_PAD_SRC);
fail_unless (pad != NULL, "Could not locate free src pad");
/* connect the fake sink */
sink = gst_element_factory_make ("fakesink", "fake_sink");
fail_unless (sink != NULL, "Could create fakesink");
fail_unless (gst_bin_add (GST_BIN (bin), sink), "Could not insert fakesink");
sinkpad = gst_element_get_static_pad (sink, "sink");
fail_unless (sinkpad != NULL, "Could not get fakesink src pad");
linkret = gst_pad_link (pad, sinkpad);
fail_unless (GST_PAD_LINK_SUCCESSFUL (linkret),
"Could not link to fake sink");
gst_object_unref (sinkpad);
/* run until we receive EOS */
loop = g_main_loop_new (NULL, FALSE);
bus = gst_element_get_bus (bin);
bus_watch = gst_bus_add_watch (bus, (GstBusFunc) eos_watch, loop);
gst_object_unref (bus);
start_pipeline (bin, pad);
g_main_loop_run (loop);
/* we're EOS now; make sure oggmux out caps have stream headers on them */
{
GstStructure *s;
GstCaps *muxcaps;
muxcaps = gst_pad_get_negotiated_caps (sinkpad);
fail_unless (muxcaps != NULL);
s = gst_caps_get_structure (muxcaps, 0);
fail_unless (gst_structure_has_name (s, "application/ogg"));
fail_unless (gst_structure_has_field (s, "streamheader"));
fail_unless (gst_structure_has_field_typed (s, "streamheader",
GST_TYPE_ARRAY));
gst_caps_unref (muxcaps);
}
stop_pipeline (bin, pad);
/* clean up */
g_main_loop_unref (loop);
g_source_remove (bus_watch);
gst_object_unref (pad);
gst_object_unref (bin);
}
bool GSCam::init_stream()
{
if(!gst_is_initialized()) {
// Initialize gstreamer pipeline
ROS_DEBUG_STREAM( "Initializing gstreamer..." );
gst_init(0,0);
}
ROS_DEBUG_STREAM( "Gstreamer Version: " << gst_version_string() );
GError *error = 0; // Assignment to zero is a gst requirement
pipeline_ = gst_parse_launch(gsconfig_.c_str(), &error);
if (pipeline_ == NULL) {
ROS_FATAL_STREAM( error->message );
return false;
}
// Create RGB sink
sink_ = gst_element_factory_make("appsink",NULL);
GstCaps * caps = image_encoding_ == sensor_msgs::image_encodings::RGB8 ?
gst_caps_new_simple("video/x-raw-rgb", NULL) :
gst_caps_new_simple("video/x-raw-gray", NULL);
gst_app_sink_set_caps(GST_APP_SINK(sink_), caps);
gst_caps_unref(caps);
// Set whether the sink should sync
// Sometimes setting this to true can cause a large number of frames to be
// dropped
gst_base_sink_set_sync(
GST_BASE_SINK(sink_),
(sync_sink_) ? TRUE : FALSE);
if(GST_IS_PIPELINE(pipeline_)) {
GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC);
g_assert(outpad);
GstElement *outelement = gst_pad_get_parent_element(outpad);
g_assert(outelement);
gst_object_unref(outpad);
if(!gst_bin_add(GST_BIN(pipeline_), sink_)) {
ROS_FATAL("gst_bin_add() failed");
gst_object_unref(outelement);
gst_object_unref(pipeline_);
return false;
}
if(!gst_element_link(outelement, sink_)) {
ROS_FATAL("GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement));
gst_object_unref(outelement);
gst_object_unref(pipeline_);
return false;
}
gst_object_unref(outelement);
} else {
GstElement* launchpipe = pipeline_;
pipeline_ = gst_pipeline_new(NULL);
g_assert(pipeline_);
gst_object_unparent(GST_OBJECT(launchpipe));
gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, NULL);
if(!gst_element_link(launchpipe, sink_)) {
ROS_FATAL("GStreamer: cannot link launchpipe -> sink");
gst_object_unref(pipeline_);
return false;
}
}
gst_element_set_state(pipeline_, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration.");
return false;
} else {
ROS_DEBUG_STREAM("Stream is PAUSED.");
}
// Create ROS camera interface
camera_pub_ = image_transport_.advertiseCamera("camera/image_raw", 1);
return true;
}
PadPtr Bin::findUnlinkedPad(PadDirection direction) const
{
return PadPtr::wrap(gst_bin_find_unlinked_pad(object<GstBin>(),
static_cast<GstPadDirection>(direction)), false);
}
int main(int argc, char** argv) {
char *config = getenv("GSCAM_CONFIG");
if (config == NULL) {
std::cout << "Problem getting GSCAM_CONFIG variable." << std::endl;
exit(-1);
}
gst_init(0,0);
std::cout << "Gstreamer Version: " << gst_version_string() << std::endl;
GError *error = 0; //assignment to zero is a gst requirement
GstElement *pipeline = gst_parse_launch(config,&error);
if (pipeline == NULL) {
std::cout << error->message << std::endl;
exit(-1);
}
GstElement * sink = gst_element_factory_make("appsink",NULL);
GstCaps * caps = gst_caps_new_simple("video/x-raw-rgb", NULL);
gst_app_sink_set_caps(GST_APP_SINK(sink), caps);
gst_caps_unref(caps);
gst_base_sink_set_sync(GST_BASE_SINK(sink), TRUE);
if(GST_IS_PIPELINE(pipeline)) {
GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline), GST_PAD_SRC);
g_assert(outpad);
GstElement *outelement = gst_pad_get_parent_element(outpad);
g_assert(outelement);
gst_object_unref(outpad);
if(!gst_bin_add(GST_BIN(pipeline), sink)) {
fprintf(stderr, "gst_bin_add() failed\n"); // TODO: do some unref
gst_object_unref(outelement);
gst_object_unref(pipeline);
return -1;
}
if(!gst_element_link(outelement, sink)) {
fprintf(stderr, "GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement));
gst_object_unref(outelement);
gst_object_unref(pipeline);
return -1;
}
gst_object_unref(outelement);
} else {
GstElement* launchpipe = pipeline;
pipeline = gst_pipeline_new(NULL);
g_assert(pipeline);
gst_object_unparent(GST_OBJECT(launchpipe));
gst_bin_add_many(GST_BIN(pipeline), launchpipe, sink, NULL);
if(!gst_element_link(launchpipe, sink)) {
fprintf(stderr, "GStreamer: cannot link launchpipe -> sink\n");
gst_object_unref(pipeline);
return -1;
}
}
gst_element_set_state(pipeline, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
std::cout << "Failed to PAUSE." << std::endl;
exit(-1);
} else {
std::cout << "stream is PAUSED." << std::endl;
}
// We could probably do something with the camera name, check
// errors or something, but at the moment, we don't care.
std::string camera_name;
if (camera_calibration_parsers::readCalibrationIni("../camera_parameters.txt", camera_name, camera_info)) {
ROS_INFO("Successfully read camera calibration. Rerun camera calibrator if it is incorrect.");
}
else {
ROS_ERROR("No camera_parameters.txt file found. Use default file if no other is available.");
}
ros::init(argc, argv, "gscam_publisher");
ros::NodeHandle nh;
int preroll;
nh.param("brown/gscam/preroll", preroll, 0);
if (preroll) {
//The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
//I am told this is needed and am erring on the side of caution.
gst_element_set_state(pipeline, GST_STATE_PLAYING);
if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
std::cout << "Failed to PLAY." << std::endl;
exit(-1);
} else {
std::cout << "stream is PLAYING." << std::endl;
}
gst_element_set_state(pipeline, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
std::cout << "Failed to PAUSE." << std::endl;
exit(-1);
} else {
std::cout << "stream is PAUSED." << std::endl;
}
}
image_transport::ImageTransport it(nh);
// image_transport::CameraPublisher pub = it.advertiseCamera("gscam/image_raw", 1);
//------------------------------------
// Added by jschoi, 2012-08-13
char topic_name[32];
if(argc==1)
sprintf(topic_name, "gscam/image_raw");
else
sprintf(topic_name,"%s",argv[1]); // To get the name of topic from the first arguement
//------------------------------------
image_transport::CameraPublisher pub = it.advertiseCamera(topic_name, 1);
ros::ServiceServer set_camera_info = nh.advertiseService("gscam/set_camera_info", setCameraInfo);
std::cout << "Processing..." << std::endl;
//processVideo
rosPad = false;
gstreamerPad = true;
gst_element_set_state(pipeline, GST_STATE_PLAYING);
while(nh.ok()) {
// This should block until a new frame is awake, this way, we'll run at the
// actual capture framerate of the device.
GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink));
if (!buf) break;
GstPad* pad = gst_element_get_static_pad(sink, "sink");
const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
GstStructure *structure = gst_caps_get_structure(caps,0);
gst_structure_get_int(structure,"width",&width);
gst_structure_get_int(structure,"height",&height);
sensor_msgs::Image msg;
msg.width = width;
msg.height = height;
msg.encoding = "rgb8";
msg.is_bigendian = false;
msg.step = width*3;
msg.data.resize(width*height*3);
std::copy(buf->data, buf->data+(width*height*3), msg.data.begin());
pub.publish(msg, camera_info);
gst_buffer_unref(buf);
ros::spinOnce();
}
//close out
std::cout << "\nquitting..." << std::endl;
gst_element_set_state(pipeline, GST_STATE_NULL);
gst_object_unref(pipeline);
return 0;
}
