Added workaround for ros-controls/gazebo_ros2_control#295

icclab · May 13, 2024 · 6fe722f · 6fe722f
1 parent 2be1428
commit 6fe722f
Showing 1 changed file with 47 additions and 35 deletions.
diff --git 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
+
+
+
+
+