From b3b8b0b21f985db35b48374ea1e7074dcbaacc6c Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Tue, 30 May 2023 20:09:32 +0000
Subject: [PATCH 1/6] Add xacro option for Ignition support

---
 .../config/panda.ros2_control.xacro             | 17 +++++++++++++++++
 .../config/panda_hand.ros2_control.xacro        |  3 +++
 2 files changed, 20 insertions(+)

diff --git a/panda_moveit_config/config/panda.ros2_control.xacro b/panda_moveit_config/config/panda.ros2_control.xacro
index addc3384..966bd341 100644
--- a/panda_moveit_config/config/panda.ros2_control.xacro
+++ b/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>
diff --git a/panda_moveit_config/config/panda_hand.ros2_control.xacro b/panda_moveit_config/config/panda_hand.ros2_control.xacro
index 06d4d177..229eb104 100644
--- a/panda_moveit_config/config/panda_hand.ros2_control.xacro
+++ b/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>

From d903c6b3442c8970ef7b600c033e6766239e2f0d Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Tue, 30 May 2023 20:15:21 +0000
Subject: [PATCH 2/6] Run Ignition with demo.launch.py

---
 panda_moveit_config/launch/demo.launch.py | 109 ++++++++++++++--------
 1 file changed, 70 insertions(+), 39 deletions(-)

diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
index 12e74017..cd8d86cd 100644
--- a/panda_moveit_config/launch/demo.launch.py
+++ b/panda_moveit_config/launch/demo.launch.py
@@ -1,10 +1,12 @@
 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_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
 
@@ -22,7 +24,7 @@ def generate_launch_description():
 
     ros2_control_hardware_type = DeclareLaunchArgument(
         "ros2_control_hardware_type",
-        default_value="mock_components",
+        default_value="sim_ignition",
         description="ROS2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
     )
 
@@ -41,6 +43,7 @@ def generate_launch_description():
         .planning_pipelines(
             pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
         )
+        .planning_scene_monitor(publish_robot_description=True)
         .to_moveit_configs()
     )
 
@@ -48,9 +51,9 @@ def generate_launch_description():
     move_group_node = Node(
         package="moveit_ros_move_group",
         executable="move_group",
-        output="screen",
+        output="log",
         parameters=[moveit_config.to_dict()],
-        arguments=["--ros-args", "--log-level", "info"],
+        arguments=["--ros-args", "--log-level", "fatal"], # MoveIt is spamming the log because of unknown '*_mimic' joints
     )
 
     # RViz
@@ -108,27 +111,47 @@ def generate_launch_description():
         parameters=[moveit_config.robot_description],
     )
 
+    # 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",
+    # 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",
+        ),
+        mappings={"ros2_control_hardware_type": "sim_ignition"}
+    )
+
+    ignition_spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        arguments=[
+                   # '-string', robot_description_config.toxml(),
+                   '-topic', 'robot_description',
+                   '-name', 'panda',
+                   '-allow-renaming', 'true'],
     )
 
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            "joint_state_broadcaster",
-            "--controller-manager",
-            "/controller_manager",
-        ],
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
     )
 
     panda_arm_controller_spawner = Node(
@@ -157,20 +180,28 @@ def generate_launch_description():
         condition=IfCondition(db_config),
     )
 
-    return LaunchDescription(
-        [
-            tutorial_arg,
-            db_arg,
-            ros2_control_hardware_type,
-            rviz_node,
-            rviz_node_tutorial,
-            static_tf_node,
-            robot_state_publisher,
-            move_group_node,
-            ros2_control_node,
-            joint_state_broadcaster_spawner,
-            panda_arm_controller_spawner,
-            panda_hand_controller_spawner,
-            mongodb_server_node,
-        ]
-    )
+    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,
+    ])

From 19328db14b59dbdc177018dc5030c9bb0881a95c Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Tue, 30 May 2023 20:17:36 +0000
Subject: [PATCH 3/6] Add gz_ros2_control as exec depend

---
 panda_moveit_config/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/panda_moveit_config/package.xml b/panda_moveit_config/package.xml
index 0b3ead36..b67558f4 100644
--- a/panda_moveit_config/package.xml
+++ b/panda_moveit_config/package.xml
@@ -21,6 +21,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>gz_ros2_control</exec_depend>
   <exec_depend>moveit_resources_panda_description</exec_depend>
   <!-- disabled to remove circular dependencies
   <exec_depend>moveit_ros_move_group</exec_depend>

From 26c49d57fd841c314a97ab3ae17e59cc3864eb5e Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Wed, 31 May 2023 15:17:44 +0000
Subject: [PATCH 4/6] Configure RViz to launch MotionPlanning panel

---
 panda_moveit_config/launch/moveit.rviz | 242 ++++++++++++++++++++++++-
 1 file changed, 233 insertions(+), 9 deletions(-)

diff --git a/panda_moveit_config/launch/moveit.rviz b/panda_moveit_config/launch/moveit.rviz
index 2f6663ee..d5a2c34d 100644
--- a/panda_moveit_config/launch/moveit.rviz
+++ b/panda_moveit_config/launch/moveit.rviz
@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 802
+    Tree Height: 435
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -121,6 +121,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Loop Animation: false
       Name: Trajectory
       Robot Alpha: 0.5
@@ -132,6 +136,7 @@ Visualization Manager:
       State Display Time: 0.05 s
       Trail Step Size: 1
       Trajectory Topic: /display_planned_path
+      Use Sim Time: false
       Value: true
     - Class: moveit_rviz_plugin/PlanningScene
       Enabled: true
@@ -213,10 +218,222 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
       Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -232,6 +449,9 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -267,26 +487,30 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.02386285550892353
-        Y: 0.15478567779064178
-        Z: 0.039489321410655975
+        X: 0.41813451051712036
+        Y: 0.3977149724960327
+        Z: 0.3874291777610779
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5953981876373291
+      Pitch: 0.46539801359176636
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.958578109741211
+      Yaw: 5.373577117919922
     Saved: ~
 Window Geometry:
+  "":
+    collapsed: false
+  " - Trajectory Slider":
+    collapsed: false
   Displays:
     collapsed: false
   Height: 1025
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c9000003abfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000023c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffbffffffff010000027d000001690000016900ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000056e000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
@@ -296,5 +520,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1853
-  X: 67
-  Y: 27
+  X: 438
+  Y: 247

From 0a5479f4f8f1a9c1cefa88e96525aa502a33a08f Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Wed, 31 May 2023 17:28:04 +0000
Subject: [PATCH 5/6] Fix Gazebo package dependencies

---
 panda_moveit_config/package.xml | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/panda_moveit_config/package.xml b/panda_moveit_config/package.xml
index b67558f4..6b84758f 100644
--- a/panda_moveit_config/package.xml
+++ b/panda_moveit_config/package.xml
@@ -21,7 +21,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <exec_depend>gz_ros2_control</exec_depend>
+  <exec_depend>ign_ros2_control</exec_depend>
   <exec_depend>moveit_resources_panda_description</exec_depend>
   <!-- disabled to remove circular dependencies
   <exec_depend>moveit_ros_move_group</exec_depend>
@@ -33,6 +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>ros2controlcli</exec_depend>
   <exec_depend>xacro</exec_depend>
   <!-- TODO(#40): Package not available in ROS 2, find equivalent when migrating launch files
   <exec_depend>topic_tools</exec_depend>

From 3daad7b2b581b19c5364cb8b11f3c003ae025ad9 Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Wed, 31 May 2023 18:54:19 +0000
Subject: [PATCH 6/6] Add demo launch args for Gazebo (Ignition) sim

---
 .../config/initial_positions_zero.yaml        |  14 ++
 panda_moveit_config/launch/demo.launch.py     | 211 ++++++++++++------
 panda_moveit_config/package.xml               |   5 +-
 3 files changed, 157 insertions(+), 73 deletions(-)
 create mode 100644 panda_moveit_config/config/initial_positions_zero.yaml

diff --git a/panda_moveit_config/config/initial_positions_zero.yaml b/panda_moveit_config/config/initial_positions_zero.yaml
new file mode 100644
index 00000000..54cfafbc
--- /dev/null
+++ 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
index cd8d86cd..68094b99 100644
--- 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
index 6b84758f..6a1b1ad8 100644
--- a/panda_moveit_config/package.xml
+++ b/panda_moveit_config/package.xml
@@ -33,7 +33,10 @@
   <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>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</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