Use chrono steady clock for frame timing (backport #173) (#174)

bjsowa · web-flow · commit 51cb5f168e67 · 2024-11-20T10:54:06.000+01:00
* Use chrono steady_clock for frame timing

* Increase cmake minimum version
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.10.2)
 project(web_video_server)
 
 ## Find catkin macros and libraries
diff --git 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 <chrono>
+
 #include <ros/ros.h>
 #include <image_transport/image_transport.h>
 #include <opencv2/opencv.hpp>
@@ -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<double> 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<double> 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
@@ -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
@@ -1,6 +1,8 @@
 #ifndef LIBAV_STREAMERS_H_
 #define LIBAV_STREAMERS_H_
 
+#include <chrono>
+
 #include <image_transport/image_transport.h>
 #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,8 +47,9 @@ class LibavStreamer : public ImageTransportImageStreamer
 private:
   AVFrame* frame_;
   struct SwsContext* sws_context_;
-  ros::Time first_image_timestamp_;
   boost::mutex encode_mutex_;
+  bool first_image_received_;
+  std::chrono::steady_clock::time_point first_image_time_;
 
   std::string format_name_;
   std::string codec_name_;
diff --git 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<std::string> 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<unsigned char> &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<unsigned char> &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
@@ -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
@@ -18,16 +18,16 @@ class RosCompressedStreamer : public ImageStreamer
   ~RosCompressedStreamer();
 
   virtual void start();
-  virtual void restreamFrame(double max_age);
+  virtual void restreamFrame(std::chrono::duration<double> 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
@@ -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<double> max_age);
   void cleanup_inactive_streams();
 
   ros::NodeHandle nh_;
diff --git 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<double> 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
@@ -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<int> 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<int> 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<std::chrono::duration<double>>(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
@@ -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_received_(false), first_image_time_(), format_name_(
         format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0)
 {
 
@@ -256,12 +256,14 @@ 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())
-  {
-    first_image_timestamp_ = time;
+  if (!first_image_received_) {
+    first_image_received_ = true;
+    first_image_time_ = time;
   }
   std::vector<uint8_t> encoded_frame;
 #if (LIBAVUTIL_VERSION_MAJOR < 53)
@@ -353,7 +355,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<std::chrono::duration<double>>(time -
+        first_image_time_).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
@@ -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<std::chrono::duration<double>>(time.time_since_epoch()).count());
   boost::shared_ptr<std::vector<async_web_server_cpp::HttpHeader> > headers(
       new std::vector<async_web_server_cpp::HttpHeader>());
   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<std::string> 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<unsigned char> &data) {
-  if (!isBusy())
-  {
+void MultipartStream::sendPartAndClear(
+  const std::chrono::steady_clock::time_point & time, const std::string & type,
+  std::vector<unsigned char> & 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<std::chrono::duration<double>>((current_time -
+        footer_time)).count() > 0.5)
+      {
         pending_footers_.pop();
       } else {
         break;
diff --git 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<int> 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<int> 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<std::chrono::duration<double>>(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
diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-cmake_minimum_required(VERSION 2.8.3)`
	`1`	`+cmake_minimum_required(VERSION 3.10.2)`
`2`	`2`	`project(web_video_server)`
`3`	`3`
`4`	`4`	`## Find catkin macros and libraries`
Original file line number	Diff line number	Diff line change
`@@ -50,14 +50,15 @@ void ImageTransportImageStreamer::initialize(const cv::Mat &)`
`50`	`50`	`{`
`51`	`51`	`}`
`52`	`52`
`53`		`-void ImageTransportImageStreamer::restreamFrame(double max_age)`
	`53`	`+void ImageTransportImageStreamer::restreamFrame(std::chrono::duration<double> max_age)`
`54`	`54`	`{`
`55`	`55`	`if (inactive_ \|\| !initialized_ )`
`56`	`56`	`return;`
`57`	`57`	`try {`
`58`		`- if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) {`
	`58`	`+ if (last_frame_ + max_age < std::chrono::steady_clock::now()) {`
`59`	`59`	`boost::mutex::scoped_lock lock(send_mutex_);`
`60`		`- sendImage(output_size_image, ros::Time::now() ); // don't update last_frame, it may remain an old value.`
	`60`	`+ // don't update last_frame, it may remain an old value.`
	`61`	`+ sendImage(output_size_image, std::chrono::steady_clock::now());`
`61`	`62`	`}`
`62`	`63`	`}`
`63`	`64`	`catch (boost::system::system_error &e)`
`@@ -148,8 +149,8 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr`
`148`	`149`	`initialized_ = true;`
`149`	`150`	`}`
`150`	`151`
`151`		`- last_frame = ros::Time::now();`
`152`		`- sendImage(output_size_image, msg->header.stamp);`
	`152`	`+ last_frame_ = std::chrono::steady_clock::now();`
	`153`	`+ sendImage(output_size_image, last_frame_);`
`153`	`154`	`}`
`154`	`155`	`catch (cv_bridge::Exception &e)`
`155`	`156`	`{`