From 8cd5be560eaeb54c21cd33228c759ddb4ca009be Mon Sep 17 00:00:00 2001 From: Brian Coltin Date: Wed, 5 Apr 2023 16:00:47 -0700 Subject: [PATCH] Convert basics but still missing main functionality. --- astrobee/launch/robot/MLP.launch.py | 9 +- management/cpu_mem_monitor/CMakeLists.txt | 116 ++++++------ management/cpu_mem_monitor/COLCON_IGNORE | 0 .../include/cpu_mem_monitor/cpu_mem_monitor.h | 70 +++----- .../launch/cpu_mem_monitor.launch | 25 --- .../cpu_mem_monitor/nodelet_plugins.xml | 9 - management/cpu_mem_monitor/package.xml | 20 +-- .../cpu_mem_monitor/src/cpu_mem_monitor.cc | 167 ++++++++++-------- 8 files changed, 194 insertions(+), 222 deletions(-) delete mode 100644 management/cpu_mem_monitor/COLCON_IGNORE delete mode 100644 management/cpu_mem_monitor/launch/cpu_mem_monitor.launch delete mode 100644 management/cpu_mem_monitor/nodelet_plugins.xml diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py index ad5561b6ec..0167200b84 100644 --- a/astrobee/launch/robot/MLP.launch.py +++ b/astrobee/launch/robot/MLP.launch.py @@ -272,11 +272,10 @@ def generate_launch_description(): # plugin='sys_monitor::SysMonitor', # name='sys_monitor', # extra_arguments=[{'use_intra_process_comms': True}]), - # ComposableNode( - # package='cpu_mem_monitor', - # plugin='cpu_mem_monitor::CpuMemMonitor', - # name='mlp_cpu_mem_monitor', - # extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='cpu_mem_monitor', + plugin='cpu_mem_monitor::CpuMemMonitor', + name='mlp_cpu_mem_monitor'), # ComposableNode( # package='disk_monitor', # plugin='disk_monitor::DiskMonitor', diff --git a/management/cpu_mem_monitor/CMakeLists.txt b/management/cpu_mem_monitor/CMakeLists.txt index e1cd69ba19..b34514ddee 100644 --- a/management/cpu_mem_monitor/CMakeLists.txt +++ b/management/cpu_mem_monitor/CMakeLists.txt @@ -15,27 +15,25 @@ # License for the specific language governing permissions and limitations # under the License. -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) project(cpu_mem_monitor) ## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) -add_definitions(-DROS1) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - nodelet - ff_hw_msgs - ff_msgs - config_reader - ff_util -) +# Find amend and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) -catkin_package( - LIBRARIES cpu_mem_monitor - CATKIN_DEPENDS roscpp nodelet ff_hw_msgs ff_msgs config_reader ff_util -) +find_package(ff_common REQUIRED) +find_package(ff_msgs REQUIRED) +find_package(ff_hw_msgs REQUIRED) +find_package(ff_util REQUIRED) +find_package(config_reader REQUIRED) ########### ## Build ## @@ -44,58 +42,66 @@ catkin_package( # Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} ) # Declare C++ libraries -add_library(cpu_mem_monitor +add_library(cpu_mem_monitor SHARED src/cpu.cc src/cpu_mem_monitor.cc ) -add_dependencies(cpu_mem_monitor ${catkin_EXPORTED_TARGETS}) -target_link_libraries(cpu_mem_monitor ${catkin_LIBRARIES}) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - # llp CPU monitor initialization fault tester - add_rostest_gtest(test_init_llp_cpu_mem_monitor - test/test_init_llp_cpu_mem_monitor.test - test/test_init_llp_cpu_mem_monitor.cc - ) - target_link_libraries(test_init_llp_cpu_mem_monitor - ${catkin_LIBRARIES} glog - ) - # mlp CPU monitor initialization fault tester - add_rostest_gtest(test_init_mlp_cpu_mem_monitor - test/test_init_mlp_cpu_mem_monitor.test - test/test_init_mlp_cpu_mem_monitor.cc - ) - - target_link_libraries(test_init_mlp_cpu_mem_monitor - ${catkin_LIBRARIES} glog - ) - -endif() +target_compile_definitions(cpu_mem_monitor + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(cpu_mem_monitor rclcpp rclcpp_components ff_common config_reader ff_msgs ff_util ff_hw_msgs) +rclcpp_components_register_nodes(cpu_mem_monitor "cpu_mem_monitor::CpuMemMonitor") + +#if(CATKIN_ENABLE_TESTING) +# find_package(rostest REQUIRED) +# # llp CPU monitor initialization fault tester +# add_rostest_gtest(test_init_llp_cpu_mem_monitor +# test/test_init_llp_cpu_mem_monitor.test +# test/test_init_llp_cpu_mem_monitor.cc +# ) +# +# target_link_libraries(test_init_llp_cpu_mem_monitor +# ${catkin_LIBRARIES} glog +# ) +# # mlp CPU monitor initialization fault tester +# add_rostest_gtest(test_init_mlp_cpu_mem_monitor +# test/test_init_mlp_cpu_mem_monitor.test +# test/test_init_mlp_cpu_mem_monitor.cc +# ) +# +# target_link_libraries(test_init_mlp_cpu_mem_monitor +# ${catkin_LIBRARIES} glog +# ) +# +#endif() ############# ## Install ## ############# -# Mark libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) +ament_export_include_directories(include) + +target_include_directories(cpu_mem_monitor + PUBLIC + $ + $) -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) -# Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + EXPORT cpu_mem_monitor + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +ament_package() diff --git a/management/cpu_mem_monitor/COLCON_IGNORE b/management/cpu_mem_monitor/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h index 22acee377e..9cbca071f7 100644 --- a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h +++ b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h @@ -20,10 +20,7 @@ #ifndef CPU_MEM_MONITOR_CPU_MEM_MONITOR_H_ #define CPU_MEM_MONITOR_CPU_MEM_MONITOR_H_ -#include - -#include -#include +#include #include #include @@ -36,12 +33,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include #include #include @@ -50,6 +47,7 @@ namespace cpu_mem_monitor { + enum LoadFaultState { ASSERTING, ASSERTED, @@ -57,32 +55,32 @@ enum LoadFaultState { CLEARED }; -class CpuMemMonitor : public ff_util::FreeFlyerNodelet { +class CpuMemMonitor : public ff_util::FreeFlyerComponent { public: /** * CPU Memory monitor allocate, register, initialize model */ - CpuMemMonitor(); + explicit CpuMemMonitor(const rclcpp::NodeOptions & options); /** * destruct model */ ~CpuMemMonitor(); protected: - virtual void Initialize(ros::NodeHandle *nh); + virtual void Initialize(NodeHandle & nh); bool ReadParams(); private: // Get the PIDs of the nodes to monitor - void GetPIDs(ros::TimerEvent const &te); + void GetPIDs(); // Assert CPU loads and report if too high - void AssertCPULoadHighFaultCallback(ros::TimerEvent const& te); - void ClearCPULoadHighFaultCallback(ros::TimerEvent const& te); + void AssertCPULoadHighFaultCallback(); + void ClearCPULoadHighFaultCallback(); // Assert Memory loads and report if too high - void AssertMemLoadHighFaultCallback(ros::TimerEvent const& te); - void ClearMemLoadHighFaultCallback(ros::TimerEvent const& te); + void AssertMemLoadHighFaultCallback(); + void ClearMemLoadHighFaultCallback(); // Assert the collected CPU and Memory status void AssertCpuStats(); @@ -95,7 +93,7 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { int CollectMemStats(); // Callback to scan and publish the CPU and Memory stats - void PublishStatsCallback(ros::TimerEvent const &te); + void PublishStatsCallback(); private: struct Load { @@ -120,15 +118,15 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { }; config_reader::ConfigReader config_params_; - ros::Publisher cpu_state_pub_; // Cpu stats publisher - ros::Publisher mem_state_pub_; // Memory stats publisher - ros::Timer reload_params_timer_; // Ckeck if parameters were updated - ros::Timer pid_timer_; // Update PIDs - ros::Timer stats_timer_; // Update stats - ros::Timer assert_cpu_load_fault_timer_; // Check cpu load limits - ros::Timer clear_cpu_load_fault_timer_; // Clear cpu fault timer - ros::Timer assert_mem_load_fault_timer_; // Check memory load limits - ros::Timer clear_mem_load_fault_timer_; // Clear memory fault timer + rclcpp::Publisher::SharedPtr cpu_state_pub_; + rclcpp::Publisher::SharedPtr mem_state_pub_; + ff_util::FreeFlyerTimer reload_params_timer_; // Ckeck if parameters were updated + ff_util::FreeFlyerTimer pid_timer_; // Update PIDs + ff_util::FreeFlyerTimer stats_timer_; // Update stats + ff_util::FreeFlyerTimer assert_cpu_load_fault_timer_; // Check cpu load limits + ff_util::FreeFlyerTimer clear_cpu_load_fault_timer_; // Clear cpu fault timer + ff_util::FreeFlyerTimer assert_mem_load_fault_timer_; // Check memory load limits + ff_util::FreeFlyerTimer clear_mem_load_fault_timer_; // Clear memory fault timer int pub_queue_size_; // Monitor publishing queue size double update_freq_hz_, update_pid_hz_; // Publishing and PID update frequency struct sysinfo mem_info_; // Scope memory info from sysinfo @@ -154,8 +152,8 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { // Status messages to publish - ff_msgs::CpuStateStamped cpu_state_msg_; - ff_msgs::MemStateStamped mem_state_msg_; + ff_msgs::msg::CpuStateStamped cpu_state_msg_; + ff_msgs::msg::MemStateStamped mem_state_msg_; // Asserting faults LoadFaultState load_fault_state_; @@ -173,20 +171,6 @@ int ParseLine(char* line) { return i; } -std::string getHostfromURI(std::string uri) { - std::size_t uri_begin = uri.find_first_of("/"); - std::size_t uri_end = uri.find_last_of(":"); - if (std::string::npos != uri_begin && std::string::npos != uri_end && - uri_begin <= uri_end) { - uri.erase(uri.begin() + uri_end, uri.end()); - uri.erase(uri.begin(), uri.begin() + uri_begin + 2); - return uri; - } else { - ROS_ERROR_STREAM("Invalid URI, returning "); - return {}; - } -} - } // namespace cpu_mem_monitor #endif // CPU_MEM_MONITOR_CPU_MEM_MONITOR_H_ diff --git a/management/cpu_mem_monitor/launch/cpu_mem_monitor.launch b/management/cpu_mem_monitor/launch/cpu_mem_monitor.launch deleted file mode 100644 index e15ab094c9..0000000000 --- a/management/cpu_mem_monitor/launch/cpu_mem_monitor.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/management/cpu_mem_monitor/nodelet_plugins.xml b/management/cpu_mem_monitor/nodelet_plugins.xml deleted file mode 100644 index cdf2ae48af..0000000000 --- a/management/cpu_mem_monitor/nodelet_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Nodelet to monitor CPU and Memory stats. - - - diff --git a/management/cpu_mem_monitor/package.xml b/management/cpu_mem_monitor/package.xml index 8549acff8c..263100d061 100644 --- a/management/cpu_mem_monitor/package.xml +++ b/management/cpu_mem_monitor/package.xml @@ -1,5 +1,5 @@ - + cpu_mem_monitor 0.0.0 @@ -14,20 +14,18 @@ Astrobee Flight Software - catkin - roscpp - nodelet + ament_cmake + rclcpp ff_msgs ff_hw_msgs config_reader ff_util - roscpp - nodelet - ff_msgs - ff_hw_msgs - config_reader - ff_util + rclcpp + ff_msgs + ff_hw_msgs + config_reader + ff_util - + ament_cmake diff --git a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc index 8c615bd90b..5bda7be75e 100644 --- a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc +++ b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc @@ -4,14 +4,31 @@ namespace cpu_mem_monitor { +FF_DEFINE_LOGGER("cpu_mem_monitor"); + namespace { constexpr char kProcStat[] = "/proc/stat"; constexpr char SysCpuPath[] = "/sys/devices/system/cpu"; constexpr char SysThermalPath[] = "/sys/class/thermal"; } // namespace -CpuMemMonitor::CpuMemMonitor() : - ff_util::FreeFlyerNodelet(), +std::string getHostfromURI(std::string uri) { + std::size_t uri_begin = uri.find_first_of("/"); + std::size_t uri_end = uri.find_last_of(":"); + if (std::string::npos != uri_begin && std::string::npos != uri_end && + uri_begin <= uri_end) { + uri.erase(uri.begin() + uri_end, uri.end()); + uri.erase(uri.begin(), uri.begin() + uri_begin + 2); + return uri; + } else { + FF_ERROR_STREAM("Invalid URI, returning "); + return {}; + } +} + +// TODO(bcoltin): figure out how to use name specified in launch file? +CpuMemMonitor::CpuMemMonitor(const rclcpp::NodeOptions & options) : + ff_util::FreeFlyerComponent(options, "cpu_mem_monitor"), pub_queue_size_(10), update_freq_hz_(1), temp_fault_triggered_(false), @@ -27,7 +44,7 @@ CpuMemMonitor::CpuMemMonitor() : CpuMemMonitor::~CpuMemMonitor() { } -void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { +void CpuMemMonitor::Initialize(NodeHandle & nh) { std::string err_msg; // First three letters of the node name specifies processor processor_name_ = GetName().substr(0, 3); @@ -37,74 +54,71 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { return; } - reload_params_timer_ = nh->createTimer(ros::Duration(1), - [this](ros::TimerEvent e) { + reload_params_timer_.createTimer(1.0, [this]() { config_params_.CheckFilesUpdated(std::bind(&CpuMemMonitor::ReadParams, this));}, - false, - true); + nh, false, true); // All state messages are latching - cpu_state_pub_ = nh->advertise( - TOPIC_MANAGEMENT_CPU_MONITOR_STATE, - pub_queue_size_); + cpu_state_pub_ = FF_CREATE_PUBLISHER(nh, ff_msgs::msg::CpuStateStamped, + TOPIC_MANAGEMENT_CPU_MONITOR_STATE, pub_queue_size_); // All state messages are latching - mem_state_pub_ = nh->advertise( - TOPIC_MANAGEMENT_MEM_MONITOR_STATE, - pub_queue_size_); + mem_state_pub_ = FF_CREATE_PUBLISHER(nh, ff_msgs::msg::MemStateStamped, + TOPIC_MANAGEMENT_MEM_MONITOR_STATE, pub_queue_size_); // Timer for asserting the cpu load too high fault - assert_cpu_load_fault_timer_ = nh->createTimer( - ros::Duration(assert_load_high_fault_timeout_sec_), - &CpuMemMonitor::AssertCPULoadHighFaultCallback, - this, + assert_cpu_load_fault_timer_.createTimer( + assert_load_high_fault_timeout_sec_, + std::bind(&CpuMemMonitor::AssertCPULoadHighFaultCallback, this), + nh, true, false); // Timer for clearing the cpu load too high fault - clear_cpu_load_fault_timer_ = nh->createTimer( - ros::Duration(clear_load_high_fault_timeout_sec_), - &CpuMemMonitor::ClearCPULoadHighFaultCallback, - this, + clear_cpu_load_fault_timer_.createTimer( + clear_load_high_fault_timeout_sec_, + std::bind(&CpuMemMonitor::ClearCPULoadHighFaultCallback, this), + nh, true, false); // Timer for asserting the memory load too high fault - assert_mem_load_fault_timer_ = nh->createTimer( - ros::Duration(assert_load_high_fault_timeout_sec_), - &CpuMemMonitor::AssertMemLoadHighFaultCallback, - this, + assert_mem_load_fault_timer_.createTimer( + assert_load_high_fault_timeout_sec_, + std::bind(&CpuMemMonitor::AssertMemLoadHighFaultCallback, this), + nh, true, false); // Timer for clearing the memory load too high fault - clear_mem_load_fault_timer_ = nh->createTimer( - ros::Duration(clear_load_high_fault_timeout_sec_), - &CpuMemMonitor::ClearMemLoadHighFaultCallback, - this, + clear_mem_load_fault_timer_.createTimer( + clear_load_high_fault_timeout_sec_, + std::bind(&CpuMemMonitor::ClearMemLoadHighFaultCallback, this), + nh, true, false); // Timer for updating PID values of interest nodes - pid_timer_ = nh->createTimer(ros::Duration(1 / update_pid_hz_), - &CpuMemMonitor::GetPIDs, - this, + pid_timer_.createTimer(1 / update_pid_hz_, + std::bind(&CpuMemMonitor::GetPIDs, this), + nh, false, true); // Timer for checking cpu and memory stats. Timer is not one shot and start it right away - stats_timer_ = nh->createTimer(ros::Duration(1 / update_freq_hz_), - &CpuMemMonitor::PublishStatsCallback, - this, + stats_timer_.createTimer(1 / update_freq_hz_, + std::bind(&CpuMemMonitor::PublishStatsCallback, this), + nh, false, true); // Initialize host name, get own URI - XmlRpc::XmlRpcValue args, result, payload; - args.setSize(2); - args[0] = ros::this_node::getName(); - args[1] = ros::this_node::getName(); - ros::master::execute("lookupNode", args, result, payload, true); - monitor_host_ = getHostfromURI(result[2]); + // TODO(bcoltin): find ros2 equivalent + // XmlRpc::XmlRpcValue args, result, payload; + // args.setSize(2); + // args[0] = ros::this_node::getName(); + // args[1] = ros::this_node::getName(); + // ros::master::execute("lookupNode", args, result, payload, true); + // monitor_host_ = getHostfromURI(result[2]); if (monitor_host_.empty()) { FF_ERROR_STREAM("URI of the memory monitor not valid"); return; @@ -138,11 +152,11 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { cpu_state_msg_.avg_loads.resize(5); // Load locatioms won't change - cpu_state_msg_.load_fields[0] = ff_msgs::CpuStateStamped::NICE; - cpu_state_msg_.load_fields[1] = ff_msgs::CpuStateStamped::USER; - cpu_state_msg_.load_fields[2] = ff_msgs::CpuStateStamped::SYS; - cpu_state_msg_.load_fields[3] = ff_msgs::CpuStateStamped::VIRT; - cpu_state_msg_.load_fields[4] = ff_msgs::CpuStateStamped::TOTAL; + cpu_state_msg_.load_fields[0] = ff_msgs::msg::CpuStateStamped::NICE; + cpu_state_msg_.load_fields[1] = ff_msgs::msg::CpuStateStamped::USER; + cpu_state_msg_.load_fields[2] = ff_msgs::msg::CpuStateStamped::SYS; + cpu_state_msg_.load_fields[3] = ff_msgs::msg::CpuStateStamped::VIRT; + cpu_state_msg_.load_fields[4] = ff_msgs::msg::CpuStateStamped::TOTAL; // Create as many cpu states as the number of cpus the freq class found cpu_state_msg_.cpus.resize(ncpus_); @@ -263,14 +277,14 @@ bool CpuMemMonitor::ReadParams() { } std::string name; if (node.GetStr("name", &name)) { - NODELET_DEBUG_STREAM("Read node " << name); + FF_DEBUG_STREAM("Read node " << name); nodes_pid_.insert(std::pair(name, 0)); } } return true; } -void CpuMemMonitor::AssertCPULoadHighFaultCallback(ros::TimerEvent const& te) { +void CpuMemMonitor::AssertCPULoadHighFaultCallback() { // Stop timer so we don't trigger the fault over and over again assert_cpu_load_fault_timer_.stop(); std::string err_msg = "CPU average load is " + @@ -282,14 +296,14 @@ void CpuMemMonitor::AssertCPULoadHighFaultCallback(ros::TimerEvent const& te) { load_fault_state_ = ASSERTED; } -void CpuMemMonitor::ClearCPULoadHighFaultCallback(ros::TimerEvent const& te) { +void CpuMemMonitor::ClearCPULoadHighFaultCallback() { // Stop timer so we don't try to clear the fault over and over again clear_cpu_load_fault_timer_.stop(); this->ClearFault(ff_util::LOAD_TOO_HIGH); load_fault_state_ = CLEARED; } -void CpuMemMonitor::AssertMemLoadHighFaultCallback(ros::TimerEvent const& te) { +void CpuMemMonitor::AssertMemLoadHighFaultCallback() { // Stop timer so we don't trigger the fault over and over again assert_mem_load_fault_timer_.stop(); std::string err_msg = "Memory average load is " + @@ -301,34 +315,36 @@ void CpuMemMonitor::AssertMemLoadHighFaultCallback(ros::TimerEvent const& te) { load_fault_state_ = ASSERTED; } -void CpuMemMonitor::ClearMemLoadHighFaultCallback(ros::TimerEvent const& te) { +void CpuMemMonitor::ClearMemLoadHighFaultCallback() { // Stop timer so we don't try to clear the fault over and over again clear_mem_load_fault_timer_.stop(); this->ClearFault(ff_util::MEMORY_USAGE_TOO_HIGH); load_fault_state_ = CLEARED; } -void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { +void CpuMemMonitor::GetPIDs() { // Go through all the node list and get the PID - XmlRpc::XmlRpcValue args, result, payload; + // XmlRpc::XmlRpcValue args, result, payload; std::map::iterator it; for ( it = nodes_pid_.begin(); it != nodes_pid_.end(); it++ ) { // Look for PID if not already on the list if (it->second == 0) { + // TODO(bcoltin): find ROS2 equivalent // Check if the node is being executed in this computer // Get URI of the node - args.setSize(2); - args[0] = ros::this_node::getName(); - args[1] = it->first; - if (!ros::master::execute("lookupNode", args, result, payload, true)) { - it->second = -1; - continue; - } - std::string node_host = getHostfromURI(result[2]); - if (node_host.empty()) { - it->second = -1; - continue; - } + // args.setSize(2); + // args[0] = ros::this_node::getName(); + // args[1] = it->first; + // if (!ros::master::execute("lookupNode", args, result, payload, true)) { + // it->second = -1; + // continue; + // } + // std::string node_host = getHostfromURI(result[2]); + // if (node_host.empty()) { + // it->second = -1; + // continue; + // } + std::string node_host; // If it is not in the same cpu if (node_host != monitor_host_) { @@ -377,7 +393,7 @@ int CpuMemMonitor::CollectCPUStats() { total, idle_all, system_all, virtual_all = 0; uint64_t total_period, user_period, nice_period, steal_period, guest_period, system_all_period; - uint64_t total_period_all; + uint64_t total_period_all = 0; uint32_t cpuid = 0; char buffer[256]; @@ -539,7 +555,7 @@ int CpuMemMonitor::CollectCPUStats() { } // Add time stamp - cpu_state_msg_.header.stamp = ros::Time::now(); + cpu_state_msg_.header.stamp = GetTimeNow(); return 0; } @@ -662,7 +678,7 @@ int CpuMemMonitor::CollectMemStats() { } // Send mem stats - mem_state_msg_.header.stamp = ros::Time::now(); + mem_state_msg_.header.stamp = GetTimeNow(); return 0; } @@ -705,29 +721,32 @@ void CpuMemMonitor::AssertMemStats() { } } -void CpuMemMonitor::PublishStatsCallback(ros::TimerEvent const &te) { +void CpuMemMonitor::PublishStatsCallback() { // Get cpu stats if (CollectCPUStats() < 0) { - NODELET_FATAL("CPU node unable to get load stats!"); + FF_FATAL("CPU node unable to get load stats!"); return; } // Assert cpu stats AssertMemStats(); // Publish CPU stats - cpu_state_pub_.publish(cpu_state_msg_); + cpu_state_pub_->publish(cpu_state_msg_); // Collect Memory stats if (CollectMemStats() < 0) { - NODELET_FATAL("Memory node unable to get load stats!"); + FF_FATAL("Memory node unable to get load stats!"); return; } // Assert memory stats AssertMemStats(); // Publish memory stats - mem_state_pub_.publish(mem_state_msg_); + mem_state_pub_->publish(mem_state_msg_); } } // namespace cpu_mem_monitor -PLUGINLIB_EXPORT_CLASS(cpu_mem_monitor::CpuMemMonitor, nodelet::Nodelet) +#include "rclcpp_components/register_node_macro.hpp" + +// Declare the plugin +RCLCPP_COMPONENTS_REGISTER_NODE(cpu_mem_monitor::CpuMemMonitor)