diff --git a/include/web_video_server/h264_streamer.h b/include/web_video_server/h264_streamer.h index 9961389..223d079 100644 --- a/include/web_video_server/h264_streamer.h +++ b/include/web_video_server/h264_streamer.h @@ -13,7 +13,7 @@ class H264Streamer : public LibavStreamer { public: H264Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~H264Streamer(); protected: virtual void initializeEncoder(); @@ -26,7 +26,7 @@ class H264StreamerType : public LibavStreamerType H264StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index ca474e8..f0dc2ad 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -17,7 +17,7 @@ class ImageStreamer public: ImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); virtual void start() = 0; virtual ~ImageStreamer(); @@ -41,7 +41,7 @@ class ImageStreamer protected: async_web_server_cpp::HttpConnectionPtr connection_; async_web_server_cpp::HttpRequest request_; - rclcpp::Node::SharedPtr nh_; + rclcpp::Node::SharedPtr node_; bool inactive_; image_transport::Subscriber image_sub_; std::string topic_; @@ -52,7 +52,7 @@ class ImageTransportImageStreamer : public ImageStreamer { public: ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); virtual ~ImageTransportImageStreamer(); virtual void start(); @@ -85,7 +85,7 @@ class ImageStreamerType public: virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) = 0; + rclcpp::Node::SharedPtr node) = 0; virtual std::string create_viewer(const async_web_server_cpp::HttpRequest &request) = 0; }; diff --git a/include/web_video_server/jpeg_streamers.h b/include/web_video_server/jpeg_streamers.h index ad788fa..a79b6e2 100644 --- a/include/web_video_server/jpeg_streamers.h +++ b/include/web_video_server/jpeg_streamers.h @@ -14,7 +14,7 @@ class MjpegStreamer : public ImageTransportImageStreamer { public: MjpegStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~MjpegStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); @@ -29,7 +29,7 @@ class MjpegStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; @@ -37,7 +37,7 @@ class JpegSnapshotStreamer : public ImageTransportImageStreamer { public: JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~JpegSnapshotStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.h index 635be1c..9bb0012 100644 --- a/include/web_video_server/libav_streamer.h +++ b/include/web_video_server/libav_streamer.h @@ -25,7 +25,7 @@ class LibavStreamer : public ImageTransportImageStreamer { public: LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh, const std::string &format_name, const std::string &codec_name, + rclcpp::Node::SharedPtr node, const std::string &format_name, const std::string &codec_name, const std::string &content_type); ~LibavStreamer(); @@ -65,7 +65,7 @@ class LibavStreamerType : public ImageStreamerType boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); diff --git a/include/web_video_server/png_streamers.h b/include/web_video_server/png_streamers.h index a6edabc..56fdcec 100644 --- a/include/web_video_server/png_streamers.h +++ b/include/web_video_server/png_streamers.h @@ -14,7 +14,7 @@ class PngStreamer : public ImageTransportImageStreamer { public: PngStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~PngStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); @@ -29,7 +29,7 @@ class PngStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; @@ -37,7 +37,7 @@ class PngSnapshotStreamer : public ImageTransportImageStreamer { public: PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~PngSnapshotStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h index f188d7e..c1f676b 100644 --- a/include/web_video_server/ros_compressed_streamer.h +++ b/include/web_video_server/ros_compressed_streamer.h @@ -14,7 +14,7 @@ class RosCompressedStreamer : public ImageStreamer { public: RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~RosCompressedStreamer(); virtual void start(); virtual void restreamFrame(double max_age); @@ -37,7 +37,7 @@ class RosCompressedStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; diff --git a/include/web_video_server/vp8_streamer.h b/include/web_video_server/vp8_streamer.h index 46e8bed..01cd7e7 100644 --- a/include/web_video_server/vp8_streamer.h +++ b/include/web_video_server/vp8_streamer.h @@ -49,7 +49,7 @@ class Vp8Streamer : public LibavStreamer { public: Vp8Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~Vp8Streamer(); protected: virtual void initializeEncoder(); @@ -63,7 +63,7 @@ class Vp8StreamerType : public LibavStreamerType Vp8StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/vp9_streamer.h b/include/web_video_server/vp9_streamer.h index 06c48f8..e6d0356 100644 --- a/include/web_video_server/vp9_streamer.h +++ b/include/web_video_server/vp9_streamer.h @@ -13,7 +13,7 @@ class Vp9Streamer : public LibavStreamer { public: Vp9Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~Vp9Streamer(); protected: virtual void initializeEncoder(); @@ -25,7 +25,7 @@ class Vp9StreamerType : public LibavStreamerType Vp9StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index dce4aa6..33c5d7e 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -29,7 +29,7 @@ class WebVideoServer * @brief Constructor * @return */ - WebVideoServer(rclcpp::Node::SharedPtr &nh); + WebVideoServer(rclcpp::Node::SharedPtr &node); /** * @brief Destructor - Cleans up @@ -59,7 +59,7 @@ class WebVideoServer void restreamFrames(double max_age); void cleanup_inactive_streams(); - rclcpp::Node::SharedPtr nh_; + rclcpp::Node::SharedPtr node_; rclcpp::WallTimer::SharedPtr cleanup_timer_; int ros_threads_; double publish_rate_; diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp index a866aa1..05a6c72 100644 --- a/src/h264_streamer.cpp +++ b/src/h264_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { H264Streamer::H264Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "mp4", "libx264", "video/mp4") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4") { /* possible quality presets: * ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo @@ -41,9 +41,9 @@ H264StreamerType::H264StreamerType() : boost::shared_ptr H264StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new H264Streamer(request, connection, nh)); + return boost::shared_ptr(new H264Streamer(request, connection, node)); } } diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index a91c31a..7dcc4cb 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -12,8 +12,8 @@ namespace web_video_server { ImageStreamer::ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - request_(request), connection_(connection), nh_(nh), inactive_(false) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + request_(request), connection_(connection), node_(node), inactive_(false) { topic_ = request.get_query_param_value_or_default("topic", ""); } @@ -23,8 +23,8 @@ ImageStreamer::~ImageStreamer() } ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), it_(nh), initialized_(false) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageStreamer(request, connection, node), it_(node), initialized_(false) { output_width_ = request.get_query_param_value_or_default("width", -1); output_height_ = request.get_query_param_value_or_default("height", -1); @@ -39,8 +39,8 @@ ImageTransportImageStreamer::~ImageTransportImageStreamer() void ImageTransportImageStreamer::start() { - image_transport::TransportHints hints(nh_.get(), default_transport_); - auto tnat = nh_->get_topic_names_and_types(); + image_transport::TransportHints hints(node_.get(), default_transport_); + auto tnat = node_->get_topic_names_and_types(); inactive_ = true; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -55,19 +55,19 @@ void ImageTransportImageStreamer::start() } // Get QoS profile from query parameter - RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); auto qos_profile = get_qos_profile_from_name(qos_profile_name_); if (!qos_profile) { qos_profile = rmw_qos_profile_default; RCLCPP_ERROR( - nh_->get_logger(), + node_->get_logger(), "Invalid QoS profile %s specified. Using default profile.", qos_profile_name_.c_str()); } // Create subscriber image_sub_ = image_transport::create_subscription( - nh_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), + node_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), default_transport_, qos_profile.value()); } @@ -80,29 +80,29 @@ void ImageTransportImageStreamer::restreamFrame(double max_age) if (inactive_ || !initialized_ ) return; try { - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { + if ( last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(output_size_image, nh_->now() ); // don't update last_frame, it may remain an old value. + sendImage(output_size_image, node_->now() ); // don't update last_frame, it may remain an old value. } } catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } @@ -170,41 +170,41 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::C initialized_ = true; } - last_frame = nh_->now(); + last_frame = node_->now(); sendImage(output_size_image, msg->header.stamp); } catch (cv_bridge::Exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; } catch (cv::Exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; } catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index 0c5cf88..c48bc4a 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -5,8 +5,8 @@ namespace web_video_server { MjpegStreamer::MjpegStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { quality_ = request.get_query_param_value_or_default("quality", 95); stream_.sendInitialHeader(); @@ -32,9 +32,9 @@ void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) boost::shared_ptr MjpegStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new MjpegStreamer(request, connection, nh)); + return boost::shared_ptr(new MjpegStreamer(request, connection, node)); } std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) @@ -48,8 +48,8 @@ std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpReq JpegSnapshotStreamer::JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) + rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node) { quality_ = request.get_query_param_value_or_default("quality", 95); } diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 0e490ac..9707b31 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -9,10 +9,10 @@ namespace web_video_server { LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node, const std::string &format_name, const std::string &codec_name, const std::string &content_type) : - ImageTransportImageStreamer(request, connection, nh), format_context_(0), codec_(0), codec_context_(0), video_stream_( + ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0), codec_context_(0), video_stream_( 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) { @@ -223,11 +223,11 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) ret = avcodec_send_frame(codec_context_, frame_); if (ret == AVERROR_EOF) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() encoder flushed\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() encoder flushed\n"); } else if (ret == AVERROR(EAGAIN)) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() need output read out\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() need output read out\n"); } if (ret < 0) { @@ -238,11 +238,11 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) bool got_packet = pkt->size > 0; if (ret == AVERROR_EOF) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); } else if (ret == AVERROR(EAGAIN)) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() needs more input\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() needs more input\n"); got_packet = false; } @@ -280,10 +280,10 @@ LibavStreamerType::LibavStreamerType(const std::string &format_name, const std:: boost::shared_ptr LibavStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { return boost::shared_ptr( - new LibavStreamer(request, connection, nh, format_name_, codec_name_, content_type_)); + new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_)); } std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) diff --git a/src/png_streamers.cpp b/src/png_streamers.cpp index 1a9f874..8d008f1 100644 --- a/src/png_streamers.cpp +++ b/src/png_streamers.cpp @@ -5,8 +5,8 @@ namespace web_video_server { PngStreamer::PngStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { quality_ = request.get_query_param_value_or_default("quality", 3); stream_.sendInitialHeader(); @@ -32,9 +32,9 @@ void PngStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) boost::shared_ptr PngStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new PngStreamer(request, connection, nh)); + return boost::shared_ptr(new PngStreamer(request, connection, node)); } std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) @@ -48,8 +48,8 @@ std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpReque PngSnapshotStreamer::PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) + rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node) { quality_ = request.get_query_param_value_or_default("quality", 3); } diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index 02f13ef..e1c89f6 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { stream_.sendInitialHeader(); qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); @@ -21,19 +21,19 @@ void RosCompressedStreamer::start() { const std::string compressed_topic = topic_ + "/compressed"; // Get QoS profile from query parameter - RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(), qos_profile_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(), qos_profile_name_.c_str()); auto qos_profile = get_qos_profile_from_name(qos_profile_name_); if (!qos_profile) { qos_profile = rmw_qos_profile_default; RCLCPP_ERROR( - nh_->get_logger(), + node_->get_logger(), "Invalid QoS profile %s specified. Using default profile.", qos_profile_name_.c_str()); } // Create subscriber const auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.value().history, 1), qos_profile.value()); - image_sub_ = nh_->create_subscription( + image_sub_ = node_->create_subscription( compressed_topic, qos, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); } @@ -42,9 +42,9 @@ void RosCompressedStreamer::restreamFrame(double max_age) if (inactive_ || (last_msg == 0)) return; - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { + if ( last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(last_msg, nh_->now() ); // don't update last_frame, it may remain an old value. + sendImage(last_msg, node_->now() ); // don't update last_frame, it may remain an old value. } } @@ -59,7 +59,7 @@ void RosCompressedStreamer::sendImage(const sensor_msgs::msg::CompressedImage::C content_type = "image/png"; } else { - RCLCPP_WARN(nh_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); + RCLCPP_WARN(node_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); return; } @@ -68,21 +68,21 @@ void RosCompressedStreamer::sendImage(const sensor_msgs::msg::CompressedImage::C catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } @@ -99,9 +99,9 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::msg::CompressedImag boost::shared_ptr RosCompressedStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new RosCompressedStreamer(request, connection, nh)); + return boost::shared_ptr(new RosCompressedStreamer(request, connection, node)); } std::string RosCompressedStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) diff --git a/src/vp8_streamer.cpp b/src/vp8_streamer.cpp index f45b7ca..33fb8ee 100644 --- a/src/vp8_streamer.cpp +++ b/src/vp8_streamer.cpp @@ -40,8 +40,8 @@ namespace web_video_server { Vp8Streamer::Vp8Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx", "video/webm") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "webm", "libvpx", "video/webm") { quality_ = request.get_query_param_value_or_default("quality", "realtime"); } @@ -83,9 +83,9 @@ Vp8StreamerType::Vp8StreamerType() : boost::shared_ptr Vp8StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new Vp8Streamer(request, connection, nh)); + return boost::shared_ptr(new Vp8Streamer(request, connection, node)); } } diff --git a/src/vp9_streamer.cpp b/src/vp9_streamer.cpp index 8fcefd8..da8fa65 100644 --- a/src/vp9_streamer.cpp +++ b/src/vp9_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { Vp9Streamer::Vp9Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx-vp9", "video/webm") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "webm", "libvpx-vp9", "video/webm") { } Vp9Streamer::~Vp9Streamer() @@ -30,9 +30,9 @@ Vp9StreamerType::Vp9StreamerType() : boost::shared_ptr Vp9StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new Vp9Streamer(request, connection, nh)); + return boost::shared_ptr(new Vp9Streamer(request, connection, node)); } } diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index dcfd012..190df47 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -33,7 +33,7 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler if (__verbose) { // TODO reenable - // RCLCPP_INFO(nh->get_logger(), "Handling Request: %s", request.uri.c_str()); + // RCLCPP_INFO(node->get_logger(), "Handling Request: %s", request.uri.c_str()); } try { @@ -43,32 +43,32 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler catch (std::exception &e) { // TODO reenable - // RCLCPP_WARN(nh->get_logger(), "Error Handling Request: %s", e.what()); + // RCLCPP_WARN(node->get_logger(), "Error Handling Request: %s", e.what()); return false; } return false; } -WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & nh) : - nh_(nh), handler_group_( +WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node) : + node_(node), handler_group_( async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) { - nh_->declare_parameter("port", 8080); - nh_->declare_parameter("verbose", true); - nh_->declare_parameter("address", "0.0.0.0"); - nh_->declare_parameter("server_threads", 1); - nh_->declare_parameter("ros_threads", 2); - nh_->declare_parameter("publish_rate", -1.0); - nh_->declare_parameter("default_stream_type", "mjpeg"); - - nh_->get_parameter("port", port_); - nh_->get_parameter("verbose", __verbose); - nh_->get_parameter("address", address_); + node_->declare_parameter("port", 8080); + node_->declare_parameter("verbose", true); + node_->declare_parameter("address", "0.0.0.0"); + node_->declare_parameter("server_threads", 1); + node_->declare_parameter("ros_threads", 2); + node_->declare_parameter("publish_rate", -1.0); + node_->declare_parameter("default_stream_type", "mjpeg"); + + node_->get_parameter("port", port_); + node_->get_parameter("verbose", __verbose); + node_->get_parameter("address", address_); int server_threads; - nh_->get_parameter("server_threads", server_threads); - nh_->get_parameter("ros_threads", ros_threads_); - nh_->get_parameter("publish_rate", publish_rate_); - nh_->get_parameter("default_stream_type", __default_stream_type); + node_->get_parameter("server_threads", server_threads); + node_->get_parameter("ros_threads", ros_threads_); + node_->get_parameter("publish_rate", publish_rate_); + node_->get_parameter("default_stream_type", __default_stream_type); stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); stream_types_["png"] = boost::shared_ptr(new PngStreamerType()); @@ -92,7 +92,7 @@ WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & nh) : } catch(boost::exception& e) { - RCLCPP_ERROR(nh_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); + RCLCPP_ERROR(node_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); throw; } } @@ -104,17 +104,17 @@ WebVideoServer::~WebVideoServer() void WebVideoServer::setup_cleanup_inactive_streams() { std::function callback = std::bind(&WebVideoServer::cleanup_inactive_streams, this); - cleanup_timer_ = nh_->create_wall_timer(500ms, callback); + cleanup_timer_ = node_->create_wall_timer(500ms, callback); } void WebVideoServer::spin() { server_->run(); - RCLCPP_INFO(nh_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); + RCLCPP_INFO(node_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); rclcpp::executors::MultiThreadedExecutor spinner(rclcpp::ExecutorOptions(), ros_threads_); - spinner.add_node(nh_); + spinner.add_node(node_); if ( publish_rate_ > 0 ) { - nh_->create_wall_timer(1s / publish_rate_, [this](){restreamFrames(1.0 / publish_rate_);}); + node_->create_wall_timer(1s / publish_rate_, [this](){restreamFrames(1.0 / publish_rate_);}); } spinner.spin(); server_->stop(); @@ -144,7 +144,7 @@ void WebVideoServer::cleanup_inactive_streams() { for (itr_type itr = new_end; itr < image_subscribers_.end(); ++itr) { - RCLCPP_INFO(nh_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); + RCLCPP_INFO(node_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); } } image_subscribers_.erase(new_end, image_subscribers_.end()); @@ -163,7 +163,7 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ if (type == std::string("ros_compressed")) { std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -178,11 +178,11 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ } if (!did_find_compressed_topic) { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); + RCLCPP_WARN(node_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); type = "mjpeg"; } } - boost::shared_ptr streamer = stream_types_[type]->create_streamer(request, connection, nh_); + boost::shared_ptr streamer = stream_types_[type]->create_streamer(request, connection, node_); streamer->start(); boost::mutex::scoped_lock lock(subscriber_mutex_); image_subscribers_.push_back(streamer); @@ -199,7 +199,7 @@ bool WebVideoServer::handle_snapshot(const async_web_server_cpp::HttpRequest &re async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end) { - boost::shared_ptr streamer(new JpegSnapshotStreamer(request, connection, nh_)); + boost::shared_ptr streamer(new JpegSnapshotStreamer(request, connection, node_)); streamer->start(); boost::mutex::scoped_lock lock(subscriber_mutex_); @@ -220,7 +220,7 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques { std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -235,7 +235,7 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques } if (!did_find_compressed_topic) { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); + RCLCPP_WARN(node_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); type = "mjpeg"; } } @@ -264,7 +264,7 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest { std::vector image_topics; std::vector camera_info_topics; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { // skip over topics with more than one type