diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py index 18ebda1f..5ccbc4c4 100644 --- a/urc_bringup/launch/sim.launch.py +++ b/urc_bringup/launch/sim.launch.py @@ -1,13 +1,16 @@ import os -from tempfile import NamedTemporaryFile from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution from launch_ros.descriptions import ParameterValue from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node -from xacro import process_file def generate_launch_description(): @@ -45,18 +48,10 @@ def generate_launch_description(): world_filename = LaunchConfiguration("world") walli_xacro_config = LaunchConfiguration("walli_xacro") - # Construct the full world path from the filename world_path = PathJoinSubstitution( [path_urc_hw_description, "world", world_filename] ) - """ - robot_urdf_file = process_file( - ParameterValue(walli_xacro_config, value_type = str), - mappings = {"use_sim": "true"} - ).toxml() - """ - robot_urdf_file = ParameterValue( Command( [ @@ -76,20 +71,6 @@ def generate_launch_description(): launch_arguments={"gz_args": world_path}.items(), ) - rover_pose_bridge = Node( - package="urc_bringup", - executable="urc_bringup_RoverPoseBridge", - name="rover_pose_bridge", - parameters=[ - { - "tf_topic": "/ground_truth_pose", - "rover_pos_topic": "/rover_ground_truth", - "use_sim_time": True, - } - ], - output="screen", - ) - bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", @@ -106,37 +87,6 @@ def generate_launch_description(): output="screen", ) - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[controller_config_file_dir], - output="screen", - remappings=[("/controller_manager/robot_description", "/robot_description")], - ) - - load_joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"], - ) - - # load_position_controller = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["-p", controller_config_file_dir, "position_controller"], - # ) - # - # load_velocity_controller = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["-p", controller_config_file_dir, "velocity_controller"], - # ) - load_swerve_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "swerve_controller"], - ) - covariances_on_imu = Node( package="urc_localization", executable="urc_localization_CovariancesOnImu", @@ -150,6 +100,20 @@ def generate_launch_description(): output="screen", ) + rover_pose_bridge = Node( + package="urc_bringup", + executable="urc_bringup_RoverPoseBridge", + name="rover_pose_bridge", + parameters=[ + { + "tf_topic": "/ground_truth_pose", + "rover_pos_topic": "/rover_ground_truth", + "use_sim_time": True, + } + ], + output="screen", + ) + spawn = Node( package="ros_gz_sim", executable="create", @@ -174,21 +138,51 @@ def generate_launch_description(): ], ) + # --- Controller Spawners --- + load_joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--param-file", + controller_config_file_dir, + ], + ) + + load_swerve_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "swerve_controller", + "--param-file", + controller_config_file_dir, + ], + ) + return LaunchDescription( [ sim_world_arg, walli_xacro, - gz_sim, - spawn, - covariances_on_imu, bridge_yaml, + gz_sim, bridge, - control_node, robot_state_publisher_node, - load_joint_state_broadcaster, - # load_position_controller, - # load_velocity_controller, - load_swerve_controller, + covariances_on_imu, rover_pose_bridge, + spawn, + # Start joint_state_broadcaster AFTER robot spawn + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn, + on_exit=[load_joint_state_broadcaster], + ) + ), + # Start swerve controller AFTER joint_state_broadcaster + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_swerve_controller], + ) + ), ] ) diff --git a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro index 820ab451..4407ed8a 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_ros2_control.xacro @@ -14,7 +14,7 @@ - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem urc_hw/RoverDrivetrain @@ -114,7 +114,7 @@ - + $(find urc_bringup)/config/test_controllers.yaml diff --git a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro index 5922e13e..45d682b7 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro @@ -87,18 +87,14 @@ - - 0.001 0.001 0.001 - + - - 0.001 0.001 0.001 - +