diff --git a/rox_bringup/launch/bringup_sim_launch.py b/rox_bringup/launch/bringup_sim_launch.py index ad70e27..6869fef 100644 --- a/rox_bringup/launch/bringup_sim_launch.py +++ b/rox_bringup/launch/bringup_sim_launch.py @@ -14,7 +14,6 @@ from param_file_utils import generate_final_yaml import os from pathlib import Path -import xacro def execution_stage(context: LaunchContext, rox_type, @@ -24,7 +23,9 @@ def execution_stage(context: LaunchContext, scanner_type, ur_dc, gripper_type, - headless_sim): + headless_sim, + spawn_x, spawn_y, spawn_z, + spawn_R, spawn_P, spawn_Y): launch_actions = [] @@ -48,7 +49,9 @@ def execution_stage(context: LaunchContext, joint_type = "revolute" # Getting the robot description xacro - urdf = os.path.join(get_package_share_directory('rox_description'), 'urdf', 'rox.urdf.xacro') + urdf = os.path.join(get_package_share_directory('rox_description'), + 'urdf', + 'rox.urdf.xacro') spawn_robot = Node( package='ros_gz_sim', @@ -57,7 +60,15 @@ def execution_stage(context: LaunchContext, output='screen', arguments=[ '-topic', "robot_description", - '-name', "rox"]) + '-name', "rox", + '-x', spawn_x.perform(context), + '-y', spawn_y.perform(context), + '-z', spawn_z.perform(context), + '-R', spawn_R.perform(context), + '-P', spawn_P.perform(context), + '-Y', spawn_Y.perform(context) + ] + ) # Define gz_args based on headless_simulation argument gz_args = f"-r {default_world_path}" @@ -239,6 +250,25 @@ def execution_stage(context: LaunchContext, def generate_launch_description(): + declare_spawn_x_cmd = DeclareLaunchArgument( + 'spawn_x', default_value='0.0', description='Initial X position of the robot in gz' + ) + declare_spawn_y_cmd = DeclareLaunchArgument( + 'spawn_y', default_value='0.0', description='Initial Y position of the robot in gz' + ) + declare_spawn_z_cmd = DeclareLaunchArgument( + 'spawn_z', default_value='0.0', description='Initial Z position of the robot in gz' + ) + declare_spawn_R_cmd = DeclareLaunchArgument( + 'spawn_R', default_value='0.0', description='Initial roll of the robot in gz' + ) + declare_spawn_P_cmd = DeclareLaunchArgument( + 'spawn_P', default_value='0.0', description='Initial pitch of the robot in gz' + ) + declare_spawn_Y_cmd = DeclareLaunchArgument( + 'spawn_Y', default_value='0.0', description='Initial yaw of the robot in gz' + ) + declare_rox_type_cmd = DeclareLaunchArgument( 'rox_type',default_value='argo', choices = ['', 'argo', 'argo-trio', 'diff', 'trike'], @@ -293,7 +323,13 @@ def generate_launch_description(): LaunchConfiguration('scanner_type'), LaunchConfiguration('use_ur_dc'), LaunchConfiguration('gripper_type'), - LaunchConfiguration('headless_simulation') + LaunchConfiguration('headless_simulation'), + LaunchConfiguration('spawn_x'), + LaunchConfiguration('spawn_y'), + LaunchConfiguration('spawn_z'), + LaunchConfiguration('spawn_R'), + LaunchConfiguration('spawn_P'), + LaunchConfiguration('spawn_Y'), ]) ld = LaunchDescription([ @@ -305,6 +341,12 @@ def generate_launch_description(): declare_ur_pwr_variant_cmd, declare_gripper_type_cmd, declare_headless_sim_cmd, + declare_spawn_x_cmd, + declare_spawn_y_cmd, + declare_spawn_z_cmd, + declare_spawn_R_cmd, + declare_spawn_P_cmd, + declare_spawn_Y_cmd, opq_function ]) return ld