diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..7810d7e
--- /dev/null
+++ b/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)
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..e0a3cbe
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,74 @@
+
+
+ ros_rtsp
+ 0.0.0
+ The ros_rtsp package
+
+
+
+
+ samar
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ cv_bridge
+ image_transport
+ roscpp
+ sensor_msgs
+ std_msgs
+ cv_bridge
+ image_transport
+ roscpp
+ sensor_msgs
+ std_msgs
+ cv_bridge
+ image_transport
+ roscpp
+ sensor_msgs
+ std_msgs
+
+
+
+
+
+
+
+
diff --git a/src/camera_publisher.cpp b/src/camera_publisher.cpp
new file mode 100644
index 0000000..336b806
--- /dev/null
+++ b/src/camera_publisher.cpp
@@ -0,0 +1,42 @@
+#include
+#include
+#include
+#include
+#include // 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();
+ }
+}
diff --git a/src/camera_subscriber.cpp b/src/camera_subscriber.cpp
new file mode 100644
index 0000000..34a0517
--- /dev/null
+++ b/src/camera_subscriber.cpp
@@ -0,0 +1,29 @@
+#include
+#include
+#include
+#include
+
+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");
+}
diff --git a/src/test-appsrc.cpp b/src/test-appsrc.cpp
new file mode 100644
index 0000000..014aec3
--- /dev/null
+++ b/src/test-appsrc.cpp
@@ -0,0 +1,268 @@
+/* GStreamer
+ * Copyright (C) 2008 Wim Taymans
+ *
+ * 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
+#include
+#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;
+
+}
diff --git a/src/test-appsrc.h b/src/test-appsrc.h
new file mode 100644
index 0000000..0eba696
--- /dev/null
+++ b/src/test-appsrc.h
@@ -0,0 +1,24 @@
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+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[]);
diff --git a/src/test-multicast2.cpp b/src/test-multicast2.cpp
new file mode 100644
index 0000000..bd393e4
--- /dev/null
+++ b/src/test-multicast2.cpp
@@ -0,0 +1,204 @@
+/* GStreamer
+ * Copyright (C) 2008 Wim Taymans
+ *
+ * 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
+#include
+
+#include
+#include
+#include
+
+
+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;
+}