System monitor node

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,34 @@
+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
+  system_monitor/system_monitor_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install launch and config files
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Install the package.xml file
+install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mission/system_monitor/config/system_monitor_config.yaml

@@ -0,0 +1,8 @@
+system_monitor:
+  ros__parameters:
+    ip_list:
+      - "10.0.1.10"
+      - "10.0.1.100"
+      - "10.0.1.240"
+      - "10.0.1.250"
+    ping_rate: 1.0

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,25 @@
+<?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>
+  <buildtool_depend>ament_cmake_python</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/system_monitor/system_monitor_node.py

@@ -0,0 +1,82 @@
+#!/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.declare_parameter('ip_list', rclpy.Parameter.Type.STRING_ARRAY)
+        self.m_ip_list = self.get_parameter('ip_list').get_parameter_value().string_array_value
+
+        self.declare_parameter("ping_rate", rclpy.Parameter.Type.DOUBLE)
+        ping_rate = self.get_parameter("ping_rate").get_parameter_value().double_value
+
+        self.m_force_publisher = self.create_publisher(Float32MultiArray, '/thrust/thruster_forces', 5)
+        self.m_timer = self.create_timer(ping_rate, self.timer_callback)
+
+        self.m_allocator_lifecycle_client = self.create_client(ChangeState, '/motion/thruster_allocator_node/change_state')
+
+        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().warn("Shutting down thruster allocator"
+         " -> Manual intervention will be necessary")
+
+        if not self.m_allocator_lifecycle_client.wait_for_service(timeout_sec=0.5):
+            self.get_logger().error('Service not available')
+            return
+
+        request = ChangeState.Request()
+        request.transition.id = Transition.TRANSITION_DEACTIVATE
+
+        future = self.m_allocator_lifecycle_client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=0.5)
+
+    def publish_zero_force(self):
+        self.get_logger().info('Publishing zero-force on shutdown...')
+        message = Float32MultiArray()
+        message.data = [0.0] * 4
+        self.m_force_publisher.publish(message)
+        self.get_logger().warn('Done! Manual intervention is now required!')
+
+
+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()

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

@@ -11,40 +11,47 @@
 #include "thruster_allocator/allocator_utils.hpp"
 #include "thruster_allocator/pseudoinverse_allocator.hpp"
 #include <eigen3/Eigen/Eigen>
-#include <rclcpp/rclcpp.hpp>
-
 #include <geometry_msgs/msg/wrench.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.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();
 
+  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 &);
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_shutdown(const rclcpp_lifecycle::State &);
+
+private:
   /**
    * @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();
+  void wrench_callback(const geometry_msgs::msg::Wrench &msg);
 
-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_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_;
-  size_t count_;
   int num_dof_;
   int num_thrusters_;
   int min_thrust_;
@@ -54,4 +61,4 @@ class ThrusterAllocator : public rclcpp::Node {
   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
-    ])
+    ])