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