Draft: Launch Panda demo with Gazebo (Ignition)

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

panda_moveit_config/config/panda.ros2_control.xacro

@@ -9,6 +9,9 @@
                 <xacro:if value="${ros2_control_hardware_type == 'mock_components'}">
                     <plugin>mock_components/GenericSystem</plugin>
                 </xacro:if>
+                <xacro:if value="${ros2_control_hardware_type == 'sim_ignition'}">
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </xacro:if>
                 <xacro:if value="${ros2_control_hardware_type == 'isaac'}">
                     <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
                     <param name="joint_commands_topic">/isaac_joint_commands</param>
@@ -65,5 +68,19 @@
                 <state_interface name="velocity"/>
             </joint>
         </ros2_control>
+
+        <xacro:if value="${ros2_control_hardware_type == 'sim_ignition'}">
+          <link name="world"/>
+          <joint name="panda_world" type="fixed">
+              <origin rpy="0 0 0" xyz="0 0 0.0" />
+              <parent link="world" />
+              <child link="panda_link0" />
+          </joint>
+          <gazebo>
+            <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+              <parameters>$(find moveit_resources_panda_moveit_config)/config/ros2_controllers.yaml</parameters>
+            </plugin>
+          </gazebo>
+        </xacro:if>
     </xacro:macro>
 </robot>

panda_moveit_config/config/panda_hand.ros2_control.xacro

@@ -7,6 +7,9 @@
                 <xacro:if value="${ros2_control_hardware_type == 'mock_components'}">
                     <plugin>mock_components/GenericSystem</plugin>
                 </xacro:if>
+                <xacro:if value="${ros2_control_hardware_type == 'sim_ignition'}">
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </xacro:if>
                 <xacro:if value="${ros2_control_hardware_type == 'isaac'}">
                     <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
                     <param name="joint_commands_topic">/isaac_joint_commands</param>

panda_moveit_config/launch/demo.launch.py

@@ -1,10 +1,21 @@
 import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+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
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
 
@@ -23,7 +34,21 @@ def generate_launch_description():
     ros2_control_hardware_type = DeclareLaunchArgument(
         "ros2_control_hardware_type",
         default_value="mock_components",
-        description="ROS2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
+        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 = (
@@ -33,24 +58,30 @@ 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")
         .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
         .planning_pipelines(
             pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
         )
+        .planning_scene_monitor(publish_robot_description=True)
         .to_moveit_configs()
     )
 
     # Start the actual move_group node/action server
     move_group_node = Node(
         package="moveit_ros_move_group",
         executable="move_group",
-        output="screen",
-        parameters=[moveit_config.to_dict()],
-        arguments=["--ros-args", "--log-level", "info"],
+        output="log",
+        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
@@ -86,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),
     )
@@ -96,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"],
     )
 
@@ -105,7 +138,7 @@ 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}],
     )
 
     # ros2_control using FakeSystem as hardware
@@ -119,28 +152,73 @@ def generate_launch_description():
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
         output="screen",
+        condition=LaunchConfigurationNotEquals(
+            "ros2_control_hardware_type", "sim_ignition"
+        ),
     )
 
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
+    ignition_spawn_entity = Node(
+        package="ros_gz_sim",
+        executable="create",
+        output="screen",
         arguments=[
+            "-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",
-            "--controller-manager",
-            "/controller_manager",
         ],
+        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
@@ -162,15 +240,35 @@ def generate_launch_description():
             tutorial_arg,
             db_arg,
             ros2_control_hardware_type,
-            rviz_node,
-            rviz_node_tutorial,
-            static_tf_node,
-            robot_state_publisher,
+            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,
-            joint_state_broadcaster_spawner,
+            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,
         ]
     )