diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp index 8646fe540bfc659addc15de5b5f3df29f1609ce5..06d483f0e44a0c4d2195e4308b37a03630fad20a 100644 --- a/src/ros_publish.cpp +++ b/src/ros_publish.cpp @@ -143,11 +143,11 @@ namespace dynamicgraph sotNOSIGNAL, MAKE_SIGNAL_STRING(name, true, "int", "trigger")), rate_ (ROS_JOINT_STATE_PUBLISHER_RATE), - lastPublicated_ () + nextPublication_ () { try { - lastPublicated_ = ros::Time::now (); + nextPublication_ = ros::Time::now (); } catch (const std::exception& exc) { throw std::runtime_error ("Failed to call ros::Time::now ():" + std::string (exc.what ())); @@ -220,7 +220,6 @@ namespace dynamicgraph } //lock the mutex to avoid deleting the signal during a call to trigger - while(! mutex_.try_lock() ){} signalDeregistration(signal); bindedSignal_.erase (signal); mutex_.unlock(); @@ -258,11 +257,10 @@ namespace dynamicgraph { typedef std::map<std::string, bindedSignal_t>::iterator iterator_t; - ros::Duration dt = ros::Time::now () - lastPublicated_; - if (dt < rate_) + if (ros::Time::now() <= nextPublication_) return dummy; - lastPublicated_ = ros::Time::now(); + nextPublication_ = ros::Time::now() + rate_; while(! mutex_.try_lock() ){} for (iterator_t it = bindedSignal_.begin (); diff --git a/src/ros_publish.hh b/src/ros_publish.hh index f52a8e6c2e00ab938e779ecdc675834c485553dd..6005f3ee295cf62f51c101f9856153dcd8e37224 100644 --- a/src/ros_publish.hh +++ b/src/ros_publish.hh @@ -93,7 +93,7 @@ namespace dynamicgraph std::map<std::string, bindedSignal_t> bindedSignal_; dynamicgraph::SignalTimeDependent<int,int> trigger_; ros::Duration rate_; - ros::Time lastPublicated_; + ros::Time nextPublication_; boost::interprocess::interprocess_mutex mutex_; }; } // end of namespace dynamicgraph.