diff --git a/include/node.h b/include/node.h index aa55635e3c515bf6f99205699f57fb056283ab5d..4e599aa5612080129cb5ae1afaeaf511a5e65dd0 100644 --- a/include/node.h +++ b/include/node.h @@ -51,9 +51,7 @@ class WolfRosNode // ROS node handle ros::NodeHandle nh_; - double solver_period_; double viz_period_; - double publisher_period_; // visualizer std::shared_ptr<Visualizer> viz_; @@ -66,7 +64,6 @@ class WolfRosNode protected: // solver SolverManagerPtr solver_; - SolverManager::ReportVerbosity solver_verbose_; // transforms tf::TransformBroadcaster tfb_; diff --git a/src/node.cpp b/src/node.cpp index dfd0557dba2b923fbc85dc7a497fb6df94b608a7..1e770d3fe77447c216573b72e1d36d29cb57dd0c 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -38,11 +38,7 @@ WolfRosNode::WolfRosNode() // SOLVER ROS_INFO("Creating solver..."); - solver_ = std::static_pointer_cast<SolverCeres>(FactorySolver::create("SolverCeres", problem_ptr_, server)); - int solver_verbose_int; - solver_period_ = server.getParam<double>("solver/period"); - solver_verbose_int = server.getParam<int>("solver/verbose"); - solver_verbose_ = static_cast<SolverManager::ReportVerbosity>(solver_verbose_int); + solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server); // ROS SUBSCRIBERS ROS_INFO("Creating subscribers..."); @@ -90,10 +86,10 @@ WolfRosNode::WolfRosNode() void WolfRosNode::solve() { - if (solver_verbose_ != SolverManager::ReportVerbosity::QUIET) + if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET) ROS_INFO("================ solve =================="); - std::string report = solver_->solve(solver_verbose_); + std::string report = solver_->solve(); if (!report.empty()) std::cout << report << std::endl; } @@ -189,12 +185,11 @@ void WolfRosNode::broadcastTf() void WolfRosNode::solveLoop() { WOLF_DEBUG("Started solver loop"); - ros::Rate solverRate(1/solver_period_); while (ros::ok()) { - solve(); - solverRate.sleep(); + if (solver_->ready()) + solve(); if(ros::isShuttingDown()) break;