Follow launch file template from other examples.

ros-controls · Aug 25, 2022 · ce69cb6 · ce69cb6
1 parent 03ae766
commit ce69cb6
Showing 1 changed file with 50 additions and 58 deletions.
diff --git a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py
@@ -15,98 +15,90 @@
 # Author: Denis Stogl (Stogl Robotics Consulting)
 #
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration
+
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
+
+import xacro
 
 
 def generate_launch_description():
+    # Launch arguments
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name='xacro')]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare(
-                    'ign_ros2_control_demos'),
-                    'urdf',
-                    'test_gripper_mimic_joint.xacro.urdf']
-            ),
-        ]
-    )
-    robot_description = {"robot_description": robot_description_content}
+    ignition_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('ign_ros2_control_demos'))
+
+    xacro_file = os.path.join(ignition_ros2_control_demos_path,
+                              'urdf',
+                              'test_gripper_mimic_joint.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[robot_description]
-    )
-
-    ignition = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"]
-        ),
-        launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])],
+        parameters=[params]
     )
 
-    spawn_entity = Node(
-        package="ros_ign_gazebo",
-        executable="create",
-        output="screen",
-        arguments=[
-            "-param",
-            "robot_description",
-            "-name",
-            "gripper",
-            "-allow_renaming",
-            "true",
-            "-x",
-            "0.0",
-            "-y",
-            "0.0",
-            "-z",
-            "0.0",
-            "-R",
-            "0.0",
-            "-P",
-            "0.0",
-            "-Y",
-            "0.0",
-        ],
-        parameters=[robot_description]
+    ignition_spawn_entity = Node(
+        package='ros_ign_gazebo',
+        executable='create',
+        output='screen',
+        arguments=['-string', doc.toxml(),
+                   '-name', 'gripper',
+                   '-allow_renaming', 'true'],
     )
 
-    load_joint_state_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
              'joint_state_broadcaster'],
         output='screen'
     )
 
     load_gripper_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
              'gripper_controller'],
         output='screen'
     )
 
     return LaunchDescription([
+        # Launch gazebo environment
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+                              'launch', 'ign_gazebo.launch.py')]),
+            launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]),
         RegisterEventHandler(
-          event_handler=OnProcessExit(
-                target_action=spawn_entity,
-                on_exit=[load_joint_state_controller],
+            event_handler=OnProcessExit(
+                target_action=ignition_spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
             )
         ),
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=load_joint_state_controller,
+                target_action=load_joint_state_broadcaster,
                 on_exit=[load_gripper_controller],
             )
         ),
-        ignition,
         node_robot_state_publisher,
-        spawn_entity,
+        ignition_spawn_entity,
+        # Launch Arguments
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value=use_sim_time,
+            description='If true, use simulated clock'),
     ])