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

bjsowa · tlwong3 · commit 048667d87e8a · 2025-06-06T12:20:39.000+08:00
* Use chrono steady_clock for frame timing
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
@@ -1,6 +1,8 @@
 #ifndef MULTIPART_STREAM_H_
 #define MULTIPART_STREAM_H_
 
+#include <chrono>
+
 #include <ros/ros.h>
 #include <async_web_server_cpp/http_connection.hpp>
 
@@ -16,10 +18,10 @@ class MultipartStream {
                   std::size_t max_queue_size=2);
 
   void sendInitialHeader();
-  void sendPartHeader(const ros::Time &time, const std::string& type, size_t payload_size);
+  void sendPartHeader(const std::chrono::steady_clock::time_point &time, const std::string& type, size_t payload_size);
   void sendPartFooter();
-  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 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,17 +18,17 @@ 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);
   virtual void convertGrayscale16toGrayscaleAlpha(const sensor_msgs::CompressedImageConstPtr &msg);
 
 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,34 +41,38 @@ 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() {
+void MultipartStream::sendPartFooter()
+{
   boost::shared_ptr<std::string> str(new std::string("\r\n--"+boundry_+"\r\n"));
   connection_->write(boost::asio::buffer(*str), str);
   if (max_queue_size_ > 0) pending_footers_.push(str);
 }
 
-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();
   }
 }
 
-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();
   }
 }
 
-bool MultipartStream::isBusy() {
+bool MultipartStream::isBusy()
+{
   while (!pending_footers_.empty() && pending_footers_.front().expired())
   {
     pending_footers_.pop();
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
@@ -22,19 +22,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<double> 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) {
@@ -107,8 +110,8 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageCons
 
   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

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`	`{`