diff --git a/src/node.cpp b/src/node.cpp index 20b975f80535d58ce7e210aacfa4ec50a3bae152..34a456b4d212edad06f786ca95ee8b164d1add65 100644 --- a/src/node.cpp +++ b/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(); }