diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index 83d6817..090faa1 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -1,6 +1,8 @@ #ifndef IMAGE_STREAMER_H_ #define IMAGE_STREAMER_H_ +#include + #include #include #include @@ -29,7 +31,7 @@ class ImageStreamer /** * Restreams the last received image frame if older than max_age. */ - virtual void restreamFrame(double max_age) = 0; + virtual void restreamFrame(std::chrono::duration max_age) = 0; std::string getTopic() { @@ -56,8 +58,8 @@ class ImageTransportImageStreamer : public ImageStreamer protected: virtual cv::Mat decodeImage(const sensor_msgs::ImageConstPtr& msg); - virtual void sendImage(const cv::Mat &, const ros::Time &time) = 0; - virtual void restreamFrame(double max_age); + virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time) = 0; + virtual void restreamFrame(std::chrono::duration max_age); virtual void initialize(const cv::Mat &); image_transport::Subscriber image_sub_; @@ -66,7 +68,7 @@ class ImageTransportImageStreamer : public ImageStreamer bool invert_; std::string default_transport_; - ros::Time last_frame; + std::chrono::steady_clock::time_point last_frame_; cv::Mat output_size_image; boost::mutex send_mutex_; diff --git a/include/web_video_server/jpeg_streamers.h b/include/web_video_server/jpeg_streamers.h index 7be2991..7f3fa67 100644 --- a/include/web_video_server/jpeg_streamers.h +++ b/include/web_video_server/jpeg_streamers.h @@ -18,7 +18,7 @@ class MjpegStreamer : public ImageTransportImageStreamer ros::NodeHandle& nh); ~MjpegStreamer(); protected: - virtual void sendImage(const cv::Mat &, const ros::Time &time); + virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time); private: MultipartStream stream_; @@ -41,7 +41,7 @@ class JpegSnapshotStreamer : public ImageTransportImageStreamer async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh); ~JpegSnapshotStreamer(); protected: - virtual void sendImage(const cv::Mat &, const ros::Time &time); + virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time); private: int quality_; diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.h index e8dc12a..b944543 100644 --- a/include/web_video_server/libav_streamer.h +++ b/include/web_video_server/libav_streamer.h @@ -1,6 +1,8 @@ #ifndef LIBAV_STREAMERS_H_ #define LIBAV_STREAMERS_H_ +#include + #include #include "web_video_server/image_streamer.h" #include "async_web_server_cpp/http_request.hpp" @@ -32,7 +34,7 @@ class LibavStreamer : public ImageTransportImageStreamer protected: virtual void initializeEncoder(); - virtual void sendImage(const cv::Mat&, const ros::Time& time); + virtual void sendImage(const cv::Mat&, const std::chrono::steady_clock::time_point& time); virtual void initialize(const cv::Mat&); AVOutputFormat* output_format_; AVFormatContext* format_context_; @@ -45,7 +47,7 @@ class LibavStreamer : public ImageTransportImageStreamer private: AVFrame* frame_; struct SwsContext* sws_context_; - ros::Time first_image_timestamp_; + std::optional first_image_timestamp_; boost::mutex encode_mutex_; std::string format_name_; diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.h index 476ee73..4eff231 100644 --- a/include/web_video_server/multipart_stream.h +++ b/include/web_video_server/multipart_stream.h @@ -10,7 +10,7 @@ namespace web_video_server { struct PendingFooter { - ros::Time timestamp; + std::chrono::steady_clock::time_point timestamp; boost::weak_ptr contents; }; @@ -21,10 +21,10 @@ class MultipartStream { std::size_t max_queue_size=1); void sendInitialHeader(); - void sendPartHeader(const ros::Time &time, const std::string& type, size_t payload_size); - void sendPartFooter(const ros::Time &time); - void sendPartAndClear(const ros::Time &time, const std::string& type, std::vector &data); - void sendPart(const ros::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, + void sendPartHeader(const std::chrono::steady_clock::time_point &time, const std::string& type, size_t payload_size); + void sendPartFooter(const std::chrono::steady_clock::time_point &time); + void sendPartAndClear(const std::chrono::steady_clock::time_point &time, const std::string& type, std::vector &data); + void sendPart(const std::chrono::steady_clock::time_point &time, const std::string& type, const boost::asio::const_buffer &buffer, async_web_server_cpp::HttpConnection::ResourcePtr resource); private: diff --git a/include/web_video_server/png_streamers.h b/include/web_video_server/png_streamers.h index 6b72b66..1d7ba4e 100644 --- a/include/web_video_server/png_streamers.h +++ b/include/web_video_server/png_streamers.h @@ -18,7 +18,7 @@ class PngStreamer : public ImageTransportImageStreamer ros::NodeHandle& nh); ~PngStreamer(); protected: - virtual void sendImage(const cv::Mat &, const ros::Time &time); + virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time); virtual cv::Mat decodeImage(const sensor_msgs::ImageConstPtr& msg); private: @@ -42,7 +42,7 @@ class PngSnapshotStreamer : public ImageTransportImageStreamer async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh); ~PngSnapshotStreamer(); protected: - virtual void sendImage(const cv::Mat &, const ros::Time &time); + virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time); virtual cv::Mat decodeImage(const sensor_msgs::ImageConstPtr& msg); private: diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h index 7e8af37..76a1ab3 100644 --- a/include/web_video_server/ros_compressed_streamer.h +++ b/include/web_video_server/ros_compressed_streamer.h @@ -18,16 +18,16 @@ class RosCompressedStreamer : public ImageStreamer ~RosCompressedStreamer(); virtual void start(); - virtual void restreamFrame(double max_age); + virtual void restreamFrame(std::chrono::duration max_age); protected: - virtual void sendImage(const sensor_msgs::CompressedImageConstPtr &msg, const ros::Time &time); + virtual void sendImage(const sensor_msgs::CompressedImageConstPtr &msg, const std::chrono::steady_clock::time_point &time); private: void imageCallback(const sensor_msgs::CompressedImageConstPtr &msg); MultipartStream stream_; ros::Subscriber image_sub_; - ros::Time last_frame; + std::chrono::steady_clock::time_point last_frame_; sensor_msgs::CompressedImageConstPtr last_msg; boost::mutex send_mutex_; }; diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index e4e0d73..ce77434 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -50,7 +50,7 @@ class WebVideoServer async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); private: - void restreamFrames(double max_age); + void restreamFrames(std::chrono::duration max_age); void cleanup_inactive_streams(); ros::NodeHandle nh_; diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 4a54523..38266c0 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -50,14 +50,15 @@ void ImageTransportImageStreamer::initialize(const cv::Mat &) { } -void ImageTransportImageStreamer::restreamFrame(double max_age) +void ImageTransportImageStreamer::restreamFrame(std::chrono::duration max_age) { if (inactive_ || !initialized_ ) return; try { - if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) { + if (last_frame_ + max_age < std::chrono::steady_clock::now()) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(output_size_image, ros::Time::now() ); // don't update last_frame, it may remain an old value. + // don't update last_frame, it may remain an old value. + sendImage(output_size_image, std::chrono::steady_clock::now()); } } catch (boost::system::system_error &e) @@ -148,8 +149,8 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr initialized_ = true; } - last_frame = ros::Time::now(); - sendImage(output_size_image, msg->header.stamp); + last_frame_ = std::chrono::steady_clock::now(); + sendImage(output_size_image, last_frame_); } catch (cv_bridge::Exception &e) { diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index 8f8eac9..e385d6d 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -18,7 +18,9 @@ MjpegStreamer::~MjpegStreamer() boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. } -void MjpegStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void MjpegStreamer::sendImage( + const cv::Mat & img, + const std::chrono::steady_clock::time_point & time) { std::vector encode_params; #if CV_VERSION_MAJOR >= 3 @@ -64,7 +66,9 @@ JpegSnapshotStreamer::~JpegSnapshotStreamer() boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. } -void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void JpegSnapshotStreamer::sendImage( + const cv::Mat & img, + const std::chrono::steady_clock::time_point & time) { std::vector encode_params; #if CV_VERSION_MAJOR >= 3 @@ -78,7 +82,8 @@ void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) cv::imencode(".jpeg", img, encoded_buffer, encode_params); char stamp[20]; - sprintf(stamp, "%.06lf", time.toSec()); + snprintf(stamp, sizeof(stamp), "%.06lf", + std::chrono::duration_cast>(time.time_since_epoch()).count()); async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) .header("Connection", "close") .header("Server", "web_video_server") diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 8c65ce8..79dd7c0 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -53,7 +53,7 @@ LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, const std::string &format_name, const std::string &codec_name, const std::string &content_type) : ImageTransportImageStreamer(request, connection, nh), output_format_(0), format_context_(0), codec_(0), codec_context_(0), video_stream_( - 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( + 0), frame_(0), sws_context_(0), first_image_timestamp_(std::nullopt), format_name_( format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) { @@ -256,11 +256,12 @@ void LibavStreamer::initializeEncoder() { } -void LibavStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void LibavStreamer::sendImage( + const cv::Mat & img, + const std::chrono::steady_clock::time_point & time) { boost::mutex::scoped_lock lock(encode_mutex_); - if (first_image_timestamp_.isZero()) - { + if (!first_image_timestamp_.has_value()) { first_image_timestamp_ = time; } std::vector encoded_frame; @@ -353,7 +354,8 @@ void LibavStreamer::sendImage(const cv::Mat &img, const ros::Time &time) std::size_t size; uint8_t *output_buf; - double seconds = (time - first_image_timestamp_).toSec(); + double seconds = std::chrono::duration_cast>(time - + first_image_timestamp_.value()).count(); // Encode video at 1/0.95 to minimize delay pkt.pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); if (pkt.pts <= 0) diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index c38056d..0fbf9ba 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -21,9 +21,13 @@ void MultipartStream::sendInitialHeader() { connection_->write("--"+boundry_+"\r\n"); } -void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& type, size_t payload_size) { +void MultipartStream::sendPartHeader( + const std::chrono::steady_clock::time_point & time, const std::string & type, + size_t payload_size) +{ char stamp[20]; - sprintf(stamp, "%.06lf", time.toSec()); + snprintf(stamp, sizeof(stamp), "%.06lf", + std::chrono::duration_cast>(time.time_since_epoch()).count()); boost::shared_ptr > headers( new std::vector()); headers->push_back(async_web_server_cpp::HttpHeader("Content-type", type)); @@ -37,7 +41,8 @@ void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& t connection_->write(async_web_server_cpp::HttpReply::to_buffers(*headers), headers); } -void MultipartStream::sendPartFooter(const ros::Time &time) { +void MultipartStream::sendPartFooter(const std::chrono::steady_clock::time_point & time) +{ boost::shared_ptr str(new std::string("\r\n--"+boundry_+"\r\n")); PendingFooter pf; pf.timestamp = time; @@ -46,36 +51,40 @@ void MultipartStream::sendPartFooter(const ros::Time &time) { if (max_queue_size_ > 0) pending_footers_.push(pf); } -void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& type, - std::vector &data) { - if (!isBusy()) - { +void MultipartStream::sendPartAndClear( + const std::chrono::steady_clock::time_point & time, const std::string & type, + std::vector & data) +{ + if (!isBusy()) { sendPartHeader(time, type, data.size()); connection_->write_and_clear(data); sendPartFooter(time); } } -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()) - { +void MultipartStream::sendPart( + const std::chrono::steady_clock::time_point & time, const std::string & type, + const boost::asio::const_buffer & buffer, + async_web_server_cpp::HttpConnection::ResourcePtr resource) +{ + if (!isBusy()) { sendPartHeader(time, type, boost::asio::buffer_size(buffer)); connection_->write(buffer, resource); sendPartFooter(time); } } -bool MultipartStream::isBusy() { - ros::Time currentTime = ros::Time::now(); - while (!pending_footers_.empty()) - { +bool MultipartStream::isBusy() +{ + auto current_time = std::chrono::steady_clock::now(); + while (!pending_footers_.empty()) { if (pending_footers_.front().contents.expired()) { pending_footers_.pop(); } else { - ros::Time footerTime = pending_footers_.front().timestamp; - if ((currentTime - footerTime).toSec() > 0.5) { + auto footer_time = pending_footers_.front().timestamp; + if (std::chrono::duration_cast>((current_time - + footer_time)).count() > 0.5) + { pending_footers_.pop(); } else { break; diff --git a/src/png_streamers.cpp b/src/png_streamers.cpp index 666c6ed..c0e9b1a 100644 --- a/src/png_streamers.cpp +++ b/src/png_streamers.cpp @@ -34,8 +34,7 @@ cv::Mat PngStreamer::decodeImage(const sensor_msgs::ImageConstPtr& msg) } } - -void PngStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void PngStreamer::sendImage(const cv::Mat & img, const std::chrono::steady_clock::time_point & time) { std::vector encode_params; #if CV_VERSION_MAJOR >= 3 @@ -95,7 +94,7 @@ cv::Mat PngSnapshotStreamer::decodeImage(const sensor_msgs::ImageConstPtr& msg) } } -void PngSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void PngSnapshotStreamer::sendImage(const cv::Mat &img, const std::chrono::steady_clock::time_point &time) { std::vector encode_params; #if CV_VERSION_MAJOR >= 3 @@ -109,7 +108,9 @@ void PngSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) cv::imencode(".png", img, encoded_buffer, encode_params); char stamp[20]; - sprintf(stamp, "%.06lf", time.toSec()); + snprintf( + stamp, sizeof(stamp), "%.06lf", + std::chrono::duration_cast>(time.time_since_epoch()).count()); async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) .header("Connection", "close") .header("Server", "web_video_server") diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index e1a142e..290ad6f 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -21,19 +21,22 @@ void RosCompressedStreamer::start() { image_sub_ = nh_.subscribe(compressed_topic, 1, &RosCompressedStreamer::imageCallback, this); } -void RosCompressedStreamer::restreamFrame(double max_age) +void RosCompressedStreamer::restreamFrame(std::chrono::duration max_age) { if (inactive_ || (last_msg == 0)) return; - if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) { + if (last_frame_ + max_age < std::chrono::steady_clock::now()) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(last_msg, ros::Time::now() ); // don't update last_frame, it may remain an old value. + // don't update last_frame, it may remain an old value. + sendImage(last_msg, std::chrono::steady_clock::now()); } } -void RosCompressedStreamer::sendImage(const sensor_msgs::CompressedImageConstPtr &msg, - const ros::Time &time) { +void RosCompressedStreamer::sendImage( + const sensor_msgs::CompressedImageConstPtr & msg, + const std::chrono::steady_clock::time_point & time) +{ try { std::string content_type; if(msg->format.find("jpeg") != std::string::npos || msg->format.find("jpg") != std::string::npos) { @@ -74,8 +77,8 @@ void RosCompressedStreamer::sendImage(const sensor_msgs::CompressedImageConstPtr void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageConstPtr &msg) { boost::mutex::scoped_lock lock(send_mutex_); // protects last_msg and last_frame last_msg = msg; - last_frame = ros::Time(msg->header.stamp.sec, msg->header.stamp.nsec); - sendImage(last_msg, last_frame); + last_frame_ = std::chrono::steady_clock::now(); + sendImage(last_msg, last_frame_); } diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index a4428ec..4e0bc77 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -14,6 +14,8 @@ #include "web_video_server/vp9_streamer.h" #include "async_web_server_cpp/http_reply.hpp" +using namespace std::chrono_literals; + namespace web_video_server { @@ -109,7 +111,7 @@ void WebVideoServer::spin() ros::Rate r(publish_rate_); while( ros::ok() ) { - this->restreamFrames( 1.0 / publish_rate_ ); + this->restreamFrames( 1s / publish_rate_ ); r.sleep(); } } else { @@ -119,7 +121,7 @@ void WebVideoServer::spin() server_->stop(); } -void WebVideoServer::restreamFrames( double max_age ) +void WebVideoServer::restreamFrames(std::chrono::duration max_age) { boost::mutex::scoped_lock lock(subscriber_mutex_);