Convert CPU Mem Monitor to ROS2

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',

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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
 
-# 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()

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 <pluginlib/class_list_macros.h>
-
-#include <ros/ros.h>
-#include <ros/package.h>
+#include <ff_common/ff_ros.h>
 
 #include <inttypes.h>
 #include <stdio.h>
@@ -36,12 +33,12 @@
 
 #include <config_reader/config_reader.h>
 #include <cpu_mem_monitor/cpu.h>
-#include <ff_msgs/CpuState.h>
-#include <ff_msgs/CpuStateStamped.h>
-#include <ff_msgs/MemState.h>
-#include <ff_msgs/MemStateStamped.h>
+#include <ff_msgs/msg/cpu_state.hpp>
+#include <ff_msgs/msg/cpu_state_stamped.hpp>
+#include <ff_msgs/msg/mem_state.hpp>
+#include <ff_msgs/msg/mem_state_stamped.hpp>
 #include <ff_common/ff_names.h>
-#include <ff_util/ff_nodelet.h>
+#include <ff_util/ff_component.h>
 
 #include <cstdint>
 #include <string>
@@ -50,39 +47,40 @@
 
 namespace cpu_mem_monitor {
 
+
 enum LoadFaultState {
   ASSERTING,
   ASSERTED,
   CLEARING,
   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<ff_msgs::msg::CpuStateStamped>::SharedPtr cpu_state_pub_;
+  rclcpp::Publisher<ff_msgs::msg::MemStateStamped>::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_

management/cpu_mem_monitor/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>cpu_mem_monitor</name>
   <version>0.0.0</version>
   <description>
@@ -14,20 +14,18 @@
   <maintainer email="astrobee-fsw@nx.arc.nasa.gov">
     Astrobee Flight Software
   </maintainer>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>nodelet</build_depend>
+  <build_depend>ament_cmake</build_depend>
+  <build_depend>rclcpp</build_depend>
   <build_depend>ff_msgs</build_depend>
   <build_depend>ff_hw_msgs</build_depend>
   <build_depend>config_reader</build_depend>
   <build_depend>ff_util</build_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>ff_msgs</run_depend>
-  <run_depend>ff_hw_msgs</run_depend>
-  <run_depend>config_reader</run_depend>
-  <run_depend>ff_util</run_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>ff_msgs</exec_depend>
+  <exec_depend>ff_hw_msgs</exec_depend>
+  <exec_depend>config_reader</exec_depend>
+  <exec_depend>ff_util</exec_depend>
   <export>
-    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <build_type>ament_cmake</build_type>
   </export>
 </package>