From cb9f1c2ff616c1884e08f1a884d8236bcb0b6690 Mon Sep 17 00:00:00 2001
From: Joe Ferreira Scholtz <joefscholtz@gmail.com>
Date: Sat, 18 May 2024 00:18:32 -0300
Subject: [PATCH] configure gazebo ros2 control

it was necessary to erase all semicolons in comments because of a [weird
bug in gazebo_ros2_control
0.4.7](https://github.com/ros-controls/gazebo_ros2_control/issues/295),
I hope it will get fixed soon because I like my "TODO:"s using
folke/todo-comments.nvim
---
 .../config/gazebo_ros2_control.yaml           | 36 +++++++
 forg_description/urdf/base.urdf.xacro         |  2 +-
 forg_description/urdf/forg_bot.urdf.xacro     |  5 +-
 forg_description/urdf/forg_core.urdf.xacro    |  6 +-
 .../urdf/inertial_macros.urdf.xacro           |  2 +-
 .../urdf/left_back_arm.urdf.xacro             |  2 +-
 .../urdf/left_front_arm.urdf.xacro            |  2 +-
 .../urdf/left_middle_arm.urdf.xacro           |  4 +-
 .../urdf/right_back_arm.urdf.xacro            |  2 +-
 .../urdf/right_front_arm.urdf.xacro           |  2 +-
 .../urdf/right_middle_arm.urdf.xacro          |  4 +-
 forg_description/urdf/ros2_control.urdf.xacro | 93 +++++++++++++++++++
 forg_description/urdf/wheel.urdf.xacro        |  8 +-
 .../urdf/wheel_steering.urdf.xacro            |  6 +-
 14 files changed, 153 insertions(+), 21 deletions(-)
 create mode 100644 forg_description/config/gazebo_ros2_control.yaml
 create mode 100644 forg_description/urdf/ros2_control.urdf.xacro

diff --git a/forg_description/config/gazebo_ros2_control.yaml b/forg_description/config/gazebo_ros2_control.yaml
new file mode 100644
index 0000000..cb113ff
--- /dev/null
+++ b/forg_description/config/gazebo_ros2_control.yaml
@@ -0,0 +1,36 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50
+    use_sim_time: True
+
+    # holonomic_rover_controller:
+    #   type: holonomic_rover_controller/HolonomicRoverController
+
+holonomic_rover_controller:
+  ros__parameters:
+    rear_wheels_steering_names:
+      [
+        "left_back_wheel_steering_gear_joint",
+        "right_back_wheel_steering_gear_joint",
+      ]
+    middle_wheel_steering_names:
+      [
+        "left_middle_wheel_steering_gear_joint",
+        "right_middle_wheel_steering_gear_joint",
+      ]
+    front_wheels_steering_names:
+      [
+        "left_front_wheel_steering_gear_joint",
+        "right_front_wheel_steering_gear_joint",
+      ]
+    rear_wheels_names: ["left_back_wheel_joint", "right_back_wheel_joint"]
+    middle_wheel_names: ["left_middle_wheel_joint", "right_middle_wheel_joint"]
+    front_wheels_names: ["left_front_wheel_joint", "right_front_wheel_joint"]
+    front_to_middle_wheel_distance: 0.44
+    middle_to_back_wheel_distance: 0.43
+    wheel_track: 0.27
+    wheels_radius: 0.06
+    base_frame_id: "base_link"
+    odom_frame_id: "odom"
+    enable_odom_tf: True
+    use_stamped_vel: False
diff --git a/forg_description/urdf/base.urdf.xacro b/forg_description/urdf/base.urdf.xacro
index b053ce0..2c6db3e 100644
--- a/forg_description/urdf/base.urdf.xacro
+++ b/forg_description/urdf/base.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/spine.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="5" x="0.17" y="0.17" z="0.17"/> <!-- TODO: replace with real values -->
+      <xacro:inertial_box mass="5" x="0.17" y="0.17" z="0.17"/> <!-- TODO replace with real values -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/forg_bot.urdf.xacro b/forg_description/urdf/forg_bot.urdf.xacro
index d042043..0ce4388 100644
--- a/forg_description/urdf/forg_bot.urdf.xacro
+++ b/forg_description/urdf/forg_bot.urdf.xacro
@@ -1,7 +1,10 @@
-<?xml version="1.0" ?>
+<?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="forg_bot">
 
+	<xacro:arg name="simulation" default="true"/>
+
   <xacro:include filename="forg_core.urdf.xacro"/>
+  <xacro:include filename="ros2_control.urdf.xacro"/>
 
 </robot>
 
diff --git a/forg_description/urdf/forg_core.urdf.xacro b/forg_description/urdf/forg_core.urdf.xacro
index a8ff0ff..ce56e20 100644
--- a/forg_description/urdf/forg_core.urdf.xacro
+++ b/forg_description/urdf/forg_core.urdf.xacro
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="base.urdf.xacro"/>
@@ -47,7 +47,7 @@
     <axis xyz="1 0 0"/>
     <mimic joint="shoulder_joint" multiplier="1"/>
     <limit effort="${10}" velocity="${360*2*pi/60}"/> 
-    <dynamics damping="0.1" friction="50"/> <!-- TODO: replace with real properties -->
+    <dynamics damping="0.1" friction="50"/> <!-- TODO replace with real properties -->
   </joint>
   <!-- <xacro:mimic_joint_plugin_gazebo name_prefix="shoulder_left" parent_joint="shoulder_joint" mimic_joint="shoulder_left_joint" multiplier="1.0" /> -->
 
@@ -61,7 +61,7 @@
     <axis xyz="1 0 0"/>
     <mimic joint="shoulder_joint" multiplier="-1"/>
     <limit effort="${10}" velocity="${360*2*pi/60}"/> 
-    <dynamics damping="0.1" friction="50"/> <!-- TODO: replace with real properties -->
+    <dynamics damping="0.1" friction="50"/> <!-- TODO replace with real properties -->
   </joint>
   <xacro:mimic_joint_plugin_gazebo name_prefix="shoulder_right" parent_joint="shoulder_left_joint" mimic_joint="shoulder_right_joint" multiplier="-1.0" debug="false"/>
 
diff --git a/forg_description/urdf/inertial_macros.urdf.xacro b/forg_description/urdf/inertial_macros.urdf.xacro
index 1de778d..dbb827c 100644
--- a/forg_description/urdf/inertial_macros.urdf.xacro
+++ b/forg_description/urdf/inertial_macros.urdf.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
 
-    <!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
+    <!-- Specify some standard inertial calculations en.wikipedia.org/wiki/List_of_moments_of_inertia -->
     <!-- These make use of xacro's mathematical functionality -->
 
     <xacro:macro name="inertial_sphere" params="mass radius">
diff --git a/forg_description/urdf/left_back_arm.urdf.xacro b/forg_description/urdf/left_back_arm.urdf.xacro
index 6ba53ad..727e170 100644
--- a/forg_description/urdf/left_back_arm.urdf.xacro
+++ b/forg_description/urdf/left_back_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/left_back_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/left_front_arm.urdf.xacro b/forg_description/urdf/left_front_arm.urdf.xacro
index cce9767..c11f4ce 100644
--- a/forg_description/urdf/left_front_arm.urdf.xacro
+++ b/forg_description/urdf/left_front_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/left_front_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/left_middle_arm.urdf.xacro b/forg_description/urdf/left_middle_arm.urdf.xacro
index 3802d1b..0c9456e 100644
--- a/forg_description/urdf/left_middle_arm.urdf.xacro
+++ b/forg_description/urdf/left_middle_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/shoulder_axis.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
     <joint name="${name}_joint" type="fixed">
@@ -46,7 +46,7 @@
 					<mesh filename="package://forg_description/meshes/middle_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/right_back_arm.urdf.xacro b/forg_description/urdf/right_back_arm.urdf.xacro
index f78a0a5..57a0398 100644
--- a/forg_description/urdf/right_back_arm.urdf.xacro
+++ b/forg_description/urdf/right_back_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/right_back_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/right_front_arm.urdf.xacro b/forg_description/urdf/right_front_arm.urdf.xacro
index 097ca5e..042f430 100644
--- a/forg_description/urdf/right_front_arm.urdf.xacro
+++ b/forg_description/urdf/right_front_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/right_front_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/right_middle_arm.urdf.xacro b/forg_description/urdf/right_middle_arm.urdf.xacro
index 5306124..8739898 100644
--- a/forg_description/urdf/right_middle_arm.urdf.xacro
+++ b/forg_description/urdf/right_middle_arm.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/shoulder_axis.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
     <joint name="${name}_joint" type="fixed">
@@ -46,7 +46,7 @@
 					<mesh filename="package://forg_description/meshes/middle_arm.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/ros2_control.urdf.xacro b/forg_description/urdf/ros2_control.urdf.xacro
new file mode 100644
index 0000000..04484d6
--- /dev/null
+++ b/forg_description/urdf/ros2_control.urdf.xacro
@@ -0,0 +1,93 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+
+	<xacro:arg name="simulation" default="true"/>
+
+  <xacro:if value="$(arg simulation)">
+    <ros2_control name="GazeboSystem" type="system">
+      <hardware>
+        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      </hardware>
+      <joint name="left_front_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_middle_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_back_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_front_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_middle_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_back_wheel_steering_gear_joint">
+        <command_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <joint name="left_front_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_middle_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_back_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_front_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_middle_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="right_back_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="effort"/>
+      </joint>
+    </ros2_control>
+
+    <gazebo>
+      <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+        <parameters>$(find forg_description)/config/gazebo_ros2_control.yaml</parameters>
+      </plugin>
+    </gazebo>
+  </xacro:if>
+
+</robot>
diff --git a/forg_description/urdf/wheel.urdf.xacro b/forg_description/urdf/wheel.urdf.xacro
index ba39cf0..8afd9f5 100644
--- a/forg_description/urdf/wheel.urdf.xacro
+++ b/forg_description/urdf/wheel.urdf.xacro
@@ -12,8 +12,8 @@
 			<child link="${name}_axis"/>
 			<xacro:insert_block name="origin"/>
 			<axis xyz="0 0 1"/>
-      <limit effort="${13.3*9.81*2.9}" velocity="${1000*2*pi/60}"/> <!-- TODO: replace with motor limits -->
-			<!--dynamics damping="0.1" friction="1"/--> <!-- TODO: replace with motor + axis properties -->
+      <limit effort="${13.3*9.81*2.9}" velocity="${1000*2*pi/60}"/> <!-- TODO replace with motor limits -->
+			<!--dynamics damping="0.1" friction="1"/--> <!-- TODO replace with motor + axis properties -->
 		</joint>
 
 		<link name="${name}_axis">
@@ -27,7 +27,7 @@
 					<mesh filename="package://forg_description/meshes/wheel_axis.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO: replace wheel axis mass with real value -->
+      <xacro:inertial_cylinder mass="0.05" length="0.17" radius="0.00625"/> <!-- TODO replace wheel axis mass with real value -->
 		</link>
 
     <joint name="${name}_axis_joint" type="fixed">
@@ -48,7 +48,7 @@
 					<mesh filename="package://forg_description/meshes/wheel.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_cylinder mass="0.2" length="0.015" radius="0.06"/> <!-- TODO: replace wheel mass with real value -->
+      <xacro:inertial_cylinder mass="0.2" length="0.015" radius="0.06"/> <!-- TODO replace wheel mass with real value -->
 		</link>
 
 	</xacro:macro>
diff --git a/forg_description/urdf/wheel_steering.urdf.xacro b/forg_description/urdf/wheel_steering.urdf.xacro
index a67a6d7..30f0648 100644
--- a/forg_description/urdf/wheel_steering.urdf.xacro
+++ b/forg_description/urdf/wheel_steering.urdf.xacro
@@ -25,7 +25,7 @@
 					<mesh filename="package://forg_description/meshes/steering_base.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
     <joint name="${name}_steering_gear_joint" type="continuous">
@@ -46,7 +46,7 @@
 					<mesh filename="package://forg_description/meshes/steering_gear.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
     <joint name="${name}_steering_joint" type="fixed">
@@ -67,7 +67,7 @@
 					<mesh filename="package://forg_description/meshes/wheel_steering.dae"/>
 				</geometry>
 			</collision>
-      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO: replace shoulder axis mass with real value -->
+      <xacro:inertial_box mass="0.05" x="0.17" y="0.00625" z="0.8"/> <!-- TODO replace shoulder axis mass with real value -->
 		</link>
 
   <xacro:wheel name="${name}" parent="${name}_steering">