3636#include < boost/algorithm/string/predicate.hpp>
3737#include < opencv2/opencv.hpp>
3838
39+ #include " rclcpp/rclcpp.hpp"
40+
3941#include " sensor_msgs/image_encodings.hpp"
4042#include " web_video_server/ros_compressed_streamer.hpp"
4143#include " web_video_server/jpeg_streamers.hpp"
@@ -51,26 +53,26 @@ using namespace boost::placeholders; // NOLINT
5153namespace web_video_server
5254{
5355
54- WebVideoServer::WebVideoServer (rclcpp::Node::SharedPtr & node )
55- : node_(node ), handler_group_(
56+ WebVideoServer::WebVideoServer (const rclcpp::NodeOptions & options )
57+ : rclcpp::Node( " web_video_server " , options ), handler_group_(
5658 async_web_server_cpp::HttpReply::stock_reply (async_web_server_cpp::HttpReply::not_found))
5759{
58- node_-> declare_parameter (" port" , 8080 );
59- node_-> declare_parameter (" verbose" , true );
60- node_-> declare_parameter (" address" , " 0.0.0.0" );
61- node_-> declare_parameter (" server_threads" , 1 );
62- node_-> declare_parameter (" ros_threads" , 2 );
63- node_-> declare_parameter (" publish_rate" , -1.0 );
64- node_-> declare_parameter (" default_stream_type" , " mjpeg" );
65-
66- node_-> get_parameter (" port" , port_);
67- node_-> get_parameter (" verbose" , verbose_);
68- node_-> get_parameter (" address" , address_);
60+ declare_parameter (" port" , 8080 );
61+ declare_parameter (" verbose" , true );
62+ declare_parameter (" address" , " 0.0.0.0" );
63+ declare_parameter (" server_threads" , 1 );
64+ declare_parameter (" ros_threads" , 2 );
65+ declare_parameter (" publish_rate" , -1.0 );
66+ declare_parameter (" default_stream_type" , " mjpeg" );
67+
68+ get_parameter (" port" , port_);
69+ get_parameter (" verbose" , verbose_);
70+ get_parameter (" address" , address_);
6971 int server_threads;
70- node_-> get_parameter (" server_threads" , server_threads);
71- node_-> get_parameter (" ros_threads" , ros_threads_);
72- node_-> get_parameter (" publish_rate" , publish_rate_);
73- node_-> get_parameter (" default_stream_type" , default_stream_type_);
72+ get_parameter (" server_threads" , server_threads);
73+ get_parameter (" ros_threads" , ros_threads_);
74+ get_parameter (" publish_rate" , publish_rate_);
75+ get_parameter (" default_stream_type" , default_stream_type_);
7476
7577 stream_types_[" mjpeg" ] = std::make_shared<MjpegStreamerType>();
7678 stream_types_[" png" ] = std::make_shared<PngStreamerType>();
@@ -102,33 +104,26 @@ WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node)
102104 );
103105 } catch (boost::exception & e) {
104106 RCLCPP_ERROR (
105- node_-> get_logger (), " Exception when creating the web server! %s:%d" ,
107+ get_logger (), " Exception when creating the web server! %s:%d" ,
106108 address_.c_str (), port_);
107109 throw ;
108110 }
111+ RCLCPP_INFO (get_logger (), " Waiting For connections on %s:%d" , address_.c_str (), port_);
112+ if (publish_rate_ > 0 ) {
113+ create_wall_timer (1s / publish_rate_, [this ]() {restreamFrames (1.0 / publish_rate_);});
114+ }
115+ server_->run ();
109116}
110117
111118WebVideoServer::~WebVideoServer ()
112119{
120+ server_->stop ();
113121}
114122
115123void WebVideoServer::setup_cleanup_inactive_streams ()
116124{
117125 std::function<void ()> callback = std::bind (&WebVideoServer::cleanup_inactive_streams, this );
118- cleanup_timer_ = node_->create_wall_timer (500ms, callback);
119- }
120-
121- void WebVideoServer::spin ()
122- {
123- server_->run ();
124- RCLCPP_INFO (node_->get_logger (), " Waiting For connections on %s:%d" , address_.c_str (), port_);
125- rclcpp::executors::MultiThreadedExecutor spinner (rclcpp::ExecutorOptions (), ros_threads_);
126- spinner.add_node (node_);
127- if (publish_rate_ > 0 ) {
128- node_->create_wall_timer (1s / publish_rate_, [this ]() {restreamFrames (1.0 / publish_rate_);});
129- }
130- spinner.spin ();
131- server_->stop ();
126+ cleanup_timer_ = create_wall_timer (500ms, callback);
132127}
133128
134129void WebVideoServer::restreamFrames (double max_age)
@@ -149,7 +144,7 @@ void WebVideoServer::cleanup_inactive_streams()
149144 [](const std::shared_ptr<ImageStreamer> & streamer) {return !streamer->isInactive ();});
150145 if (verbose_) {
151146 for (auto itr = new_end; itr < image_subscribers_.end (); ++itr) {
152- RCLCPP_INFO (node_-> get_logger (), " Removed Stream: %s" , (*itr)->getTopic ().c_str ());
147+ RCLCPP_INFO (get_logger (), " Removed Stream: %s" , (*itr)->getTopic ().c_str ());
153148 }
154149 }
155150 image_subscribers_.erase (new_end, image_subscribers_.end ());
@@ -162,13 +157,13 @@ bool WebVideoServer::handle_request(
162157 const char * end)
163158{
164159 if (verbose_) {
165- RCLCPP_INFO (node_-> get_logger (), " Handling Request: %s" , request.uri .c_str ());
160+ RCLCPP_INFO (get_logger (), " Handling Request: %s" , request.uri .c_str ());
166161 }
167162
168163 try {
169164 return handler_group_ (request, connection, begin, end);
170165 } catch (std::exception & e) {
171- RCLCPP_WARN (node_-> get_logger (), " Error Handling Request: %s" , e.what ());
166+ RCLCPP_WARN (get_logger (), " Error Handling Request: %s" , e.what ());
172167 return false ;
173168 }
174169}
@@ -184,7 +179,7 @@ bool WebVideoServer::handle_stream(
184179 // Fallback for topics without corresponding compressed topics
185180 if (type == std::string (" ros_compressed" )) {
186181 std::string compressed_topic_name = topic + " /compressed" ;
187- auto tnat = node_-> get_topic_names_and_types ();
182+ auto tnat = get_topic_names_and_types ();
188183 bool did_find_compressed_topic = false ;
189184 for (auto topic_and_types : tnat) {
190185 if (topic_and_types.second .size () > 1 ) {
@@ -201,13 +196,13 @@ bool WebVideoServer::handle_stream(
201196 }
202197 if (!did_find_compressed_topic) {
203198 RCLCPP_WARN (
204- node_-> get_logger (),
199+ get_logger (),
205200 " Could not find compressed image topic for %s, falling back to mjpeg" , topic.c_str ());
206201 type = " mjpeg" ;
207202 }
208203 }
209204 std::shared_ptr<ImageStreamer> streamer = stream_types_[type]->create_streamer (
210- request, connection, node_ );
205+ request, connection, shared_from_this () );
211206 streamer->start ();
212207 std::scoped_lock lock (subscriber_mutex_);
213208 image_subscribers_.push_back (streamer);
@@ -224,7 +219,7 @@ bool WebVideoServer::handle_snapshot(
224219 const char * end)
225220{
226221 std::shared_ptr<ImageStreamer> streamer = std::make_shared<JpegSnapshotStreamer>(
227- request, connection, node_ );
222+ request, connection, shared_from_this () );
228223 streamer->start ();
229224
230225 std::scoped_lock lock (subscriber_mutex_);
@@ -243,7 +238,7 @@ bool WebVideoServer::handle_stream_viewer(
243238 // Fallback for topics without corresponding compressed topics
244239 if (type == std::string (" ros_compressed" )) {
245240 std::string compressed_topic_name = topic + " /compressed" ;
246- auto tnat = node_-> get_topic_names_and_types ();
241+ auto tnat = get_topic_names_and_types ();
247242 bool did_find_compressed_topic = false ;
248243 for (auto topic_and_types : tnat) {
249244 if (topic_and_types.second .size () > 1 ) {
@@ -260,7 +255,7 @@ bool WebVideoServer::handle_stream_viewer(
260255 }
261256 if (!did_find_compressed_topic) {
262257 RCLCPP_WARN (
263- node_-> get_logger (),
258+ get_logger (),
264259 " Could not find compressed image topic for %s, falling back to mjpeg" , topic.c_str ());
265260 type = " mjpeg" ;
266261 }
@@ -292,7 +287,7 @@ bool WebVideoServer::handle_list_streams(
292287{
293288 std::vector<std::string> image_topics;
294289 std::vector<std::string> camera_info_topics;
295- auto tnat = node_-> get_topic_names_and_types ();
290+ auto tnat = get_topic_names_and_types ();
296291 for (auto topic_and_types : tnat) {
297292 if (topic_and_types.second .size () > 1 ) {
298293 // skip over topics with more than one type
@@ -380,14 +375,9 @@ bool WebVideoServer::handle_list_streams(
380375
381376} // namespace web_video_server
382377
383- int main (int argc, char ** argv)
384- {
385- rclcpp::init (argc, argv);
386- auto node = std::make_shared<rclcpp::Node>(" web_video_server" );
387-
388- web_video_server::WebVideoServer server (node);
389- server.setup_cleanup_inactive_streams ();
390- server.spin ();
378+ #include " rclcpp_components/register_node_macro.hpp"
391379
392- return 0 ;
393- }
380+ // Register the component with class_loader.
381+ // This acts as a sort of entry point, allowing the component to be discoverable when its library
382+ // is being loaded into a running process.
383+ RCLCPP_COMPONENTS_REGISTER_NODE (web_video_server::WebVideoServer)
0 commit comments