mirror of
https://github.com/CircusMonkey/ros_rtsp.git
synced 2025-01-20 14:58:21 -04:00
Merge pull request #2 from Kapernikov/master
Add option for custom port and fix indentation
This commit is contained in:
commit
019150d3c5
@ -1,4 +1,5 @@
|
|||||||
# Set up your streams to rtsp here.
|
# Set up your streams to rtsp here.
|
||||||
|
port: "8554"
|
||||||
streams: # Cannot rename - must leave this as is.
|
streams: # Cannot rename - must leave this as is.
|
||||||
|
|
||||||
stream-x:
|
stream-x:
|
||||||
@ -13,4 +14,4 @@ streams: # Cannot rename - must leave this as is.
|
|||||||
mountpoint: /back
|
mountpoint: /back
|
||||||
caps: video/x-raw,framerate=10/1,width=640,height=480
|
caps: video/x-raw,framerate=10/1,width=640,height=480
|
||||||
bitrate: 500
|
bitrate: 500
|
||||||
|
|
||||||
|
@ -2,25 +2,26 @@
|
|||||||
#define IMAGE_TO_RTSP_H
|
#define IMAGE_TO_RTSP_H
|
||||||
|
|
||||||
namespace image2rtsp {
|
namespace image2rtsp {
|
||||||
class Image2RTSPNodelet : public nodelet::Nodelet {
|
class Image2RTSPNodelet : public nodelet::Nodelet {
|
||||||
public:
|
public:
|
||||||
GstRTSPServer *rtsp_server;
|
GstRTSPServer *rtsp_server;
|
||||||
void onInit();
|
void onInit();
|
||||||
void url_connected(std::string url);
|
void url_connected(std::string url);
|
||||||
void url_disconnected(std::string url);
|
void url_disconnected(std::string url);
|
||||||
void print_info(char *s);
|
void print_info(char *s);
|
||||||
void print_error(char *s);
|
void print_error(char *s);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::map<std::string, ros::Subscriber> subs;
|
std::string port;
|
||||||
std::map<std::string, GstAppSrc*> appsrc;
|
std::map<std::string, ros::Subscriber> subs;
|
||||||
std::map<std::string, int> num_of_clients;
|
std::map<std::string, GstAppSrc*> appsrc;
|
||||||
GstCaps* gst_caps_new_from_image(const sensor_msgs::Image::ConstPtr &msg);
|
std::map<std::string, int> num_of_clients;
|
||||||
void imageCallback(const sensor_msgs::Image::ConstPtr& msg, const std::string& topic);
|
GstCaps* gst_caps_new_from_image(const sensor_msgs::Image::ConstPtr &msg);
|
||||||
void video_mainloop_start();
|
void imageCallback(const sensor_msgs::Image::ConstPtr& msg, const std::string& topic);
|
||||||
void rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc);
|
void video_mainloop_start();
|
||||||
GstRTSPServer *rtsp_server_create();
|
void rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc);
|
||||||
};
|
GstRTSPServer *rtsp_server_create(const std::string& port);
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -3,9 +3,9 @@
|
|||||||
<!-- Start the RTSP server -->
|
<!-- Start the RTSP server -->
|
||||||
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
|
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
|
||||||
|
|
||||||
<node pkg="nodelet" type="nodelet" name="Image2RTSPNodelet" args="load image2rtsp/Image2RTSPNodelet standalone_nodelet" output="screen">
|
<node pkg="nodelet" type="nodelet" name="Image2RTSPNodelet" args="load image2rtsp/Image2RTSPNodelet standalone_nodelet" output="screen">
|
||||||
<!-- Read the stream setup file -->
|
<!-- Read the stream setup file -->
|
||||||
<rosparam command="load" file="$(find ros_rtsp)/config/stream_setup.yaml" />
|
<rosparam command="load" file="$(find ros_rtsp)/config/stream_setup.yaml" />
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
</launch>
|
</launch>
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
<library path="lib/libimage_to_rtsp_nodelet">
|
<library path="lib/libimage_to_rtsp_nodelet">
|
||||||
<class name="image2rtsp/Image2RTSPNodelet" type="image2rtsp::Image2RTSPNodelet" base_class_type="nodelet::Nodelet">
|
<class name="image2rtsp/Image2RTSPNodelet" type="image2rtsp::Image2RTSPNodelet" base_class_type="nodelet::Nodelet">
|
||||||
<description>
|
<description>
|
||||||
Nodelet to transfert the /camera/image topic from openni2_camera over rtsp.
|
Nodelet to transfert the /camera/image topic from openni2_camera over rtsp.
|
||||||
</description>
|
</description>
|
||||||
</class>
|
</class>
|
||||||
</library>
|
</library>
|
||||||
|
@ -16,191 +16,191 @@ using namespace image2rtsp;
|
|||||||
|
|
||||||
|
|
||||||
void Image2RTSPNodelet::onInit() {
|
void Image2RTSPNodelet::onInit() {
|
||||||
string pipeline, mountpoint, bitrate, caps;
|
string pipeline, mountpoint, bitrate, caps;
|
||||||
string pipeline_tail = " key-int-max=30 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )"; // Gets completed based on rosparams below
|
string pipeline_tail = " key-int-max=30 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )"; // Gets completed based on rosparams below
|
||||||
|
|
||||||
NODELET_DEBUG("Initializing image2rtsp nodelet...");
|
NODELET_DEBUG("Initializing image2rtsp nodelet...");
|
||||||
|
|
||||||
if (getenv((char*)"GST_DEBUG") == NULL) {
|
if (getenv((char*)"GST_DEBUG") == NULL) {
|
||||||
// set GST_DEBUG to warning if unset
|
// set GST_DEBUG to warning if unset
|
||||||
putenv((char*)"GST_DEBUG=*:1");
|
putenv((char*)"GST_DEBUG=*:1");
|
||||||
}
|
}
|
||||||
|
|
||||||
ros::NodeHandle& nh = getPrivateNodeHandle();
|
ros::NodeHandle& nh = getPrivateNodeHandle();
|
||||||
|
|
||||||
video_mainloop_start();
|
// Get the parameters from the rosparam server
|
||||||
rtsp_server = rtsp_server_create();
|
XmlRpc::XmlRpcValue streams;
|
||||||
|
nh.getParam("streams", streams);
|
||||||
|
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
||||||
|
ROS_DEBUG("Number of RTSP streams: %d", streams.size());
|
||||||
|
nh.getParam("port", this->port);
|
||||||
|
|
||||||
// Get the parameters from the rosparam server
|
video_mainloop_start();
|
||||||
XmlRpc::XmlRpcValue streams;
|
rtsp_server = rtsp_server_create(port);
|
||||||
nh.getParam("streams", streams);
|
|
||||||
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
|
||||||
|
|
||||||
ROS_DEBUG("Number of RTSP streams: %d", streams.size());
|
// Go through and parse each stream
|
||||||
|
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
||||||
|
{
|
||||||
|
XmlRpc::XmlRpcValue stream = streams[it->first];
|
||||||
|
ROS_DEBUG_STREAM("Found stream: " << (std::string)(it->first) << " ==> " << stream);
|
||||||
|
|
||||||
// Go through and parse each stream
|
// Convert to string for ease of use
|
||||||
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
||||||
{
|
bitrate = std::to_string(static_cast<int>(stream["bitrate"]));
|
||||||
XmlRpc::XmlRpcValue stream = streams[it->first];
|
|
||||||
ROS_DEBUG_STREAM("Found stream: " << (std::string)(it->first) << " ==> " << stream);
|
|
||||||
|
|
||||||
// Convert to string for ease of use
|
|
||||||
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
|
||||||
bitrate = std::to_string(static_cast<int>(stream["bitrate"]));
|
|
||||||
|
|
||||||
// uvc camera?
|
// uvc camera?
|
||||||
if (stream["type"]=="cam")
|
if (stream["type"]=="cam")
|
||||||
{
|
{
|
||||||
pipeline = "( " + static_cast<std::string>(stream["source"]) + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail;
|
pipeline = "( " + static_cast<std::string>(stream["source"]) + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail;
|
||||||
|
|
||||||
rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), NULL);
|
rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), NULL);
|
||||||
}
|
}
|
||||||
// ROS Image topic?
|
// ROS Image topic?
|
||||||
else if (stream["type"]=="topic")
|
else if (stream["type"]=="topic")
|
||||||
{
|
{
|
||||||
/* Keep record of number of clients connected to each topic.
|
/* Keep record of number of clients connected to each topic.
|
||||||
* so we know to stop subscribing when no-one is connected. */
|
* so we know to stop subscribing when no-one is connected. */
|
||||||
num_of_clients[mountpoint] = 0;
|
num_of_clients[mountpoint] = 0;
|
||||||
appsrc[mountpoint] = NULL;
|
appsrc[mountpoint] = NULL;
|
||||||
caps = static_cast<std::string>(stream["caps"]);
|
caps = static_cast<std::string>(stream["caps"]);
|
||||||
|
|
||||||
// Setup the full pipeline
|
// Setup the full pipeline
|
||||||
pipeline = "( appsrc name=imagesrc do-timestamp=true min-latency=0 max-latency=0 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! " + caps + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail;
|
pipeline = "( appsrc name=imagesrc do-timestamp=true min-latency=0 max-latency=0 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! " + caps + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail;
|
||||||
|
|
||||||
// Add the pipeline to the rtsp server
|
// Add the pipeline to the rtsp server
|
||||||
rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), (GstElement **)&(appsrc[mountpoint]));
|
rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), (GstElement **)&(appsrc[mountpoint]));
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
ROS_ERROR("Undefined stream type. Check your stream_setup.yaml file.");
|
ROS_ERROR("Undefined stream type. Check your stream_setup.yaml file.");
|
||||||
}
|
}
|
||||||
NODELET_INFO("Stream available at rtsp://%s:8554%s", gst_rtsp_server_get_address(rtsp_server), mountpoint.c_str());
|
NODELET_INFO("Stream available at rtsp://%s:%s%s", gst_rtsp_server_get_address(rtsp_server), port.c_str(), mountpoint.c_str());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Modified from https://github.com/ProjectArtemis/gst_video_server/blob/master/src/server_nodelet.cpp */
|
/* Modified from https://github.com/ProjectArtemis/gst_video_server/blob/master/src/server_nodelet.cpp */
|
||||||
GstCaps* Image2RTSPNodelet::gst_caps_new_from_image(const sensor_msgs::Image::ConstPtr &msg)
|
GstCaps* Image2RTSPNodelet::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
|
// http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html
|
||||||
static const ros::M_string known_formats = {{
|
static const ros::M_string known_formats = {{
|
||||||
{sensor_msgs::image_encodings::RGB8, "RGB"},
|
{sensor_msgs::image_encodings::RGB8, "RGB"},
|
||||||
{sensor_msgs::image_encodings::RGB16, "RGB16"},
|
{sensor_msgs::image_encodings::RGB16, "RGB16"},
|
||||||
{sensor_msgs::image_encodings::RGBA8, "RGBA"},
|
{sensor_msgs::image_encodings::RGBA8, "RGBA"},
|
||||||
{sensor_msgs::image_encodings::RGBA16, "RGBA16"},
|
{sensor_msgs::image_encodings::RGBA16, "RGBA16"},
|
||||||
{sensor_msgs::image_encodings::BGR8, "BGR"},
|
{sensor_msgs::image_encodings::BGR8, "BGR"},
|
||||||
{sensor_msgs::image_encodings::BGR16, "BGR16"},
|
{sensor_msgs::image_encodings::BGR16, "BGR16"},
|
||||||
{sensor_msgs::image_encodings::BGRA8, "BGRA"},
|
{sensor_msgs::image_encodings::BGRA8, "BGRA"},
|
||||||
{sensor_msgs::image_encodings::BGRA16, "BGRA16"},
|
{sensor_msgs::image_encodings::BGRA16, "BGRA16"},
|
||||||
{sensor_msgs::image_encodings::MONO8, "GRAY8"},
|
{sensor_msgs::image_encodings::MONO8, "GRAY8"},
|
||||||
{sensor_msgs::image_encodings::MONO16, "GRAY16_LE"},
|
{sensor_msgs::image_encodings::MONO16, "GRAY16_LE"},
|
||||||
}};
|
}};
|
||||||
|
|
||||||
if (msg->is_bigendian) {
|
if (msg->is_bigendian) {
|
||||||
ROS_ERROR("GST: big endian image format is not supported");
|
ROS_ERROR("GST: big endian image format is not supported");
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto format = known_formats.find(msg->encoding);
|
auto format = known_formats.find(msg->encoding);
|
||||||
if (format == known_formats.end()) {
|
if (format == known_formats.end()) {
|
||||||
ROS_ERROR("GST: image format '%s' unknown", msg->encoding.c_str());
|
ROS_ERROR("GST: image format '%s' unknown", msg->encoding.c_str());
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
return gst_caps_new_simple("video/x-raw",
|
return gst_caps_new_simple("video/x-raw",
|
||||||
"format", G_TYPE_STRING, format->second.c_str(),
|
"format", G_TYPE_STRING, format->second.c_str(),
|
||||||
"width", G_TYPE_INT, msg->width,
|
"width", G_TYPE_INT, msg->width,
|
||||||
"height", G_TYPE_INT, msg->height,
|
"height", G_TYPE_INT, msg->height,
|
||||||
"framerate", GST_TYPE_FRACTION, 10, 1,
|
"framerate", GST_TYPE_FRACTION, 10, 1,
|
||||||
nullptr);
|
nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Image2RTSPNodelet::imageCallback(const sensor_msgs::Image::ConstPtr& msg, const std::string& topic) {
|
void Image2RTSPNodelet::imageCallback(const sensor_msgs::Image::ConstPtr& msg, const std::string& topic) {
|
||||||
GstBuffer *buf;
|
GstBuffer *buf;
|
||||||
|
|
||||||
GstCaps *caps;
|
GstCaps *caps;
|
||||||
char *gst_type, *gst_format=(char *)"";
|
char *gst_type, *gst_format=(char *)"";
|
||||||
// g_print("Image encoding: %s\n", msg->encoding.c_str());
|
// g_print("Image encoding: %s\n", msg->encoding.c_str());
|
||||||
if (appsrc[topic] != NULL) {
|
if (appsrc[topic] != NULL) {
|
||||||
// Set caps from message
|
// Set caps from message
|
||||||
caps = gst_caps_new_from_image(msg);
|
caps = gst_caps_new_from_image(msg);
|
||||||
gst_app_src_set_caps(appsrc[topic], caps);
|
gst_app_src_set_caps(appsrc[topic], caps);
|
||||||
|
|
||||||
buf = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr);
|
buf = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr);
|
||||||
gst_buffer_fill(buf, 0, msg->data.data(), msg->data.size());
|
gst_buffer_fill(buf, 0, msg->data.data(), msg->data.size());
|
||||||
GST_BUFFER_FLAG_SET(buf, GST_BUFFER_FLAG_LIVE);
|
GST_BUFFER_FLAG_SET(buf, GST_BUFFER_FLAG_LIVE);
|
||||||
|
|
||||||
gst_app_src_push_buffer(appsrc[topic], buf);
|
gst_app_src_push_buffer(appsrc[topic], buf);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Image2RTSPNodelet::url_connected(string url) {
|
void Image2RTSPNodelet::url_connected(string url) {
|
||||||
std::string mountpoint, source, type;
|
std::string mountpoint, source, type;
|
||||||
|
|
||||||
NODELET_INFO("Client connected: %s", url.c_str());
|
NODELET_INFO("Client connected: %s", url.c_str());
|
||||||
ros::NodeHandle& nh = getPrivateNodeHandle();
|
ros::NodeHandle& nh = getPrivateNodeHandle();
|
||||||
|
|
||||||
// Get the parameters from the rosparam server
|
// Get the parameters from the rosparam server
|
||||||
XmlRpc::XmlRpcValue streams;
|
XmlRpc::XmlRpcValue streams;
|
||||||
nh.getParam("streams", streams);
|
nh.getParam("streams", streams);
|
||||||
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
||||||
|
|
||||||
// Go through and parse each stream
|
// Go through and parse each stream
|
||||||
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
||||||
{
|
{
|
||||||
XmlRpc::XmlRpcValue stream = streams[it->first];
|
XmlRpc::XmlRpcValue stream = streams[it->first];
|
||||||
type = static_cast<std::string>(stream["type"]);
|
type = static_cast<std::string>(stream["type"]);
|
||||||
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
||||||
source = static_cast<std::string>(stream["source"]);
|
source = static_cast<std::string>(stream["source"]);
|
||||||
|
|
||||||
// Check which stream the client has connected to
|
// Check which stream the client has connected to
|
||||||
if (type=="topic" && url==mountpoint) {
|
if (type=="topic" && url==mountpoint) {
|
||||||
|
|
||||||
if (num_of_clients[url]==0) {
|
if (num_of_clients[url]==0) {
|
||||||
// Subscribe to the ROS topic
|
// Subscribe to the ROS topic
|
||||||
subs[url] = nh.subscribe<sensor_msgs::Image>(source, 1, boost::bind(&Image2RTSPNodelet::imageCallback, this, _1, url));
|
subs[url] = nh.subscribe<sensor_msgs::Image>(source, 1, boost::bind(&Image2RTSPNodelet::imageCallback, this, _1, url));
|
||||||
}
|
}
|
||||||
num_of_clients[url]++;
|
num_of_clients[url]++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Image2RTSPNodelet::url_disconnected(string url) {
|
void Image2RTSPNodelet::url_disconnected(string url) {
|
||||||
std::string mountpoint;
|
std::string mountpoint;
|
||||||
|
|
||||||
NODELET_INFO("Client disconnected: %s", url.c_str());
|
NODELET_INFO("Client disconnected: %s", url.c_str());
|
||||||
ros::NodeHandle& nh = getPrivateNodeHandle();
|
ros::NodeHandle& nh = getPrivateNodeHandle();
|
||||||
|
|
||||||
// Get the parameters from the rosparam server
|
// Get the parameters from the rosparam server
|
||||||
XmlRpc::XmlRpcValue streams;
|
XmlRpc::XmlRpcValue streams;
|
||||||
nh.getParam("streams", streams);
|
nh.getParam("streams", streams);
|
||||||
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
ROS_ASSERT(streams.getType() == XmlRpc::XmlRpcValue::TypeStruct);
|
||||||
|
|
||||||
// Go through and parse each stream
|
// Go through and parse each stream
|
||||||
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = streams.begin(); it != streams.end(); ++it)
|
||||||
{
|
{
|
||||||
XmlRpc::XmlRpcValue stream = streams[it->first];
|
XmlRpc::XmlRpcValue stream = streams[it->first];
|
||||||
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
mountpoint = static_cast<std::string>(stream["mountpoint"]);
|
||||||
|
|
||||||
// Check which stream the client has disconnected from
|
// Check which stream the client has disconnected from
|
||||||
if (url==mountpoint) {
|
if (url==mountpoint) {
|
||||||
if (num_of_clients[url] > 0) num_of_clients[url]--;
|
if (num_of_clients[url] > 0) num_of_clients[url]--;
|
||||||
if (num_of_clients[url] == 0) {
|
if (num_of_clients[url] == 0) {
|
||||||
// No-one else is connected. Stop the subscription.
|
// No-one else is connected. Stop the subscription.
|
||||||
subs[url].shutdown();
|
subs[url].shutdown();
|
||||||
appsrc[url] = NULL;
|
appsrc[url] = NULL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Image2RTSPNodelet::print_info(char *s) {
|
void Image2RTSPNodelet::print_info(char *s) {
|
||||||
NODELET_INFO("%s",s);
|
NODELET_INFO("%s",s);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Image2RTSPNodelet::print_error(char *s) {
|
void Image2RTSPNodelet::print_error(char *s) {
|
||||||
NODELET_ERROR("%s",s);
|
NODELET_ERROR("%s",s);
|
||||||
}
|
}
|
||||||
|
|
||||||
PLUGINLIB_EXPORT_CLASS(image2rtsp::Image2RTSPNodelet, nodelet::Nodelet)
|
PLUGINLIB_EXPORT_CLASS(image2rtsp::Image2RTSPNodelet, nodelet::Nodelet)
|
||||||
|
173
src/video.cpp
173
src/video.cpp
@ -16,147 +16,150 @@ using namespace image2rtsp;
|
|||||||
|
|
||||||
|
|
||||||
static void *mainloop(void *arg) {
|
static void *mainloop(void *arg) {
|
||||||
GMainLoop *loop = g_main_loop_new(NULL, FALSE);
|
GMainLoop *loop = g_main_loop_new(NULL, FALSE);
|
||||||
|
|
||||||
g_main_loop_run(loop);
|
g_main_loop_run(loop);
|
||||||
|
|
||||||
g_main_destroy(loop);
|
g_main_destroy(loop);
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Image2RTSPNodelet::video_mainloop_start() {
|
void Image2RTSPNodelet::video_mainloop_start() {
|
||||||
pthread_t tloop;
|
pthread_t tloop;
|
||||||
|
|
||||||
gst_init(NULL, NULL);
|
gst_init(NULL, NULL);
|
||||||
pthread_create(&tloop, NULL, &mainloop, NULL);
|
pthread_create(&tloop, NULL, &mainloop, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static void client_options(GstRTSPClient *client, GstRTSPContext *state, Image2RTSPNodelet *nodelet) {
|
static void client_options(GstRTSPClient *client, GstRTSPContext *state, Image2RTSPNodelet *nodelet) {
|
||||||
if (state->uri) {
|
if (state->uri) {
|
||||||
nodelet->url_connected(state->uri->abspath);
|
nodelet->url_connected(state->uri->abspath);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static void client_teardown(GstRTSPClient *client, GstRTSPContext *state, Image2RTSPNodelet *nodelet) {
|
static void client_teardown(GstRTSPClient *client, GstRTSPContext *state, Image2RTSPNodelet *nodelet) {
|
||||||
if (state->uri) {
|
if (state->uri) {
|
||||||
nodelet->url_disconnected(state->uri->abspath);
|
nodelet->url_disconnected(state->uri->abspath);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static void new_client(GstRTSPServer *server, GstRTSPClient *client, Image2RTSPNodelet *nodelet) {
|
static void new_client(GstRTSPServer *server, GstRTSPClient *client, Image2RTSPNodelet *nodelet) {
|
||||||
nodelet->print_info((char *)"New RTSP client");
|
nodelet->print_info((char *)"New RTSP client");
|
||||||
g_signal_connect(client, "options-request", G_CALLBACK(client_options), nodelet);
|
g_signal_connect(client, "options-request", G_CALLBACK(client_options), nodelet);
|
||||||
g_signal_connect(client, "teardown-request", G_CALLBACK(client_teardown), nodelet);
|
g_signal_connect(client, "teardown-request", G_CALLBACK(client_teardown), nodelet);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* this function is periodically run to clean up the expired sessions from the pool. */
|
/* this function is periodically run to clean up the expired sessions from the pool. */
|
||||||
static gboolean session_cleanup(Image2RTSPNodelet *nodelet, gboolean ignored)
|
static gboolean session_cleanup(Image2RTSPNodelet *nodelet, gboolean ignored)
|
||||||
{
|
{
|
||||||
GstRTSPServer *server = nodelet->rtsp_server;
|
GstRTSPServer *server = nodelet->rtsp_server;
|
||||||
GstRTSPSessionPool *pool;
|
GstRTSPSessionPool *pool;
|
||||||
int num;
|
int num;
|
||||||
|
|
||||||
pool = gst_rtsp_server_get_session_pool(server);
|
pool = gst_rtsp_server_get_session_pool(server);
|
||||||
num = gst_rtsp_session_pool_cleanup(pool);
|
num = gst_rtsp_session_pool_cleanup(pool);
|
||||||
g_object_unref(pool);
|
g_object_unref(pool);
|
||||||
|
|
||||||
if (num > 0) {
|
if (num > 0) {
|
||||||
char s[32];
|
char s[32];
|
||||||
snprintf(s, 32, (char *)"Sessions cleaned: %d", num);
|
snprintf(s, 32, (char *)"Sessions cleaned: %d", num);
|
||||||
nodelet->print_info(s);
|
nodelet->print_info(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
|
|
||||||
GstRTSPServer *Image2RTSPNodelet::rtsp_server_create() {
|
GstRTSPServer *Image2RTSPNodelet::rtsp_server_create(const std::string& port) {
|
||||||
GstRTSPServer *server;
|
GstRTSPServer *server;
|
||||||
|
|
||||||
/* create a server instance */
|
/* create a server instance */
|
||||||
server = gst_rtsp_server_new();
|
server = gst_rtsp_server_new();
|
||||||
|
|
||||||
/* attach the server to the default maincontext */
|
// char *port = (char *) port;
|
||||||
gst_rtsp_server_attach(server, NULL);
|
g_object_set(server, "service", port.c_str(), NULL);
|
||||||
|
|
||||||
g_signal_connect(server, "client-connected", G_CALLBACK(new_client), this);
|
/* attach the server to the default maincontext */
|
||||||
|
gst_rtsp_server_attach(server, NULL);
|
||||||
|
|
||||||
/* add a timeout for the session cleanup */
|
g_signal_connect(server, "client-connected", G_CALLBACK(new_client), this);
|
||||||
g_timeout_add_seconds(2, (GSourceFunc)session_cleanup, this);
|
|
||||||
|
|
||||||
return server;
|
/* add a timeout for the session cleanup */
|
||||||
|
g_timeout_add_seconds(2, (GSourceFunc)session_cleanup, this);
|
||||||
|
|
||||||
|
return server;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/* called when a new media pipeline is constructed. We can query the
|
/* called when a new media pipeline is constructed. We can query the
|
||||||
* pipeline and configure our appsrc */
|
* pipeline and configure our appsrc */
|
||||||
static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc)
|
static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc)
|
||||||
{ if (appsrc) {
|
{ if (appsrc) {
|
||||||
GstElement *pipeline = gst_rtsp_media_get_element(media);
|
GstElement *pipeline = gst_rtsp_media_get_element(media);
|
||||||
|
|
||||||
*appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "imagesrc");
|
*appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "imagesrc");
|
||||||
|
|
||||||
/* this instructs appsrc that we will be dealing with timed buffer */
|
/* this instructs appsrc that we will be dealing with timed buffer */
|
||||||
gst_util_set_object_arg(G_OBJECT(*appsrc), "format", "time");
|
gst_util_set_object_arg(G_OBJECT(*appsrc), "format", "time");
|
||||||
|
|
||||||
gst_object_unref(pipeline);
|
gst_object_unref(pipeline);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
guint i, n_streams;
|
guint i, n_streams;
|
||||||
n_streams = gst_rtsp_media_n_streams (media);
|
n_streams = gst_rtsp_media_n_streams (media);
|
||||||
|
|
||||||
for (i = 0; i < n_streams; i++) {
|
for (i = 0; i < n_streams; i++) {
|
||||||
GstRTSPAddressPool *pool;
|
GstRTSPAddressPool *pool;
|
||||||
GstRTSPStream *stream;
|
GstRTSPStream *stream;
|
||||||
gchar *min, *max;
|
gchar *min, *max;
|
||||||
|
|
||||||
stream = gst_rtsp_media_get_stream (media, i);
|
stream = gst_rtsp_media_get_stream (media, i);
|
||||||
|
|
||||||
/* make a new address pool */
|
/* make a new address pool */
|
||||||
pool = gst_rtsp_address_pool_new ();
|
pool = gst_rtsp_address_pool_new ();
|
||||||
|
|
||||||
min = g_strdup_printf ("224.3.0.%d", (2 * i) + 1);
|
min = g_strdup_printf ("224.3.0.%d", (2 * i) + 1);
|
||||||
max = g_strdup_printf ("224.3.0.%d", (2 * i) + 2);
|
max = g_strdup_printf ("224.3.0.%d", (2 * i) + 2);
|
||||||
gst_rtsp_address_pool_add_range (pool, min, max,
|
gst_rtsp_address_pool_add_range (pool, min, max,
|
||||||
5000 + (10 * i), 5010 + (10 * i), 1);
|
5000 + (10 * i), 5010 + (10 * i), 1);
|
||||||
g_free (min);
|
g_free (min);
|
||||||
g_free (max);
|
g_free (max);
|
||||||
|
|
||||||
gst_rtsp_stream_set_address_pool (stream, pool);
|
gst_rtsp_stream_set_address_pool (stream, pool);
|
||||||
g_object_unref (pool);
|
g_object_unref (pool);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Image2RTSPNodelet::rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc) {
|
void Image2RTSPNodelet::rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc) {
|
||||||
GstRTSPMountPoints *mounts;
|
GstRTSPMountPoints *mounts;
|
||||||
GstRTSPMediaFactory *factory;
|
GstRTSPMediaFactory *factory;
|
||||||
|
|
||||||
/* get the mount points for this server, every server has a default object
|
/* get the mount points for this server, every server has a default object
|
||||||
* that be used to map uri mount points to media factories */
|
* that be used to map uri mount points to media factories */
|
||||||
mounts = gst_rtsp_server_get_mount_points(rtsp_server);
|
mounts = gst_rtsp_server_get_mount_points(rtsp_server);
|
||||||
|
|
||||||
/* make a media factory for a test stream. The default media factory can use
|
/* make a media factory for a test stream. The default media factory can use
|
||||||
* gst-launch syntax to create pipelines.
|
* gst-launch syntax to create pipelines.
|
||||||
* any launch line works as long as it contains elements named pay%d. Each
|
* any launch line works as long as it contains elements named pay%d. Each
|
||||||
* element with pay%d names will be a stream */
|
* element with pay%d names will be a stream */
|
||||||
factory = gst_rtsp_media_factory_new();
|
factory = gst_rtsp_media_factory_new();
|
||||||
gst_rtsp_media_factory_set_launch(factory, sPipeline);
|
gst_rtsp_media_factory_set_launch(factory, sPipeline);
|
||||||
|
|
||||||
/* notify when our media is ready, This is called whenever someone asks for
|
/* notify when our media is ready, This is called whenever someone asks for
|
||||||
* the media and a new pipeline is created */
|
* the media and a new pipeline is created */
|
||||||
g_signal_connect(factory, "media-configure", (GCallback)media_configure, appsrc);
|
g_signal_connect(factory, "media-configure", (GCallback)media_configure, appsrc);
|
||||||
|
|
||||||
gst_rtsp_media_factory_set_shared(factory, TRUE);
|
gst_rtsp_media_factory_set_shared(factory, TRUE);
|
||||||
|
|
||||||
/* attach the factory to the url */
|
/* attach the factory to the url */
|
||||||
gst_rtsp_mount_points_add_factory(mounts, url, factory);
|
gst_rtsp_mount_points_add_factory(mounts, url, factory);
|
||||||
|
|
||||||
/* don't need the ref to the mounts anymore */
|
/* don't need the ref to the mounts anymore */
|
||||||
g_object_unref(mounts);
|
g_object_unref(mounts);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user