From 7a9a59a5d26a0114b635647529ae4717917388b5 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sat, 10 Feb 2024 14:52:40 +0100 Subject: [PATCH] fixed diagnostics bug in constraint and gain managers --- src/constraint_manager.cpp | 46 ++++++++++++++++++++------------- src/gain_manager.cpp | 52 +++++++++++++++++++++++--------------- 2 files changed, 59 insertions(+), 39 deletions(-) diff --git a/src/constraint_manager.cpp b/src/constraint_manager.cpp index 28ad72d9..38c31627 100644 --- a/src/constraint_manager.cpp +++ b/src/constraint_manager.cpp @@ -41,7 +41,8 @@ class ConstraintManager : public nodelet::Nodelet { private: ros::NodeHandle nh_; - bool is_initialized_ = false; + + std::atomic is_initialized_ = false; // | ----------------------- parameters ----------------------- | @@ -230,7 +231,7 @@ void ConstraintManager::onInit() { _map_type_default_constraints_.insert(std::pair(*it, temp_str)); } - ROS_INFO("[ConstraintManager]: done loading dynamical params"); + ROS_INFO("[ConstraintManager]: done loading dynamic params"); current_constraints_ = ""; last_estimator_name_ = ""; @@ -258,7 +259,7 @@ void ConstraintManager::onInit() { // | ----------------------- publishers ----------------------- | - ph_diagnostics_ = mrs_lib::PublisherHandler(nh_, "diagnostics_out", 1); + ph_diagnostics_ = mrs_lib::PublisherHandler(nh_, "diagnostics_out", 10); // | ------------------------- timers ------------------------- | @@ -383,7 +384,7 @@ bool ConstraintManager::callbackSetConstraints(mrs_msgs::String::Request& req, m return true; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); if (!stringInVector(req.value, _constraint_names_)) { @@ -396,7 +397,7 @@ bool ConstraintManager::callbackSetConstraints(mrs_msgs::String::Request& req, m return true; } - if (!stringInVector(req.value, _map_type_allowed_constraints_.at(estimation_diagnostics.current_state_estimator))) { + if (!stringInVector(req.value, _map_type_allowed_constraints_.at(estimation_diagnostics->current_state_estimator))) { ss << "the constraints '" << req.value.c_str() << "' are not allowed given the current odometry type"; @@ -483,38 +484,38 @@ void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) return; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); // | --- automatically set constraints when the state estimator changes -- | - if (estimation_diagnostics.current_state_estimator != last_estimator_name_) { + if (estimation_diagnostics->current_state_estimator != last_estimator_name_) { ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(), - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); std::map::iterator it; - it = _map_type_default_constraints_.find(estimation_diagnostics.current_state_estimator); + it = _map_type_default_constraints_.find(estimation_diagnostics->current_state_estimator); if (it == _map_type_default_constraints_.end()) { ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the state estimator type '%s' was not specified in the constraint_manager's config!", - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); } else { // if the current constraints are within the allowed state estimator types, do nothing - if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics.current_state_estimator))) { + if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics->current_state_estimator))) { - last_estimator_name = estimation_diagnostics.current_state_estimator; + last_estimator_name = estimation_diagnostics->current_state_estimator; // else, try to set the initial constraints } else { ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the current constraints '%s' are not within the allowed constraints for '%s'", current_constraints.c_str(), - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); if (setConstraints(it->second)) { - last_estimator_name = estimation_diagnostics.current_state_estimator; + last_estimator_name = estimation_diagnostics->current_state_estimator; ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: constraints set to initial: '%s'", it->second.c_str()); @@ -561,10 +562,14 @@ void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) { return; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_); + if (current_constraints == "") { // this could happend just before timerConstraintManagement() finishes + return; + } + mrs_msgs::ConstraintManagerDiagnostics diagnostics; diagnostics.stamp = ros::Time::now(); @@ -574,11 +579,11 @@ void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) { // get the available constraints { std::map>::iterator it; - it = _map_type_allowed_constraints_.find(estimation_diagnostics.current_state_estimator); + it = _map_type_allowed_constraints_.find(estimation_diagnostics->current_state_estimator); if (it == _map_type_allowed_constraints_.end()) { - ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the odometry.type '%s' was not specified in the constraint_manager's config!", - estimation_diagnostics.current_state_estimator.c_str()); + ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the state estimator '%s' was not specified in the constraint_manager's config!", + estimation_diagnostics->current_state_estimator.c_str()); } else { diagnostics.available = it->second; } @@ -589,6 +594,11 @@ void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) { std::map::iterator it; it = _constraints_.find(current_constraints); + if (it == _constraints_.end()) { + ROS_ERROR("[ConstraintManager]: current constraints '%s' not found in the constraint list!", current_constraints.c_str()); + return; + } + diagnostics.current_values = it->second.constraints; } diff --git a/src/gain_manager.cpp b/src/gain_manager.cpp index b83aa4a5..25cace1e 100644 --- a/src/gain_manager.cpp +++ b/src/gain_manager.cpp @@ -52,7 +52,8 @@ class GainManager : public nodelet::Nodelet { private: ros::NodeHandle nh_; - bool is_initialized_ = false; + + std::atomic is_initialized_ = false; // | ----------------------- parameters ----------------------- | @@ -254,7 +255,7 @@ void GainManager::onInit() { // | ----------------------- publishers ----------------------- | - ph_diagnostics_ = mrs_lib::PublisherHandler(nh_, "diagnostics_out", 1); + ph_diagnostics_ = mrs_lib::PublisherHandler(nh_, "diagnostics_out", 10); // | ------------------------- timers ------------------------- | @@ -410,7 +411,7 @@ bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::Str return true; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); if (!stringInVector(req.value, _gain_names_)) { @@ -423,7 +424,7 @@ bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::Str return true; } - if (!stringInVector(req.value, _map_type_allowed_gains_.at(estimation_diagnostics.current_state_estimator))) { + if (!stringInVector(req.value, _map_type_allowed_gains_.at(estimation_diagnostics->current_state_estimator))) { ss << "the gains '" << req.value.c_str() << "' are not allowed given the current state estimator"; @@ -463,7 +464,7 @@ bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::Str // | timers | // -------------------------------------------------------------- -/* gainManagementTimer() //{ */ +/* timerGainManagement() //{ */ void GainManager::timerGainManagement(const ros::TimerEvent& event) { @@ -478,47 +479,47 @@ void GainManager::timerGainManagement(const ros::TimerEvent& event) { return; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); - if (!sh_control_manager_diag_.hasMsg()) { return; } - auto control_manager_diagnostics = *sh_estimation_diag_.getMsg(); + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); + + auto control_manager_diagnostics = sh_estimation_diag_.getMsg(); auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_); auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_); // | --- automatically set _gains_ when currrent state estimator changes -- | - if (estimation_diagnostics.current_state_estimator != last_estimator_name) { + if (estimation_diagnostics->current_state_estimator != last_estimator_name) { ROS_INFO_THROTTLE(1.0, "[GainManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(), - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); std::map::iterator it; - it = _map_type_default_gains_.find(estimation_diagnostics.current_state_estimator); + it = _map_type_default_gains_.find(estimation_diagnostics->current_state_estimator); if (it == _map_type_default_gains_.end()) { ROS_WARN_THROTTLE(1.0, "[GainManager]: the state estimator '%s' was not specified in the gain_manager's config!", - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); } else { // if the current gains are within the allowed estimator types, do nothing - if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics.current_state_estimator))) { + if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics->current_state_estimator))) { - last_estimator_name = estimation_diagnostics.current_state_estimator; + last_estimator_name = estimation_diagnostics->current_state_estimator; // else, try to set the default gains } else { ROS_WARN_THROTTLE(1.0, "[GainManager]: the current gains '%s' are not within the allowed gains for '%s'", current_gains.c_str(), - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); if (setGains(it->second)) { - last_estimator_name = estimation_diagnostics.current_state_estimator; + last_estimator_name = estimation_diagnostics->current_state_estimator; ROS_INFO_THROTTLE(1.0, "[GainManager]: gains set to default: '%s'", it->second.c_str()); @@ -535,7 +536,7 @@ void GainManager::timerGainManagement(const ros::TimerEvent& event) { //} -/* dignosticsTimer() //{ */ +/* timerDiagnostics() //{ */ void GainManager::timerDiagnostics(const ros::TimerEvent& event) { @@ -556,10 +557,14 @@ void GainManager::timerDiagnostics(const ros::TimerEvent& event) { return; } - auto estimation_diagnostics = *sh_estimation_diag_.getMsg(); - auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_); + if (current_gains == "") { // this could happend just before timerGainManagement() finishes + return; + } + + auto estimation_diagnostics = sh_estimation_diag_.getMsg(); + mrs_msgs::GainManagerDiagnostics diagnostics; diagnostics.stamp = ros::Time::now(); @@ -569,11 +574,11 @@ void GainManager::timerDiagnostics(const ros::TimerEvent& event) { // get the available gains { std::map>::iterator it; - it = _map_type_allowed_gains_.find(estimation_diagnostics.current_state_estimator); + it = _map_type_allowed_gains_.find(estimation_diagnostics->current_state_estimator); if (it == _map_type_allowed_gains_.end()) { ROS_WARN_THROTTLE(1.0, "[GainManager]: the estimator name '%s' was not specified in the gain_manager's config!", - estimation_diagnostics.current_state_estimator.c_str()); + estimation_diagnostics->current_state_estimator.c_str()); } else { diagnostics.available = it->second; } @@ -584,6 +589,11 @@ void GainManager::timerDiagnostics(const ros::TimerEvent& event) { std::map::iterator it; it = _gains_.find(current_gains); + if (it == _gains_.end()) { + ROS_ERROR("[GainManager]: current gains '%s' not found in the gain list!", current_gains.c_str()); + return; + } + diagnostics.current_values.kpxy = it->second.kpxy; diagnostics.current_values.kvxy = it->second.kvxy; diagnostics.current_values.kaxy = it->second.kaxy;