Fixing diff controller tests

rae_hw/CMakeLists.txt

@@ -155,6 +155,7 @@ install(
   scripts/mock_speakers.py
   scripts/mock_wheels.py
   scripts/lifecycle_manager.py
+  scripts/diff_controller_test.py
   DESTINATION lib/${PROJECT_NAME}
 )
 ament_package()

rae_hw/launch/control.launch.py

@@ -4,8 +4,8 @@
 
 from launch import LaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.actions import IncludeLaunchDescription, OpaqueFunction, DeclareLaunchArgument, RegisterEventHandler
-from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription, OpaqueFunction, DeclareLaunchArgument, RegisterEventHandler, ExecuteProcess
+from launch.substitutions import LaunchConfiguration, FindExecutable
 from launch.conditions import IfCondition
 from launch_ros.event_handlers import OnStateTransition
 from launch_ros.actions import Node, LifecycleNode
@@ -74,8 +74,9 @@ def launch_setup(context, *args, **kwargs):
         },
     )
 
-    diff_controller = Node(
+    diff_controller = LifecycleNode(
         package='controller_manager',
+        name='diff_controller',
         namespace=LaunchConfiguration('namespace'),
         executable='spawner',
         arguments=['diff_controller', '-c', '/controller_manager'],
@@ -127,6 +128,13 @@ def launch_setup(context, *args, **kwargs):
         executable='battery_node',
     )
 
+    diff_controller_test = Node(
+    package='rae_hw',
+    executable='diff_controller_test.py',
+    name='diff_controller_test',
+    namespace=LaunchConfiguration('namespace')
+)
+
     return [
         lifecycle_manager,
         led,
@@ -142,10 +150,15 @@ def launch_setup(context, *args, **kwargs):
                       imu_comp_filt,
                       controller_manager,
                       diff_controller,
-                      joint_state_broadcaster,
+                      joint_state_broadcaster
                       ]
-        ))
-
+        )
+        ),
+        RegisterEventHandler(OnStateTransition(
+        target_lifecycle_node=diff_controller,
+        goal_state='active',
+        entities=[diff_controller_test]
+    ))
     ]
 
 

rae_hw/scripts/diff_controller_test.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+import os
+import rclpy
+import time
+from rclpy.node import Node
+from controller_manager_msgs.srv import ListControllers
+
+class DiffControllerTest(Node):
+    def __init__(self):
+        super().__init__('rae_hw_diff_controller_test')
+        self.get_logger().info('Initializing Diff Controller Test')
+        self._mock = self.declare_parameter('mock', False).value
+        self._controllers_client = self.create_client(
+            ListControllers, 'controller_manager/list_controllers')
+        self.get_logger().info('Diff Controller Test initialized')
+
+    def check_diff_controller(self):
+        if self._mock:
+            return
+        self.get_logger().info('Checking diff controller...')
+        diff_controller_running = False
+        while not diff_controller_running:
+            self._controllers_client.wait_for_service()
+            req = ListControllers.Request()
+            future = self._controllers_client.call_async(req)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                for controller in future.result().controller:
+                    if controller.name == 'diff_controller' and controller.state == 'active':
+                        self.get_logger().info('Diff controller running')
+                        diff_controller_running = True
+                        break
+                else:
+                    self.get_logger().error('Failed to get controller list')
+            time.sleep(1.0)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DiffControllerTest()
+    node.check_diff_controller()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

rae_hw/scripts/lifecycle_manager.py

@@ -52,7 +52,6 @@ def startup(self):
         self.startup_visual_nodes()
         self.startup_rest()
 
-        self.check_diff_controller()
 
         self.finish_indicator()
 
@@ -70,25 +69,6 @@ def startup_rest(self):
             self.configure_and_activate(node_name)
             self.update_progress_indicators()
 
-    def check_diff_controller(self):
-        if self._mock:
-            return
-        self.get_logger().info('Checking diff controller...')
-        diff_controller_running = False
-        while not diff_controller_running:
-            self._controllers_client.wait_for_service()
-            req = ListControllers.Request()
-            future = self._controllers_client.call_async(req)
-            rclpy.spin_until_future_complete(self, future)
-            if future.result() is not None:
-                for controller in future.result().controller:
-                    if controller.name == 'diff_controller':
-                        if controller.state == 'active':
-                            self.get_logger().info('Diff controller running')
-                        diff_controller_running = True
-                        break
-            else:
-                self.get_logger().error('Failed to get controller list')
 
     def finish_indicator(self):
         if self._silent_startup: