|
| 1 | +// Copyright 2024 The Autoware Foundation |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "component_monitor_node.hpp" |
| 16 | + |
| 17 | +#include "unit_conversions.hpp" |
| 18 | + |
| 19 | +#include <rclcpp/rclcpp.hpp> |
| 20 | + |
| 21 | +#include <autoware_internal_msgs/msg/resource_usage_report.hpp> |
| 22 | + |
| 23 | +#include <boost/process.hpp> |
| 24 | + |
| 25 | +#include <cctype> |
| 26 | +#include <cstdint> |
| 27 | +#include <exception> |
| 28 | +#include <functional> |
| 29 | +#include <sstream> |
| 30 | +#include <string> |
| 31 | +#include <unordered_map> |
| 32 | +#include <utility> |
| 33 | +#include <vector> |
| 34 | + |
| 35 | +namespace autoware::component_monitor |
| 36 | +{ |
| 37 | +ComponentMonitor::ComponentMonitor(const rclcpp::NodeOptions & node_options) |
| 38 | +: Node("component_monitor", node_options), publish_rate_(declare_parameter<double>("publish_rate")) |
| 39 | +{ |
| 40 | + usage_pub_ = |
| 41 | + create_publisher<ResourceUsageReport>("~/component_system_usage", rclcpp::SensorDataQoS()); |
| 42 | + |
| 43 | + // Make sure top ins installed and is in path |
| 44 | + const auto path_top = boost::process::search_path("top"); |
| 45 | + if (path_top.empty()) { |
| 46 | + RCLCPP_ERROR_STREAM(get_logger(), "Couldn't find 'top' in path."); |
| 47 | + rclcpp::shutdown(); |
| 48 | + } |
| 49 | + |
| 50 | + // Get the PID of the current process |
| 51 | + int pid = getpid(); |
| 52 | + |
| 53 | + environment_ = boost::this_process::environment(); |
| 54 | + environment_["LC_NUMERIC"] = "en_US.UTF-8"; |
| 55 | + |
| 56 | + on_timer_tick_wrapped_ = std::bind(&ComponentMonitor::on_timer_tick, this, pid); |
| 57 | + |
| 58 | + timer_ = rclcpp::create_timer( |
| 59 | + this, get_clock(), rclcpp::Rate(publish_rate_).period(), on_timer_tick_wrapped_); |
| 60 | +} |
| 61 | + |
| 62 | +void ComponentMonitor::on_timer_tick(const int pid) const |
| 63 | +{ |
| 64 | + if (usage_pub_->get_subscription_count() == 0) return; |
| 65 | + |
| 66 | + try { |
| 67 | + auto usage_msg = pid_to_report(pid); |
| 68 | + usage_msg.header.stamp = this->now(); |
| 69 | + usage_msg.pid = pid; |
| 70 | + usage_pub_->publish(usage_msg); |
| 71 | + } catch (std::exception & e) { |
| 72 | + RCLCPP_ERROR(get_logger(), "%s", e.what()); |
| 73 | + } catch (...) { |
| 74 | + RCLCPP_ERROR(get_logger(), "An unknown error occurred."); |
| 75 | + } |
| 76 | +} |
| 77 | + |
| 78 | +ComponentMonitor::ResourceUsageReport ComponentMonitor::pid_to_report(const pid_t & pid) const |
| 79 | +{ |
| 80 | + const auto std_out = run_system_command("top -b -n 1 -E k -p " + std::to_string(pid)); |
| 81 | + |
| 82 | + const auto fields = parse_lines_into_words(std_out); |
| 83 | + |
| 84 | + ResourceUsageReport report; |
| 85 | + report.cpu_cores_utilized = std::stof(fields.back().at(8)) / 100.0f; |
| 86 | + report.total_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(3))); |
| 87 | + report.free_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(5))); |
| 88 | + report.process_memory_bytes = parse_memory_res(fields.back().at(5)); |
| 89 | + |
| 90 | + return report; |
| 91 | +} |
| 92 | + |
| 93 | +std::stringstream ComponentMonitor::run_system_command(const std::string & cmd) const |
| 94 | +{ |
| 95 | + int out_fd[2]; |
| 96 | + if (pipe2(out_fd, O_CLOEXEC) != 0) { |
| 97 | + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on out_fd"); |
| 98 | + } |
| 99 | + boost::process::pipe out_pipe{out_fd[0], out_fd[1]}; |
| 100 | + boost::process::ipstream is_out{std::move(out_pipe)}; |
| 101 | + |
| 102 | + int err_fd[2]; |
| 103 | + if (pipe2(err_fd, O_CLOEXEC) != 0) { |
| 104 | + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on err_fd"); |
| 105 | + } |
| 106 | + boost::process::pipe err_pipe{err_fd[0], err_fd[1]}; |
| 107 | + boost::process::ipstream is_err{std::move(err_pipe)}; |
| 108 | + |
| 109 | + boost::process::child c( |
| 110 | + cmd, environment_, boost::process::std_out > is_out, boost::process::std_err > is_err); |
| 111 | + c.wait(); |
| 112 | + |
| 113 | + if (c.exit_code() != 0) { |
| 114 | + std::ostringstream os; |
| 115 | + is_err >> os.rdbuf(); |
| 116 | + RCLCPP_ERROR_STREAM(get_logger(), "Error running command: " << os.str()); |
| 117 | + } |
| 118 | + |
| 119 | + std::stringstream sstream; |
| 120 | + sstream << is_out.rdbuf(); |
| 121 | + return sstream; |
| 122 | +} |
| 123 | + |
| 124 | +ComponentMonitor::VecVecStr ComponentMonitor::parse_lines_into_words( |
| 125 | + const std::stringstream & std_out) |
| 126 | +{ |
| 127 | + VecVecStr fields; |
| 128 | + std::string line; |
| 129 | + std::istringstream input{std_out.str()}; |
| 130 | + |
| 131 | + while (std::getline(input, line)) { |
| 132 | + std::istringstream iss_line{line}; |
| 133 | + std::string word; |
| 134 | + std::vector<std::string> words; |
| 135 | + |
| 136 | + while (iss_line >> word) { |
| 137 | + words.push_back(word); |
| 138 | + } |
| 139 | + |
| 140 | + fields.push_back(words); |
| 141 | + } |
| 142 | + |
| 143 | + return fields; |
| 144 | +} |
| 145 | + |
| 146 | +std::uint64_t ComponentMonitor::parse_memory_res(const std::string & mem_res) |
| 147 | +{ |
| 148 | + // example 1: 12.3g |
| 149 | + // example 2: 123 (without suffix, just bytes) |
| 150 | + static const std::unordered_map<char, std::function<std::uint64_t(double)>> unit_map{ |
| 151 | + {'k', unit_conversions::kib_to_bytes<double>}, {'m', unit_conversions::mib_to_bytes<double>}, |
| 152 | + {'g', unit_conversions::gib_to_bytes<double>}, {'t', unit_conversions::tib_to_bytes<double>}, |
| 153 | + {'p', unit_conversions::pib_to_bytes<double>}, {'e', unit_conversions::eib_to_bytes<double>}}; |
| 154 | + |
| 155 | + if (std::isdigit(mem_res.back())) { |
| 156 | + return std::stoull(mem_res); // Handle plain bytes without any suffix |
| 157 | + } |
| 158 | + |
| 159 | + // Extract the numeric part and the unit suffix |
| 160 | + double value = std::stod(mem_res.substr(0, mem_res.size() - 1)); |
| 161 | + char suffix = mem_res.back(); |
| 162 | + |
| 163 | + // Find the appropriate function from the map |
| 164 | + auto it = unit_map.find(suffix); |
| 165 | + if (it != unit_map.end()) { |
| 166 | + const auto & conversion_function = it->second; |
| 167 | + return conversion_function(value); |
| 168 | + } |
| 169 | + |
| 170 | + // Throw an exception or handle the error as needed if the suffix is not recognized |
| 171 | + throw std::runtime_error("Unsupported unit suffix: " + std::string(1, suffix)); |
| 172 | +} |
| 173 | + |
| 174 | +} // namespace autoware::component_monitor |
| 175 | + |
| 176 | +#include <rclcpp_components/register_node_macro.hpp> |
| 177 | +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_monitor::ComponentMonitor) |
0 commit comments