Skip to content

Commit

Permalink
ControlManager: fixed race conditions in tracker/controller switching
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Jan 27, 2024
1 parent 539cf34 commit de8e80c
Showing 1 changed file with 78 additions and 95 deletions.
173 changes: 78 additions & 95 deletions src/control_manager/control_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -670,8 +670,8 @@ class ControlManager : public nodelet::Nodelet {

// | ------------------ emergency triggered? ------------------ |

bool failsafe_triggered_ = false;
bool eland_triggered_ = false;
std::atomic<bool> failsafe_triggered_ = false;
std::atomic<bool> eland_triggered_ = false;

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

Expand Down Expand Up @@ -7110,6 +7110,7 @@ std::tuple<bool, std::string> ControlManager::hover(void) {
/* //{ ehover() */

std::tuple<bool, std::string> ControlManager::ehover(void) {

if (!is_initialized_)
return std::tuple(false, "the ControlManager is not initialized");

Expand Down Expand Up @@ -7178,6 +7179,7 @@ std::tuple<bool, std::string> ControlManager::ehover(void) {
/* eland() //{ */

std::tuple<bool, std::string> ControlManager::eland(void) {

if (!is_initialized_)
return std::tuple(false, "the ControlManager is not initialized");

Expand Down Expand Up @@ -7929,6 +7931,7 @@ void ControlManager::toggleOutput(const bool& input) {
/* switchTracker() //{ */

std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {

mrs_lib::Routine profiler_routine = profiler_.createRoutine("switchTracker");
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);

Expand Down Expand Up @@ -8176,61 +8179,58 @@ std::tuple<bool, std::string> ControlManager::switchController(const std::string
/* updateTrackers() //{ */

void ControlManager::updateTrackers(void) {

mrs_lib::Routine profiler_routine = profiler_.createRoutine("updateTrackers");
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);

// copy member variables
auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);

// --------------------------------------------------------------
// | Update the trackers |
// --------------------------------------------------------------

std::optional<mrs_msgs::TrackerCommand> tracker_command;

// for each tracker
for (size_t i = 0; i < tracker_list_.size(); i++) {

if (i == active_tracker_idx) {

try {
std::scoped_lock lock(mutex_tracker_list_);

// active tracker => update and retrieve the command
tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
}
catch (std::runtime_error& exrun) {
unsigned int active_tracker_idx;

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active tracker");
{
std::scoped_lock lock(mutex_tracker_list_);

eland();
}
active_tracker_idx = active_tracker_idx_;

} else {
// for each tracker
for (size_t i = 0; i < tracker_list_.size(); i++) {

try {
std::scoped_lock lock(mutex_tracker_list_);
if (i == active_tracker_idx) {

// nonactive tracker => just update without retrieving the command
tracker_list_[i]->update(uav_state, last_control_output);
}
catch (std::runtime_error& exrun) {
try {
// active tracker => update and retrieve the command
tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
}
catch (std::runtime_error& exrun) {
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
tracker_command = {};
}

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the tracker '%s'", _tracker_names_[i].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the tracker");
} else {

eland();
try {
// nonactive tracker => just update without retrieving the command
tracker_list_[i]->update(uav_state, last_control_output);
}
catch (std::runtime_error& exrun) {
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_[i].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
}
}
}
}

if (active_tracker_idx == _null_tracker_idx_) {
return;
if (active_tracker_idx == _null_tracker_idx_) {
return;
}
}

if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
Expand All @@ -8251,32 +8251,22 @@ void ControlManager::updateTrackers(void) {

} else {

if (active_tracker_idx != _null_tracker_idx_) {
if (active_tracker_idx == _ehover_tracker_idx_) {

if (active_tracker_idx == _ehover_tracker_idx_) {
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
failsafe();

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the ehover tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
} else {

failsafe();
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());

if (_tracker_error_action_ == ELAND_STR) {
eland();
} else if (_tracker_error_action_ == EHOVER_STR) {
ehover();
} else {

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());

if (_tracker_error_action_ == ELAND_STR) {
eland();
} else if (_tracker_error_action_ == EHOVER_STR) {
ehover();
} else {
failsafe();
}
failsafe();
}

} else {

std::scoped_lock lock(mutex_last_tracker_cmd_);

last_tracker_cmd_ = tracker_command;
}
}
}
Expand All @@ -8286,19 +8276,19 @@ void ControlManager::updateTrackers(void) {
/* updateControllers() //{ */

void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {

mrs_lib::Routine profiler_routine = profiler_.createRoutine("updateControllers");
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);

// copy member variables
auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);

// | ----------------- update the controllers ----------------- |

// the trackers are not running
if (!last_tracker_cmd) {

ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: tracker command is empty, giving controller just the uav_state");
ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: tracker command is empty, giving controllers just the uav_state");

// give the controllers current uav state
{
Expand All @@ -8315,49 +8305,40 @@ void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {

Controller::ControlOutput control_output;

// for each controller
for (size_t i = 0; i < controller_list_.size(); i++) {

if (i == active_controller_idx) {
unsigned int active_controller_idx;

try {
std::scoped_lock lock(mutex_controller_list_);

// active controller => update and retrieve the command
control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
}
catch (std::runtime_error& exrun) {

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active controller (%s)", _controller_names_[active_controller_idx].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
{
std::scoped_lock lock(mutex_controller_list_);

if (eland_triggered_) {
active_controller_idx = active_controller_idx_;

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering failsafe due to an exception in the active controller (eland is already active)");
failsafe();
// for each controller
for (size_t i = 0; i < controller_list_.size(); i++) {

} else {
if (i == active_controller_idx) {

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active controller");
eland();
try {
// active controller => update and retrieve the command
control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
}
}

} else {
catch (std::runtime_error& exrun) {

try {
std::scoped_lock lock(mutex_controller_list_);
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
_controller_names_[active_controller_idx].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
}

// nonactive controller => just update without retrieving the command
controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
}
catch (std::runtime_error& exrun) {
} else {

ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland (somebody should notice this)");
try {
// nonactive controller => just update without retrieving the command
controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
}
catch (std::runtime_error& exrun) {

eland();
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
}
}
}
}
Expand Down Expand Up @@ -8385,21 +8366,21 @@ void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {

if (controller_status) {

if (active_controller_idx_ == _failsafe_controller_idx_) {
if (failsafe_triggered_) {

ROS_ERROR("[ControlManager]: failsafe controller returned empty command, disabling control");
ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");

toggleOutput(false);

} else if (active_controller_idx_ == _eland_controller_idx_) {
} else if (eland_triggered_) {

ROS_ERROR("[ControlManager]: triggering failsafe, the emergency controller returned empty or invalid command");
ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");

failsafe();

} else {

ROS_ERROR("[ControlManager]: triggering eland, the controller returned empty or invalid command");
ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");

eland();
}
Expand All @@ -8412,6 +8393,7 @@ void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
/* publish() //{ */

void ControlManager::publish(void) {

mrs_lib::Routine profiler_routine = profiler_.createRoutine("publish");
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);

Expand Down Expand Up @@ -8708,6 +8690,7 @@ void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::opt
/* initializeControlOutput() //{ */

void ControlManager::initializeControlOutput(void) {

Controller::ControlOutput controller_output;

controller_output.diagnostics.total_mass = _uav_mass_;
Expand Down

0 comments on commit de8e80c

Please sign in to comment.