From 95f19f3e6539e0346e3fc87a271938c7aa13ab9b Mon Sep 17 00:00:00 2001
From: Jan Hernas <40989249+jasiex01@users.noreply.github.com>
Date: Wed, 18 Oct 2023 13:10:59 +0200
Subject: [PATCH] Adjust description package to work in new Gazebo simulation
 (#5)

* Add enviormental variable hooks

* Adjust simulation xacro for new simulation

* Macros.xacro cleanup

* Addd surface element to wheels

* Add custom topic name for joint state plugin

* Fix gazebo launch warnings

* Change bullet parameters

* Update diff system plugin name

* Update enviormental hooks

* Remove joint prefix

* Add imu system plugin

* Bring back joint_prefix param

* Add joints prefix param to all joints

* Remove unnecessary link prefixes
---
 leo_description/CMakeLists.txt                |   2 +
 .../hooks/gz_sim_resource_path.dsv            |   1 +
 leo_description/urdf/macros.xacro             | 333 ++++++++++--------
 3 files changed, 195 insertions(+), 141 deletions(-)
 create mode 100644 leo_description/hooks/gz_sim_resource_path.dsv

diff --git a/leo_description/CMakeLists.txt b/leo_description/CMakeLists.txt
index 0861db0..31ea79f 100644
--- a/leo_description/CMakeLists.txt
+++ b/leo_description/CMakeLists.txt
@@ -12,4 +12,6 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
+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/urdf/macros.xacro b/leo_description/urdf/macros.xacro
index 61e0d27..9ac093e 100644
--- a/leo_description/urdf/macros.xacro
+++ b/leo_description/urdf/macros.xacro
@@ -231,7 +231,7 @@
   <!-- LEO GAZEBO -->
 
   <xacro:macro name="leo_gazebo"
-               params="robot_ns:=''">
+               params="robot_ns:='''">
 
     <xacro:property name="link_prefix" value=""/>
     <xacro:if value="${robot_ns != '' and robot_ns != '/'}">
@@ -240,191 +240,242 @@
 
     <!-- base ODE properties -->
     <gazebo reference="${link_prefix}base_footprint">
-      <kp>1e6</kp>
-      <kd>1.0</kd>
-      <mu1>0.3</mu1>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>0.3</friction>
+              <friction2>0.3</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>
-      <mu1>0.3</mu1>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>0.3</friction>
+              <friction2>0.3</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>
-      <mu1>0.3</mu1>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>0.3</friction>
+              <friction2>0.3</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>
-      <mu1>0.3</mu1>
-      <mu2>0.3</mu2>
-      <minDepth>0.003</minDepth>
+      <collision>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.3</mu>
+              <mu2>0.3</mu2>
+            </ode>
+            <bullet>
+              <friction>0.3</friction>
+              <friction2>0.3</friction2>
+              <rolling_friction>0.1</rolling_friction>
+            </bullet>
+          </friction>
+        </surface>
+      </collision>
     </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>
+      <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">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <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">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <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">
-      <kp>1e6</kp>
-      <kd>100.0</kd>
-      <mu1>3.0</mu1>
-      <mu2>1.0</mu2>
-      <fdir1>1 0 0</fdir1>
-      <minDepth>0.003</minDepth>
+      <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 -->
     <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>${link_prefix}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_gz_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">
+        <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">
       <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>${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
+        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>${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>
     </gazebo>
-
   </xacro:macro>
 
   <xacro:macro name="leo_sim"