From 52ea5123f8162a091856f4886f0498f637a3a634 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 9 Sep 2020 09:57:27 +0200
Subject: [PATCH] WIP

---
 include/node.h |  2 ++
 src/node.cpp   | 33 ++++++++++++++++++++++++---------
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/include/node.h b/include/node.h
index 4e599aa..46fa0e4 100644
--- a/include/node.h
+++ b/include/node.h
@@ -82,9 +82,11 @@ class WolfRosNode
         void solve();
         void solveLoop();
 
+
         void broadcastTf();
 
         void visualize();
+        void visualizeLoop();
 
         bool updateTf();
 };
diff --git a/src/node.cpp b/src/node.cpp
index 34a456b..fdf83ab 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -179,6 +179,7 @@ void WolfRosNode::solveLoop()
 {
     ros::Rate solverRate(1/solver_->getPeriod());
     WOLF_DEBUG("Started solver loop");
+    WOLF_TRACE("HI SOLVER");
 
     while (ros::ok())
     {
@@ -191,6 +192,27 @@ void WolfRosNode::solveLoop()
         solverRate.sleep();
     }
     WOLF_DEBUG("Solver loop finished");
+    WOLF_TRACE("BYE SOLVER");
+}
+
+void WolfRosNode::visualizeLoop()
+{
+    ros::Rate visualizerRate(1/viz_period_);
+    // visualize periodically
+    WOLF_DEBUG("Started visualize loop");
+    WOLF_TRACE("HI VISUALIZE");
+    while (ros::ok())
+    {
+        visualize();
+
+        if(ros::isShuttingDown())
+            break;
+
+        // relax to fit output rate
+        visualizerRate.sleep();
+    }
+    WOLF_DEBUG("Visualize loop finished");
+    WOLF_TRACE("BYE VISUALIZE");
 }
 
 int main(int argc, char **argv)
@@ -208,6 +230,8 @@ int main(int argc, char **argv)
 
     // Solver thread
     std::thread solver_thread(&WolfRosNode::solveLoop, &wolf_node);
+    // Visualizer thread
+    std::thread visualizer_thread(&WolfRosNode::visualizeLoop, &wolf_node);
 
     while (ros::ok())
     {
@@ -215,15 +239,6 @@ int main(int argc, char **argv)
         wolf_node.updateTf();
         wolf_node.broadcastTf();
 
-        // visualize periodically
-        auto start3 = std::chrono::high_resolution_clock::now();
-        if ((ros::Time::now() - last_viz_time).toSec() >= wolf_node.viz_period_)
-        {
-            std::cout << "Last Viz since/viz_period_ " << (ros::Time::now() - last_viz_time).toSec() << " / " << wolf_node.viz_period_ << std::endl;
-            wolf_node.visualize();
-            last_viz_time = ros::Time::now();
-        }
-
         // publish periodically
         for(auto pub : wolf_node.publishers_)
             if (pub->ready())
-- 
GitLab