Merge pull request #31 from ctu-mrs/ft_ros2_ground_truth

[ROS2] Ground truth plugin + Refactoring

Author: stibipet

config/uav_ros_gz_bridge_config.yaml.jinja

@@ -22,6 +22,14 @@
   direction: GZ_TO_ROS
 {% endfor %}
 
+{%- for topic in odometry_topic_list -%}
+- ros_topic_name: "{{ topic["ros"] }}"
+  gz_topic_name: "{{ topic["gazebo"] }}"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
+{% endfor %}
+
 {%- for topic in imu_topic_list -%}
 - ros_topic_name: "{{ topic["ros"] }}"
   gz_topic_name: "{{ topic["gazebo"] }}"

models/mrs_robots_description/sdf/README.md

@@ -350,6 +350,8 @@ gz topic -i -t topic_name
 - if there is `no subscriber` the topic is not being subscribed by `ros_gz_bridge`
 Both issues are most likely caused by incorrect topic names. Verify that the topic names in your sensor’s SDF file do not violate the rules described above, and check the autogenerated `ros_gz_bridge` config file located in the `/tmp` directory.
 
+For more information about which message types can be bridged, we recommend the following [documentation](https://docs.ros.org/en/rolling/p/ros_gz_bridge/).
+
 #### Tests
 There are unit tests that can partially verify the Jinja syntax within macros.
 Be aware that these tests only ensure the Jinja templates can be rendered successfully. They do not check whether the resulting SDF is valid. Additionally, there is a `test_camera_topic_names` test that verifies the naming conventions for all types of camera sensors.

models/mrs_robots_description/sdf/components/component_snippets.sdf.jinja

@@ -43,26 +43,30 @@ limitations under the License.
 
   {%- set spawner_keyword = 'enable-ground-truth' -%}
   {%- set spawner_description = 'Enable ROS topic with ground truth odometry published under model namespace' -%}
-  {%- set spawner_default_args = {'topic_name': 'ground_truth', 'frame_name': 'world', 'update_rate': 150} -%}
+  {%- set spawner_default_args = {'topic_name': 'ground_truth', 'frame_name': 'world', 'update_rate': 150, 'noise' : 0} -%}
 
   {%- if spawner_keyword in spawner_args.keys() -%}
     {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }}
-
-    <!-- Ground truth {-->
-    {{ generic.odometry_sensor_macro(
-      odometry_sensor_name = 'ground_truth',
-      parent_link = parent_link,
-      topic_name = spawner_args[spawner_keyword]['topic_name'],
-      noise = 0,
-      frame_name = spawner_args[spawner_keyword]['frame_name'],
-      update_rate = spawner_args[spawner_keyword]['update_rate'],
-      x = x,
-      y = y,
-      z = z,
-      roll = roll,
-      pitch = pitch,
-      yaw = yaw)
-    }}
+    
+ 
+    {%- set world_frame_name = spawner_args[spawner_keyword]['frame_name'] -%}
+    {%- set robot_base_frame = parent_link -%}
+    {%- set topic_name = spawner_args['name'] + '/' + spawner_args[spawner_keyword]['topic_name']-%}
+    {%- set update_rate = spawner_args[spawner_keyword]['update_rate'] -%}
+    {%- set noise = spawner_args[spawner_keyword]['noise'] -%}
+
+    <plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+      <odom_frame>{{ world_frame_name }}</odom_frame>
+      <robot_base_frame>{{ robot_base_frame }}</robot_base_frame>
+      <odom_publish_frequency>{{ update_rate }}</odom_publish_frequency>
+      <odom_topic>{{ topic_name }}</odom_topic>
+      <odom_covariance_topic>{{ topic_name }}_with_covariance</odom_covariance_topic> 
+      <tf_topic>{{ topic_name }}/pose</tf_topic>
+      <dimensions>3</dimensions>
+      <xyz_offset>{{ x }} {{ y }} {{ z }}</xyz_offset>
+      <rpy_offset>{{ roll }} {{ pitch }} {{ yaw }}</rpy_offset>
+      <gaussian_noise>{{ noise }}</gaussian_noise>
+    </plugin>
     <!--}-->
 
   {%- endif -%}

models/mrs_robots_description/sdf/components/generic_components.sdf.jinja

@@ -633,21 +633,42 @@ limitations under the License.
 {%- endmacro -%}
 {# <!--}--> #}
 
+{# odometry_sensor_macro {--> #}
+{%- macro odometry_sensor_macro(odometry_sensor_name, topic_name, 
+                                noise, frame_name, update_rate, 
+                                x, y, z, roll, pitch, yaw
+                                ) -%}
+  <plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+    <odom_frame>{{ frame_name }}</odom_frame>
+    <robot_base_frame>{{ odometry_sensor_name }}</robot_base_frame>
+    <odom_publish_frequency>{{ update_rate }}</odom_publish_frequency>
+    <odom_topic>{{ topic_name }}</odom_topic>
+    <odom_covariance_topic>{{ topic_name }}_with_covariance</odom_covariance_topic> 
+    <tf_topic>{{ odometry_sensor_name }}/pose</tf_topic>
+    <dimensions>3</dimensions>
+    <xyz_offset>{{ x }} {{ y }} {{ z }}</xyz_offset>
+    <rpy_offset>{{ roll }} {{ pitch }} {{ yaw }}</rpy_offset>
+    <gaussian_noise>{{ noise }}</gaussian_noise>
+  </plugin>
+{%- endmacro -%}
+{# <!--}--> #}
+
 {# imu_sensor_macro {--> #}
 {%- macro imu_sensor_macro(name, update_rate, 
-                          imu_sdf_topic_name, imu_gz_topic_name, imu_ros_topic_name,
+                          imu_sdf_topic_name, imu_gz_frame_id, imu_gz_topic_name, imu_ros_topic_name,
                           sensor_x, sensor_y, sensor_z, sensor_roll, sensor_pitch, sensor_yaw,
                           angular_velocity_noise_mean_x, angular_velocity_noise_std_x, 
                           angular_velocity_noise_mean_y, angular_velocity_noise_std_y, 
                           angular_velocity_noise_mean_z, angular_velocity_noise_std_z, 
                           linear_acceleration_noise_mean_x, linear_acceleration_noise_std_x, 
                           linear_acceleration_noise_mean_y, linear_acceleration_noise_std_y, 
                           linear_acceleration_noise_mean_z, linear_acceleration_noise_std_z) -%}
-<sensor name="{{ name }}/imu" type="imu">
+<sensor name="{{ imu_gz_frame_id }}" type="imu">
   <pose>{{ sensor_x }} {{ sensor_y }} {{ sensor_z }} {{ sensor_roll }} {{ sensor_pitch }} {{ sensor_yaw }}</pose>
   <always_on>1</always_on>
   <update_rate>{{ update_rate }}</update_rate>
   <topic>{{ imu_sdf_topic_name }}</topic>
+  <gz_frame_id>{{ imu_gz_frame_id }}</gz_frame_id>
   <imu_gz_topic>{{ imu_gz_topic_name }}</imu_gz_topic>
   <imu_ros_topic>{{ imu_ros_topic_name }}</imu_ros_topic>
   <imu>

models/mrs_robots_description/sdf/components/lidar/ouster/ouster.sdf.jinja

@@ -16,6 +16,7 @@
     {%- set ouster_model = spawner_args[spawner_keyword]['model'] -%}
     {%- set sensor_link_name = sensor_name -%}
     {%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
+    {%- set imu_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/imu' -%}
 
     {# OS0 {--> #}
 
@@ -186,6 +187,7 @@
       {{ generic.imu_sensor_macro(
         name = sensor_name,
         update_rate = 250,
+        imu_gz_frame_id = imu_gz_frame_id,
         imu_sdf_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',
         imu_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',
         imu_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',

models/mrs_robots_description/sdf/components/lidar/scanse_sweep/scanse_sweep.sdf.jinja

@@ -14,7 +14,7 @@
     <!-- scanse sweep {-->
     {%- set sensor_name = 'scanse_sweep' -%}
     {%- set sensor_link_name = sensor_name -%}
-    {%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
+    {%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
 
     {# -- tf from link to sensor -- #}
     {%- set lidar_x = 0.0 -%}

models/mrs_robots_description/sdf/components/lidar/velodyne/velodyne.sdf.jinja

@@ -29,7 +29,7 @@
     <!-- Velodyne {-->
     {%- set sensor_name = 'velodyne' -%}
     {%- set sensor_link_name = sensor_name -%}
-    {%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
+    {%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
 
     <link name="{{ sensor_link_name }}">
       <pose relative_to="{{ parent_link }}">{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>

models/mrs_robots_description/sdf/drones/f330.sdf.jinja

@@ -540,6 +540,21 @@
     {# ||                 optional sensor definitions                  || #}
     {# ================================================================== #}
 
+    {# Ground truth {--> #}
+    {{ components.ground_truth_macro(
+      parent_link = root,
+      x = 0,
+      y = 0,
+      z = 0,
+      roll = 0,
+      pitch = 0,
+      yaw = 0,
+      spawner_args = spawner_args)
+    }}
+    {# <!--}--> #}
+
+    {# ======================= rangefinder sensors ====================== #}
+
     {# Garmin down {--> #}
     {{ components.garmin_down_macro(
       parent_link = root,

models/mrs_robots_description/sdf/drones/f450.sdf.jinja

@@ -662,6 +662,19 @@
     {# ||                 optional sensor definitions                  || #}
     {# ================================================================== #}
 
+    {# Ground truth {--> #}
+    {{ components.ground_truth_macro(
+      parent_link = root,
+      x = 0,
+      y = 0,
+      z = 0,
+      roll = 0,
+      pitch = 0,
+      yaw = 0,
+      spawner_args = spawner_args)
+    }}
+    {# <!--}--> #}
+
     {# ======================= rangefinder sensors ====================== #}
 
     {# Garmin down {--> #}

models/mrs_robots_description/sdf/drones/f550.sdf.jinja

@@ -710,6 +710,19 @@
     {# ||                  optional sensor definitions                 || #}
     {# ================================================================== #}
 
+    {# Ground truth {--> #}
+    {{ components.ground_truth_macro(
+      parent_link = root,
+      x = 0,
+      y = 0,
+      z = 0,
+      roll = 0,
+      pitch = 0,
+      yaw = 0,
+      spawner_args = spawner_args)
+    }}
+    {# <!--}--> #}
+
     {# ======================= rangefinder sensors ====================== #}
 
     {# Garmin down {--> #}