From 95b6ee5613dc9d64c3df50252818c31caa3bc98f Mon Sep 17 00:00:00 2001
From: Klaus Ening <klaus.ening@ntnu.no>
Date: Wed, 5 Jun 2024 14:34:25 +0200
Subject: [PATCH 1/4] changes for gz-sim: from ign-gazebo to gz-sim

---
 launch/default.launch.py                    |  2 +-
 launch/robots/robot_panda.launch.py         | 13 ++++++++++---
 launch/worlds/world_default.launch.py       |  6 +++---
 launch/worlds/world_follow_target.launch.py |  8 ++++----
 launch/worlds/world_throw_object.launch.py  |  6 +++---
 package.xml                                 |  8 ++++----
 worlds/follow_target.sdf                    | 10 +++++-----
 worlds/throw_object.sdf                     |  8 ++++----
 8 files changed, 34 insertions(+), 27 deletions(-)

diff --git a/launch/default.launch.py b/launch/default.launch.py
index d42ca2e..034ea36 100755
--- a/launch/default.launch.py
+++ b/launch/default.launch.py
@@ -89,7 +89,7 @@ def generate_launch_description() -> LaunchDescription:
                 )
             ),
             launch_arguments=[
-                ("ros2_control_plugin", "ign"),
+                ("ros2_control_plugin", "gz"),
                 ("ros2_control_command_interface", "effort"),
                 # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled
                 ("collision_arm", "false"),
diff --git a/launch/robots/robot_panda.launch.py b/launch/robots/robot_panda.launch.py
index 3992d7e..98bf067 100755
--- a/launch/robots/robot_panda.launch.py
+++ b/launch/robots/robot_panda.launch.py
@@ -1,8 +1,11 @@
 #!/usr/bin/env -S ros2 launch
 """Launch script for spawning Franka Emika Panda into Ignition Gazebo world"""
 
+from os import path
 from typing import List
 
+from ament_index_python.packages import get_package_share_directory
+
 from launch_ros.actions import Node
 
 from launch import LaunchDescription
@@ -10,6 +13,7 @@
 from launch.substitutions import LaunchConfiguration
 
 
+
 def generate_launch_description() -> LaunchDescription:
     # Declare all launch arguments
     declared_arguments = generate_declared_arguments()
@@ -23,7 +27,7 @@ def generate_launch_description() -> LaunchDescription:
     nodes = [
         # ros_ign_gazebo_create
         Node(
-            package="ros_ign_gazebo",
+            package="ros_gz_sim",
             executable="create",
             output="log",
             arguments=["-file", model, "--ros-args", "--log-level", log_level],
@@ -43,8 +47,11 @@ def generate_declared_arguments() -> List[DeclareLaunchArgument]:
         # Model for Ignition Gazebo
         DeclareLaunchArgument(
             "model",
-            default_value="panda",
-            description="Name or filepath of model to load.",
+            default_value=path.join(
+                get_package_share_directory("panda_description"),
+                "panda",
+                "model.sdf",
+            ),
         ),
         # Miscellaneous
         DeclareLaunchArgument(
diff --git a/launch/worlds/world_default.launch.py b/launch/worlds/world_default.launch.py
index 043b9ef..710c26b 100755
--- a/launch/worlds/world_default.launch.py
+++ b/launch/worlds/world_default.launch.py
@@ -29,9 +29,9 @@ def generate_launch_description() -> LaunchDescription:
             PythonLaunchDescriptionSource(
                 PathJoinSubstitution(
                     [
-                        FindPackageShare("ros_ign_gazebo"),
+                        FindPackageShare("ros_gz_sim"),
                         "launch",
-                        "ign_gazebo.launch.py",
+                        "gz_sim.launch.py",
                     ]
                 )
             ),
@@ -43,7 +43,7 @@ def generate_launch_description() -> LaunchDescription:
     nodes = [
         # ros_ign_bridge (clock -> ROS 2)
         Node(
-            package="ros_ign_bridge",
+            package="ros_gz_bridge",
             executable="parameter_bridge",
             output="log",
             arguments=[
diff --git a/launch/worlds/world_follow_target.launch.py b/launch/worlds/world_follow_target.launch.py
index b5e9bb0..569633d 100755
--- a/launch/worlds/world_follow_target.launch.py
+++ b/launch/worlds/world_follow_target.launch.py
@@ -31,9 +31,9 @@ def generate_launch_description() -> LaunchDescription:
             PythonLaunchDescriptionSource(
                 PathJoinSubstitution(
                     [
-                        FindPackageShare("ros_ign_gazebo"),
+                        FindPackageShare("ros_gz_sim"),
                         "launch",
-                        "ign_gazebo.launch.py",
+                        "gz_sim.launch.py",
                     ]
                 )
             ),
@@ -45,7 +45,7 @@ def generate_launch_description() -> LaunchDescription:
     nodes = [
         # ros_ign_bridge (clock -> ROS 2)
         Node(
-            package="ros_ign_bridge",
+            package="ros_gz_bridge",
             executable="parameter_bridge",
             output="log",
             arguments=[
@@ -58,7 +58,7 @@ def generate_launch_description() -> LaunchDescription:
         ),
         # ros_ign_bridge (target pose -> ROS 2)
         Node(
-            package="ros_ign_bridge",
+            package="ros_gz_bridge",
             executable="parameter_bridge",
             output="log",
             arguments=[
diff --git a/launch/worlds/world_throw_object.launch.py b/launch/worlds/world_throw_object.launch.py
index 61f55ea..e7b968a 100755
--- a/launch/worlds/world_throw_object.launch.py
+++ b/launch/worlds/world_throw_object.launch.py
@@ -31,9 +31,9 @@ def generate_launch_description() -> LaunchDescription:
             PythonLaunchDescriptionSource(
                 PathJoinSubstitution(
                     [
-                        FindPackageShare("ros_ign_gazebo"),
+                        FindPackageShare("ros_gz_sim"),
                         "launch",
-                        "ign_gazebo.launch.py",
+                        "gz_sim.launch.py",
                     ]
                 )
             ),
@@ -45,7 +45,7 @@ def generate_launch_description() -> LaunchDescription:
     nodes = [
         # ros_ign_bridge (clock -> ROS 2)
         Node(
-            package="ros_ign_bridge",
+            package="ros_gz_bridge",
             executable="parameter_bridge",
             output="log",
             arguments=[
diff --git a/package.xml b/package.xml
index 4af70cd..6a8d64e 100644
--- a/package.xml
+++ b/package.xml
@@ -2,7 +2,7 @@
 <package format="3">
   <name>ign_moveit2_examples</name>
   <version>1.0.0</version>
-  <description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside Ignition Gazebo simulation environment</description>
+  <description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside GazeboSim simulation environment</description>
   <maintainer email="orsula.andrej@gmail.com">Andrej Orsula</maintainer>
   <author>Andrej Orsula</author>
   <license>BSD</license>
@@ -13,12 +13,12 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
 
-  <exec_depend>ign_ros2_control</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
   <exec_depend>moveit</exec_depend>
   <exec_depend>pymoveit2</exec_depend>
   <exec_depend>rclpy</exec_depend>
-  <exec_depend>ros_ign_bridge</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>rviz2</exec_depend>
 
   <exec_depend>panda_description</exec_depend>
diff --git a/worlds/follow_target.sdf b/worlds/follow_target.sdf
index b887c48..4a4c78d 100644
--- a/worlds/follow_target.sdf
+++ b/worlds/follow_target.sdf
@@ -3,9 +3,9 @@
   <world name="ign_moveit2_follow_target_world">
 
     <!-- Physics -->
-    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics">
       <engine>
-        <filename>ignition-physics-dartsim-plugin</filename>
+        <filename>gz-physics-dartsim-plugin</filename>
       </engine>
       <dart>
         <collision_detector>bullet</collision_detector>
@@ -17,7 +17,7 @@
     </physics>
 
     <!-- Scene -->
-    <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster">
     </plugin>
     <scene>
       <ambient>0.8 0.8 0.8</ambient>
@@ -25,7 +25,7 @@
     </scene>
 
     <!-- User Commands (transform control) -->
-    <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands">
     </plugin>
 
 
@@ -96,7 +96,7 @@
         </visual>
       </link>
       <!-- Pose publisher plugin, used to determine the target pose to reach -->
-      <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
+      <plugin filename="gz-sim-pose-publisher-system" name="gz::sim::systems::PosePublisher">
         <publish_nested_model_pose>true</publish_nested_model_pose>
         <publish_link_pose>false</publish_link_pose>
         <publish_collision_pose>false</publish_collision_pose>
diff --git a/worlds/throw_object.sdf b/worlds/throw_object.sdf
index 14ab3b3..8654419 100644
--- a/worlds/throw_object.sdf
+++ b/worlds/throw_object.sdf
@@ -3,9 +3,9 @@
   <world name="ign_moveit2_follow_target_world">
 
     <!-- Physics -->
-    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics">
       <engine>
-        <filename>ignition-physics-dartsim-plugin</filename>
+        <filename>gz-physics-dartsim-plugin</filename>
       </engine>
       <dart>
         <collision_detector>bullet</collision_detector>
@@ -17,7 +17,7 @@
     </physics>
 
     <!-- Scene -->
-    <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster">
     </plugin>
     <scene>
       <ambient>0.8 0.8 0.8</ambient>
@@ -25,7 +25,7 @@
     </scene>
 
     <!-- User Commands (transform control) -->
-    <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands">
     </plugin>
 
 

From d303218ab07d9ab6bba5d26acb33c4b305259d0e Mon Sep 17 00:00:00 2001
From: Klaus Ening <klaus.ening@ntnu.no>
Date: Mon, 10 Jun 2024 15:34:17 +0200
Subject: [PATCH 2/4] Joint2 was in collision. Found a Problem with
 wait_for_execution: Does not wait when still in planning

---
 examples/py/ex_throw_object.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/py/ex_throw_object.py b/examples/py/ex_throw_object.py
index 7136bef..e8c37e3 100755
--- a/examples/py/ex_throw_object.py
+++ b/examples/py/ex_throw_object.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-
+import time
 from copy import deepcopy
 from threading import Thread
 
@@ -64,7 +64,7 @@ def throw(self):
             position=position_above_object,
             quat_xyzw=default_quat,
         )
-        self._moveit2.wait_until_executed()
+        self._moveit2.wait_until_executed()  #TODO: "wait_until_executed" does not wait when path is still planned!!
 
         # Move to grasp position
         self._moveit2.move_to_pose(
@@ -89,7 +89,7 @@ def throw(self):
         self._moveit2.wait_until_executed()
 
         # Move to pre-throw configuration
-        joint_configuration_pre_throw = [0.0, -1.75, 0.0, -0.1, 0.0, 3.6, 0.8]
+        joint_configuration_pre_throw = [0.0, -1.5, 0.0, -0.2, 0.0, 3.6, 0.8]
         self._moveit2.move_to_configuration(joint_configuration_pre_throw)
         self._moveit2.wait_until_executed()
 

From a8eeea023a30ff9d721df09c37e4cf326e355ec0 Mon Sep 17 00:00:00 2001
From: Klaus Ening <k.ening@gmx.de>
Date: Tue, 11 Jun 2024 13:25:29 +0200
Subject: [PATCH 3/4] remove unused import

Co-authored-by: Andrej Orsula <orsula.andrej@gmail.com>
---
 examples/py/ex_throw_object.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/examples/py/ex_throw_object.py b/examples/py/ex_throw_object.py
index e8c37e3..caff2da 100755
--- a/examples/py/ex_throw_object.py
+++ b/examples/py/ex_throw_object.py
@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import time
 from copy import deepcopy
 from threading import Thread
 

From 7c233a0d3ef96b67fae313eb9c78e66ea11fe17a Mon Sep 17 00:00:00 2001
From: Klaus Ening <k.ening@gmx.de>
Date: Tue, 11 Jun 2024 13:26:04 +0200
Subject: [PATCH 4/4] Added Author

Co-authored-by: Andrej Orsula <orsula.andrej@gmail.com>
---
 package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/package.xml b/package.xml
index 6a8d64e..22ecf09 100644
--- a/package.xml
+++ b/package.xml
@@ -5,6 +5,7 @@
   <description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside GazeboSim simulation environment</description>
   <maintainer email="orsula.andrej@gmail.com">Andrej Orsula</maintainer>
   <author>Andrej Orsula</author>
+  <author>Klaus Ening</author>
   <license>BSD</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>