diff --git a/rktl_control2/launch/ball.launch.py b/rktl_control2/launch/ball.launch.py index 757202ca..22237402 100644 --- a/rktl_control2/launch/ball.launch.py +++ b/rktl_control2/launch/ball.launch.py @@ -9,13 +9,15 @@ def generate_launch_description(): ld = launch.LaunchDescription([ launch_ros.actions.Node( + namespace='ball', package='rktl_control', executable='mean_odom_filter', name='mean_odom_filter', output='screen', parameters=[ - get_package_share_directory( - 'rktl_control') + '/config/mean_odom_filter.yaml' + { + launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/mean_odom_filter.yaml') + } ] ) ]) diff --git a/rktl_control2/launch/car.launch.py b/rktl_control2/launch/car.launch.py index b1d67a5b..11ec08fd 100644 --- a/rktl_control2/launch/car.launch.py +++ b/rktl_control2/launch/car.launch.py @@ -14,6 +14,55 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument( name='use_particle_filter', default_value='true' + ), + launch.actions.GroupAction( + actions=[ + launch_ros.actions.PushRosNamespace("cars/" + launch.substitutions.LaunchConfiguration("car_name")), + + launch_ros.actions.Node( + package='rktl_control', + executable='particle_odom_filter', + name='particle_odom_filter', + output='screen', + condition=launch.conditions.LaunchConfigurationEquals('use_particle_filter', True), + parameters=[ + { + launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/particle_odom_filter.yaml') + }, + { + 'frame_ids/body': launch.substitutions.LaunchConfiguration('car_name') + } + ] + ), + + launch_ros.actions.Node( + package='rktl_control', + executable='mean_odom_filter', + name='mean_odom_filter', + output='screen', + condition=launch.conditions.LaunchConfigurationNotEquals('use_particle_filter', True), + parameters=[ + { + launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/mean_odom_filter.yaml') + }, + { + 'frame_ids/body': launch.substitutions.LaunchConfiguration('car_name') + } + ] + ), + + launch_ros.actions.Node( + package='rktl_control', + executable='controller', + name='controller', + output='screen', + parameters=[ + { + launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/controller.yaml') + }, + ] + ) + ] ) ]) return ld diff --git a/rktl_control2/launch/hardware_interface.launch.py b/rktl_control2/launch/hardware_interface.launch.py index d0960cb1..23ba6a08 100644 --- a/rktl_control2/launch/hardware_interface.launch.py +++ b/rktl_control2/launch/hardware_interface.launch.py @@ -13,8 +13,9 @@ def generate_launch_description(): executable='serial_node.py', name='hardware_interface', parameters=[ - get_package_share_directory( - 'rktl_control') + '/config/hardware_interface.yaml' + { + launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/hardware_interface.yaml') + } ] ) ]) diff --git a/rktl_control2/launch/keyboard_control.launch.py b/rktl_control2/launch/keyboard_control.launch.py index 6842aeaf..74251cbc 100644 --- a/rktl_control2/launch/keyboard_control.launch.py +++ b/rktl_control2/launch/keyboard_control.launch.py @@ -12,6 +12,7 @@ def generate_launch_description(): default_value='car0' ), launch_ros.actions.Node( + namespace=launch.substitutions.PathJoinSubstitution('cars/', launch_ros.substitutions.FindPackageShare('car_name')), package='rktl_control', executable='keyboard_interface', name='keyboard_interface', diff --git a/rktl_control2/launch/xbox_control.launch.py b/rktl_control2/launch/xbox_control.launch.py index 7ee0f430..6e512bdb 100644 --- a/rktl_control2/launch/xbox_control.launch.py +++ b/rktl_control2/launch/xbox_control.launch.py @@ -18,7 +18,54 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument( name='delay', default_value='0.1' + ), + launch.actions.GroupAction( + actions=[ + launch_ros.actions.PushRosNamespace("cars/" + launch.substitutions.LaunchConfiguration("car_name")), + + launch_ros.actions.Node( + package='joy', + executable='joy_node', + name='joy_node', + output='screen', + parameters=[ + { + 'dev': launch.substitutions.LaunchConfiguration('device') + }, + { + 'default_trig_val': 'true' + } + ] + ), + launch_ros.actions.Node( + package='rktl_control', + executable='xbox_interface', + name='xbox_interface', + output='screen', + parameters=[ + { + 'base_throttle': '0.75' + }, + { + 'boost_throttle': '1.25' + }, + { + 'cooldown_ratio': '3' + }, + { + 'max_boost': '2' + } + ], + actions=[ + launch_ros.actions.SetRemap( + src="joy", + dst="joy)mux", + ) + ] + ) + ] ) + ]) return ld