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++; }