Add initial system monitor draft and lifeycle node management

Signed-off-by: Christopher Strøm <[email protected]>

asv_setup/launch/freya.launch.py

@@ -12,6 +12,13 @@ def generate_launch_description():
         value='[${severity}] [${time}] [${node}]: ${message}'
     )
 
+    # System Monitor launch
+    system_monitor_launch = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+            path.join(get_package_share_directory('system_monitor'),'launch','system_monitor.launch.py')
+        )
+    )
+
     # Thruster Allocator launch
     thruster_allocator_launch = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(
@@ -29,6 +36,7 @@ def generate_launch_description():
     # Return launch description
     return LaunchDescription([
         set_env_var,
+        system_monitor_launch,
         thruster_allocator_launch,
-        thruster_interface_launch
+        thruster_interface_launch,
     ])

mission/system_monitor/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+project(system_monitor)
+
+# Find required packages
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/system_monitor_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install launch and config files
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

mission/system_monitor/config/system_monitor_config.yaml

@@ -0,0 +1,3 @@
+system_monitor:
+  ros__parameters:
+    ip_list: ["10.24.166.212"]

mission/system_monitor/launch/system_monitor.launch.py

@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('system_monitor'),
+        'config',
+        'system_monitor_config.yaml'
+    )
+
+    system_monitor_node = Node(
+        package='system_monitor',
+        executable='system_monitor_node.py',
+        name='system_monitor',
+        output='screen',
+        emulate_tty=True,
+        parameters=[config]
+    )
+
+    return LaunchDescription([
+        system_monitor_node
+    ])

mission/system_monitor/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>system_monitor</name>
+  <version>0.0.0</version>
+  <description>A global system monitor to ensure safe autonomous operations</description>
+  <maintainer email="[email protected]">Christopher Strøm</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mission/system_monitor/scripts/system_monitor_node.py

@@ -0,0 +1,74 @@
+#!/usr/bin/python3
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32MultiArray
+from lifecycle_msgs.srv import ChangeState
+from lifecycle_msgs.msg import Transition
+import os
+import sys
+
+class SystemMonitor(Node):
+    def __init__(self):
+        super().__init__('system_monitor')
+
+        self.m_ip_list = ["10.24.166.212"]
+
+        self.m_force_publisher = self.create_publisher(Float32MultiArray, '/thrust/thruster_forces', 5)
+        self.m_timer = self.create_timer(1.0, self.timer_callback)
+
+        self.get_logger().info('SystemMonitor initialized')
+
+    def timer_callback(self):
+        all_ips_responsive = all(self.ping_ip(ip) for ip in self.m_ip_list)
+
+        if not all_ips_responsive:
+            self.get_logger().warn('One or more IPs are unresponsive. Initiating shutdown sequence.')
+            self.shutdown_thruster_allocator()
+            self.publish_zero_force()
+            sys.exit(0)
+
+    def ping_ip(self, ip):
+        response = os.system(f"ping -c 1 -W 1 {ip} > /dev/null 2>&1")
+        if response != 0:
+            self.get_logger().error(f'Failed to ping IP: {ip}')
+            return False
+        return True
+
+    def shutdown_thruster_allocator(self):
+        self.get_logger().info('Attempting to shutdown thruster allocator')
+
+        client = self.create_client(ChangeState, '/motion/thruster_allocator_node/change_state')
+        if not client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().error('Service not available')
+            return
+
+        request = ChangeState.Request()
+        request.transition.id = Transition.TRANSITION_DEACTIVATE
+
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+
+    def publish_zero_force(self):
+        self.get_logger().info('Publishing zero force command')
+        message = Float32MultiArray()
+        message.data = [0.0] * 4
+        self.m_force_publisher.publish(message)
+        self.get_logger().info('Zero force command published')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SystemMonitor()
+    node.get_logger().info('Starting SystemMonitor node')
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.get_logger().info('SystemMonitor node shutting down')
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

mission/system_monitor/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'system_monitor'
+
+setup(
+    name=package_name,
+    version='0.0.1',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='[email protected]',
+    description='System Monitor for ROS2',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'system_monitor_node = system_monitor.system_monitor_node:main'
+        ],
+    },
+)

motion/thruster_allocator/CMakeLists.txt

@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.8)
 project(thruster_allocator)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
+  set(CMAKE_CXX_STANDARD 17)
+endif() 
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -16,6 +16,7 @@ find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 
 include_directories(include)
 
@@ -32,6 +33,8 @@ ament_target_dependencies(${PROJECT_NAME}_node
   Eigen3
   )
 
+target_link_libraries(${PROJECT_NAME}_node   rclcpp_lifecycle::rclcpp_lifecycle)
+
 install(
   DIRECTORY include/
   DESTINATION include

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -1,57 +1,49 @@
-/**
- * @file allocator_ros.hpp
- * @brief ThrusterAllocator class, which
- * allocates thrust to the ASV's thrusters based on the desired body frame
- * forces.
- */
-
 #ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 #define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 
 #include "thruster_allocator/allocator_utils.hpp"
 #include "thruster_allocator/pseudoinverse_allocator.hpp"
 #include <eigen3/Eigen/Eigen>
-#include <rclcpp/rclcpp.hpp>
-
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
 
 using namespace std::chrono_literals;
 
-class ThrusterAllocator : public rclcpp::Node {
+class ThrusterAllocator : public rclcpp_lifecycle::LifecycleNode {
 public:
-  explicit ThrusterAllocator();
+    explicit ThrusterAllocator();
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+    on_configure(const rclcpp_lifecycle::State &);
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+    on_activate(const rclcpp_lifecycle::State &);
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+    on_deactivate(const rclcpp_lifecycle::State &);
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+    on_cleanup(const rclcpp_lifecycle::State &);
 
-  /**
-   * @brief Calculates the allocated
-   * thrust based on the body frame forces. It then saturates the output vector
-   * between min and max values and publishes the thruster forces to the topic
-   * "thrust/thruster_forces".
-   */
-  void calculate_thrust_timer_cb();
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+    on_shutdown(const rclcpp_lifecycle::State &);
 
 private:
-  Eigen::MatrixXd thrust_configuration;
-  /**
-   * @brief Callback function for the wrench input subscription. Extracts the
-   * surge, sway and yaw values from the received wrench msg
-   * and stores them in the body_frame_forces_ Eigen vector.
-   * @param msg The received geometry_msgs::msg::Wrench message.
-   */
-  void wrench_callback(const geometry_msgs::msg::Wrench &msg);
-  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr
-      thruster_forces_publisher_;
-  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr
-      wrench_subscriber_;
-  rclcpp::TimerBase::SharedPtr calculate_thrust_timer_;
-  size_t count_;
-  int num_dof_;
-  int num_thrusters_;
-  int min_thrust_;
-  int max_thrust_;
-  Eigen::Vector3d body_frame_forces_;
-  std::vector<int64_t> direction_;
-  PseudoinverseAllocator pseudoinverse_allocator_;
+    void calculate_thrust_timer_cb();
+    void wrench_callback(const geometry_msgs::msg::Wrench &msg);
+
+    Eigen::MatrixXd thrust_configuration;
+    rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float32MultiArray>::SharedPtr thruster_forces_publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr wrench_subscriber_;
+    rclcpp::TimerBase::SharedPtr calculate_thrust_timer_;
+    int num_dof_;
+    int num_thrusters_;
+    int min_thrust_;
+    int max_thrust_;
+    Eigen::Vector3d body_frame_forces_;
+    std::vector<int64_t> direction_;
+    PseudoinverseAllocator pseudoinverse_allocator_;
 };
 
-#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
+#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP

motion/thruster_allocator/launch/thruster_allocator.launch.py

@@ -1,19 +1,20 @@
 from os import path
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import LifecycleNode
 
 def generate_launch_description():
-    config = path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml')
-
-    thruster_allocator_node = Node(
+    config = path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml')
+    
+    thruster_allocator_node = LifecycleNode(
         package='thruster_allocator',
         executable='thruster_allocator_node',
         name='thruster_allocator_node',
+        namespace='motion',
         parameters=[config],
-        output='screen'
+        output='screen',
     )
-    
+
     return LaunchDescription([
         thruster_allocator_node
-    ])
+    ])