From 4529ceb473ddda9f83b1c0639c13d996242333ba Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sun, 11 Oct 2020 15:08:18 +0200
Subject: [PATCH] more precise rate of publishing
---
include/publisher.h | 14 ++++++++++----
1 file changed, 10 insertions(+), 4 deletions(-)
diff --git a/include/publisher.h b/include/publisher.h
index d962d61..d8e06d3 100644
--- a/include/publisher.h
+++ b/include/publisher.h
@@ -48,7 +48,8 @@ class Publisher
const ParamsServer& _server,
const ProblemPtr _problem) :
problem_(_problem),
- last_publish_time_(ros::Time(0)),
+ first_publish_time_(ros::Time(0)),
+ n_published_(0),
prefix_("ROS publisher/" + _unique_name)
{
period_ = _server.getParam<double>(prefix_ + "/period");
@@ -72,20 +73,25 @@ class Publisher
ProblemPtr problem_;
ros::Publisher publisher_;
double period_;
- ros::Time last_publish_time_;
+ ros::Time first_publish_time_;
+ long unsigned int n_published_;
std::string prefix_;
std::string topic_;
};
inline void Publisher::publish()
{
- last_publish_time_ = ros::Time::now();
+ if (n_published_ == 0)
+ first_publish_time_ = ros::Time::now();
+
+ n_published_++;
publishDerived();
}
inline bool Publisher::ready()
{
- return (ros::Time::now() - last_publish_time_).toSec() > period_;
+ long unsigned int n_pub = (long unsigned int)((ros::Time::now() - first_publish_time_).toSec() / period_);
+ return n_pub > n_published_;
}
inline std::string Publisher::getTopic() const
--
GitLab