diff --git a/include/node.h b/include/node.h index 4e599aa5612080129cb5ae1afaeaf511a5e65dd0..46fa0e4e8a11c0bc6526d3f34937f17ba1841cad 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 34a456b4d212edad06f786ca95ee8b164d1add65..fdf83abc9935f32d85dd30ec3e154680b5222eb8 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())