diff --git a/config/stream_setup.yaml b/config/stream_setup.yaml
new file mode 100644
index 0000000..474a825
--- /dev/null
+++ b/config/stream_setup.yaml
@@ -0,0 +1,11 @@
+streams:
+ stream1:
+ name: stream1
+ type: cam
+ source: /dev/video0
+ mountpoint: /front
+ stream2:
+ name: stream2
+ type: topic
+ source: /usb_cam/image_raw
+ mountpoint: /back
\ No newline at end of file
diff --git a/launch/test_rtsp.launch b/launch/test_rtsp.launch
index ea08ba5..c1906e7 100644
--- a/launch/test_rtsp.launch
+++ b/launch/test_rtsp.launch
@@ -1,6 +1,6 @@
-
+
-
-
+
+
+
diff --git a/src/image2rtsp.cpp b/src/image2rtsp.cpp
index 932bb19..6b6f5a2 100644
--- a/src/image2rtsp.cpp
+++ b/src/image2rtsp.cpp
@@ -16,8 +16,9 @@ using namespace image2rtsp;
void Image2RTSPNodelet::onInit() {
- string mountpoint_1;
- string pipeline_1;
+ string pipeline_cam = "( appsrc name=imagesrc do-timestamp=true max-latency=50 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! video/x-raw,width=800,height=640 ! videoconvert ! videorate ! video/x-raw,framerate=10/1 ! queue ! x264enc tune=zerolatency bitrate=1000 key-int-max=90 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )";
+
+ string pipeline_appsrc = "( appsrc name=imagesrc do-timestamp=true max-latency=50 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! video/x-raw,width=800,height=640 ! videoconvert ! videorate ! video/x-raw,framerate=10/1 ! queue ! x264enc tune=zerolatency bitrate=1000 key-int-max=90 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )";
NODELET_DEBUG("Initializing image2rtsp nodelet...");
@@ -28,15 +29,36 @@ void Image2RTSPNodelet::onInit() {
num = 0;
appsrc = NULL;
- ros::NodeHandle& node = getPrivateNodeHandle();
+ ros::NodeHandle& nh = getPrivateNodeHandle();
video_mainloop_start();
rtsp_server = rtsp_server_create();
- node.getParam("mountpoint_1", mountpoint_1);
- node.getParam("pipeline_1", pipeline_1);
+ XmlRpc::XmlRpcValue streams;
+ nh.getParam("streams", streams);
+ ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
- rtsp_server_add_url(mountpoint_1.c_str(), pipeline_1.c_str(), (GstElement **)&appsrc);
+ ROS_INFO("Number of streams: %d", streams.size());
+
+ for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
+ {
+ ROS_INFO_STREAM("Found stream: " << (std::string)(it->first) << " ==> " << streams[it->first]);
+
+ if (streams[it->first]["type"]=="cam")
+ {
+ ROS_INFO("ADDING CAMERA");
+ rtsp_server_add_url(static_cast(streams[it->first]["mountpoint"]).c_str(), pipeline_cam.c_str(), NULL);
+ }
+ else if (streams[it->first]["type"]=="topic")
+ {
+ ROS_INFO("ADDING topic");
+ rtsp_server_add_url(static_cast(streams[it->first]["mountpoint"]).c_str(), pipeline_appsrc.c_str(), (GstElement **)&appsrc);
+ }
+
+ }
+
+
+
}
/* Borrowed from https://github.com/ProjectArtemis/gst_video_server/blob/master/src/server_nodelet.cpp */
@@ -151,9 +173,9 @@ void Image2RTSPNodelet::url_connected(string url) {
if (url == "/front") {
if (num == 0) {
- ros::NodeHandle& node = getPrivateNodeHandle();
- node.getParam("topic_1", topic);
- sub = node.subscribe(topic, 10, &Image2RTSPNodelet::imageCallback, this);
+ ros::NodeHandle& nh = getPrivateNodeHandle();
+ nh.getParam("topic_1", topic);
+ sub = nh.subscribe(topic, 10, &Image2RTSPNodelet::imageCallback, this);
}
num++;
}