Skip to content
Snippets Groups Projects
Commit 4529ceb4 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

more precise rate of publishing

parent 4b0fb236
No related branches found
No related tags found
3 merge requests!11new release,!10new release,!4Gauss
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment