From 676f86bcd0c0219b7cab4a1e17638fc565d7dbfd Mon Sep 17 00:00:00 2001 From: David Date: Fri, 10 Oct 2025 18:58:30 -0500 Subject: [PATCH] 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 | 22 ++-- .../config/ros2_controllers.yaml | 8 +- .../launch/load_ros2_controllers.launch.py | 118 ++++++++++++++++++ 3 files changed, 133 insertions(+), 15 deletions(-) create mode 100644 src/core_rover_description/launch/load_ros2_controllers.launch.py diff --git a/src/core_gazebo/launch/core.gazebo.launch.py b/src/core_gazebo/launch/core.gazebo.launch.py index 7b991cd..746bf0c 100644 --- a/src/core_gazebo/launch/core.gazebo.launch.py +++ b/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 - # load_controllers_cmd = IncludeLaunchDescription( - # PythonLaunchDescriptionSource([ - # os.path.join(pkg_share_moveit, 'launch', 'load_ros2_controllers.launch.py') - # ]), - # launch_arguments={ - # 'use_sim_time': use_sim_time - # }.items(), - # condition=IfCondition(load_controllers) - # ) + # Include ROS 2 Controllers launch file if enabled + load_controllers_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(pkg_share_description, 'launch', 'load_ros2_controllers.launch.py') + ]), + launch_arguments={ + 'use_sim_time': use_sim_time + }.items(), + 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) diff --git a/src/core_rover_description/config/ros2_controllers.yaml b/src/core_rover_description/config/ros2_controllers.yaml index f83e968..a0e158a 100644 --- a/src/core_rover_description/config/ros2_controllers.yaml +++ b/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 diff --git a/src/core_rover_description/launch/load_ros2_controllers.launch.py b/src/core_rover_description/launch/load_ros2_controllers.launch.py new file mode 100644 index 0000000..8cc981c --- /dev/null +++ b/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