diff --git a/summit_xl_gazebo/launch/description.launch.py b/summit_xl_gazebo/launch/description.launch.py index 87033dc..4fb3283 100644 --- a/summit_xl_gazebo/launch/description.launch.py +++ b/summit_xl_gazebo/launch/description.launch.py @@ -24,48 +24,21 @@ # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import os +import re import launch -from launch.actions import LogInfo +from launch.actions import LogInfo, OpaqueFunction import launch_ros from ament_index_python.packages import get_package_share_directory from robotnik_common.launch import RewrittenYaml -def generate_launch_description(): - - ld = launch.LaunchDescription() +def launch_setup(context, *args, **kwargs): use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time') controllers_file = launch.substitutions.LaunchConfiguration('controllers_file') robot_id = launch.substitutions.LaunchConfiguration('robot_id') robot_xacro = launch.substitutions.LaunchConfiguration('robot_xacro') - # Declare the launch options - ld.add_action(launch.actions.DeclareLaunchArgument( - name='use_sim_time', - description='Use simulation (Gazebo) clock if true', - choices=['true', 'false'], - default_value='true') - ) - - ld.add_action(launch.actions.DeclareLaunchArgument( - name='controllers_file', - description='ROS 2 controller file.', - default_value=[get_package_share_directory('summit_xl_gazebo'), '/config/controller.yml']) - ) - - ld.add_action(launch.actions.DeclareLaunchArgument( - name='robot_id', - description='Robot ID used to create the robot namespace', - default_value='robot') - ) - - ld.add_action(launch.actions.DeclareLaunchArgument( - name='robot_xacro', - description='Robot xacro file path for the robot model', - default_value=os.path.join(get_package_share_directory('summit_xl_description'), 'robots', 'summit_xls_icclab.urdf.xacro')) - ) - config_file_rewritten = RewrittenYaml( source_file=controllers_file, param_rewrites={}, @@ -85,8 +58,10 @@ def generate_launch_description(): ] ) - # Create parameter - robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str) + # Get rid of XML comments + # Workaround because of this bug: https://github.com/ros-controls/gazebo_ros2_control/issues/295 + pattern = r'' + robot_description_param_no_comments = re.sub(pattern, '', robot_description_content.perform(context), flags=re.DOTALL) robot_state_publisher = launch_ros.actions.Node( package='robot_state_publisher', @@ -96,13 +71,50 @@ def generate_launch_description(): remappings= [('/tf', 'tf'), ('/tf_static', 'tf_static')], parameters=[{ 'use_sim_time': use_sim_time, - 'robot_description': robot_description_param, + 'robot_description': robot_description_param_no_comments, 'publish_frequency': 100.0, 'frame_prefix': "", # [params['robot_id'], '/'], }], ) - ld.add_action(robot_state_publisher) - ld.add_action(LogInfo(msg=["description.launch.py", " robot_description_param: \n", robot_description_content])) + return [robot_state_publisher, LogInfo(msg=["description.launch.py", " robot_description_param: \n", robot_description_content])] + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + # Declare the launch options + ld.add_action(launch.actions.DeclareLaunchArgument( + name='use_sim_time', + description='Use simulation (Gazebo) clock if true', + choices=['true', 'false'], + default_value='true') + ) + + ld.add_action(launch.actions.DeclareLaunchArgument( + name='controllers_file', + description='ROS 2 controller file.', + default_value=[get_package_share_directory('summit_xl_gazebo'), '/config/controller.yml']) + ) + + ld.add_action(launch.actions.DeclareLaunchArgument( + name='robot_id', + description='Robot ID used to create the robot namespace', + default_value='robot') + ) + + ld.add_action(launch.actions.DeclareLaunchArgument( + name='robot_xacro', + description='Robot xacro file path for the robot model', + default_value=os.path.join(get_package_share_directory('summit_xl_description'), 'robots', 'summit_xls_icclab.urdf.xacro')) + ) + + ld.add_action(OpaqueFunction(function=launch_setup)) return ld + + + + +