From d859a69b5b3cee9613e181b007bb9119c084e60e Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Mon, 2 Oct 2023 12:53:40 +0200
Subject: [PATCH 01/14] Add enviormental variable hooks

---
 leo_description/CMakeLists.txt               | 3 +++
 leo_description/hooks/leo_description.dsv.in | 1 +
 leo_description/hooks/leo_description.sh.in  | 1 +
 3 files changed, 5 insertions(+)
 create mode 100644 leo_description/hooks/leo_description.dsv.in
 create mode 100644 leo_description/hooks/leo_description.sh.in

diff --git a/leo_description/CMakeLists.txt b/leo_description/CMakeLists.txt
index 0861db0..cab1b84 100644
--- a/leo_description/CMakeLists.txt
+++ b/leo_description/CMakeLists.txt
@@ -12,4 +12,7 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+
 ament_package()
diff --git a/leo_description/hooks/leo_description.dsv.in b/leo_description/hooks/leo_description.dsv.in
new file mode 100644
index 0000000..2a38ccc
--- /dev/null
+++ b/leo_description/hooks/leo_description.dsv.in
@@ -0,0 +1 @@
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share;@CMAKE_INSTALL_PREFIX@/share
\ No newline at end of file
diff --git a/leo_description/hooks/leo_description.sh.in b/leo_description/hooks/leo_description.sh.in
new file mode 100644
index 0000000..33fa6b2
--- /dev/null
+++ b/leo_description/hooks/leo_description.sh.in
@@ -0,0 +1 @@
+ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/models"
\ No newline at end of file

From 1800dc788fdb5cce78a354ac482511709f5219eb Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Mon, 2 Oct 2023 12:54:22 +0200
Subject: [PATCH 02/14] Adjust simulation xacro for new simulation

---
 leo_description/urdf/macros.xacro | 181 ++++++++++++++++++++++--------
 1 file changed, 134 insertions(+), 47 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 61e0d27..d5b3c5e 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -231,7 +231,8 @@
   <!-- LEO GAZEBO -->
 
   <xacro:macro name="leo_gazebo"
-               params="robot_ns:=''">
+               params="robot_ns:=''
+               joint_prefix:=''">
 
     <xacro:property name="link_prefix" value=""/>
     <xacro:if value="${robot_ns != '' and robot_ns != '/'}">
@@ -310,65 +311,75 @@
 
     <!-- camera -->
     <gazebo reference="${link_prefix}camera_frame">
-      <sensor type="camera" name="camera">
-        <always_on>true</always_on>
-        <update_rate>30.0</update_rate>
-        <visualize>false</visualize> 
-        <camera name="leo_camera">
-          <horizontal_fov>1.9</horizontal_fov>
-          <image>
-            <width>640</width>
-            <height>480</height>
-            <format>R8G8B8</format>
-          </image>
-          <clip>
-            <near>0.02</near>
-            <far>300</far>
-          </clip>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.007</stddev>
-          </noise>
-          <distortion>
-            <k1>-0.279817</k1>
-            <k2>0.060321</k2>
-            <k3>0.000487</k3>
-            <p1>0.000310</p1>
-            <p2>0.000000</p2>
-            <center>0.5 0.5</center>
-          </distortion>
-        </camera>
-        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
-          <ros><namespace>${robot_ns}</namespace></ros>
-          <frame_name>camera_optical_frame</frame_name>
-        </plugin>
+      <sensor name="camera" type="camera">
+          <camera name="leo_camera">
+              <horizontal_fov>1.9</horizontal_fov>
+              <image>
+                  <width>640</width>
+                  <height>480</height>
+                  <format>R8G8B8</format>
+              </image>
+              <clip>
+                  <near>0.2</near>
+                  <far>300</far>
+              </clip>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.007</stddev>
+              </noise>
+              <distortion>
+                <k1>-0.279817</k1>
+                <k2>0.060321</k2>
+                <k3>0.000487</k3>
+                <p1>0.000310</p1>
+                <p2>0.000000</p2>
+                <center>0.5 0.5</center>
+              </distortion>              
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <topic>camera/image_raw</topic>
+          <ignition_frame_id>${link_prefix}camera_frame</ignition_frame_id>
       </sensor>
     </gazebo>
 
     <!-- rocker differential -->
     <gazebo>
-      <plugin name="rocker_differential" filename="libleo_gazebo_differential_plugin.so">
+      <plugin name="leo_gz::DifferentialSystem" filename="libleo_gazebo_differential_plugin.so">
         <jointA>rocker_L_joint</jointA>
         <jointB>rocker_R_joint</jointB>
         <forceConstant>100.0</forceConstant>
       </plugin>
     </gazebo>
 
+    <gazebo>
+      <plugin
+        filename="ignition-gazebo-joint-state-publisher-system"
+        name="ignition::gazebo::systems::JointStatePublisher">
+      </plugin>
+    <!--
+      <plugin
+        filename="ignition-gazebo-pose-publisher-system"
+        name="ignition::gazebo::systems::PosePublisher">
+        <publish_link_pose>true</publish_link_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
+      </plugin>-->
+    </gazebo>
+
     <gazebo reference="${link_prefix}imu_frame">
       <sensor type="imu" name="leo_imu_sensor">
         <update_rate>100</update_rate>
-        <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
-          <robotNamespace>${robot_ns}</robotNamespace>
-          <topicName>imu/data_raw</topicName>
-          <frameName>${link_prefix}imu_frame</frameName>
-          <updateRateHZ>100.0</updateRateHZ>
-          <gaussianNoise>0.01</gaussianNoise>
-          <initialOrientationAsReference>false</initialOrientationAsReference>
-        </plugin>
+        <visualize>true</visualize>
+        <topic>imu/data_raw</topic>
+        <always_on>1</always_on>
+        <ignition_frame_id>${link_prefix}imu_frame</ignition_frame_id>
       </sensor>
     </gazebo>
-
+<!--
     <ros2_control name="GazeboSystem" type="system">
       <hardware>
         <plugin>gazebo_ros2_control/GazeboSystem</plugin>
@@ -416,15 +427,91 @@
         <state_interface name="effort"/>
       </joint>
     </ros2_control>
-
+-->
     <gazebo>
-      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <!--<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
         <robot_param>robot_description</robot_param>
         <robot_param_node>robot_state_publisher</robot_param_node>
         <parameters>package://leo_description/config/controllers.yaml</parameters>
+      </plugin>-->
+      <plugin
+        filename="libignition-gazebo-diff-drive-system.so"
+        name="ignition::gazebo::systems::DiffDrive">
+        <left_joint>wheel_FL_joint</left_joint>
+        <right_joint>wheel_FR_joint</right_joint>
+        <left_joint>wheel_RL_joint</left_joint>
+        <right_joint>wheel_RR_joint</right_joint>
+        <wheel_separation>0.358</wheel_separation>
+        <wheel_radius>0.0625</wheel_radius>
+        <frame_id>odom</frame_id>
+        <child_frame_id>${link_prefix}base_footprint</child_frame_id>
+        <odom_publish_frequency>20</odom_publish_frequency>
+        <topic>cmd_vel</topic>
+        <max_linear_acceleration>2.5</max_linear_acceleration>
+        <max_angular_acceleration>3</max_angular_acceleration>
+        <min_linear_acceleration>-2.5</min_linear_acceleration>
+        <min_angular_acceleration>-3</min_angular_acceleration>
       </plugin>
+      <!--
+      <plugin filename="libignition-gazebo-mecanum-drive-system.so" name="ignition::gazebo::systems::MecanumDrive">
+        <front_left_joint>wheel_FL_joint</front_left_joint>
+        <front_right_joint>wheel_FR_joint</front_right_joint>
+        <back_left_joint>wheel_RL_joint</back_left_joint>
+        <back_right_joint>wheel_RR_joint</back_right_joint>
+        <wheel_separation>0.358</wheel_separation>
+        <wheel_radius>0.0625</wheel_radius>
+        <wheelbase>0.3</wheelbase>
+        <odom_publish_frequency>20</odom_publish_frequency>
+        <topic>cmd_vel</topic>
+        <frame_id>odom</frame_id>
+        <child_frame_id>${link_prefix}base_footprint</child_frame_id>
+      </plugin>      -->
+<!--
+       <plugin filename="libignition-gazebo-wheel-slip-system.so"
+          name="systems::WheelSlip">
+          <wheel link_name="wheel_FL_link">
+            <slip_compliance_lateral>0</slip_compliance_lateral>
+            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
+            <wheel_normal_force>100</wheel_normal_force>
+            <wheel_radius>0.0625</wheel_radius>
+          </wheel>
+          <wheel link_name="wheel_FR_link">
+            <slip_compliance_lateral>0</slip_compliance_lateral>
+            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
+            <wheel_normal_force>100</wheel_normal_force>
+            <wheel_radius>0.0625</wheel_radius>
+          </wheel>
+          <wheel link_name="wheel_RL_link">
+            <slip_compliance_lateral>0</slip_compliance_lateral>
+            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
+            <wheel_normal_force>80</wheel_normal_force>
+            <wheel_radius>0.0625</wheel_radius>
+          </wheel>
+          <wheel link_name="wheel_RR_link">
+            <slip_compliance_lateral>0</slip_compliance_lateral>
+            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
+            <wheel_normal_force>80</wheel_normal_force>
+            <wheel_radius>0.0625</wheel_radius>
+          </wheel>
+        </plugin>-->
     </gazebo>
-
+<!--
+    <gazebo reference="${joint_prefix}base_joint">
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo>
+    <gazebo reference="${joint_prefix}antenna_joint">
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo>
+    <gazebo reference="${joint_prefix}camera_joint">
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo>
+    <gazebo reference="${joint_prefix}camera_optical_joint">
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo>
+    <gazebo reference="${joint_prefix}imu_joint">
+      <preserveFixedJoint>true</preserveFixedJoint>
+    </gazebo> 
+-->
   </xacro:macro>
 
   <xacro:macro name="leo_sim" 

From 332cb0532bb65d4762751cfc1c3322beecb9a67f Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 10 Oct 2023 10:39:39 +0200
Subject: [PATCH 03/14] Macros.xacro cleanup

---
 leo_description/urdf/macros.xacro | 152 ++++--------------------------
 1 file changed, 18 insertions(+), 134 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index d5b3c5e..50c22f6 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -311,8 +311,8 @@
 
     <!-- camera -->
     <gazebo reference="${link_prefix}camera_frame">
-      <sensor name="camera" type="camera">
-          <camera name="leo_camera">
+      <sensor name="${link_prefix}camera" type="camera">
+          <camera name="${link_prefix}leo_camera">
               <horizontal_fov>1.9</horizontal_fov>
               <image>
                   <width>640</width>
@@ -340,7 +340,7 @@
           <always_on>1</always_on>
           <update_rate>30</update_rate>
           <visualize>true</visualize>
-          <topic>camera/image_raw</topic>
+          <topic>${link_prefix}camera/image_raw</topic>
           <ignition_frame_id>${link_prefix}camera_frame</ignition_frame_id>
       </sensor>
     </gazebo>
@@ -359,159 +359,39 @@
         filename="ignition-gazebo-joint-state-publisher-system"
         name="ignition::gazebo::systems::JointStatePublisher">
       </plugin>
-    <!--
-      <plugin
-        filename="ignition-gazebo-pose-publisher-system"
-        name="ignition::gazebo::systems::PosePublisher">
-        <publish_link_pose>true</publish_link_pose>
-        <use_pose_vector_msg>true</use_pose_vector_msg>
-        <static_publisher>true</static_publisher>
-        <static_update_frequency>1</static_update_frequency>
-      </plugin>-->
     </gazebo>
 
     <gazebo reference="${link_prefix}imu_frame">
-      <sensor type="imu" name="leo_imu_sensor">
+      <sensor type="imu" name="${link_prefix}leo_imu_sensor">
         <update_rate>100</update_rate>
         <visualize>true</visualize>
-        <topic>imu/data_raw</topic>
+        <topic>${link_prefix}imu/data_raw</topic>
         <always_on>1</always_on>
         <ignition_frame_id>${link_prefix}imu_frame</ignition_frame_id>
       </sensor>
     </gazebo>
-<!--
-    <ros2_control name="GazeboSystem" type="system">
-      <hardware>
-        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-      </hardware>
-      <joint name="rocker_L_joint">
-        <state_interface name="position"/>
-      </joint>
-      <joint name="rocker_R_joint">
-        <state_interface name="position"/>
-      </joint>
-      <joint name="wheel_FL_joint">
-        <command_interface name="velocity">
-          <param name="min">-6</param>
-          <param name="max">6</param>
-        </command_interface>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      <joint name="wheel_RL_joint">
-        <command_interface name="velocity">
-          <param name="min">-6</param>
-          <param name="max">6</param>
-        </command_interface>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      <joint name="wheel_FR_joint">
-        <command_interface name="velocity">
-          <param name="min">-6</param>
-          <param name="max">6</param>
-        </command_interface>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      <joint name="wheel_RR_joint">
-        <command_interface name="velocity">
-          <param name="min">-6</param>
-          <param name="max">6</param>
-        </command_interface>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-    </ros2_control>
--->
     <gazebo>
-      <!--<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-        <robot_param>robot_description</robot_param>
-        <robot_param_node>robot_state_publisher</robot_param_node>
-        <parameters>package://leo_description/config/controllers.yaml</parameters>
-      </plugin>-->
       <plugin
         filename="libignition-gazebo-diff-drive-system.so"
         name="ignition::gazebo::systems::DiffDrive">
-        <left_joint>wheel_FL_joint</left_joint>
-        <right_joint>wheel_FR_joint</right_joint>
-        <left_joint>wheel_RL_joint</left_joint>
-        <right_joint>wheel_RR_joint</right_joint>
+        <left_joint>${joint_prefix}wheel_FL_joint</left_joint>
+        <right_joint>${joint_prefix}wheel_FR_joint</right_joint>
+        <left_joint>${joint_prefix}wheel_RL_joint</left_joint>
+        <right_joint>${joint_prefix}wheel_RR_joint</right_joint>
         <wheel_separation>0.358</wheel_separation>
         <wheel_radius>0.0625</wheel_radius>
         <frame_id>odom</frame_id>
         <child_frame_id>${link_prefix}base_footprint</child_frame_id>
         <odom_publish_frequency>20</odom_publish_frequency>
-        <topic>cmd_vel</topic>
+        <topic>${link_prefix}cmd_vel</topic>
+        <odom_topic>${link_prefix}odom</odom_topic>
+        <tf_topic>${link_prefix}tf</tf_topic>
         <max_linear_acceleration>2.5</max_linear_acceleration>
         <max_angular_acceleration>3</max_angular_acceleration>
         <min_linear_acceleration>-2.5</min_linear_acceleration>
         <min_angular_acceleration>-3</min_angular_acceleration>
       </plugin>
-      <!--
-      <plugin filename="libignition-gazebo-mecanum-drive-system.so" name="ignition::gazebo::systems::MecanumDrive">
-        <front_left_joint>wheel_FL_joint</front_left_joint>
-        <front_right_joint>wheel_FR_joint</front_right_joint>
-        <back_left_joint>wheel_RL_joint</back_left_joint>
-        <back_right_joint>wheel_RR_joint</back_right_joint>
-        <wheel_separation>0.358</wheel_separation>
-        <wheel_radius>0.0625</wheel_radius>
-        <wheelbase>0.3</wheelbase>
-        <odom_publish_frequency>20</odom_publish_frequency>
-        <topic>cmd_vel</topic>
-        <frame_id>odom</frame_id>
-        <child_frame_id>${link_prefix}base_footprint</child_frame_id>
-      </plugin>      -->
-<!--
-       <plugin filename="libignition-gazebo-wheel-slip-system.so"
-          name="systems::WheelSlip">
-          <wheel link_name="wheel_FL_link">
-            <slip_compliance_lateral>0</slip_compliance_lateral>
-            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
-            <wheel_normal_force>100</wheel_normal_force>
-            <wheel_radius>0.0625</wheel_radius>
-          </wheel>
-          <wheel link_name="wheel_FR_link">
-            <slip_compliance_lateral>0</slip_compliance_lateral>
-            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
-            <wheel_normal_force>100</wheel_normal_force>
-            <wheel_radius>0.0625</wheel_radius>
-          </wheel>
-          <wheel link_name="wheel_RL_link">
-            <slip_compliance_lateral>0</slip_compliance_lateral>
-            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
-            <wheel_normal_force>80</wheel_normal_force>
-            <wheel_radius>0.0625</wheel_radius>
-          </wheel>
-          <wheel link_name="wheel_RR_link">
-            <slip_compliance_lateral>0</slip_compliance_lateral>
-            <slip_compliance_longitudinal>0.1</slip_compliance_longitudinal>
-            <wheel_normal_force>80</wheel_normal_force>
-            <wheel_radius>0.0625</wheel_radius>
-          </wheel>
-        </plugin>-->
-    </gazebo>
-<!--
-    <gazebo reference="${joint_prefix}base_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <gazebo reference="${joint_prefix}antenna_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <gazebo reference="${joint_prefix}camera_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo>
-    <gazebo reference="${joint_prefix}camera_optical_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
     </gazebo>
-    <gazebo reference="${joint_prefix}imu_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
-    </gazebo> 
--->
   </xacro:macro>
 
   <xacro:macro name="leo_sim" 
@@ -520,15 +400,19 @@
                        robot_ns:=''">
 
     <xacro:property name="link_prefix" value=""/>
+    <xacro:property name="joint_prefix" value=""/>
     <xacro:if value="${robot_ns != '' and robot_ns != '/'}">
       <xacro:property name="link_prefix" value="${robot_ns}/"/>
+      <xacro:property name="joint_prefix" value="${robot_ns}/"/>
     </xacro:if>
 
     <xacro:leo default_antenna="${default_antenna}"
                rockers_fixed="false"
-               link_prefix="${link_prefix}"/>
+               link_prefix="${link_prefix}"
+               joint_prefix="${joint_prefix}"/>
 
-    <xacro:leo_gazebo robot_ns="${robot_ns}"/>
+    <xacro:leo_gazebo robot_ns="${robot_ns}"
+    joint_prefix="${joint_prefix}"/>
 
     <xacro:if value="${fixed}">
       <link name="world"/>

From a2726c10f665e65a884af9a70523055f6dc07451 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 10 Oct 2023 13:34:41 +0200
Subject: [PATCH 04/14] Addd surface element to wheels

---
 leo_description/urdf/macros.xacro | 89 +++++++++++++++++++++----------
 1 file changed, 61 insertions(+), 28 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 50c22f6..2eb44d3 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -243,7 +243,7 @@
     <gazebo reference="${link_prefix}base_footprint">
       <kp>1e6</kp>
       <kd>1.0</kd>
-      <mu1>0.3</mu1>
+      <mu>0.3</mu>
       <mu2>0.3</mu2>
       <minDepth>0.003</minDepth>
     </gazebo>
@@ -251,7 +251,7 @@
     <gazebo reference="${link_prefix}base_link">
       <kp>1e6</kp>
       <kd>1.0</kd>
-      <mu1>0.3</mu1>
+      <mu>0.3</mu>
       <mu2>0.3</mu2>
       <minDepth>0.003</minDepth>
     </gazebo>
@@ -260,7 +260,7 @@
     <gazebo reference="${link_prefix}rocker_L_link">
       <kp>1e6</kp>
       <kd>1.0</kd>
-      <mu1>0.3</mu1>
+      <mu>0.3</mu>
       <mu2>0.3</mu2>
       <minDepth>0.003</minDepth>
     </gazebo>
@@ -268,45 +268,77 @@
     <gazebo reference="${link_prefix}rocker_R_link">
       <kp>1e6</kp>
       <kd>1.0</kd>
-      <mu1>0.3</mu1>
+      <mu>0.3</mu>
       <mu2>0.3</mu2>
       <minDepth>0.003</minDepth>
     </gazebo>
     <!-- wheel ODE properties -->
     <gazebo reference="${link_prefix}wheel_FL_link">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <surface>
+        <friction>
+          <ode>
+            <mu>3.0</mu>
+            <mu2>1.0</mu2>
+            <fdir1>1 0 0</fdir1>
+          </ode>
+          <bullet>
+            <friction>1</friction>
+            <friction2>1</friction2>
+            <rolling_friction>0.1</rolling_friction>
+          </bullet>
+        </friction>
+      </surface>
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_FR_link">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <surface>
+        <friction>
+          <ode>
+            <mu>3.0</mu>
+            <mu2>1.0</mu2>
+            <fdir1>1 0 0</fdir1>
+          </ode>
+          <bullet>
+            <friction>1</friction>
+            <friction2>1</friction2>
+            <rolling_friction>0.1</rolling_friction>
+          </bullet>
+        </friction>
+      </surface>  
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_RL_link">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <surface>
+        <friction>
+          <ode>
+            <mu>3.0</mu>
+            <mu2>1.0</mu2>
+            <fdir1>1 0 0</fdir1>
+          </ode>
+          <bullet>
+            <friction>1</friction>
+            <friction2>1</friction2>
+            <rolling_friction>0.1</rolling_friction>
+          </bullet>
+        </friction>
+      </surface>
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_RR_link">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <surface>
+        <friction>
+          <ode>
+            <mu>3.0</mu>
+            <mu2>1.0</mu2>
+            <fdir1>1 0 0</fdir1>
+          </ode>
+          <bullet>
+            <friction>1</friction>
+            <friction2>1</friction2>
+            <rolling_friction>0.1</rolling_friction>
+          </bullet>
+        </friction>
+      </surface>
     </gazebo>
 
     <!-- camera -->
@@ -371,6 +403,7 @@
       </sensor>
     </gazebo>
     <gazebo>
+    
       <plugin
         filename="libignition-gazebo-diff-drive-system.so"
         name="ignition::gazebo::systems::DiffDrive">

From e0afce678ec0f833c1085b924dfc174594723dfc Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Wed, 11 Oct 2023 14:29:07 +0200
Subject: [PATCH 05/14] Add custom topic name for joint state plugin

---
 leo_description/urdf/macros.xacro | 1 +
 1 file changed, 1 insertion(+)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 2eb44d3..5aab947 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -390,6 +390,7 @@
       <plugin
         filename="ignition-gazebo-joint-state-publisher-system"
         name="ignition::gazebo::systems::JointStatePublisher">
+        <topic>${link_prefix}joint_states</topic>
       </plugin>
     </gazebo>
 

From 0412c6edac55f85d6697eb2969a79c882cc8b24b Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Fri, 13 Oct 2023 10:43:21 +0200
Subject: [PATCH 06/14] Fix gazebo launch warnings

---
 leo_description/urdf/macros.xacro | 200 ++++++++++++++++++------------
 1 file changed, 124 insertions(+), 76 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 5aab947..e1a45a5 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -241,104 +241,152 @@
 
     <!-- base ODE properties -->
     <gazebo reference="${link_prefix}base_footprint">
-      <kp>1e6</kp>
-      <kd>1.0</kd>
-      <mu>0.3</mu>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <gazebo reference="${link_prefix}base_link">
-      <kp>1e6</kp>
-      <kd>1.0</kd>
-      <mu>0.3</mu>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <!-- rocker ODE properties -->
     <gazebo reference="${link_prefix}rocker_L_link">
-      <kp>1e6</kp>
-      <kd>1.0</kd>
-      <mu>0.3</mu>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <gazebo reference="${link_prefix}rocker_R_link">
-      <kp>1e6</kp>
-      <kd>1.0</kd>
-      <mu>0.3</mu>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
     <!-- wheel ODE properties -->
     <gazebo reference="${link_prefix}wheel_FL_link">
-      <surface>
-        <friction>
-          <ode>
-            <mu>3.0</mu>
-            <mu2>1.0</mu2>
-            <fdir1>1 0 0</fdir1>
-          </ode>
-          <bullet>
-            <friction>1</friction>
-            <friction2>1</friction2>
-            <rolling_friction>0.1</rolling_friction>
-          </bullet>
-        </friction>
-      </surface>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+              <fdir1>0 0 0</fdir1>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_FR_link">
-      <surface>
-        <friction>
-          <ode>
-            <mu>3.0</mu>
-            <mu2>1.0</mu2>
-            <fdir1>1 0 0</fdir1>
-          </ode>
-          <bullet>
-            <friction>1</friction>
-            <friction2>1</friction2>
-            <rolling_friction>0.1</rolling_friction>
-          </bullet>
-        </friction>
-      </surface>  
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+              <fdir1>0 0 0</fdir1>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_RL_link">
-      <surface>
-        <friction>
-          <ode>
-            <mu>3.0</mu>
-            <mu2>1.0</mu2>
-            <fdir1>1 0 0</fdir1>
-          </ode>
-          <bullet>
-            <friction>1</friction>
-            <friction2>1</friction2>
-            <rolling_friction>0.1</rolling_friction>
-          </bullet>
-        </friction>
-      </surface>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+              <fdir1>0 0 0</fdir1>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <gazebo reference="${link_prefix}wheel_RR_link">
-      <surface>
-        <friction>
-          <ode>
-            <mu>3.0</mu>
-            <mu2>1.0</mu2>
-            <fdir1>1 0 0</fdir1>
-          </ode>
-          <bullet>
-            <friction>1</friction>
-            <friction2>1</friction2>
-            <rolling_friction>0.1</rolling_friction>
-          </bullet>
-        </friction>
-      </surface>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+              <fdir1>0 0 0</fdir1>
+            </ode>
+            <bullet>
+              <friction>1</friction>
+              <friction2>1</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </gazebo>
 
     <!-- camera -->

From 4846f848fde6f29714f6f212240568f60f9eff21 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Fri, 13 Oct 2023 11:04:18 +0200
Subject: [PATCH 07/14] Change bullet parameters

---
 leo_description/urdf/macros.xacro | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index e1a45a5..75fb9fa 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -249,8 +249,8 @@
               <mu2>0.3</mu2>
             </ode>
             <bullet>
-              <friction>1</friction>
-              <friction2>1</friction2>
+              <friction>0.3</friction>
+              <friction2>0.3</friction2>
               <rolling_friction>0.1</rolling_friction>
             </bullet>
           </friction>
@@ -267,8 +267,8 @@
               <mu2>0.3</mu2>
             </ode>
             <bullet>
-              <friction>1</friction>
-              <friction2>1</friction2>
+              <friction>0.3</friction>
+              <friction2>0.3</friction2>
               <rolling_friction>0.1</rolling_friction>
             </bullet>
           </friction>
@@ -286,8 +286,8 @@
               <mu2>0.3</mu2>
             </ode>
             <bullet>
-              <friction>1</friction>
-              <friction2>1</friction2>
+              <friction>0.3</friction>
+              <friction2>0.3</friction2>
               <rolling_friction>0.1</rolling_friction>
             </bullet>
           </friction>
@@ -304,8 +304,8 @@
               <mu2>0.3</mu2>
             </ode>
             <bullet>
-              <friction>1</friction>
-              <friction2>1</friction2>
+              <friction>0.3</friction>
+              <friction2>0.3</friction2>
               <rolling_friction>0.1</rolling_friction>
             </bullet>
           </friction>

From 289bc18b91cfc523b25ad34e348c1867f39a9aee Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 17 Oct 2023 11:26:58 +0200
Subject: [PATCH 08/14] Update diff system plugin name

---
 leo_description/urdf/macros.xacro | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 75fb9fa..d7e8638 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -427,7 +427,7 @@
 
     <!-- rocker differential -->
     <gazebo>
-      <plugin name="leo_gz::DifferentialSystem" filename="libleo_gazebo_differential_plugin.so">
+      <plugin name="leo_gz::DifferentialSystem" filename="libleo_gz_differential_plugin.so">
         <jointA>rocker_L_joint</jointA>
         <jointB>rocker_R_joint</jointB>
         <forceConstant>100.0</forceConstant>

From 51449746a00a8f6da27b0a2ea8ea97614d383ef2 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 17 Oct 2023 12:06:48 +0200
Subject: [PATCH 09/14] Update enviormental hooks

---
 leo_description/CMakeLists.txt                 | 3 +--
 leo_description/hooks/gz_sim_resource_path.dsv | 1 +
 leo_description/hooks/leo_description.dsv.in   | 1 -
 leo_description/hooks/leo_description.sh.in    | 1 -
 4 files changed, 2 insertions(+), 4 deletions(-)
 create mode 100644 leo_description/hooks/gz_sim_resource_path.dsv
 delete mode 100644 leo_description/hooks/leo_description.dsv.in
 delete mode 100644 leo_description/hooks/leo_description.sh.in

diff --git a/leo_description/CMakeLists.txt b/leo_description/CMakeLists.txt
index cab1b84..31ea79f 100644
--- a/leo_description/CMakeLists.txt
+++ b/leo_description/CMakeLists.txt
@@ -12,7 +12,6 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
-ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
-ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/gz_sim_resource_path.dsv")
 
 ament_package()
diff --git a/leo_description/hooks/gz_sim_resource_path.dsv b/leo_description/hooks/gz_sim_resource_path.dsv
new file mode 100644
index 0000000..4426101
--- /dev/null
+++ b/leo_description/hooks/gz_sim_resource_path.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share;
\ No newline at end of file
diff --git a/leo_description/hooks/leo_description.dsv.in b/leo_description/hooks/leo_description.dsv.in
deleted file mode 100644
index 2a38ccc..0000000
--- a/leo_description/hooks/leo_description.dsv.in
+++ /dev/null
@@ -1 +0,0 @@
-prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share;@CMAKE_INSTALL_PREFIX@/share
\ No newline at end of file
diff --git a/leo_description/hooks/leo_description.sh.in b/leo_description/hooks/leo_description.sh.in
deleted file mode 100644
index 33fa6b2..0000000
--- a/leo_description/hooks/leo_description.sh.in
+++ /dev/null
@@ -1 +0,0 @@
-ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/models"
\ No newline at end of file

From fc4bde04925eb619675bfd030a500398787f53d7 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 17 Oct 2023 12:23:40 +0200
Subject: [PATCH 10/14] Remove joint prefix

---
 leo_description/urdf/macros.xacro | 39 ++++++++++++++-----------------
 1 file changed, 18 insertions(+), 21 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index d7e8638..f8cc509 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -5,8 +5,7 @@
                params="default_antenna:=true
                        rockers_fixed:=true
                        footprint_link:=true
-                       link_prefix:=''
-                       joint_prefix:=''
+                       link_prefix:='' 
                        imu_translation:='0.0628 -0.0314 -0.0393'">
 
     <xacro:macro name="rocker_link" params="name">
@@ -131,7 +130,7 @@
     <!-- JOINTS -->
 
     <xacro:if value="${footprint_link}">
-      <joint name="${joint_prefix}base_joint" type="fixed">
+      <joint name="base_joint" type="fixed">
         <origin xyz="0 0 0.19783" rpy="0 0 0"/>
         <parent link="${link_prefix}base_footprint"/>
         <child link="${link_prefix}base_link"/>
@@ -145,7 +144,7 @@
       <xacro:property name="rockers_joint_type" value="revolute"/>
     </xacro:unless>
 
-    <joint name="${joint_prefix}rocker_L_joint" type="${rockers_joint_type}">
+    <joint name="rocker_L_joint" type="${rockers_joint_type}">
       <origin xyz="0.00263 0.14167 -0.04731" rpy="0 0 ${pi}"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}rocker_L_link"/>
@@ -154,7 +153,7 @@
       <dynamics friction="1.0" damping="0.1"/>
     </joint>
 
-    <joint name="${joint_prefix}rocker_R_joint" type="${rockers_joint_type}">
+    <joint name="rocker_R_joint" type="${rockers_joint_type}">
       <origin xyz="0.00263 -0.14167 -0.04731" rpy="0 0 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}rocker_R_link"/>
@@ -164,7 +163,7 @@
       <mimic joint="rocker_L_joint"/>
     </joint>
 
-    <joint name="${joint_prefix}wheel_FL_joint" type="continuous">
+    <joint name="wheel_FL_joint" type="continuous">
       <origin xyz="-0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_L_link"/>
       <child link="${link_prefix}wheel_FL_link"/>
@@ -173,7 +172,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="${joint_prefix}wheel_RL_joint" type="continuous">
+    <joint name="wheel_RL_joint" type="continuous">
       <origin xyz="0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_L_link"/>
       <child link="${link_prefix}wheel_RL_link"/>
@@ -182,7 +181,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="${joint_prefix}wheel_FR_joint" type="continuous">
+    <joint name="wheel_FR_joint" type="continuous">
       <origin xyz="0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_R_link"/>
       <child link="${link_prefix}wheel_FR_link"/>
@@ -191,7 +190,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="${joint_prefix}wheel_RR_joint" type="continuous">
+    <joint name="wheel_RR_joint" type="continuous">
       <origin xyz="-0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_R_link"/>
       <child link="${link_prefix}wheel_RR_link"/>
@@ -201,26 +200,26 @@
     </joint>
 
     <xacro:if value="${default_antenna}">
-      <joint name="${joint_prefix}antenna_joint" type="fixed">
+      <joint name="antenna_joint" type="fixed">
         <origin xyz="-0.0052 0.056 -0.0065" rpy="0 0 0"/>
         <parent link="${link_prefix}base_link"/>
         <child link="${link_prefix}antenna_link"/>
       </joint>
     </xacro:if>
 
-    <joint name="${joint_prefix}camera_joint" type="fixed">
+    <joint name="camera_joint" type="fixed">
       <origin xyz="0.0971 0 -0.0427" rpy="0 0.2094 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}camera_frame"/>
     </joint>
 
-    <joint name="${joint_prefix}camera_optical_joint" type="fixed">
+    <joint name="camera_optical_joint" type="fixed">
       <origin xyz="0 0 0" rpy="-${pi/2} 0.0 -${pi/2}"/>
       <parent link="${link_prefix}camera_frame"/>
       <child link="${link_prefix}camera_optical_frame"/>
     </joint>
 
-    <joint name="${joint_prefix}imu_joint" type="fixed">
+    <joint name="imu_joint" type="fixed">
       <origin xyz="${imu_translation}" rpy="0 0 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}imu_frame"/>
@@ -456,10 +455,10 @@
       <plugin
         filename="libignition-gazebo-diff-drive-system.so"
         name="ignition::gazebo::systems::DiffDrive">
-        <left_joint>${joint_prefix}wheel_FL_joint</left_joint>
-        <right_joint>${joint_prefix}wheel_FR_joint</right_joint>
-        <left_joint>${joint_prefix}wheel_RL_joint</left_joint>
-        <right_joint>${joint_prefix}wheel_RR_joint</right_joint>
+        <left_joint>wheel_FL_joint</left_joint>
+        <right_joint>wheel_FR_joint</right_joint>
+        <left_joint>wheel_RL_joint</left_joint>
+        <right_joint>wheel_RR_joint</right_joint>
         <wheel_separation>0.358</wheel_separation>
         <wheel_radius>0.0625</wheel_radius>
         <frame_id>odom</frame_id>
@@ -490,11 +489,9 @@
 
     <xacro:leo default_antenna="${default_antenna}"
                rockers_fixed="false"
-               link_prefix="${link_prefix}"
-               joint_prefix="${joint_prefix}"/>
+               link_prefix="${link_prefix}"/>
 
-    <xacro:leo_gazebo robot_ns="${robot_ns}"
-    joint_prefix="${joint_prefix}"/>
+    <xacro:leo_gazebo robot_ns="${robot_ns}"/>
 
     <xacro:if value="${fixed}">
       <link name="world"/>

From b6efd5050cb4920106495a7f1d7148f6a867866e Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 17 Oct 2023 12:52:29 +0200
Subject: [PATCH 11/14] Add imu system plugin

---
 leo_description/urdf/macros.xacro | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index f8cc509..c2c7dd2 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -439,6 +439,9 @@
         name="ignition::gazebo::systems::JointStatePublisher">
         <topic>${link_prefix}joint_states</topic>
       </plugin>
+      <plugin filename="libignition-gazebo-imu-system.so"
+        name="ignition::gazebo::systems::Imu">
+      </plugin>
     </gazebo>
 
     <gazebo reference="${link_prefix}imu_frame">

From 45b1dd585745c39976bf4bb7a2105b9a62ca3006 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Tue, 17 Oct 2023 15:48:43 +0200
Subject: [PATCH 12/14] Bring back joint_prefix param

---
 leo_description/urdf/macros.xacro | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index c2c7dd2..c9583d9 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -5,7 +5,8 @@
                params="default_antenna:=true
                        rockers_fixed:=true
                        footprint_link:=true
-                       link_prefix:='' 
+                       link_prefix:=''
+                       joint_prefix:='' 
                        imu_translation:='0.0628 -0.0314 -0.0393'">
 
     <xacro:macro name="rocker_link" params="name">

From 58cf2c57c0c435dd31be4a084896b5d1d4d0714c Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Wed, 18 Oct 2023 12:29:44 +0200
Subject: [PATCH 13/14] Add joints prefix param to all joints

---
 leo_description/urdf/macros.xacro | 29 +++++++++++++----------------
 1 file changed, 13 insertions(+), 16 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index c9583d9..2b44294 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -6,7 +6,7 @@
                        rockers_fixed:=true
                        footprint_link:=true
                        link_prefix:=''
-                       joint_prefix:='' 
+                       joint_prefix:=''
                        imu_translation:='0.0628 -0.0314 -0.0393'">
 
     <xacro:macro name="rocker_link" params="name">
@@ -131,7 +131,7 @@
     <!-- JOINTS -->
 
     <xacro:if value="${footprint_link}">
-      <joint name="base_joint" type="fixed">
+      <joint name="${joint_prefix}base_joint" type="fixed">
         <origin xyz="0 0 0.19783" rpy="0 0 0"/>
         <parent link="${link_prefix}base_footprint"/>
         <child link="${link_prefix}base_link"/>
@@ -145,7 +145,7 @@
       <xacro:property name="rockers_joint_type" value="revolute"/>
     </xacro:unless>
 
-    <joint name="rocker_L_joint" type="${rockers_joint_type}">
+    <joint name="${joint_prefix}rocker_L_joint" type="${rockers_joint_type}">
       <origin xyz="0.00263 0.14167 -0.04731" rpy="0 0 ${pi}"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}rocker_L_link"/>
@@ -154,7 +154,7 @@
       <dynamics friction="1.0" damping="0.1"/>
     </joint>
 
-    <joint name="rocker_R_joint" type="${rockers_joint_type}">
+    <joint name="${joint_prefix}rocker_R_joint" type="${rockers_joint_type}">
       <origin xyz="0.00263 -0.14167 -0.04731" rpy="0 0 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}rocker_R_link"/>
@@ -164,7 +164,7 @@
       <mimic joint="rocker_L_joint"/>
     </joint>
 
-    <joint name="wheel_FL_joint" type="continuous">
+    <joint name="${joint_prefix}wheel_FL_joint" type="continuous">
       <origin xyz="-0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_L_link"/>
       <child link="${link_prefix}wheel_FL_link"/>
@@ -173,7 +173,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="wheel_RL_joint" type="continuous">
+    <joint name="${joint_prefix}wheel_RL_joint" type="continuous">
       <origin xyz="0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_L_link"/>
       <child link="${link_prefix}wheel_RL_link"/>
@@ -182,7 +182,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="wheel_FR_joint" type="continuous">
+    <joint name="${joint_prefix}wheel_FR_joint" type="continuous">
       <origin xyz="0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_R_link"/>
       <child link="${link_prefix}wheel_FR_link"/>
@@ -191,7 +191,7 @@
       <dynamics friction="0.3125" damping="0.1"/>
     </joint>
 
-    <joint name="wheel_RR_joint" type="continuous">
+    <joint name="${joint_prefix}wheel_RR_joint" type="continuous">
       <origin xyz="-0.15256 -0.08214 -0.08802" rpy="0 0 0"/>
       <parent link="${link_prefix}rocker_R_link"/>
       <child link="${link_prefix}wheel_RR_link"/>
@@ -201,26 +201,26 @@
     </joint>
 
     <xacro:if value="${default_antenna}">
-      <joint name="antenna_joint" type="fixed">
+      <joint name="${joint_prefix}antenna_joint" type="fixed">
         <origin xyz="-0.0052 0.056 -0.0065" rpy="0 0 0"/>
         <parent link="${link_prefix}base_link"/>
         <child link="${link_prefix}antenna_link"/>
       </joint>
     </xacro:if>
 
-    <joint name="camera_joint" type="fixed">
+    <joint name="${joint_prefix}camera_joint" type="fixed">
       <origin xyz="0.0971 0 -0.0427" rpy="0 0.2094 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}camera_frame"/>
     </joint>
 
-    <joint name="camera_optical_joint" type="fixed">
+    <joint name="${joint_prefix}camera_optical_joint" type="fixed">
       <origin xyz="0 0 0" rpy="-${pi/2} 0.0 -${pi/2}"/>
       <parent link="${link_prefix}camera_frame"/>
       <child link="${link_prefix}camera_optical_frame"/>
     </joint>
 
-    <joint name="imu_joint" type="fixed">
+    <joint name="${joint_prefix}imu_joint" type="fixed">
       <origin xyz="${imu_translation}" rpy="0 0 0"/>
       <parent link="${link_prefix}base_link"/>
       <child link="${link_prefix}imu_frame"/>
@@ -231,8 +231,7 @@
   <!-- LEO GAZEBO -->
 
   <xacro:macro name="leo_gazebo"
-               params="robot_ns:=''
-               joint_prefix:=''">
+               params="robot_ns:='''">
 
     <xacro:property name="link_prefix" value=""/>
     <xacro:if value="${robot_ns != '' and robot_ns != '/'}">
@@ -485,10 +484,8 @@
                        robot_ns:=''">
 
     <xacro:property name="link_prefix" value=""/>
-    <xacro:property name="joint_prefix" value=""/>
     <xacro:if value="${robot_ns != '' and robot_ns != '/'}">
       <xacro:property name="link_prefix" value="${robot_ns}/"/>
-      <xacro:property name="joint_prefix" value="${robot_ns}/"/>
     </xacro:if>
 
     <xacro:leo default_antenna="${default_antenna}"

From c2166229b85f4d85b8dafe448e767ded93b526a8 Mon Sep 17 00:00:00 2001
From: Jan Hernas <hernasj@wp.pl>
Date: Wed, 18 Oct 2023 13:02:56 +0200
Subject: [PATCH 14/14] Remove unnecessary link prefixes

---
 leo_description/urdf/macros.xacro | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/leo_description/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 2b44294..9ac093e 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -390,8 +390,8 @@
 
     <!-- camera -->
     <gazebo reference="${link_prefix}camera_frame">
-      <sensor name="${link_prefix}camera" type="camera">
-          <camera name="${link_prefix}leo_camera">
+      <sensor name="camera" type="camera">
+          <camera name="leo_camera">
               <horizontal_fov>1.9</horizontal_fov>
               <image>
                   <width>640</width>
@@ -445,7 +445,7 @@
     </gazebo>
 
     <gazebo reference="${link_prefix}imu_frame">
-      <sensor type="imu" name="${link_prefix}leo_imu_sensor">
+      <sensor type="imu" name="leo_imu_sensor">
         <update_rate>100</update_rate>
         <visualize>true</visualize>
         <topic>${link_prefix}imu/data_raw</topic>