From 3471833c717bc3f040221668561f10ee1de9cfc8 Mon Sep 17 00:00:00 2001 From: QuentinTorg Date: Mon, 4 Nov 2024 18:14:42 -0500 Subject: [PATCH 1/2] fix issue where images timestamped in a different time domain from host failed to stop streams after connection is closed --- include/web_video_server/multipart_stream.h | 2 +- src/multipart_stream.cpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.h index 476ee73..6de4fd2 100644 --- a/include/web_video_server/multipart_stream.h +++ b/include/web_video_server/multipart_stream.h @@ -28,7 +28,7 @@ class MultipartStream { async_web_server_cpp::HttpConnection::ResourcePtr resource); private: - bool isBusy(); + bool isBusy(const ros::Time ¤tTime); private: const std::size_t max_queue_size_; diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index c38056d..00a4f5b 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -48,7 +48,7 @@ void MultipartStream::sendPartFooter(const ros::Time &time) { void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& type, std::vector &data) { - if (!isBusy()) + if (!isBusy(time)) { sendPartHeader(time, type, data.size()); connection_->write_and_clear(data); @@ -59,7 +59,7 @@ void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& void MultipartStream::sendPart(const ros::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, async_web_server_cpp::HttpConnection::ResourcePtr resource) { - if (!isBusy()) + if (!isBusy(time)) { sendPartHeader(time, type, boost::asio::buffer_size(buffer)); connection_->write(buffer, resource); @@ -67,8 +67,7 @@ void MultipartStream::sendPart(const ros::Time &time, const std::string& type, } } -bool MultipartStream::isBusy() { - ros::Time currentTime = ros::Time::now(); +bool MultipartStream::isBusy(const ros::Time ¤tTime) { while (!pending_footers_.empty()) { if (pending_footers_.front().contents.expired()) { From 77368b5ae6b62ccd407d8f1425cc3267eb9cd70c Mon Sep 17 00:00:00 2001 From: QuentinTorg Date: Mon, 4 Nov 2024 18:29:14 -0500 Subject: [PATCH 2/2] use ros::Time::now() instead of camera timestamp when publishing --- include/web_video_server/multipart_stream.h | 2 +- src/image_streamer.cpp | 2 +- src/multipart_stream.cpp | 7 ++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.h index 6de4fd2..476ee73 100644 --- a/include/web_video_server/multipart_stream.h +++ b/include/web_video_server/multipart_stream.h @@ -28,7 +28,7 @@ class MultipartStream { async_web_server_cpp::HttpConnection::ResourcePtr resource); private: - bool isBusy(const ros::Time ¤tTime); + bool isBusy(); private: const std::size_t max_queue_size_; diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 4a54523..07f1356 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -149,7 +149,7 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr } last_frame = ros::Time::now(); - sendImage(output_size_image, msg->header.stamp); + sendImage(output_size_image, last_frame); } catch (cv_bridge::Exception &e) { diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index 00a4f5b..c38056d 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -48,7 +48,7 @@ void MultipartStream::sendPartFooter(const ros::Time &time) { void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& type, std::vector &data) { - if (!isBusy(time)) + if (!isBusy()) { sendPartHeader(time, type, data.size()); connection_->write_and_clear(data); @@ -59,7 +59,7 @@ void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& void MultipartStream::sendPart(const ros::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, async_web_server_cpp::HttpConnection::ResourcePtr resource) { - if (!isBusy(time)) + if (!isBusy()) { sendPartHeader(time, type, boost::asio::buffer_size(buffer)); connection_->write(buffer, resource); @@ -67,7 +67,8 @@ void MultipartStream::sendPart(const ros::Time &time, const std::string& type, } } -bool MultipartStream::isBusy(const ros::Time ¤tTime) { +bool MultipartStream::isBusy() { + ros::Time currentTime = ros::Time::now(); while (!pending_footers_.empty()) { if (pending_footers_.front().contents.expired()) {