From 37c4dc787c7fbdd6e45cd84656f30a9a8bc7d5ca Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Thu, 28 Mar 2024 21:36:55 +0100
Subject: [PATCH] Working version.

---
 example_9/CMakeLists.txt                      |  2 +-
 ...c.launch.py => rrbot_gazebo_sim.launch.py} | 43 +++++++----
 .../launch/rrbot_gazebo_sim.launch.xml        | 64 +++++++++++++++++
 .../description/gazebo/rrbot.gazebo.xacro     |  7 +-
 .../ros2_control/rrbot.ros2_control.xacro     |  8 +--
 example_9/description/urdf/rrbot.urdf.xacro   |  6 +-
 example_9/package.xml                         |  2 +-
 example_9/worlds/empty.world                  | 72 +++++++++++++++++++
 8 files changed, 179 insertions(+), 25 deletions(-)
 rename example_9/bringup/launch/{rrbot_gazebo_classic.launch.py => rrbot_gazebo_sim.launch.py} (79%)
 create mode 100644 example_9/bringup/launch/rrbot_gazebo_sim.launch.xml
 create mode 100644 example_9/worlds/empty.world

diff --git a/example_9/CMakeLists.txt b/example_9/CMakeLists.txt
index 1c6980484..ab585baee 100644
--- a/example_9/CMakeLists.txt
+++ b/example_9/CMakeLists.txt
@@ -49,7 +49,7 @@ install(
   DESTINATION include/ros2_control_demo_example_9
 )
 install(
-  DIRECTORY description/launch description/ros2_control description/urdf description/gazebo
+  DIRECTORY description/launch description/ros2_control description/urdf description/gazebo worlds
   DESTINATION share/ros2_control_demo_example_9
 )
 install(
diff --git a/example_9/bringup/launch/rrbot_gazebo_classic.launch.py b/example_9/bringup/launch/rrbot_gazebo_sim.launch.py
similarity index 79%
rename from example_9/bringup/launch/rrbot_gazebo_classic.launch.py
rename to example_9/bringup/launch/rrbot_gazebo_sim.launch.py
index 1fd754f77..ba378abcb 100644
--- a/example_9/bringup/launch/rrbot_gazebo_classic.launch.py
+++ b/example_9/bringup/launch/rrbot_gazebo_sim.launch.py
@@ -36,13 +36,6 @@ def generate_launch_description():
     # Initialize Arguments
     gui = LaunchConfiguration("gui")
 
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])]
-        ),
-        launch_arguments={"verbose": "false"}.items(),
-    )
-
     # Get URDF via xacro
     robot_description_content = Command(
         [
@@ -52,13 +45,10 @@ def generate_launch_description():
                 [FindPackageShare("ros2_control_demo_example_9"), "urdf", "rrbot.urdf.xacro"]
             ),
             " ",
-            "use_gazebo_classic:=true",
+            "use_gazebo_sim:=true",
         ]
     )
     robot_description = {"robot_description": robot_description_content}
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
-    )
 
     node_robot_state_publisher = Node(
         package="robot_state_publisher",
@@ -67,10 +57,30 @@ def generate_launch_description():
         parameters=[robot_description],
     )
 
+    # Start Gazebo
+    gazebo_world = PathJoinSubstitution(
+        [FindPackageShare("ros2_control_demo_example_9"), "worlds", "empty.world"]
+    )
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [PathJoinSubstitution([FindPackageShare("ros_gz_sim"), "launch", "gz_sim.launch.py"])]
+        ),
+        launch_arguments=[("gz_args", [" -r -v 4 ", gazebo_world])],
+    )
+
     spawn_entity = Node(
-        package="gazebo_ros",
-        executable="spawn_entity.py",
-        arguments=["-topic", "robot_description", "-entity", "rrbot_system_position"],
+        package="ros_gz_sim",
+        executable="create",
+        arguments=["-topic", "robot_description", "-name", "rrbot_system_position"],
+        output="screen",
+    )
+
+    clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="clock_bridge",
+        parameters=[{"use_sim_time": True}],
+        arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
         output="screen",
     )
 
@@ -85,6 +95,10 @@ def generate_launch_description():
         executable="spawner",
         arguments=["forward_position_controller", "--controller-manager", "/controller_manager"],
     )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
+    )
     rviz_node = Node(
         package="rviz2",
         executable="rviz2",
@@ -98,6 +112,7 @@ def generate_launch_description():
         gazebo,
         node_robot_state_publisher,
         spawn_entity,
+        clock_bridge,
         joint_state_broadcaster_spawner,
         robot_controller_spawner,
         rviz_node,
diff --git a/example_9/bringup/launch/rrbot_gazebo_sim.launch.xml b/example_9/bringup/launch/rrbot_gazebo_sim.launch.xml
new file mode 100644
index 000000000..8b2c9c43b
--- /dev/null
+++ b/example_9/bringup/launch/rrbot_gazebo_sim.launch.xml
@@ -0,0 +1,64 @@
+<!--
+Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+
+Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+
+Author: Dr. Denis
+-->
+
+<launch>
+  <arg name="gui"
+       default="false"
+       description="Start RViz2 automatically with this launch file."/>
+
+  <!-- Load description and start controllers -->
+  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share ros2_control_demo_example_9)/urdf/rrbot.urdf.xacro use_gazebo_sim:=true')" />
+
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
+    <param name="robot_description" value="$(var robot_description_content)" />
+    <param name="use_sim_time" value="true" />
+  </node>
+
+  <!-- Start Gazebo -->
+  <let name="gazebo_world" value="$(find-pkg-share ros2_control_demo_example_9)/worlds/empty.world" />
+  <include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py" >
+    <arg name="gz_args" value=" -r -v 4 $(var gazebo_world)"/>
+  </include>
+
+  <node pkg="ros_gz_bridge" exec="parameter_bridge" args="
+        /clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"
+        output="screen">
+  </node>
+
+  <!-- Create robot in Gazebo -->
+  <node pkg="ros_gz_sim" exec="create" output="screen" args="-topic /robot_description"/>
+
+  <!-- Start Controllers -->
+  <!--joint_state_braodcaster_spawner-->
+  <node pkg="controller_manager" exec="spawner" args="joint_state_broadcaster">
+    <param name="use_sim_time" value="true" />
+  </node>
+  <!--Mobile Base Controller-->
+  <node pkg="controller_manager" exec="spawner" args="forward_position_controller">
+    <param name="use_sim_time" value="true" />
+  </node>
+
+  <!-- Rviz -->
+  <node if="$(var gui)" pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var ros2_control_demo_description))/rrbot/rviz/rrbot.rviz">
+    <param name="use_sim_time" value="true" />
+  </node>
+
+</launch>
diff --git a/example_9/description/gazebo/rrbot.gazebo.xacro b/example_9/description/gazebo/rrbot.gazebo.xacro
index 1de74bcc8..e2400617c 100644
--- a/example_9/description/gazebo/rrbot.gazebo.xacro
+++ b/example_9/description/gazebo/rrbot.gazebo.xacro
@@ -7,10 +7,13 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
 
   <xacro:macro name="rrbot_gazebo" params="prefix">
 
-    <!-- ros_control plugin -->
+    <!-- ros2_control GZ Sim plugin -->
+    <gazebo reference="world">
+    </gazebo>
     <gazebo>
-      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
         <parameters>$(find ros2_control_demo_example_9)/config/rrbot_controllers.yaml</parameters>
+        <controller_manager_node_name>${prefix}controller_manager</controller_manager_node_name>
       </plugin>
     </gazebo>
 
diff --git a/example_9/description/ros2_control/rrbot.ros2_control.xacro b/example_9/description/ros2_control/rrbot.ros2_control.xacro
index 0d57bf036..4d2340d7f 100644
--- a/example_9/description/ros2_control/rrbot.ros2_control.xacro
+++ b/example_9/description/ros2_control/rrbot.ros2_control.xacro
@@ -1,14 +1,14 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="rrbot_ros2_control" params="name prefix use_gazebo_classic:=^|false">
+  <xacro:macro name="rrbot_ros2_control" params="name prefix use_gazebo_sim:=^|false">
 
     <ros2_control name="${name}" type="system">
       <hardware>
-        <xacro:if value="${use_gazebo_classic}">
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        <xacro:if value="${use_gazebo_sim}">
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </xacro:if>
-        <xacro:unless value="${use_gazebo_classic}">
+        <xacro:unless value="${use_gazebo_sim}">
           <plugin>ros2_control_demo_example_9/RRBotSystemPositionOnlyHardware</plugin>
           <param name="example_param_hw_start_duration_sec">0</param>
           <param name="example_param_hw_stop_duration_sec">3.0</param>
diff --git a/example_9/description/urdf/rrbot.urdf.xacro b/example_9/description/urdf/rrbot.urdf.xacro
index a77033cee..01d7f14ae 100644
--- a/example_9/description/urdf/rrbot.urdf.xacro
+++ b/example_9/description/urdf/rrbot.urdf.xacro
@@ -6,7 +6,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
   <xacro:arg name="prefix" default="" />
-  <xacro:arg name="use_gazebo_classic" default="false" />
+  <xacro:arg name="use_gazebo_sim" default="false" />
 
   <!-- Import RRBot macro -->
   <xacro:include filename="$(find ros2_control_demo_description)/rrbot/urdf/rrbot_description.urdf.xacro" />
@@ -25,9 +25,9 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
   </xacro:rrbot>
 
   <xacro:rrbot_ros2_control
-    name="RRBot" prefix="$(arg prefix)" use_gazebo_classic="$(arg use_gazebo_classic)"/>
+    name="RRBot" prefix="$(arg prefix)" use_gazebo_sim="$(arg use_gazebo_sim)"/>
 
-  <xacro:if value="$(arg use_gazebo_classic)">
+  <xacro:if value="$(arg use_gazebo_sim)">
     <!-- Import Gazebo Classic definitions + plugin -->
     <xacro:include filename="$(find ros2_control_demo_example_9)/gazebo/rrbot.gazebo.xacro" />
     <xacro:rrbot_gazebo prefix="$(arg prefix)"/>
diff --git a/example_9/package.xml b/example_9/package.xml
index 580b31746..b40b6b968 100644
--- a/example_9/package.xml
+++ b/example_9/package.xml
@@ -24,7 +24,7 @@
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>forward_command_controller</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
-  <exec_depend>gazebo_ros2_control</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
diff --git a/example_9/worlds/empty.world b/example_9/worlds/empty.world
new file mode 100644
index 000000000..3a2872c24
--- /dev/null
+++ b/example_9/worlds/empty.world
@@ -0,0 +1,72 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="empty">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-contact-system"
+      name="gz::sim::systems::Contact">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>