Add demo launch args for Gazebo (Ignition) sim

henningkayser · henningkayser · commit 83aaa02f1bf5 · 2023-06-01T09:57:44.000Z
diff --git a/panda_moveit_config/config/initial_positions_zero.yaml b/panda_moveit_config/config/initial_positions_zero.yaml
@@ -0,0 +1,14 @@
+# Initial positions for the panda arm's ros2_control Gazebo support
+# NOTE: These modified values from initial_positions.yaml move the default state
+# closer to 0 for Humble backwards support. The original values would cause
+# a strong initial motion, leading to self-collision. Reason is that initial_values
+# are not supported by ign_ros2_control
+# See: https://github.com/ros-controls/gz_ros2_control/pull/27
+initial_positions:
+  panda_joint1: 0.0
+  panda_joint2: 0.0
+  panda_joint3: 0.0
+  panda_joint4: 0.0
+  panda_joint5: 0.0
+  panda_joint6: 1.571
+  panda_joint7: 0.785
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
@@ -1,8 +1,17 @@
 import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
 from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import (
+    IfCondition,
+    UnlessCondition,
+    LaunchConfigurationNotEquals,
+    LaunchConfigurationEquals,
+)
 from launch_ros.actions import Node
 from launch.actions import ExecuteProcess
 from launch.event_handlers import OnProcessExit
@@ -24,8 +33,22 @@ def generate_launch_description():
 
     ros2_control_hardware_type = DeclareLaunchArgument(
         "ros2_control_hardware_type",
-        default_value="sim_ignition",
-        description="ROS2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
+        default_value="mock_components",
+        description="ROS2 control hardware interface type to use for the launch file",
+        choices=["mock_components", "isaac", "sim_ignition"],
+    )
+
+    declare_initial_positions_file = DeclareLaunchArgument(
+        "initial_positions_file",
+        default_value="initial_positions.yaml",
+        description="Initial joint positions to use for ros2_control fake components and simulation -- expected to be a yaml file inside the config directory",
+    )
+
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="False",
+        description="Use simulation (Gazebo) clock if True",
     )
 
     moveit_config = (
@@ -35,7 +58,8 @@ def generate_launch_description():
             mappings={
                 "ros2_control_hardware_type": LaunchConfiguration(
                     "ros2_control_hardware_type"
-                )
+                ),
+                "initial_positions_file": LaunchConfiguration("initial_positions_file"),
             },
         )
         .robot_description_semantic(file_path="config/panda.srdf")
@@ -52,8 +76,12 @@ def generate_launch_description():
         package="moveit_ros_move_group",
         executable="move_group",
         output="log",
-        parameters=[moveit_config.to_dict()],
-        arguments=["--ros-args", "--log-level", "fatal"], # MoveIt is spamming the log because of unknown '*_mimic' joints
+        parameters=[moveit_config.to_dict(), {"use_sim_time": use_sim_time}],
+        arguments=[
+            "--ros-args",
+            "--log-level",
+            "fatal",
+        ],  # MoveIt is spamming the log because of unknown '*_mimic' joints
     )
 
     # RViz
@@ -89,6 +117,7 @@ def generate_launch_description():
             moveit_config.planning_pipelines,
             moveit_config.robot_description_kinematics,
             moveit_config.joint_limits,
+            {"use_sim_time": use_sim_time},
         ],
         condition=UnlessCondition(tutorial_mode),
     )
@@ -99,6 +128,7 @@ def generate_launch_description():
         executable="static_transform_publisher",
         name="static_transform_publisher",
         output="log",
+        parameters=[{"use_sim_time": use_sim_time}],
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
     )
 
@@ -108,62 +138,87 @@ def generate_launch_description():
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="both",
-        parameters=[moveit_config.robot_description],
+        parameters=[moveit_config.robot_description, {"use_sim_time": use_sim_time}],
     )
 
-    # NOTE: Comment out ros2_control for now
     # ros2_control using FakeSystem as hardware
-    # ros2_controllers_path = os.path.join(
-    #     get_package_share_directory("moveit_resources_panda_moveit_config"),
-    #     "config",
-    #     "ros2_controllers.yaml",
-    # )
-    # ros2_control_node = Node(
-    #     package="controller_manager",
-    #     executable="ros2_control_node",
-    #     parameters=[moveit_config.robot_description, ros2_controllers_path],
-    #     output="screen",
-    # )
-
-    # Parse xacro file to pass as string to Ignition
-    import xacro
-    robot_description_config = xacro.process_file(
-        os.path.join(
-            get_package_share_directory("moveit_resources_panda_moveit_config"),
-            "config",
-            "panda.urdf.xacro",
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="screen",
+        condition=LaunchConfigurationNotEquals(
+            "ros2_control_hardware_type", "sim_ignition"
         ),
-        mappings={"ros2_control_hardware_type": "sim_ignition"}
     )
 
     ignition_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
+        package="ros_gz_sim",
+        executable="create",
+        output="screen",
         arguments=[
-                   # '-string', robot_description_config.toxml(),
-                   '-topic', 'robot_description',
-                   '-name', 'panda',
-                   '-allow-renaming', 'true'],
+            "-topic",
+            "robot_description",
+            "-name",
+            "panda",
+            "-allow-renaming",
+            "true",
+        ],
+        condition=LaunchConfigurationEquals(
+            "ros2_control_hardware_type", "sim_ignition"
+        ),
     )
 
+    # Clock Bridge
+    sim_clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
+        output="screen",
+        condition=LaunchConfigurationEquals(
+            "ros2_control_hardware_type", "sim_ignition"
+        ),
+    )
 
     load_joint_state_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_state_broadcaster'],
-        output='screen'
+        cmd=[
+            "ros2",
+            "control",
+            "load_controller",
+            "--set-state",
+            "active",
+            "joint_state_broadcaster",
+        ],
+        output="screen",
     )
 
-    panda_arm_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    panda_arm_controller_spawner = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "control",
+            "load_controller",
+            "--set-state",
+            "active",
+            "panda_arm_controller",
+        ],
+        output="screen",
     )
 
-    panda_hand_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    panda_hand_controller_spawner = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "control",
+            "load_controller",
+            "--set-state",
+            "active",
+            "panda_hand_controller",
+        ],
+        output="screen",
     )
 
     # Warehouse mongodb server
@@ -180,28 +235,40 @@ def generate_launch_description():
         condition=IfCondition(db_config),
     )
 
-    return LaunchDescription([
-        tutorial_arg,
-        db_arg,
-        ros2_control_hardware_type,
-        # Launch gazebo environment
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_ign_gazebo'),
-                              'launch', 'ign_gazebo.launch.py')]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        move_group_node,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=ignition_spawn_entity,
-                on_exit=[load_joint_state_broadcaster,
-                         panda_arm_controller_spawner,
-                         panda_hand_controller_spawner,],
-            )
-        ),
-        rviz_node,
-        rviz_node_tutorial,
-        static_tf_node,
-        robot_state_publisher,
-        mongodb_server_node,
-    ])
+    return LaunchDescription(
+        [
+            tutorial_arg,
+            db_arg,
+            ros2_control_hardware_type,
+            declare_use_sim_time_cmd,
+            declare_initial_positions_file,
+            sim_clock_bridge,
+            # Launch gazebo environment
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    [
+                        os.path.join(
+                            get_package_share_directory("ros_gz_sim"),
+                            "launch",
+                            "gz_sim.launch.py",
+                        )
+                    ]
+                ),
+                launch_arguments=[("gz_args", [" -r -v 4 empty.sdf"])],
+                condition=LaunchConfigurationEquals(
+                    "ros2_control_hardware_type", "sim_ignition"
+                ),
+            ),
+            move_group_node,
+            ignition_spawn_entity,
+            ros2_control_node,
+            load_joint_state_broadcaster,
+            panda_arm_controller_spawner,
+            panda_hand_controller_spawner,
+            rviz_node,
+            rviz_node_tutorial,
+            static_tf_node,
+            robot_state_publisher,
+            mongodb_server_node,
+        ]
+    )
diff --git a/panda_moveit_config/package.xml b/panda_moveit_config/package.xml
@@ -33,7 +33,8 @@
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>ros_ign_gazebo</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>ros2controlcli</exec_depend>
   <exec_depend>xacro</exec_depend>
   <!-- TODO(#40): Package not available in ROS 2, find equivalent when migrating launch files
