Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(system_error_monitor): output emergency reason for debugging #510

Merged
merged 7 commits into from
Mar 18, 2022
Merged

feat(system_error_monitor): output emergency reason for debugging #510

Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3eee4e9
feat(system_error_monitor): output emergency reason for debugging
takayuki5168 Mar 11, 2022
eeb8406
print all emergency reasons with 5 sec throttle using macros
takayuki5168 Mar 12, 2022
0ee9e03
not use macros
takayuki5168 Mar 12, 2022
8ecf0af
use const std::shared_ptr for argument of clock
takayuki5168 Mar 12, 2022
a767e1a
use enum class for DebugLevel
takayuki5168 Mar 12, 2022
142b5a5
resolved review's comment
takayuki5168 Mar 12, 2022
ce58358
Merge branch 'main' into feature/print-emergency-reaon
takayuki5168 Mar 16, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 37 additions & 0 deletions system/system_error_monitor/src/system_error_monitor_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,35 @@

namespace
{
enum class DebugLevel { DEBUG, INFO, WARN, ERROR, FATAL };

template <DebugLevel debug_level>
void logThrottledNamed(
const std::string & logger_name, const rclcpp::Clock::SharedPtr clock, const double duration_ms,
const std::string & message)
{
static std::unordered_map<std::string, rclcpp::Time> last_output_time;
if (last_output_time.count(logger_name) != 0) {
const auto time_from_last_output = clock->now() - last_output_time.at(logger_name);
if (time_from_last_output.seconds() * 1000.0 < duration_ms) {
return;
}
}

last_output_time[logger_name] = clock->now();
if constexpr (debug_level == DebugLevel::DEBUG) {
RCLCPP_DEBUG(rclcpp::get_logger(logger_name), message.c_str());
} else if constexpr (debug_level == DebugLevel::INFO) {
RCLCPP_INFO(rclcpp::get_logger(logger_name), message.c_str());
} else if constexpr (debug_level == DebugLevel::WARN) {
RCLCPP_WARN(rclcpp::get_logger(logger_name), message.c_str());
} else if constexpr (debug_level == DebugLevel::ERROR) {
RCLCPP_ERROR(rclcpp::get_logger(logger_name), message.c_str());
} else if constexpr (debug_level == DebugLevel::FATAL) {
RCLCPP_FATAL(rclcpp::get_logger(logger_name), message.c_str());
}
kenji-miyake marked this conversation as resolved.
Show resolved Hide resolved
}

std::vector<std::string> split(const std::string & str, const char delim)
{
std::vector<std::string> elems;
Expand Down Expand Up @@ -111,9 +140,17 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray(
diag_array.status.push_back(decorateDiag(hazard_diag, "[Safe Fault]"));
}
for (const auto & hazard_diag : hazard_status.diag_latent_fault) {
const std::string logger_name = "system_error_monitor " + hazard_diag.name;
logThrottledNamed<DebugLevel::WARN>(
logger_name, clock, 5000, "[Latent Fault]: " + hazard_diag.message);

diag_array.status.push_back(decorateDiag(hazard_diag, "[Latent Fault]"));
}
for (const auto & hazard_diag : hazard_status.diag_single_point_fault) {
const std::string logger_name = "system_error_monitor " + hazard_diag.name;
logThrottledNamed<DebugLevel::ERROR>(
logger_name, clock, 5000, "[Single Point Fault]: " + hazard_diag.message);

diag_array.status.push_back(decorateDiag(hazard_diag, "[Single Point Fault]"));
}

Expand Down