diff --git a/launch/mrs_drone_spawner.launch.py b/launch/mrs_drone_spawner.launch.py index 4cf5430..80397ac 100755 --- a/launch/mrs_drone_spawner.launch.py +++ b/launch/mrs_drone_spawner.launch.py @@ -3,8 +3,8 @@ import launch import os -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, LogInfo from launch.substitutions import LaunchConfiguration, IfElseSubstitution, PythonExpression, PathJoinSubstitution, EnvironmentVariable from ament_index_python.packages import get_package_share_directory @@ -19,36 +19,35 @@ def generate_launch_description(): # Launch arguments declaration ld.add_action(DeclareLaunchArgument( 'custom_config', - default_value="", + default_value = "", description="Path to the custom configuration file. The path can be absolute, starting with '/' or relative to the current working directory", - )) + )) ld.add_action(DeclareLaunchArgument( - 'spawner_params', + 'default_spawner_config', default_value = PathJoinSubstitution([ pkg_mrs_uav_gazebo_simulator, 'config', 'spawner_params.yaml' ]), description='Path to the default spawner configuration file. The path can be absolute, starting with "/" or relative to the current working directory', )) + ld.add_action(DeclareLaunchArgument( 'debug', default_value = 'false', description='Run spawner with debug log level' )) - # This logic correctly creates a substitution that resolves to an absolute path. - custom_config_path = IfElseSubstitution( - condition=PythonExpression(['"', LaunchConfiguration('custom_config'), '" != "" and ', 'not "', LaunchConfiguration('custom_config'), '".startswith("/")']), - if_value=PathJoinSubstitution([EnvironmentVariable('PWD'), LaunchConfiguration('custom_config')]), - else_value=LaunchConfiguration('custom_config') - ) - - # Conditionally set the log level using PythonExpression log_level = PythonExpression([ "'debug' if '", LaunchConfiguration('debug'), "' == 'true' else 'info'" ]) + log_config_args = LogInfo(msg=[ + '\n[mrs_drone_spawner.launch.py] Custom config file: ', LaunchConfiguration('custom_config'), + '\n[mrs_drone_spawner.launch.py] Default config file: ', LaunchConfiguration('default_spawner_config') + ]) + ld.add_action(log_config_args) + ld.add_action( Node( name='mrs_drone_spawner', @@ -57,9 +56,8 @@ def generate_launch_description(): output="screen", arguments=['--ros-args', '--log-level', log_level], parameters=[ - # This passes the custom_config path as a parameter named 'custom_config' - {'custom_config': custom_config_path}, - LaunchConfiguration('spawner_params'), + LaunchConfiguration('default_spawner_config'), + LaunchConfiguration('custom_config') ], remappings=[ diff --git a/launch/simulation.launch.py b/launch/simulation.launch.py index 4a6f99a..6371095 100755 --- a/launch/simulation.launch.py +++ b/launch/simulation.launch.py @@ -28,9 +28,7 @@ def generate_launch_description(): declare_spawner_config_arg = DeclareLaunchArgument( 'spawner_config', - default_value = PathJoinSubstitution([ - pkg_mrs_uav_gazebo_simulator, 'config', 'spawner_params.yaml' - ]), + default_value = "", description='Configuration file for the custom spawner.' ) @@ -127,7 +125,7 @@ def generate_launch_description(): ]) ), launch_arguments={ - 'spawner_config': LaunchConfiguration('spawner_config'), + 'custom_config': LaunchConfiguration('spawner_config'), 'debug': LaunchConfiguration('spawner_debug'), }.items() ) diff --git a/mrs_uav_gazebo_simulator/mrs_drone_spawner.py b/mrs_uav_gazebo_simulator/mrs_drone_spawner.py index b9fcd0c..ced7653 100755 --- a/mrs_uav_gazebo_simulator/mrs_drone_spawner.py +++ b/mrs_uav_gazebo_simulator/mrs_drone_spawner.py @@ -128,7 +128,8 @@ def __init__(self): self.declare_parameter('firmware_launch_delay', 0.0) - self.declare_parameter('extra_resource_paths', []) + self.declare_parameter('extra_resource_paths', [""]) + # Get all parameters try: @@ -141,6 +142,7 @@ def __init__(self): self.firmware_launch_delay = float(self.get_parameter('firmware_launch_delay').value) + except rclpy.exceptions.ParameterNotDeclaredException as e: self.get_logger().error(f'Could not load required param. {e}') raise RuntimeError(f'Could not load required param. {e}') @@ -217,7 +219,6 @@ def __init__(self): # #{ launch_px4_firmware(self, robot_params) def launch_px4_firmware(self, robot_params): - if self.firmware_launch_delay > 0: self.get_logger().info(f'Waiting for {self.firmware_launch_delay} s before launching firmware') time.sleep(self.firmware_launch_delay) @@ -227,7 +228,10 @@ def launch_px4_firmware(self, robot_params): package_name = self.jinja_templates[robot_params['model']].package_name package_path = get_package_share_directory(package_name) + romfs_path = os.path.join(str(package_path), 'ROMFS') + + if not os.path.exists(romfs_path) or not os.path.isdir(romfs_path): self.get_logger().error(f'Could not start PX4 firmware for {name}. ROMFS folder not found') raise CouldNotLaunch('ROMFS folder not found')