Visualization period was wrongly read from yaml file. Visualization takes too...

Visualization period was wrongly read from yaml file. Visualization takes too long when everything is activated

src/node.cpp

@@ -58,7 +58,7 @@ WolfRosNode::WolfRosNode()
 
     viz_ = std::make_shared<Visualizer>();
     viz_->initialize(nh_);
-    viz_period_ = server.getParam<int>("visualizer/period");
+    viz_period_ = server.getParam<double>("visualizer/period");
 
     // ROS PUBLISHERS
     ROS_INFO("Creating publishers...");
@@ -82,8 +82,9 @@ void WolfRosNode::solve()
         ROS_INFO("================ solve ==================");
 
     std::string report = solver_->solve();
-    if (!report.empty())
-        std::cout << report << std::endl;
+    // if (!report.empty()){
+    //     std::cout << report << std::endl;
+    // }
 }
 
 void WolfRosNode::visualize()
@@ -93,7 +94,7 @@ void WolfRosNode::visualize()
     viz_->visualize(problem_ptr_);
     auto stop     = std::chrono::high_resolution_clock::now();
     auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
-    //std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
+    std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
 }
 
 bool WolfRosNode::updateTf()
@@ -218,6 +219,7 @@ int main(int argc, char **argv)
         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();
         }