From a195faf361f263ca54c7f7ee0d2c92ac758d97cf Mon Sep 17 00:00:00 2001
From: TheNoobInventor <noobinventor@tutamail.com>
Date: Thu, 22 Aug 2024 17:13:13 +0100
Subject: [PATCH] Gazebo Fortress successfully integrated with ros2_control

---
 lidarbot_gz/config/lidarbot_bridge.yaml | 30 ---------------------
 lidarbot_gz/launch/gz_launch.py         | 35 ++++++++-----------------
 lidarbot_gz/package.xml                 |  1 -
 lidarbot_gz/urdf/lidarbot_gz.urdf.xacro |  9 +++----
 4 files changed, 15 insertions(+), 60 deletions(-)

diff --git a/lidarbot_gz/config/lidarbot_bridge.yaml b/lidarbot_gz/config/lidarbot_bridge.yaml
index b552e71..80747ea 100644
--- a/lidarbot_gz/config/lidarbot_bridge.yaml
+++ b/lidarbot_gz/config/lidarbot_bridge.yaml
@@ -5,37 +5,7 @@
   gz_type_name: "gz.msgs.Clock"
   direction: GZ_TO_ROS
 
-# gz topic published by JointState plugin
-# - ros_topic_name: "joint_states"
-#   gz_topic_name: "joint_states"
-#   ros_type_name: "sensor_msgs/msg/JointState"
-#   gz_type_name: "gz.msgs.Model"
-#   direction: GZ_TO_ROS
-
-# gz topic published by DiffDrive plugin
-# - ros_topic_name: "odom"
-#   gz_topic_name: "odom"
-#   ros_type_name: "nav_msgs/msg/Odometry"
-#   gz_type_name: "gz.msgs.Odometry"
-#   direction: GZ_TO_ROS
-
-# gz topic published by DiffDrive plugin
-# - ros_topic_name: "tf"
-#   gz_topic_name: "tf"
-#   ros_type_name: "tf2_msgs/msg/TFMessage"
-#   gz_type_name: "gz.msgs.Pose_V"
-#   direction: GZ_TO_ROS
-
-# gz topic subscribed to by DiffDrive plugin
-# - ros_topic_name: "cmd_vel"
-#   gz_topic_name: "cmd_vel"
-#   ros_type_name: "geometry_msgs/msg/Twist"
-#   gz_type_name: "gz.msgs.Twist"
-#   direction: ROS_TO_GZ
-
 # gz topic published by IMU plugin
-# - ros_topic_name: "imu"
-#   gz_topic_name: "imu"
 - ros_topic_name: "imu_broadcaster/imu"
   gz_topic_name: "imu_broadcaster/imu"
   ros_type_name: "sensor_msgs/msg/Imu"
diff --git a/lidarbot_gz/launch/gz_launch.py b/lidarbot_gz/launch/gz_launch.py
index 31327a5..141a53e 100644
--- a/lidarbot_gz/launch/gz_launch.py
+++ b/lidarbot_gz/launch/gz_launch.py
@@ -27,7 +27,9 @@ def generate_launch_description():
         "lidarbot_description"
     )
     pkg_teleop = FindPackageShare(package="lidarbot_teleop").find("lidarbot_teleop")
-    pkg_ros_gz_sim = FindPackageShare(package="ros_gz_sim").find("ros_gz_sim")
+    pkg_ros_ign_gazebo = FindPackageShare(package="ros_ign_gazebo").find(
+        "ros_ign_gazebo"
+    )
     pkg_navigation = FindPackageShare(package="lidarbot_navigation").find(
         "lidarbot_navigation"
     )
@@ -95,7 +97,7 @@ def generate_launch_description():
     # Launch Gazebo
     start_gazebo_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            [os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")]
+            [os.path.join(pkg_ros_ign_gazebo, "launch", "ign_gazebo.launch.py")]
         ),
         launch_arguments={
             "gz_args": ["-r -v4 ", world],
@@ -131,6 +133,7 @@ def generate_launch_description():
         output="screen",
     )
 
+    # Start joint_state_broadcaster
     load_joint_state_broadcaster = ExecuteProcess(
         cmd=[
             "ros2",
@@ -140,10 +143,10 @@ def generate_launch_description():
             "active",
             "joint_broadcaster",
         ],
-        # 'joint_state_broadcaster'],
         output="screen",
     )
 
+    # Start diff_drive_controller
     load_diff_drive_controller = ExecuteProcess(
         cmd=[
             "ros2",
@@ -153,26 +156,9 @@ def generate_launch_description():
             "active",
             "diff_controller",
         ],
-        # 'diff_drive_base_controller'],
         output="screen",
     )
 
-    # Spawn diff_controller
-    start_diff_controller_cmd = Node(
-        condition=IfCondition(use_ros2_control),
-        package="controller_manager",
-        executable="spawner",
-        arguments=["diff_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    # Spawn joint_state_broadcaser
-    start_joint_broadcaster_cmd = Node(
-        condition=IfCondition(use_ros2_control),
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_broadcaster", "--controller-manager", "/controller_manager"],
-    )
-
     # Start robot localization using an Extended Kalman Filter
     start_robot_localization_cmd = Node(
         condition=IfCondition(use_robot_localization),
@@ -213,14 +199,15 @@ def generate_launch_description():
     ld.add_action(start_spawner_cmd)
     ld.add_action(start_gazebo_ros_bridge_cmd)
     ld.add_action(start_gazebo_ros_image_bridge_cmd)
-    # ld.add_action(start_diff_controller_cmd)
-    # ld.add_action(start_joint_broadcaster_cmd)
     ld.add_action(start_robot_localization_cmd)
     ld.add_action(load_diff_drive_controller)
     ld.add_action(load_joint_state_broadcaster)
-    # ld.add_action(start_joystick_cmd)
-    # ld.add_action(start_twist_mux_cmd)
+    ld.add_action(start_joystick_cmd)
+    ld.add_action(start_twist_mux_cmd)
 
+    # TODO:
+    # Sort out colors in simulation
     # Bring up rviz2 and mapping/localization stuff too (?)
+    # Launch processes after previous ones have started (?)
 
     return ld
diff --git a/lidarbot_gz/package.xml b/lidarbot_gz/package.xml
index 14b5013..df49954 100644
--- a/lidarbot_gz/package.xml
+++ b/lidarbot_gz/package.xml
@@ -15,7 +15,6 @@
   <depend>lidarbot_description</depend>
   <depend>lidarbot_bringup</depend>
 
-  <!-- <depend>gz_ros2_control</depend> -->
   <depend>ign_ros2_control</depend>
   <depend>ros_gz_bridge</depend>
   <depend>ros_gz_image</depend>
diff --git a/lidarbot_gz/urdf/lidarbot_gz.urdf.xacro b/lidarbot_gz/urdf/lidarbot_gz.urdf.xacro
index 9804b0c..2094836 100644
--- a/lidarbot_gz/urdf/lidarbot_gz.urdf.xacro
+++ b/lidarbot_gz/urdf/lidarbot_gz.urdf.xacro
@@ -5,9 +5,9 @@
   <xacro:include filename="sensors.xacro"/>
 
   <!-- ros2control -->
-  <ros2_control name="GazeboSimSystem" type="system">
+  <ros2_control name="IgnitionSystem" type="system">
     <hardware>
-      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+      <plugin>ign_ros2_control/IgnitionSystem</plugin>
     </hardware>
     <joint name="left_wheel_joint">
       <command_interface name="velocity">
@@ -28,10 +28,9 @@
   </ros2_control>
 
   <gazebo>
-    <!-- Joint state publisher -->
-    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
       <parameters>$(find lidarbot_bringup)/config/controllers.yaml</parameters>
-      <!-- <parameters>$(find lidarbot_gz)/config/diff_drive_controller_velocity.yaml</parameters> -->
     </plugin>
   </gazebo>
+
 </robot>