diff --git a/CMakeLists.txt b/CMakeLists.txt index c5334b0..1d88066 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,14 +52,21 @@ file(GLOB SUPER_MEDIATOR_FILES ) add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super) ament_target_dependencies(am_super ${dependencies}) +add_executable(resource_monitor src/resource_monitor/resource_status_class.cpp + src/resource_monitor/resource_monitor_main.cpp + src/resource_monitor/resource_monitor_node.cpp) +ament_target_dependencies(resource_monitor ${dependencies}) + install(DIRECTORY include/ DESTINATION include/ ) install(TARGETS am_super + resource_monitor DESTINATION lib/${PROJECT_NAME} ) diff --git a/include/resource_monitor/resource_monitor_node.h b/include/resource_monitor/resource_monitor_node.h new file mode 100644 index 0000000..7dcf914 --- /dev/null +++ b/include/resource_monitor/resource_monitor_node.h @@ -0,0 +1,31 @@ +#ifndef AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_ +#define AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_ + + +#include + +namespace am +{ +class ResourceMonitorNode : public AMLifeCycle +{ +public: + ResourceMonitorNode(const std::string &node_name); + + ~ResourceMonitorNode(); + + std::shared_ptr resource_status_ = nullptr; + + std::shared_ptr getAMClass(); + + void setAMClass(std::shared_ptr am_class); + + bool configured_ = false; + + // AMLifeCycle overrides + void heartbeatCB() override; + bool onCleanup() override; + bool onConfigure() override; +}; +} + +#endif /*AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_*/ \ No newline at end of file diff --git a/include/resource_monitor/resource_monitor_stats.h b/include/resource_monitor/resource_monitor_stats.h new file mode 100644 index 0000000..45885f6 --- /dev/null +++ b/include/resource_monitor/resource_monitor_stats.h @@ -0,0 +1,50 @@ +#ifndef AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ +#define AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ + +#include +#include +#include +#include +#include + +namespace am +{ + +class ResourceMonitorStats +{ +public: + AMStatStatus statStatus = AMStatStatus("ss", "AMStatStatus"); + + + AMStat tf_stats = AMStat("tf_s", "Transform Stats", 1, 2, 80, 99); + AMStat node_stats = AMStat("n_s", "Nodes Stats", 1, 2, 80, 99); + AMStat cpu_stats = AMStat("cpu_s", "CPU Stats", 1, 2, 80, 99); + AMStat gpu_stats = AMStat("gpu_s", "GPU Stats", 1, 2, 80, 99); + AMStat ram_stats = AMStat("ram_s", "RAM Stats", 1, 2, 80, 99); + AMStat drive_stats = AMStat("drive_s", "Drive Stats", 1, 2, 80, 99); + AMStat lidar_ip = AMStat("lidar_ip_s", "Lidar IP Stats", 1, 2, 80, 99); + AMStat fl_ip = AMStat("fl_s", "FL IP Stats", 1, 2, 80, 99); + AMStat fr_ip = AMStat("fr_s", "FR IP Stats", 1, 2, 80, 99); + AMStat rl_ip = AMStat("rl_s", "RL IP Stats", 1, 2, 80, 99); + AMStat rr_ip = AMStat("rr_s", "RR IP Stats", 1, 2, 80, 99); + + ResourceMonitorStats(AMStatList &stat_list) + { + stat_list.add(&statStatus); + stat_list.add(&tf_stats); + stat_list.add(&node_stats); + stat_list.add(&gpu_stats); + stat_list.add(&cpu_stats); + stat_list.add(&ram_stats); + stat_list.add(&drive_stats); + stat_list.add(&lidar_ip); + stat_list.add(&fl_ip); + stat_list.add(&fr_ip); + stat_list.add(&rl_ip); + stat_list.add(&rr_ip); + } +}; + +}; // namespace + +#endif /* AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ */ diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h new file mode 100644 index 0000000..0f24eb6 --- /dev/null +++ b/include/resource_monitor/resource_status_class.h @@ -0,0 +1,159 @@ +#ifndef AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include // For statvfs +#include // For std::setprecision + + +namespace am +{ + +struct MemoryInfo +{ + unsigned long total; + unsigned long free; + unsigned long used; + unsigned long available; + int used_percent; +}; + +struct GpuInfo +{ + std::string gpu_name; + int temp; + int load_percent; +}; + +struct CpuInfo +{ + unsigned long long user; + unsigned long long nice; + unsigned long long system; + unsigned long long idle; + unsigned long long iowait; + unsigned long long irq; + unsigned long long softirq; + unsigned long long steal; + unsigned long long total; +}; + +struct DiskInfo { + unsigned long long totalSpace; // Total space in bytes + unsigned long long availableSpace; // Available space in bytes (matches `df`) + unsigned long long usedSpace; // Used space in bytes + double percentUsed; // Percentage used +}; + +class ResourceStatus +{ +public: + ResourceStatus(std::shared_ptr stats); + + ~ResourceStatus(); + + am::MemoryInfo& getMemoryInfo(); + + am::CpuInfo getCPUInfo(); + + am::GpuInfo getGPUInfo(); + + void getCPUInfo(std::vector &infos); + + DiskInfo getDiskInfo(const std::string& path = "/"); + + double calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old); + + double getUpTime(); + + void updateInfos(); + + void print(); + + bool isReachable(const std::string &ipAddress); + + void getParams(); + + std::unordered_set getActiveIPs(const std::string& subnet = "192.168.1.0/24"); + + std::shared_ptr getStats(); + + std::vector getInetAddresses(); + + // AMLifeCycle passthrus + bool onConfigure(); + + bool onCleanup(); + + void heartbeatCB(); + +private: + + std::shared_ptr stats_; + + rclcpp::Subscription::SharedPtr status_sub_; + + rclcpp::Subscription::SharedPtr stat_sub_; + + void statusCB(const std_msgs::msg::Int32::SharedPtr msg); + + void statCB(const std_msgs::msg::Int32::SharedPtr msg); + + int getCPUCoresCount(); + + std::string readFile(const std::string& path); + + am::CpuInfo parseCpuLine(const std::string &line); + + int cpu_cnt_= -1; + + double cpu_usage_; + + double uptime_seconds_; + + bool ip_check_ {false}; + + bool is_first_time_ {true}; + + std::vector cpu_loads_; + + am::MemoryInfo mi; + + std::vector cpu_infos_; + + std::vector cpu_infos_old_; + + am::GpuInfo gpu_info_; + + std::vector sub_nets_add_; + + std::map ip_addresses_; //IPAddress, Name + + /*ROS Infrastructure Checking tools*/ + std::shared_ptr transformer_; + + std::vector> transform_list_; + + rclcpp::TimerBase::SharedPtr timer_; + + void timerCB(); + + void checkNodeNames(); + + void checkTransforms(); + + void checkSensorIPs(); +}; +} + +#endif /*AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_*/ \ No newline at end of file diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 1105089..0c975cb 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -207,6 +207,7 @@ class AMSuper rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; + rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; @@ -342,6 +343,8 @@ class AMSuper current_enu_sub = am::Node::node->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, am::getSensorQoS(1), std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); + + } ~AMSuper() @@ -353,6 +356,8 @@ class AMSuper } private: + + /** * process LifeCycleState messages from nodes * diff --git a/src/resource_monitor/resource_monitor_main.cpp b/src/resource_monitor/resource_monitor_main.cpp new file mode 100644 index 0000000..81a4f72 --- /dev/null +++ b/src/resource_monitor/resource_monitor_main.cpp @@ -0,0 +1,28 @@ +#include + +#include +#include + +std::shared_ptr am::Node::node = nullptr; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + // create the AMLifeCycle object with stats and assign it to the AMNode singleton + std::shared_ptr am_node = std::make_shared("resource_monitor"); + std::shared_ptr stats = std::make_shared(am_node->stats_list_); + am::Node::node = am_node; + + // create the buisness logic object and give the AMLifecycle class a pointer to it + std::shared_ptr am_class = std::make_shared(stats); + am_node->setAMClass(am_class); + + ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); + + rclcpp::spin(am::Node::node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/resource_monitor/resource_monitor_node.cpp b/src/resource_monitor/resource_monitor_node.cpp new file mode 100644 index 0000000..b56f100 --- /dev/null +++ b/src/resource_monitor/resource_monitor_node.cpp @@ -0,0 +1,65 @@ +#include +#include +#include +#include + +namespace am +{ + +ResourceMonitorNode::ResourceMonitorNode(const std::string & node_name) : AMLifeCycle(node_name) +{ +} + +ResourceMonitorNode::~ResourceMonitorNode() +{ + +} + +void ResourceMonitorNode::setAMClass(std::shared_ptr am_class) +{ + resource_status_= am_class; +} + +std::shared_ptr ResourceMonitorNode::getAMClass() +{ + return resource_status_; +} + +bool ResourceMonitorNode::onConfigure() +{ + if(configured_) + { + return AMLifeCycle::onConfigure(); + } + + ROS_INFO("onConfigure"); + + if(!resource_status_->onConfigure()) + { + ROS_WARN("am_class_->onConfigure() failed"); + resource_status_->onCleanup(); + return false; + } + else + { + configured_ = true; + return AMLifeCycle::onConfigure(); + } +} + +bool ResourceMonitorNode::onCleanup() +{ + ROS_INFO("onCleanup"); + + resource_status_->onCleanup(); + return AMLifeCycle::onCleanup(); +} + +void ResourceMonitorNode::heartbeatCB() +{ + resource_status_->heartbeatCB(); + AMLifeCycle::heartbeatCB(); +} + + +} // namespace \ No newline at end of file diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp new file mode 100644 index 0000000..2e94962 --- /dev/null +++ b/src/resource_monitor/resource_status_class.cpp @@ -0,0 +1,681 @@ + + +#include +#include +#include // For popen and fgets +#include // For std::unique_ptr +#include // For std::regex +#include + +namespace am +{ +ResourceStatus::ResourceStatus(std::shared_ptr stats) : stats_(stats) +{ + transformer_ = std::make_shared(); + + sub_nets_add_ = getInetAddresses(); + + for(const std::string &ip : sub_nets_add_) + { + //ROS_INFO("subnet: %s", ip.c_str()); + if(ip == "192.168.1.1") + { + ip_check_ = true; + ROS_INFO("Resource Monitor: looking for sensors on 192.168.1.1"); + } + } + + getParams(); + + timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); + + cpu_cnt_ = getCPUCoresCount(); +} + +ResourceStatus::~ResourceStatus() +{ + +} + +void ResourceStatus::getParams() +{ + + //getting the ip sensor parameters + int counter = 0; + am::getParam("ip_sensor_cnt", counter, counter); + for(int i = 0; i < counter; i++) + { + std::string ip_check_str = "ip_sensor_" + std::to_string(i); + std::string ip_address = ""; + std::string sensor_name = ""; + am::getParam(ip_check_str + std::string(".ip_address") , ip_address, ip_address); + am::getParam(ip_check_str + std::string(".name") , sensor_name, sensor_name); + if(ip_address == "" || sensor_name == "") + { + ROS_ERROR("ip sensor %d has configuration issues: ip: %s and name: %s", i, ip_address.c_str(), sensor_name.c_str()); + continue; + } + + ip_addresses_[ip_address] = sensor_name; + ROS_INFO(GREEN "IP Sensor[%s] is configured as %s" COLOR_RESET, ip_address.c_str(), sensor_name.c_str()); + } + + //getting the transform list + counter = 0; + am::getParam("transform_cnt", counter, counter); + for(int i = 0; i < counter; i++) + { + std::string transform_str = "transform_" + std::to_string(i); + std::string src = ""; + std::string target = ""; + am::getParam(transform_str + std::string(".source") , src, src); + am::getParam(transform_str + std::string(".target") , target, target); + + if(src == "" || target == "") + { + ROS_ERROR("transform %d has configuration issues: source: %s and target: %s", i, src.c_str(), target.c_str()); + continue; + } + transform_list_.push_back(std::make_pair(src, target)); + ROS_INFO(GREEN "Transform check is set between source %s and target %s" COLOR_RESET, src.c_str(), target.c_str()); + } +} + +std::shared_ptr ResourceStatus::getStats() +{ + return stats_; +} + +bool ResourceStatus::onConfigure() +{ + status_sub_ = am::Node::node->create_subscription(std::string(am::Node::node->get_name()) + "/status", 100, std::bind(&ResourceStatus::statusCB, this, std::placeholders::_1)); + + stat_sub_ = am::Node::node->create_subscription(std::string(am::Node::node->get_name()) + "/stat", 100, std::bind(&ResourceStatus::statCB, this, std::placeholders::_1)); + + return true; +} + +bool ResourceStatus::onCleanup() +{ + status_sub_.reset(); + stat_sub_.reset(); + return true; +} + +void ResourceStatus::statusCB(const std_msgs::msg::Int32::SharedPtr msg) +{ + stats_->statStatus = msg->data; +} + +void ResourceStatus::statCB(const std_msgs::msg::Int32::SharedPtr msg) +{ + +} + +void ResourceStatus::heartbeatCB() +{ +} + + +int ResourceStatus::getCPUCoresCount() +{ + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Unable to open /proc/stat"); + } + + int coreCount = 0; + std::string line; + + while (std::getline(file, line)) + { + if (line.compare(0, 3, "cpu") == 0 && line[3] >= '0' && line[3] <= '9') + { + coreCount++; + } + } + + file.close(); + return coreCount; +} + +am::CpuInfo ResourceStatus::parseCpuLine(const std::string& line) +{ + am::CpuInfo info = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + std::istringstream iss(line); + std::string cpuLabel; // e.g., "cpu0", "cpu1", etc. + iss >> cpuLabel >> info.user >> info.nice >> info.system >> info.idle + >> info.iowait >> info.irq >> info.softirq >> info.steal; + + info.total = info.user + info.nice + info.system + info.idle + + info.iowait + info.irq + info.softirq + info.steal; + return info; +} + +void ResourceStatus::updateInfos() +{ + getMemoryInfo(); + if(cpu_cnt_ < 0) + { + cpu_cnt_ = getCPUCoresCount(); + if(cpu_cnt_ < 0) + { + ROS_ERROR("Cannot get CPU count"); + return; + } + } + + cpu_infos_.resize(cpu_cnt_); + cpu_infos_old_.resize(cpu_cnt_); + cpu_loads_.resize(cpu_cnt_); + + getCPUInfo(cpu_infos_); + + if(is_first_time_) + { + getCPUInfo(cpu_infos_old_); + is_first_time_ = false; + } + double avg_load = 0.0; + for(int i = 0; i < cpu_infos_.size(); i++) + { + double load = calculateCpuLoad(cpu_infos_[i], cpu_infos_old_[i]); + avg_load+=load; + cpu_loads_[i] = load; + } + + if(cpu_cnt_ > 0) + { + avg_load = avg_load/cpu_cnt_; + stats_->cpu_stats = (avg_load > 80.0?100:50); + } + + + uptime_seconds_ = getUpTime(); + + cpu_infos_old_ = cpu_infos_; + + gpu_info_ = getGPUInfo(); + stats_->gpu_stats = 50; + //ROS_INFO("GPU Load Percent: %d , Temp: %d", gpu_info_.load_percent, gpu_info_.temp); + if(gpu_info_.load_percent > 90) + { + stats_->gpu_stats = 100; + ROS_ERROR("GPU Issues: LOAD Percent: %d, Temp: %d", gpu_info_.load_percent, gpu_info_.temp); + } + + + //check the drive stats + stats_->drive_stats = 50; + am::DiskInfo disk_info = getDiskInfo(); + if(disk_info.percentUsed > 98.0) + { + stats_->drive_stats = 100; + int coef = 1024*1024; + ROS_ERROR("Disk total: %lld MB, available: %lld MB, used: %lld MB, percentage: %f", (disk_info.totalSpace/coef), (disk_info.availableSpace/coef), (disk_info.usedSpace/coef), disk_info.percentUsed); + } + +} + + +am::MemoryInfo& ResourceStatus::getMemoryInfo() +{ + mi = {0, 0, 0}; + std::ifstream file("/proc/meminfo"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/meminfo"); + return mi; + } + + std::string line; + while (std::getline(file, line)) { + std::istringstream iss(line); + std::string key; + unsigned long value; + std::string unit; + + iss >> key >> value >> unit; + + if (key == "MemTotal:") { + mi.total = value; // in kB + } else if (key == "MemFree:") { + mi.free = value; // in kB + } else if (key == "Buffers:" || key == "Cached:") { + mi.used += value; // Add buffers and cached to used + }else if(key == "MemAvailable:"){ + mi.available = value; + } + + } + + // Calculate used memory + mi.used = mi.total - mi.free; + mi.used_percent = (mi.used / mi.total) * 100; + stats_->ram_stats = (mi.used_percent > 80?100:50); + file.close(); + return mi; +} + +// Function to read the content of a file +std::string ResourceStatus::readFile(const std::string& path) +{ + std::ifstream file(path); + if (!file.is_open()) { + throw std::runtime_error("Error: Unable to open file " + path); + } + + std::string content; + std::getline(file, content); + file.close(); + return content; +} + +am::GpuInfo ResourceStatus::getGPUInfo() +{ + am::GpuInfo gpu_info; + + // "/sys/devices/gpu.0/load" exists only in Jetpack + //in contrast, nvidia-smi only exists in amd64 architure + const std::string loadPath = "/sys/devices/gpu.0/load"; + if (boost::filesystem::exists(loadPath)) + { + std::string loadStr = readFile(loadPath); + gpu_info.load_percent = std::stoi(loadStr) / 1000; // Convert to percentage + + const std::string baseThermalPath = "/sys/class/thermal/"; + const std::string typeSuffix = "/type"; + const std::string tempSuffix = "/temp"; + + bool found_gpu_file = false; + for (int i = 0; i < 10; ++i) + { // Check up to 10 thermal zones + try { + std::string typePath = baseThermalPath + "thermal_zone" + std::to_string(i) + typeSuffix; + //ROS_INFO("Type file: %s", typePath.c_str()); + if(!boost::filesystem::exists(typePath)) + { + continue; + } + std::string type = readFile(typePath); + if (type.find("GPU") != std::string::npos) + { // Look for the GPU thermal zone + found_gpu_file = true; + std::string tempPath = baseThermalPath + "thermal_zone" + std::to_string(i) + tempSuffix; + std::string tempStr = readFile(tempPath); + gpu_info.temp = std::stoi(tempStr) / 1000; // Convert millidegrees to degrees Celsius + break; + } + } catch (...) + { + // Ignore errors and continue checking other zones + } + } + + return gpu_info; + } + + + // Execute the nvidia-smi command and read the output directly + const std::string command = "nvidia-smi --query-gpu=name,utilization.gpu,temperature.gpu,memory.used,memory.free --format=csv,nounits,noheader"; + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) + { + ROS_ERROR("Error: Unable to execute nvidia-smi. Ensure it's installed and available in PATH."); + return gpu_info; + } + + char buffer[128]; + std::ostringstream result; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) + { + result << buffer; + } + pclose(pipe); + + // Parse the command output + std::istringstream iss(result.str()); + std::string line; + while (std::getline(iss, line)) + { + //ROS_INFO(GREEN "%s" COLOR_RESET, line.c_str()); + std::istringstream lineStream(line); + // Parse memory used and free values + std::string gpuName; + int gpuUtilization, gpuTemperature, memoryUsed, memoryFree; + + // Using ',' to split the values + std::getline(lineStream, gpuName, ','); // Get the GPU name + + // Extracting the other values (removing leading/trailing spaces) + lineStream >> gpuUtilization; + lineStream.ignore(); // Ignore the space after the utilization + lineStream >> gpuTemperature; + lineStream.ignore(); // Ignore the space after the temperature + lineStream >> memoryUsed; + lineStream.ignore(); // Ignore the space after the memory used + lineStream >> memoryFree; + + gpu_info.gpu_name = gpuName; + gpu_info.load_percent = gpuUtilization; + gpu_info.temp = gpuTemperature; + + return gpu_info; + } + + + + return gpu_info; +} + + +void ResourceStatus::getCPUInfo(std::vector &infos) +{ + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/stat"); + return; + } + + std::string line; + int cnt = 0; + while (std::getline(file, line)) + { + if (line.find("cpu") == 0 && line.find("cpu ") != 0) + { + // Only process lines like "cpu0", "cpu1", etc. + infos[cnt] = parseCpuLine(line); + cnt++; + } + } + + file.close(); +} + +bool ResourceStatus::isReachable(const std::string &ip_address) +{ + std::string command = std::string("ping -c 1 -W 0.2 ") + ip_address + std::string(" >/dev/null 2>&1"); + + int result = std::system(command.c_str()); + + return result == 0; +} + +am::CpuInfo ResourceStatus::getCPUInfo() +{ + if(cpu_cnt_ < 0) + { + ROS_ERROR("Cannot get CPU Core Count: %d", cpu_cnt_); + } + + am::CpuInfo cpu_info = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/stat"); + return cpu_info; + } + + std::string line; + if (std::getline(file, line)) { + std::istringstream iss(line); + std::string cpu; + iss >> cpu >> cpu_info.user >> cpu_info.nice >> cpu_info.system + >> cpu_info.idle >> cpu_info.iowait >> cpu_info.irq + >> cpu_info.softirq >> cpu_info.steal; + + // Total CPU time + cpu_info.total = cpu_info.user + cpu_info.nice + cpu_info.system + + cpu_info.idle + cpu_info.iowait + cpu_info.irq + + cpu_info.softirq + cpu_info.steal; + } + + file.close(); + return cpu_info; +} + +double ResourceStatus::calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old) +{ + unsigned long long totalDiff = ci.total - ci_old.total; + unsigned long long idleDiff = (ci.idle + ci.iowait) - (ci_old.idle + ci_old.iowait); + + cpu_usage_ = (totalDiff - idleDiff) * 100.0 / totalDiff; + + return cpu_usage_; +} + +double ResourceStatus::getUpTime() +{ + std::ifstream file("/proc/uptime"); + if (!file.is_open()) { + ROS_INFO("Error: Unable to open /proc/uptime"); + + return 0.0; + } + + double idleSeconds; + file >> uptime_seconds_ >> idleSeconds; + file.close(); + + return uptime_seconds_; +} + +void ResourceStatus::print() +{ + ROS_INFO("MemoryInfo---> Total: %ld MB, Free: %ld MB, Used: %ld MB, Available: %ld MB", (mi.total / 1024), (mi.free / 1024), (mi.used / 1024), (mi.available / 1024)); + + std::string msg = ""; + for(int i = 0; i < cpu_loads_.size(); i++) + { + msg += std::string(" Core[") + std::to_string(i) + "] Usage: " + std::to_string(cpu_loads_[i]); + } + + ROS_INFO("CPUInfo---> Cores: %d , %s", cpu_cnt_, msg.c_str()); + + ROS_INFO("UpTime: %f", uptime_seconds_); + + msg = ""; + msg += "GPU: Temp[C] = " + std::to_string(gpu_info_.temp) + ", Used[%]: " + std::to_string(gpu_info_.load_percent); + + ROS_INFO("%s", msg.c_str()); +} + +void ResourceStatus::checkNodeNames() +{ + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph = am::Node::node->get_node_graph_interface(); + + std::vector running_nodes = node_graph->get_node_names(); + + std::unordered_map string_count; + + // Count occurrences of each string + for (const std::string& str : running_nodes) + { + if(str.find("plugin_name") != std::string::npos) + { + continue; + } + string_count[str]++; + } + + // Collect strings that appear more than once + bool node_check = true; + for (const auto& [str, count] : string_count) + { + if (count > 1) + { + ROS_ERROR("Found a duplicate Node: %s", str.c_str()); + node_check = false; + } + } + stats_->node_stats = (node_check?50:100); +} + +void ResourceStatus::checkTransforms() +{ + bool tf_check = true; + for(std::pair &tf_str : transform_list_) + { + geometry_msgs::msg::TransformStamped transform; + if(!transformer_->getTransform(tf_str.first, tf_str.second, transform, 1.0, false)) + { + //ROS_ERROR("Transform tree is broken: %s, %s", tf_str.first.c_str(), tf_str.second.c_str()); + tf_check = false; + } + } + stats_->tf_stats = (tf_check?50:100); +} + +void ResourceStatus::checkSensorIPs() +{ + + //IP Address Check + stats_->lidar_ip = 50; + stats_->fl_ip = 50; + stats_->fr_ip = 50; + stats_->rl_ip = 50; + stats_->rr_ip = 50; + //Only if you have the subnet + if(ip_check_) + { + std::map::iterator it = ip_addresses_.begin(); + for(; it != ip_addresses_.end(); ++it) + { + + if(!isReachable(it->first)) + { + //THE DEVICE CANNOT BE REACHED + if(it->second == "lidar") + { + stats_->lidar_ip = 100; + ROS_ERROR("Lidar is not reachable"); + } + if(it->second == "front_left") + { + stats_->fl_ip = 100; + ROS_ERROR("Front Left Camera is not reachable"); + } + if(it->second == "front_right") + { + stats_->fr_ip = 100; + ROS_ERROR("Front Right Camera is not reachable"); + } + if(it->second == "rear_right") + { + stats_->rr_ip = 100; + ROS_ERROR("Rear Right Camera is not reachable"); + } + if(it->second == "rear_left") + { + stats_->rl_ip = 100; + ROS_ERROR("Rear Left Camera is not reachable"); + } + } + } + } +} + + +// Function to execute the nmap command and capture the output +std::unordered_set ResourceStatus::getActiveIPs(const std::string& subnet) +{ + std::unordered_set activeIPs; + std::string command = "nmap -sn " + subnet; + + // Open a pipe to execute the command and read its output + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + if (!pipe) { + std::cerr << "Error: Failed to run nmap command.\n"; + return activeIPs; + } + + // Read the command output line by line + char buffer[128]; + std::string line; + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) + { + line = buffer; + // Check if the line contains "Nmap scan report for", indicating a live IP + if (line.find("Nmap scan report for") != std::string::npos) + { + std::string ip = line.substr(line.find_last_of(' ') + 1); + ip.erase(ip.find('\n')); // Remove the newline character + activeIPs.insert(ip); + } + } + + return activeIPs; +} + + +// Function to execute ifconfig and extract inet addresses +std::vector ResourceStatus::getInetAddresses() +{ + std::vector inetAddresses; + std::string command = "ifconfig"; + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + + if (!pipe) { + std::cerr << "Error: Failed to run ifconfig command.\n"; + return inetAddresses; + } + + char buffer[256]; + std::string output; + + // Read the entire output of ifconfig + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) { + output += buffer; + } + + // Regular expression to match inet (IPv4) addresses + std::regex inetRegex(R"(inet\s+(\d+\.\d+\.\d+\.\d+))"); + std::smatch match; + + // Search for inet addresses in the output + auto begin = output.cbegin(); + auto end = output.cend(); + while (std::regex_search(begin, end, match, inetRegex)) { + inetAddresses.push_back(match[1]); + begin = match.suffix().first; // Move to the next match + } + + return inetAddresses; +} + + +// Function to get disk usage information +DiskInfo ResourceStatus::getDiskInfo(const std::string& path) +{ + struct statvfs stat; + + if (statvfs(path.c_str(), &stat) != 0) { + throw std::runtime_error("Error: Unable to get disk information for " + path); + } + + DiskInfo info; + info.totalSpace = stat.f_blocks * stat.f_frsize; // Total blocks * block size + info.availableSpace = stat.f_bavail * stat.f_frsize; // Available blocks * block size + info.usedSpace = info.totalSpace - info.availableSpace; + info.percentUsed = (info.usedSpace * 100.0) / info.totalSpace; + + return info; +} + +/* + Timer Callback: this is where everything is updated + */ +void ResourceStatus::timerCB() +{ + //Checking the repeated node name + checkNodeNames(); + + //Transform check + checkTransforms(); + + //sensor ip check + checkSensorIPs(); + + //Resource Check + updateInfos(); + +} +} \ No newline at end of file