feat: make ros2 controllers start automatically

Adds load_ros2_controllers.launch.py
Weird issue where if the update rate on the controller is 10, then the controller manager doesn't respond to requests, but if you set it to 100 (still less than gazebo's rate), then it works...

src/core_gazebo/launch/core.gazebo.launch.py

@@ -179,16 +179,16 @@ def generate_launch_description():
         condition=IfCondition(use_robot_state_pub)
     )
 
-    # # Include ROS 2 Controllers launch file if enabled
+    # Include ROS 2 Controllers launch file if enabled
-    # load_controllers_cmd = IncludeLaunchDescription(
+    load_controllers_cmd = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource([
+        PythonLaunchDescriptionSource([
-    #         os.path.join(pkg_share_moveit, 'launch', 'load_ros2_controllers.launch.py')
+            os.path.join(pkg_share_description, 'launch', 'load_ros2_controllers.launch.py')
-    #     ]),
+        ]),
-    #     launch_arguments={
+        launch_arguments={
-    #         'use_sim_time': use_sim_time
+            'use_sim_time': use_sim_time
-    #     }.items(),
+        }.items(),
-    #     condition=IfCondition(load_controllers)
+        condition=IfCondition(load_controllers)
-    # )
+    )
 
     # Set Gazebo model path - include both models directory and ROS packages
     set_env_vars_resources = AppendEnvironmentVariable(
@@ -277,7 +277,7 @@ def generate_launch_description():
     ld.add_action(set_env_vars_resources)
     ld.add_action(set_env_vars_packages)
     ld.add_action(robot_state_publisher_cmd)
-    # ld.add_action(load_controllers_cmd)
+    ld.add_action(load_controllers_cmd)
     ld.add_action(start_gazebo_cmd)
     ld.add_action(start_gazebo_ros_bridge_cmd)
     ld.add_action(start_gazebo_ros_image_bridge_cmd)

src/core_rover_description/config/ros2_controllers.yaml

@@ -1,13 +1,13 @@
 controller_manager:
   ros__parameters:
-    update_rate: 10
+    update_rate: 100
     use_sim_time: true
 
     diff_controller:
-      type: "diff_drive_controller/DiffDriveController"
+      type: diff_drive_controller/DiffDriveController
 
     joint_broadcaster:
-      type: "joint_state_broadcaster/JointStateBroadcaster"
+      type: joint_state_broadcaster/JointStateBroadcaster
 
 diff_controller: 
   ros__parameters:
@@ -30,6 +30,6 @@ diff_controller:
     publish_limited_velocity: false
 
     use_stamped_vel: false
-    publish_rate: 10.0
+    publish_rate: 100.0
 
     # TODO: add reasonable velocity and acceleration limits

src/core_rover_description/launch/load_ros2_controllers.launch.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+"""
+Launch ROS 2 controllers for the rover.
+
+This script creates a launch description that starts the necessary controllers
+for operating the rover's differential drive system in a specific sequence.
+
+Launched Controllers:
+    1. Joint State Broadcaster: Publishes joint states to /joint_states
+    2. Diff Drive Controller: Controls the rover's differential drive movement
+
+Launch Sequence:
+    1. Wait 5 seconds for Gazebo and controller manager to initialize
+    2. Load Joint State Broadcaster (unconfigured state)
+    3. Configure Joint State Broadcaster (unconfigured -> inactive)
+    4. Activate Joint State Broadcaster (inactive -> active)
+    5. Load Diff Drive Controller (unconfigured state)
+    6. Configure Diff Drive Controller (unconfigured -> inactive)
+    7. Activate Diff Drive Controller (inactive -> active)
+
+Note: ROS2 controllers follow the state machine: unconfigured -> inactive -> active.
+Each state transition must be done explicitly.
+"""
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessExit
+
+
+def generate_launch_description():
+    """Generate a launch description for loading, configuring and activating robot controllers.
+
+    Controllers follow the ROS2 control state machine: unconfigured -> inactive -> active.
+    Each transition must be done explicitly in sequence.
+
+    Returns:
+        LaunchDescription: Launch description containing sequenced controller state transitions
+    """
+
+    # Load joint state broadcaster first
+    load_joint_state_broadcaster_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', 'joint_broadcaster'],
+        output='screen')
+
+    # Configure joint state broadcaster (unconfigured -> inactive)
+    configure_joint_state_broadcaster_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'set_controller_state', 'joint_broadcaster', 'inactive'],
+        output='screen')
+
+    # Activate joint state broadcaster (inactive -> active)
+    start_joint_state_broadcaster_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'set_controller_state', 'joint_broadcaster', 'active'],
+        output='screen')
+
+    # Load diff drive controller
+    load_diff_drive_controller_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', 'diff_controller'],
+        output='screen')
+
+    # Configure diff drive controller (unconfigured -> inactive)
+    configure_diff_drive_controller_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'set_controller_state', 'diff_controller', 'inactive'],
+        output='screen')
+
+    # Activate diff drive controller (inactive -> active)
+    start_diff_drive_controller_cmd = ExecuteProcess(
+        cmd=['ros2', 'control', 'set_controller_state', 'diff_controller', 'active'],
+        output='screen')
+
+    # Add delay to allow controller manager to fully initialize
+    delayed_start = TimerAction(
+        period=5.0,
+        actions=[load_joint_state_broadcaster_cmd]
+    )
+
+    # Register event handlers for sequencing
+    # Configure joint state broadcaster after loading it (unconfigured -> inactive)
+    configure_joint_broadcaster_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=load_joint_state_broadcaster_cmd,
+            on_exit=[configure_joint_state_broadcaster_cmd]))
+
+    # Activate joint state broadcaster after configuring it (inactive -> active)
+    activate_joint_broadcaster_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=configure_joint_state_broadcaster_cmd,
+            on_exit=[start_joint_state_broadcaster_cmd]))
+
+    # Load diff controller after joint state broadcaster is activated
+    load_diff_controller_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=start_joint_state_broadcaster_cmd,
+            on_exit=[load_diff_drive_controller_cmd]))
+
+    # Configure diff controller after loading it (unconfigured -> inactive)
+    configure_diff_controller_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=load_diff_drive_controller_cmd,
+            on_exit=[configure_diff_drive_controller_cmd]))
+
+    # Activate diff controller after configuring it (inactive -> active)
+    activate_diff_controller_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=configure_diff_drive_controller_cmd,
+            on_exit=[start_diff_drive_controller_cmd]))
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Add the actions to the launch description in sequence
+    ld.add_action(delayed_start)
+    ld.add_action(configure_joint_broadcaster_event)
+    ld.add_action(activate_joint_broadcaster_event)
+    ld.add_action(load_diff_controller_event)
+    ld.add_action(configure_diff_controller_event)
+    ld.add_action(activate_diff_controller_event)
+
+    return ld