Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node into devel

include/publisher.h

@@ -83,6 +83,10 @@ class Publisher
 
         virtual ~Publisher(){};
 
+        virtual void run() final;
+        virtual void stop() final;
+        virtual void loop() final;
+
         virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
 
         virtual void publish() final;
@@ -109,6 +113,8 @@ class Publisher
         std::string prefix_;
         std::string topic_;
 
+        std::thread pub_thread_;
+
         // PROFILING
         unsigned int n_publish_;
         std::chrono::microseconds acc_duration_;
@@ -140,14 +146,7 @@ inline void Publisher::publish()
 
 inline bool Publisher::ready()
 {
-    long unsigned int n_period = (long unsigned int)((ros::Time::now() - first_publish_time_).toSec() / period_);
-    if (n_period > last_n_period_)
-    {
-        last_n_period_ = n_period;
-        return true;
-    }
-
-    return false;
+    return true;
 }
 
 inline void Publisher::printProfiling(std::ostream &_stream) const
@@ -159,6 +158,35 @@ inline void Publisher::printProfiling(std::ostream &_stream) const
             << "\n\tmax time:             " << 1e-3 * max_duration_.count() << " ms" << std::endl;
 }
 
+inline void Publisher::run()
+{
+    pub_thread_ = std::thread(&Publisher::loop, this);
+}
+
+inline void Publisher::stop()
+{
+    pub_thread_.join();
+}
+
+inline void Publisher::loop()
+{
+    auto awake_time = std::chrono::system_clock::now();
+    auto period = std::chrono::duration<int,std::milli>((int)(period_*1e3));
+
+    WOLF_DEBUG("Started publisher ", name_, " loop");
+
+    while (ros::ok())
+    {
+        if (ready())
+            publish();
+
+        // relax to fit output rate
+        awake_time += period;
+        std::this_thread::sleep_until(awake_time);
+    }
+    WOLF_DEBUG("Publisher ", name_, " loop finished");
+}
+
 template<typename T>
 inline T Publisher::getParamWithDefault(const ParamsServer &_server,
                                         const std::string &_param_name,

src/node.cpp

@@ -19,6 +19,7 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
+#include <chrono>
 #include "node.h"
 #include "ros/time.h"
 #include "core/solver/factory_solver.h"
@@ -142,7 +143,8 @@ void WolfRosNode::solve()
 
 void WolfRosNode::solveLoop()
 {
-    ros::Rate solverRate(1/(solver_->getPeriod()+1e-9)); // 1ns added to allow pausing if rosbag paused if period==0
+    auto awake_time = std::chrono::system_clock::now();
+    auto period = std::chrono::duration<int,std::milli>((int)(solver_->getPeriod()*1e3+1)); // 1ms added to allow pausing if rosbag paused if period==0
     WOLF_DEBUG("Started solver loop");
 
     while (ros::ok())
@@ -153,7 +155,8 @@ void WolfRosNode::solveLoop()
             break;
 
         // relax to fit output rate
-        solverRate.sleep();
+        awake_time += period;
+        std::this_thread::sleep_until(awake_time);
     }
     WOLF_DEBUG("Solver loop finished");
 }
@@ -225,13 +228,12 @@ int main(int argc, char **argv)
     int policy=SCHED_FIFO;
     pthread_setschedparam(solver_thread.native_handle(), SCHED_FIFO, &Priority_Param);
 
+    // Init publishers threads
+    for(auto pub : wolf_node.publishers_)
+        pub->run();
+
     while (ros::ok())
     {
-        // publish periodically
-        for(auto pub : wolf_node.publishers_)
-            if (pub->ready())
-                pub->publish();
-
         // check that subscribers received data (every second)
         if ((ros::Time::now() - last_check).toSec() > 1)
         {
@@ -249,10 +251,16 @@ int main(int argc, char **argv)
         // relax to fit output rate
         loopRate.sleep();
     }
+
+    // Stop solver thread
     WOLF_DEBUG("Node is shutting down outside loop... waiting for the thread to stop...");
     solver_thread.join();
     WOLF_DEBUG("thread stopped.");
 
+    // Stop publishers threads
+    for(auto pub : wolf_node.publishers_)
+        pub->stop();
+
     // PROFILING ========================================
     wolf_node.createProfilingFile();