diff --git a/launch/estimation_manager.launch b/launch/estimation_manager.launch index 59df8b4..08f0c5a 100644 --- a/launch/estimation_manager.launch +++ b/launch/estimation_manager.launch @@ -70,6 +70,7 @@ + diff --git a/src/estimation_manager/estimation_manager.cpp b/src/estimation_manager/estimation_manager.cpp index 91a0dcb..bcb6d40 100644 --- a/src/estimation_manager/estimation_manager.cpp +++ b/src/estimation_manager/estimation_manager.cpp @@ -345,6 +345,9 @@ class EstimationManager : public nodelet::Nodelet { bool callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res); int estimator_switch_count_ = 0; + ros::ServiceServer srvs_reset_estimator_; + bool callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res); + ros::ServiceServer srvs_toggle_callbacks_; bool callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); @@ -1042,6 +1045,7 @@ void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEve /*//{ initialize service servers */ srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this); + srvs_reset_estimator_ = nh_.advertiseService("reset_estimator_in", &EstimationManager::callbackResetEstimator, this); srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this); /*//}*/ @@ -1131,6 +1135,74 @@ bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, } /*//}*/ +/*//{ callbackResetEstimator() */ +bool EstimationManager::callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) { + + if (!sm_->isInitialized()) { + return false; + } + + if (!callbacks_enabled_) { + res.success = false; + res.message = ("Service callbacks are disabled"); + ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str()); + return true; + } + + + bool target_estimator_found = false; + boost::shared_ptr target_estimator; + for (auto estimator : estimator_list_) { + if (estimator->getName() == req.value) { + target_estimator = estimator; + target_estimator_found = true; + break; + } + } + + if (target_estimator_found) { + + + if (target_estimator->getName() == active_estimator_->getName()) { + res.success = false; + res.message = ("Cannot reset active estimator"); + ROS_WARN("[%s]: Ignoring service call. Cannot reset active estimator.", getName().c_str()); + return true; + } + + target_estimator->reset(); + ROS_INFO("[EstimationManager]: Estimator %s reset", target_estimator->getName().c_str()); + + double t_wait_left = 5; + while (t_wait_left > 0) { + ROS_INFO("[EstimationManager]: Attempting starting %s estimator", target_estimator->getName().c_str()); + target_estimator->start(); + + if (target_estimator->isRunning()) { + ROS_INFO("[EstimationManager]: Reset of %s estimator successful", target_estimator->getName().c_str()); + break; + } + + const double start_period = 1.0; + ros::Duration(start_period).sleep(); + t_wait_left -= start_period; + + } + + } else { + ROS_WARN("[%s]: Reset of invalid estimator %s requested", getName().c_str(), req.value.c_str()); + res.success = false; + res.message = ("Not a valid estimator type"); + return true; + } + + res.success = true; + res.message = "Estimator reset successful"; + + return true; +} +/*//}*/ + /* //{ callbackToggleServiceCallbacks() */ bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {