Skip to content

Commit

Permalink
fixed diagnostics bug in constraint and gain managers
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Feb 10, 2024
1 parent dd7c2da commit 7a9a59a
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 39 deletions.
46 changes: 28 additions & 18 deletions src/constraint_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,8 @@ class ConstraintManager : public nodelet::Nodelet {

private:
ros::NodeHandle nh_;
bool is_initialized_ = false;

std::atomic<bool> is_initialized_ = false;

// | ----------------------- parameters ----------------------- |

Expand Down Expand Up @@ -230,7 +231,7 @@ void ConstraintManager::onInit() {
_map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
}

ROS_INFO("[ConstraintManager]: done loading dynamical params");
ROS_INFO("[ConstraintManager]: done loading dynamic params");

current_constraints_ = "";
last_estimator_name_ = "";
Expand Down Expand Up @@ -258,7 +259,7 @@ void ConstraintManager::onInit() {

// | ----------------------- publishers ----------------------- |

ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 1);
ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 10);

// | ------------------------- timers ------------------------- |

Expand Down Expand Up @@ -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_)) {

Expand All @@ -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";

Expand Down Expand Up @@ -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<std::string, std::string>::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());

Expand Down Expand Up @@ -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();
Expand All @@ -574,11 +579,11 @@ void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
// get the available constraints
{
std::map<std::string, std::vector<std::string>>::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;
}
Expand All @@ -589,6 +594,11 @@ void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::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;
}

Expand Down
52 changes: 31 additions & 21 deletions src/gain_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class GainManager : public nodelet::Nodelet {

private:
ros::NodeHandle nh_;
bool is_initialized_ = false;

std::atomic<bool> is_initialized_ = false;

// | ----------------------- parameters ----------------------- |

Expand Down Expand Up @@ -254,7 +255,7 @@ void GainManager::onInit() {

// | ----------------------- publishers ----------------------- |

ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 1);
ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 10);

// | ------------------------- timers ------------------------- |

Expand Down Expand Up @@ -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_)) {

Expand All @@ -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";

Expand Down Expand Up @@ -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) {

Expand All @@ -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<std::string, std::string>::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());

Expand All @@ -535,7 +536,7 @@ void GainManager::timerGainManagement(const ros::TimerEvent& event) {

//}

/* dignosticsTimer() //{ */
/* timerDiagnostics() //{ */

void GainManager::timerDiagnostics(const ros::TimerEvent& event) {

Expand All @@ -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();
Expand All @@ -569,11 +574,11 @@ void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
// get the available gains
{
std::map<std::string, std::vector<std::string>>::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;
}
Expand All @@ -584,6 +589,11 @@ void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
std::map<std::string, Gains_t>::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;
Expand Down

0 comments on commit 7a9a59a

Please sign in to comment.