Minor cleanup

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

mission/system_monitor/config/system_monitor_config.yaml

@@ -1,3 +1,7 @@
 system_monitor:
   ros__parameters:
-    ip_list: ["10.24.166.212"]
+    ip_list:
+      - "10.0.1.100"
+      - "10.0.1.240"
+      - "10.0.1.250"
+    ping_rate: 1.0

mission/system_monitor/scripts/system_monitor_node.py

@@ -12,11 +12,17 @@ class SystemMonitor(Node):
     def __init__(self):
         super().__init__('system_monitor')
 
-        self.m_ip_list = ["10.24.166.212"]
-
+        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()
+
         self.m_force_publisher = self.create_publisher(Float32MultiArray, '/thrust/thruster_forces', 5)
         self.m_timer = self.create_timer(1.0, 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):
@@ -36,25 +42,26 @@ def ping_ip(self, ip):
         return True
 
     def shutdown_thruster_allocator(self):
-        self.get_logger().info('Attempting to shutdown thruster allocator')
+        self.get_logger().warn("Shutting down thruster allocator"
+         " -> Manual intervention will be necessary")
 
-        client = self.create_client(ChangeState, '/motion/thruster_allocator_node/change_state')
-        if not client.wait_for_service(timeout_sec=5.0):
+        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 = client.call_async(request)
-        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+        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 command')
+        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().info('Zero force command published')
+        self.get_logger().warn('Done! Manual intervention is now required!')
+
 
 def main(args=None):
     rclpy.init(args=args)
@@ -70,5 +77,6 @@ def main(args=None):
         node.destroy_node()
         rclpy.shutdown()
 
+
 if __name__ == '__main__':
     main()

motion/thruster_allocator/src/thruster_allocator_node.cpp

@@ -7,6 +7,9 @@ int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto allocator = std::make_shared<ThrusterAllocator>();
 
+   // Usually this would be done in the launchfile. However, we only want this node
+   // to be a lifecycle-node so that it can be shut down externally. Thus, we immediately
+   // transition to activate here.
    allocator->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
    allocator->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);