Init commit

CMakeLists.txt

@@ -0,0 +1,225 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_rtsp)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(OpenCV REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(GST REQUIRED 
+  gstreamer-1.0
+  gstreamer-app-1.0
+  gstreamer-rtsp-server-1.0
+)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  image_transport
+  roscpp
+  sensor_msgs
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ros_rtsp
+ CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${GST_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ros_rtsp.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(camera_publisher src/camera_publisher.cpp)
+add_executable(camera_subscriber src/camera_subscriber.cpp)
+add_executable(rtsp_server src/test-appsrc.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(camera_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(camera_subscriber ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(rtsp_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(camera_publisher
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
+target_link_libraries(camera_subscriber
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
+target_link_libraries(rtsp_server
+  ${catkin_LIBRARIES}
+  ${GST_LIBRARIES}
+)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_rtsp.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_rtsp</name>
+  <version>0.0.0</version>
+  <description>The ros_rtsp package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="samar@todo.todo">samar</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ros_rtsp</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>image_transport</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/camera_publisher.cpp

@@ -0,0 +1,42 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sstream> // for converting the command line parameter to integer
+
+int main(int argc, char** argv)
+{
+  // Check if video source has been passed as a parameter
+  if(argv[1] == NULL) return 1;
+
+  ros::init(argc, argv, "image_publisher");
+  ros::NodeHandle nh;
+  image_transport::ImageTransport it(nh);
+  image_transport::Publisher pub = it.advertise("camera/image", 1);
+
+  // Convert the passed as command line parameter index for the video device to an integer
+  std::istringstream video_sourceCmd(argv[1]);
+  int video_source;
+  // Check if it is indeed a number
+  if(!(video_sourceCmd >> video_source)) return 1;
+
+  cv::VideoCapture cap(video_source);
+  // Check if video device can be opened with the given index
+  if(!cap.isOpened()) return 1;
+  cv::Mat frame;
+  sensor_msgs::ImagePtr msg;
+
+  ros::Rate loop_rate(30);
+  while (nh.ok()) {
+    cap >> frame;
+    // Check if grabbed frame is actually full with some content
+    if(!frame.empty()) {
+      msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
+      pub.publish(msg);
+      cv::waitKey(1);
+    }
+
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+}

src/camera_subscriber.cpp

@@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+  try
+  {
+    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
+    cv::waitKey(30);
+  }
+  catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+  }
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "image_listener");
+  ros::NodeHandle nh;
+  cv::namedWindow("view");
+  cv::startWindowThread();
+  image_transport::ImageTransport it(nh);
+  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
+  ros::spin();
+  cv::destroyWindow("view");
+}

src/test-appsrc.cpp

@@ -0,0 +1,268 @@
+/* GStreamer
+ * Copyright (C) 2008 Wim Taymans <wim.taymans at gmail.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#include <string>
+#include <thread>
+#include "test-appsrc.h"
+
+
+GstElement *appsrc;
+GstBuffer *buffer;
+
+
+
+static gboolean
+timeout(GstRTSPServer *server)
+{
+    GstRTSPSessionPool *pool;
+
+    pool = gst_rtsp_server_get_session_pool(server);
+    gst_rtsp_session_pool_cleanup(pool);
+    g_object_unref(pool);
+
+    return TRUE;
+}
+
+/* called when we need to give data to appsrc */
+static void
+need_data (GstElement * appsrc, guint unused)
+{
+    g_print("NEED DATA. Source is dry.");
+
+}
+
+
+/* called when a new media pipeline is constructed. We can query the
+ * pipeline and configure our appsrc */
+static void
+media_constructed(GstRTSPMediaFactory *factory, GstRTSPMedia *media,
+                gpointer user_data)
+{
+    GstElement *element;
+    g_print ("YAY MEDIA BEING CONSTRUCTED!!!\n");
+
+    /* Setup for multicasting */
+    guint i, n_streams;
+
+    n_streams = gst_rtsp_media_n_streams(media);
+
+    for (i = 0; i < n_streams; i++)
+    {
+        GstRTSPAddressPool *pool;
+        GstRTSPStream *stream;
+        gchar *min, *max;
+
+        stream = gst_rtsp_media_get_stream(media, i);
+
+        /* make a new address pool */
+        pool = gst_rtsp_address_pool_new();
+
+        min = g_strdup_printf("224.3.0.%d", (2 * i) + 1);
+        max = g_strdup_printf("224.3.0.%d", (2 * i) + 2);
+        gst_rtsp_address_pool_add_range(pool, min, max,
+                                        5000 + (10 * i), 5010 + (10 * i), 1);
+        g_free(min);
+        g_free(max);
+
+        gst_rtsp_stream_set_address_pool(stream, pool);
+        g_object_unref(pool);
+    }
+
+
+    /* get the element used for providing the streams of the media */
+    element = gst_rtsp_media_get_element (media);
+
+    /* get our appsrc, we named it 'mysrc' with the name property */
+    appsrc = gst_bin_get_by_name_recurse_up (GST_BIN (element), "mysrc");
+
+    /* this instructs appsrc that we will be dealing with timed buffer */
+    gst_util_set_object_arg (G_OBJECT (appsrc), "format", "time");
+
+    g_signal_connect (appsrc, "need-data", (GCallback) need_data, NULL);
+
+
+}
+
+
+/* Borrowed from https://github.com/ProjectArtemis/gst_video_server/blob/master/src/server_nodelet.cpp */
+namespace enc = sensor_msgs::image_encodings;
+
+GstCaps* gst_caps_new_from_image(const sensor_msgs::Image::ConstPtr &msg)
+{
+	// http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html
+	static const ros::M_string known_formats = {{
+		{enc::RGB8, "RGB"},
+		{enc::RGB16, "RGB16"},
+		{enc::RGBA8, "RGBA"},
+		{enc::RGBA16, "RGBA16"},
+		{enc::BGR8, "BGR"},
+		{enc::BGR16, "BGR16"},
+		{enc::BGRA8, "BGRA"},
+		{enc::BGRA16, "BGRA16"},
+		{enc::MONO8, "GRAY8"},
+		{enc::MONO16, "GRAY16_LE"},
+	}};
+
+	if (msg->is_bigendian) {
+		ROS_ERROR("GST: big endian image format is not supported");
+		return nullptr;
+	}
+
+	auto format = known_formats.find(msg->encoding);
+    g_print("Format found = %s\n", format);
+	if (format == known_formats.end()) {
+		ROS_ERROR("GST: image format '%s' unknown", msg->encoding.c_str());
+		return nullptr;
+	}
+
+	return gst_caps_new_simple("video/x-raw",
+			"format", G_TYPE_STRING, format->second.c_str(),
+			"width", G_TYPE_INT, msg->width,
+			"height", G_TYPE_INT, msg->height,
+			"framerate", GST_TYPE_FRACTION, 0, 1,	// 0/1 = dynamic
+			nullptr);
+}
+
+/* Borrowed from https://github.com/ProjectArtemis/gst_video_server/blob/master/src/server_nodelet.cpp */
+GstSample* gst_sample_new_from_image(const sensor_msgs::Image::ConstPtr &msg)
+{
+    g_print("Image details: %s\n", msg->encoding.c_str());
+
+
+	// unfortunately we may not move image data because it is immutable. copying.
+	auto buffer = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr);
+	g_assert(buffer);
+
+	/* NOTE(vooon):
+	 * I found that best is to use `do-timestamp=true` parameter
+	 * and forgot about stamping stamp problem (PTS).
+	 */
+	//auto ts = gst_time_from_stamp(msg->header.stamp);
+	//NODELET_INFO("TS: %lld, %lld", ts, gst_util_uint64_scale_int(1, GST_SECOND, 40));
+
+	gst_buffer_fill(buffer, 0, msg->data.data(), msg->data.size());
+	GST_BUFFER_FLAG_SET(buffer, GST_BUFFER_FLAG_LIVE);
+	//GST_BUFFER_PTS(buffer) = ts;
+
+	auto caps = gst_caps_new_from_image(msg);
+    g_print("Got caps from msg\n");
+	if (caps == nullptr) {
+		gst_object_unref(GST_OBJECT(buffer));
+		return nullptr;
+	}
+
+	auto sample = gst_sample_new(buffer, caps, nullptr, nullptr);
+    g_print("Assigned a sample\n");
+	gst_buffer_unref(buffer);
+	gst_caps_unref(caps);
+
+	return sample;
+}
+
+
+
+void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+    GstSample *sample;
+    GstFlowReturn ret;
+
+    g_print("Got new ROS image callback\n");
+    /* Create the sample from the ros msg */
+    sample = gst_sample_new_from_image(msg);
+
+    /* Get the pipeline to push to */
+    ret = gst_app_src_push_sample (GST_APP_SRC (appsrc), sample);
+    gst_object_unref (sample);
+
+}
+
+
+
+int main(int argc, char *argv[])
+{
+    GMainLoop *loop;
+    GstRTSPServer *server;
+    GstRTSPMountPoints *mounts;
+    GstRTSPMediaFactory *factory;
+
+    gst_init(&argc, &argv);
+
+    loop = g_main_loop_new(NULL, FALSE);
+
+    /* create a server instance */
+    server = gst_rtsp_server_new();
+
+    /* get the mount points for this server, every server has a default object
+    * that be used to map uri mount points to media factories */
+    mounts = gst_rtsp_server_get_mount_points(server);
+
+    /* make a media factory for a test stream. The default media factory can use
+    * gst-launch syntax to create pipelines.
+    * any launch line works as long as it contains elements named pay%d. Each
+    * element with pay%d names will be a stream */
+    factory = gst_rtsp_media_factory_new();
+    gst_rtsp_media_factory_set_launch(factory, "( "
+        "appsrc name=mysrc ! videoconvert ! videoscale ! video/x-raw,framerate=15/1,width=1280,height=720 ! videoconvert ! "
+        "x264enc tune=zerolatency bitrate=1000 key-int-max=90 ! "
+        "rtph264pay name=pay0 pt=96"
+        " )");
+
+    gst_rtsp_media_factory_set_shared(factory, TRUE);
+
+    /* notify when our media is ready, This is called whenever someone asks for
+    * the media and a new pipeline with our appsrc is created */
+    g_signal_connect(factory, "media-constructed", (GCallback)media_constructed,
+                     NULL);
+
+    /* attach the test factory to the /test url */
+    gst_rtsp_mount_points_add_factory(mounts, "/test", factory);
+
+    /* don't need the ref to the mounts anymore */
+    g_object_unref(mounts);
+
+    /* attach the server to the default maincontext */
+    if (gst_rtsp_server_attach(server, NULL) == 0)
+    {
+        g_print("failed to attach the server\n");
+        return -1;
+    }
+    g_timeout_add_seconds(2, (GSourceFunc)timeout, server);
+
+    /* start serving */
+    g_print("stream ready at rtsp://127.0.0.1:8554/test\n");
+
+
+    /* Initialise the ros subscriber node */
+    ros::init(argc, argv, "image_listener");
+    ros::NodeHandle nh;
+    image_transport::ImageTransport it(nh);
+    image_transport::Subscriber sub = it.subscribe("camera/image", 1,  imageCallback);
+    
+    /* Spin up ROS, but use the non-blocking spinner so
+    * our g_main_loop can still listen for RTSP connections */
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    g_main_loop_run(loop);
+
+    spinner.stop();
+
+    return 0;
+
+}

src/test-appsrc.h

@@ -0,0 +1,24 @@
+#include <gst/gst.h>
+#include <gst/rtsp-server/rtsp-server.h>
+#include <gst/app/gstappsrc.h>
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+
+static gboolean
+timeout(GstRTSPServer *server);
+
+static void
+media_constructed(GstRTSPMediaFactory *factory, GstRTSPMedia *media,
+                gpointer user_data);
+
+GstCaps* 
+gst_caps_new_from_image(const sensor_msgs::Image::ConstPtr &msg);
+
+GstSample* 
+gst_sample_new_from_image(const sensor_msgs::Image::ConstPtr &msg);
+
+
+int 
+ros_main(int argc, char *argv[]);

src/test-multicast2.cpp

@@ -0,0 +1,204 @@
+/* GStreamer
+ * Copyright (C) 2008 Wim Taymans <wim.taymans at gmail.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+
+#include <gst/gst.h>
+#include <gst/app/gstappsrc.h>
+#include <gst/rtsp-server/rtsp-server.h>
+
+
+static gboolean
+timeout(GstRTSPServer *server)
+{
+    GstRTSPSessionPool *pool;
+
+    pool = gst_rtsp_server_get_session_pool(server);
+    gst_rtsp_session_pool_cleanup(pool);
+    g_object_unref(pool);
+
+    return TRUE;
+}
+
+static void
+media_constructed(GstRTSPMediaFactory *factory, GstRTSPMedia *media)
+{
+    guint i, n_streams;
+
+    n_streams = gst_rtsp_media_n_streams(media);
+
+    for (i = 0; i < n_streams; i++)
+    {
+        GstRTSPAddressPool *pool;
+        GstRTSPStream *stream;
+        gchar *min, *max;
+
+        stream = gst_rtsp_media_get_stream(media, i);
+
+        /* make a new address pool */
+        pool = gst_rtsp_address_pool_new();
+
+        min = g_strdup_printf("224.3.0.%d", (2 * i) + 1);
+        max = g_strdup_printf("224.3.0.%d", (2 * i) + 2);
+        gst_rtsp_address_pool_add_range(pool, min, max,
+                                        5000 + (10 * i), 5010 + (10 * i), 1);
+        g_free(min);
+        g_free(max);
+
+        gst_rtsp_stream_set_address_pool(stream, pool);
+        g_object_unref(pool);
+    }
+}
+
+
+
+/* called when there is a new buffer ready for
+ * processing */
+static void 
+wrapper_imageCallback(void* pt2Object, const sensor_msgs::ImageConstPtr& msg)
+{
+    myImages* mySelf = (myImages*) pt2Object; // explicitly cast to a pointer to class myImages
+    mySelf->imageCallback(msg );          // call member
+}
+
+
+static GstFlowReturn
+imageCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+    GstSample *sample;
+    GstFlowReturn ret;
+
+    GstCaps *caps;
+	char *gst_type, *gst_format = (char *)"";
+
+    GstBuffer *buf;
+	void *imgdata;
+	GstMapInfo map;
+	static GstClockTime timestamp=0;
+
+    appsrc_front = gst_bin_get_by_name(GST_BIN(data->sink), "appsrc_front");
+
+	if (appsrc_front != NULL) 
+    {
+		// Set caps from message
+		get_format(msg->encoding.c_str(), &gst_type, &gst_format);
+		caps = gst_caps_new_simple (gst_type,
+				"format", G_TYPE_STRING, gst_format,
+				"width", G_TYPE_INT, msg->width,
+				"height", G_TYPE_INT, msg->height,
+				NULL);
+		gst_app_src_set_caps(appsrc_front, caps);
+
+		buf = gst_buffer_new_and_alloc(msg->step*msg->height);
+
+		gst_buffer_map(buf, &map, GST_MAP_READ);
+		imgdata = map.data;
+
+		GST_BUFFER_PTS(buf) = timestamp;
+		GST_BUFFER_DURATION(buf) = gst_util_uint64_scale_int(1, GST_SECOND, 15);
+		timestamp += GST_BUFFER_DURATION(buf);
+
+
+		memcpy(imgdata, &msg->data[0], msg->step*msg->height);
+
+		gst_buffer_unmap(buf, &map);
+		gst_app_src_push_buffer(appsrc_rgb, buf);
+	}
+
+
+
+    /* get the sample from appsink */
+    sample = msg; //Get from ROS
+
+    /* get source an push new sample */
+    source = gst_bin_get_by_name(GST_BIN(source), "appsrc_front");
+    ret = gst_app_src_push_sample(GST_APP_SRC(source), sample);
+    gst_object_unref(source);
+
+    return ret;
+}
+
+
+
+int main(int argc, char **argv)
+{
+
+    std::thread rosthr(ros_main)
+
+    GMainLoop *loop;
+    GstRTSPServer *server;
+    GstRTSPMountPoints *mounts;
+    GstRTSPMediaFactory *factory_front, *factory_back;
+
+    gst_init(&argc, &argv);
+
+    loop = g_main_loop_new(NULL, FALSE);
+
+    /* create a server instance */
+    server = gst_rtsp_server_new();
+
+    /* get the mount points for this server, every server has a default object
+    * that be used to map uri mount points to media factories */
+    mounts = gst_rtsp_server_get_mount_points(server);
+
+    /* make a media factory for a test stream. The default media factory can use
+    * gst-launch syntax to create pipelines. 
+    * any launch line works as long as it contains elements named pay%d. Each
+    * element with pay%d names will be a stream */
+    factory_front = gst_rtsp_media_factory_new();
+    gst_rtsp_media_factory_set_launch(factory_front, "( "
+        "appsrc name=appsrc_front ! videoconvert ! videoscale ! video/x-raw,framerate=15/1,width=1280,height=720 ! "
+        "x264enc tune=zerolatency bitrate=1000 key-int-max=90 ! "
+        "rtph264pay name=pay0 pt=96 "
+        ")");
+
+    gst_rtsp_media_factory_set_shared(factory_front, TRUE);
+
+    g_signal_connect(factory_front, "media-constructed", (GCallback)media_constructed, NULL);
+
+    /* attach the test factory to the /test url */
+    gst_rtsp_mount_points_add_factory(mounts, "/front", factory_front);
+
+    /* don't need the ref to the mapper anymore */
+    g_object_unref(mounts);
+
+    /* attach the server to the default maincontext */
+    if (gst_rtsp_server_attach(server, NULL) == 0)
+    {
+        g_print("failed to attach the server\n");
+        return -1;
+    }
+
+    g_timeout_add_seconds(2, (GSourceFunc)timeout, server);
+
+    /* start serving */
+    g_print("stream ready at rtsp://127.0.0.1:8554/front\n");
+
+    // Use the ros::spin main loop instead.
+    // g_main_loop_run(loop);
+
+    ros::init(argc, argv, "image_listener");
+    ros::NodeHandle nh;
+    image_transport::ImageTransport it(nh);
+    image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
+    ros::spin();
+
+    return 0;
+}