From 789df0269a1e2266fd8bd13ea00314c809dfa6ed Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Mon, 18 Mar 2024 20:58:12 +0100
Subject: [PATCH] added static_tf plugin to vio

---
 .../sdf/component_snippets.sdf.jinja          | 22 ++++++++++++++++---
 .../sdf/generic_components.sdf.jinja          | 10 +++++++++
 2 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
index b5f591b..8d1ee50 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
@@ -2395,10 +2395,10 @@ limitations under the License.
     <!-- imu {-->
     {{ generic.imu_sensor_macro(
       sensor_name = sensor_name + '_imu',
-      parent_link = parent_link,
+      parent_link = sensor_name + '_link',
       update_rate = spawner_args[spawner_keyword]['imu_rate'],
       topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu',
-      frame_name = spawner_args['name'] + '/' + sensor_name,
+      frame_name = spawner_args['name'] + '/' + sensor_name + '_imu',
       noise_mean = spawner_args[spawner_keyword]['imu_noise'],
       x = 0,
       y = 0,
@@ -2409,6 +2409,14 @@ limitations under the License.
     }}
     <!--}-->
 
+    <!-- static tf {-->
+  {{ generic.tf_static_macro(
+     namespace = spawner_args['name'] + '/',
+     parent_link = sensor_name + "_link",
+     child_link = sensor_name + "_imu_link")
+  }}
+  {# <!--}--> #}
+
     <!-- mount {-->
     {{ mount if mount }}
     <!--}-->
@@ -2476,7 +2484,7 @@ limitations under the License.
     <!-- imu {-->
     {{ generic.imu_sensor_macro(
       sensor_name = sensor_name + '_imu',
-      parent_link = parent_link,
+      parent_link = sensor_name,
       update_rate = spawner_args[spawner_keyword]['imu_rate'],
       topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu',
       frame_name = spawner_args['name'] + '/' + sensor_name,
@@ -2490,6 +2498,14 @@ limitations under the License.
     }}
     <!--}-->
 
+    <!-- static tf {-->
+    {{ generic.tf_static_macro(
+       namespace = spawner_args['name'] + '/',
+       parent_link = sensor_name + "_link",
+       child_link = sensor_name + "_imu_link")
+    }}
+    {# <!--}--> #}
+
     <!-- mount {-->
     {{ mount if mount }}
     <!--}-->
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
index 0b4c2e7..63f8814 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
@@ -789,6 +789,16 @@ limitations under the License.
 {%- endmacro -%}
 {# <!--}--} #}
 
+{# tf static publisher macro {--> #}
+{%- macro tf_static_macro(namespace, parent_link, child_link) -%}
+<plugin name="{{ parent_link }}_to_{{ child_link }}_tf_plugin" filename="libMrsGazeboCommonResources_LinkStaticTFPublisher.so">
+  <namespace>{{ namespace }}</namespace>
+  <parentLink>{{ parent_link }}</parentLink>
+  <childLink>{{ child_link }}</childLink>
+</plugin>
+{%- endmacro -%}
+{# <!--}--> #}
+
 {# ========================== gazebo plugins ========================= #}
 
 {# gazebo_gps_macro {--> #}