From d0c72e08b0f02c2e9ac705e3382ee19a45e73ab7 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 6 Dec 2024 09:35:49 +0100
Subject: [PATCH] Fix simulation

---
 .../config/WH01_controller.yaml                   |  2 +-
 .../config/WH02_controller.yaml                   |  2 +-
 .../config/WH04_controller.yaml                   |  2 +-
 .../config/WH05_controller.yaml                   |  2 +-
 .../urdf/lynx/lynx_macro.urdf.xacro               | 15 +++++----------
 .../urdf/panther/body.urdf.xacro                  |  2 +-
 .../launch/simulate_robot.launch.py               |  5 ++++-
 .../config/enu_localization.yaml                  |  2 +-
 .../config/enu_localization_with_gps.yaml         |  2 +-
 .../config/relative_localization.yaml             |  2 +-
 .../config/relative_localization_with_gps.yaml    |  2 +-
 11 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/husarion_ugv_controller/config/WH01_controller.yaml b/husarion_ugv_controller/config/WH01_controller.yaml
index 4187aa50..26738b0b 100644
--- a/husarion_ugv_controller/config/WH01_controller.yaml
+++ b/husarion_ugv_controller/config/WH01_controller.yaml
@@ -41,7 +41,7 @@
 
       publish_rate: 100.0
       odom_frame_id: odom
-      base_frame_id: base_link
+      base_frame_id: base_footprint
       twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Values measured experimentally
 
       # Whether to use feedback or commands for odometry calculations
diff --git a/husarion_ugv_controller/config/WH02_controller.yaml b/husarion_ugv_controller/config/WH02_controller.yaml
index 6d109509..efadc514 100644
--- a/husarion_ugv_controller/config/WH02_controller.yaml
+++ b/husarion_ugv_controller/config/WH02_controller.yaml
@@ -42,7 +42,7 @@
 
       publish_rate: 100.0
       odom_frame_id: odom
-      base_frame_id: base_link
+      base_frame_id: base_footprint
       twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
diff --git a/husarion_ugv_controller/config/WH04_controller.yaml b/husarion_ugv_controller/config/WH04_controller.yaml
index b13bbd11..94cfe52d 100644
--- a/husarion_ugv_controller/config/WH04_controller.yaml
+++ b/husarion_ugv_controller/config/WH04_controller.yaml
@@ -42,7 +42,7 @@
 
       publish_rate: 100.0
       odom_frame_id: odom
-      base_frame_id: base_link
+      base_frame_id: base_footprint
       twist_covariance_diagonal: [2.7e-5, 2.7e-5, 0.0, 0.0, 0.0, 1.0e-4] # Selected intuitively based on WH01 results
 
       # Whether to use feedback or commands for odometry calculations
diff --git a/husarion_ugv_controller/config/WH05_controller.yaml b/husarion_ugv_controller/config/WH05_controller.yaml
index 7499d0fb..5d9f891c 100644
--- a/husarion_ugv_controller/config/WH05_controller.yaml
+++ b/husarion_ugv_controller/config/WH05_controller.yaml
@@ -41,7 +41,7 @@
 
       publish_rate: 100.0
       odom_frame_id: odom
-      base_frame_id: base_link
+      base_frame_id: base_footprint
       twist_covariance_diagonal: [5.4e-5, 5.4e-5, 0.0, 0.0, 0.0, 1.9e-4] # Values measured experimentally
 
       # Whether to use feedback or commands for odometry calculations
diff --git a/husarion_ugv_description/urdf/lynx/lynx_macro.urdf.xacro b/husarion_ugv_description/urdf/lynx/lynx_macro.urdf.xacro
index 4bff5bf8..c126d80c 100644
--- a/husarion_ugv_description/urdf/lynx/lynx_macro.urdf.xacro
+++ b/husarion_ugv_description/urdf/lynx/lynx_macro.urdf.xacro
@@ -11,15 +11,9 @@
             imu_rpy:='0.0 0.0 -1.57'
             namespace:=''">
 
-    <xacro:if value="${namespace == ''}">
-      <xacro:property name="ns" value="" />
-    </xacro:if>
-    <xacro:unless value="${namespace == ''}">
-        <xacro:property name="ns" value="${namespace}/" />
-    </xacro:unless>
+    <xacro:property name="ns" value='${namespace + "/" if namespace else ""}' />
 
     <xacro:property name="wheel_config" value="${xacro.load_yaml(wheel_config_file)}" />
-    <!-- TODO find real value -->
     <xacro:property name="wheel_separation_x" value="0.35" />
 
     <!-- INCLUDE ROBOT PARTS DEFINITIONS -->
@@ -61,7 +55,7 @@
           <param name="e_stop_initial_state">true</param>
         </xacro:if>
 
-        <xacro:unless value="$(arg use_sim)">
+        <xacro:unless value="${use_sim}">
           <plugin>husarion_ugv_hardware_interfaces/LynxSystem</plugin>
 
           <param name="encoder_resolution">1600</param>
@@ -182,8 +176,9 @@
       </xacro:if>
       <xacro:gazebo.controller config_file="${controller_config_file}" namespace="${namespace}" />
       <xacro:gazebo.imu reference_frame="imu_link" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" namespace="${namespace}" />
-      <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" namespace="${namespace}" />
+      <!-- TODO Add lights visualization -->
+      <!-- <xacro:gazebo.light reference_frame="lights_channel_1_link" name="lights_channel_1" topic="lights/channel_1_frame" namespace="${namespace}" /> -->
+      <!-- <xacro:gazebo.light reference_frame="lights_channel_2_link" name="lights_channel_2" topic="lights/channel_2_frame" namespace="${namespace}" /> -->
     </xacro:if>
   </xacro:macro>
 
diff --git a/husarion_ugv_description/urdf/panther/body.urdf.xacro b/husarion_ugv_description/urdf/panther/body.urdf.xacro
index 4d44ebef..5a05a6be 100644
--- a/husarion_ugv_description/urdf/panther/body.urdf.xacro
+++ b/husarion_ugv_description/urdf/panther/body.urdf.xacro
@@ -30,7 +30,7 @@
       </collision>
 
       <inertial>
-        <!-- TODO: Fix oringin -->
+        <!-- TODO Fix oringin -->
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
         <mass value="43.0" />
         <inertia ixx="2.630036" ixy="-0.023480" ixz="0.052320"
diff --git a/husarion_ugv_gazebo/launch/simulate_robot.launch.py b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
index 0a714908..c15dc4c5 100644
--- a/husarion_ugv_gazebo/launch/simulate_robot.launch.py
+++ b/husarion_ugv_gazebo/launch/simulate_robot.launch.py
@@ -122,6 +122,7 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
+        condition=UnlessCondition(PythonExpression(["'", robot_model, "' == 'lynx'"])),
     )
 
     manager_launch = IncludeLaunchDescription(
@@ -131,7 +132,9 @@ def generate_launch_description():
             )
         ),
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
-        condition=UnlessCondition(disable_manager),
+        condition=UnlessCondition(
+            PythonExpression(["'", robot_model, "' == 'lynx' or ", disable_manager])
+        ),
     )
 
     controller_launch = IncludeLaunchDescription(
diff --git a/husarion_ugv_localization/config/enu_localization.yaml b/husarion_ugv_localization/config/enu_localization.yaml
index 6b2668cc..9cd7071e 100644
--- a/husarion_ugv_localization/config/enu_localization.yaml
+++ b/husarion_ugv_localization/config/enu_localization.yaml
@@ -10,7 +10,7 @@
 
       map_frame: map
       odom_frame: odom
-      base_link_frame: base_link
+      base_link_frame: base_footprint
       world_frame: odom
       publish_tf: true
       publish_acceleration: false
diff --git a/husarion_ugv_localization/config/enu_localization_with_gps.yaml b/husarion_ugv_localization/config/enu_localization_with_gps.yaml
index ef1fa6ff..e9e31763 100644
--- a/husarion_ugv_localization/config/enu_localization_with_gps.yaml
+++ b/husarion_ugv_localization/config/enu_localization_with_gps.yaml
@@ -10,7 +10,7 @@
 
       map_frame: map
       odom_frame: odom
-      base_link_frame: base_link
+      base_link_frame: base_footprint
       world_frame: odom
       publish_tf: true
       publish_acceleration: false
diff --git a/husarion_ugv_localization/config/relative_localization.yaml b/husarion_ugv_localization/config/relative_localization.yaml
index 5f420735..98d4e91f 100644
--- a/husarion_ugv_localization/config/relative_localization.yaml
+++ b/husarion_ugv_localization/config/relative_localization.yaml
@@ -10,7 +10,7 @@
 
       map_frame: map
       odom_frame: odom
-      base_link_frame: base_link
+      base_link_frame: base_footprint
       world_frame: odom
       publish_tf: true
       publish_acceleration: false
diff --git a/husarion_ugv_localization/config/relative_localization_with_gps.yaml b/husarion_ugv_localization/config/relative_localization_with_gps.yaml
index fa842675..444ed7fc 100644
--- a/husarion_ugv_localization/config/relative_localization_with_gps.yaml
+++ b/husarion_ugv_localization/config/relative_localization_with_gps.yaml
@@ -10,7 +10,7 @@
 
       map_frame: map
       odom_frame: odom
-      base_link_frame: base_link
+      base_link_frame: base_footprint
       world_frame: odom
       publish_tf: true
       publish_acceleration: false