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