From f2d91bedc53ae2826c1a8e6d9e6bca3826abe011 Mon Sep 17 00:00:00 2001
From: stibipet <stibinger@fly4future.com>
Date: Fri, 3 Nov 2023 15:57:48 +0100
Subject: [PATCH] update lidar param definitions

---
 .../config/spawner_params.yaml                |  3 +-
 .../sdf/component_snippets.sdf.jinja          | 48 +++++++++++++------
 .../mrs_robots_description/sdf/f450.sdf.jinja |  5 +-
 .../mrs_robots_description/sdf/f550.sdf.jinja |  5 +-
 .../mrs_robots_description/sdf/naki.sdf.jinja |  3 +-
 .../mrs_robots_description/sdf/t650.sdf.jinja |  5 +-
 .../mrs_robots_description/sdf/x500.sdf.jinja |  3 +-
 7 files changed, 49 insertions(+), 23 deletions(-)

diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
index 14bdd93..3206f51 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
+++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -28,7 +28,8 @@ enable_velodyne: [False, 'Add Velodyne PUCK Lite laser scanner to the vehicle',
 enable_ouster: [False, 'Add Ouster laser scanner to the vehicle', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
 ouster_model: ['OS1-16', 'Choose the Ouster model. Options: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
 use_gpu_ray: [False, 'Laser ray casting will be handled by GPU shaders', [f330, f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
-horizontal_samples: [None, 'Override default base number of horizontal samples for LiDARs', [f450, f550, t650, x500, naki]]
+horizontal_samples: [None, 'Override default number of horizontal samples for LiDARs', [f450, f550, t650, x500, naki]]
+rotation_freq: [None, 'Override default update rate for LiDARs', [f450, f550, t650, x500, naki]]
 enable_light: [False, 'Add light to the vehicle', [f450, f550, t650, x500, naki]]
 enable_servo_camera: [False, 'Add camera on virtual servo to the vehicle', [f450, f550, t650, naki]]
 enable_water_gun: [False, 'Add water gun for fire fighting', [t650]]
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 7d3b374..9eddbfb 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
@@ -1501,12 +1501,20 @@ limitations under the License.
 <!--}-->
 
   <!-- Macro to add a RPlidar A3{-->
-{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
+{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, rotation_freq, x, y, z, roll, pitch, yaw) -%}
+
 {% if horizontal_samples == 'None' %}
-  {%- set samples = 14400 -%}
+  {%- set samples = 1600 -%}
 {% else %}
   {%- set samples = horizontal_samples | int -%}
 {% endif %}
+
+{% if rotation_freq == 'None' %}
+  {%- set update_rate = 20 -%}
+{% else %}
+  {%- set update_rate = rotation_freq | int -%}
+{% endif %}
+
 <link name="rplidar_link">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1542,11 +1550,11 @@ limitations under the License.
   </visual>
   <sensor name='rplidar_sensor' type='ray'>
     <visualize>false</visualize>
-    <update_rate>20</update_rate>
+    <update_rate>{{ update_rate }}</update_rate>
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ samples/20 }}</samples>
+          <samples>{{ samples }}</samples>
           <resolution>1</resolution>
           <min_angle>-3.1241390751</min_angle>
           <max_angle>3.1241390751</max_angle>
@@ -1585,7 +1593,7 @@ limitations under the License.
 <!--}-->
 
 <!-- Macro to add a Velodyne Lidar {-->
-{%- macro velodyne_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
+{%- macro velodyne_macro(namespace, parent_link, sensor_name, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
 
 {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #}
 {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #}
@@ -1618,11 +1626,17 @@ limitations under the License.
 {% endif %}
 
 {% if horizontal_samples == 'None' %}
-  {%- set samples = 291200 -%}
+  {%- set samples = 3600 -%}
 {% else %}
   {%- set samples = horizontal_samples | int -%}
 {% endif %}
 
+{% if rotation_freq == 'None' %}
+  {%- set update_rate = 20 -%}
+{% else %}
+  {%- set update_rate = rotation_freq | int -%}
+{% endif %}
+
 <link name="velodyne_link">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1675,11 +1689,11 @@ limitations under the License.
     <pose>{{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }}</pose>
     {# <pose>0 0 0 0 0 0</pose> #}
     <visualize>false</visualize>
-    <update_rate>{{ rot_freq }}</update_rate>
+    <update_rate>{{ update_rate }}</update_rate>
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ samples/(rot_freq*lasers) }}</samples>
+          <samples>{{ samples }}</samples>
           <resolution>1</resolution>
           <min_angle>{{ -rad180 }}</min_angle>
           <max_angle>{{ rad180 }}</max_angle>
@@ -1733,7 +1747,7 @@ limitations under the License.
 <!-- Macro to add an Ouster Lidar {-->
 
 <!-- Macro to add an Generic Ouster Lidar {-->
-{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
+{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
 
 {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #}
 {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #}
@@ -1777,11 +1791,17 @@ limitations under the License.
 {% endif %}
 
 {% if horizontal_samples == 'None' %}
-  {%- set samples = 20480 -%}
+  {%- set samples = 2048 -%}
 {% else %}
   {%- set samples = horizontal_samples | int -%}
 {% endif %}
 
+{% if rotation_freq == 'None' %}
+  {%- set update_rate = 10 -%}
+{% else %}
+  {%- set update_rate = rotation_freq | int -%}
+{% endif %}
+
 <link name="{{ sensor_link }}">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1852,11 +1872,11 @@ limitations under the License.
     <pose>{{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }}</pose>
     {# <pose>0 0 0 0 0 0</pose> #}
     <visualize>false</visualize>
-    <update_rate>{{ rot_freq }}</update_rate>
+    <update_rate>{{ update_rate }}</update_rate>
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ samples/rot_freq }}</samples>
+          <samples>{{ samples }}</samples>
           <resolution>1</resolution>
           <min_angle>0</min_angle>
           <max_angle>{{ 2*pi }}</max_angle>
@@ -1919,7 +1939,7 @@ limitations under the License.
 {%- endmacro -%}
 <!--}-->
 
-{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_freq, noise, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
+{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rotation_freq, noise, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
 
 <!-- Outster type selection {-->
 {#  default: OS1-16 Generation 1 - is specified in the config file #}
@@ -2010,7 +2030,7 @@ limitations under the License.
   namespace = namespace,
   parent_link = parent_link,
   sensor_name = sensor_name,
-  rot_freq = rot_freq,
+  rotation_freq = rotation_freq,
   lasers = lasers,
   vfov_angle = vfov_angle,
   max_range = range,
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja
index 4d34495..cd5796f 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja
@@ -858,6 +858,7 @@
       namespace = namespace,
       parent_link = root,
       horizontal_samples = horizontal_samples,
+      rotation_freq = rotation_freq,
       x = 0.0,
       y = 0.0,
       z = 0.1,
@@ -877,7 +878,7 @@
       namespace = namespace,
       parent_link = root,
       sensor_name = "velodyne",
-      rot_freq = 20,
+      rotation_freq = rotation_freq,
       lasers = 16,
       vfov_angle = 30,
       max_range = 100,
@@ -903,7 +904,7 @@
       parent_link = root,
       sensor_name = "os",
       ouster_model = ouster_model,
-      rot_freq = 10,
+      rotation_freq = rotation_freq,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
       horizontal_samples = horizontal_samples,
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja
index 83934df..3d6c0f5 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja
@@ -1007,6 +1007,7 @@
       namespace = namespace,
       parent_link = root,
       horizontal_samples = horizontal_samples,
+      rotation_freq = rotation_freq,
       x = 0.0,
       y = 0.0,
       z = 0.1,
@@ -1026,7 +1027,7 @@
       namespace = namespace,
       parent_link = root,
       sensor_name = "velodyne",
-      rot_freq = 20,
+      rotation_freq = rotation_freq,
       lasers = 16,
       vfov_angle = 30,
       max_range = 100,
@@ -1052,7 +1053,7 @@
       parent_link = root,
       sensor_name = "os",
       ouster_model = ouster_model,
-      rot_freq = 10,
+      rotation_freq = rotation_freq,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
       horizontal_samples = horizontal_samples,
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja
index b821cb8..d3be1fe 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja
@@ -783,6 +783,7 @@
       namespace = namespace,
       parent_link = root,
       horizontal_samples = horizontal_samples,
+      rotation_freq = rotation_freq,
       x = 0.0,
       y = 0.0,
       z = 0.09,
@@ -802,7 +803,7 @@
       parent_link = root,
       sensor_name = "os",
       ouster_model = ouster_model,
-      rot_freq = 10,
+      rotation_freq = rotation_freq,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
       horizontal_samples = horizontal_samples,
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja
index 9d0ee68..fe80036 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja
@@ -761,6 +761,7 @@
       namespace = namespace,
       parent_link = root,
       horizontal_samples = horizontal_samples,
+      rotation_freq = rotation_freq,
       x = 0.0,
       y = 0.0,
       z = 0.107,
@@ -780,7 +781,7 @@
       namespace = namespace,
       parent_link = root,
       sensor_name = "velodyne",
-      rot_freq = 20,
+      rotation_freq = rotation_freq,
       lasers = 16,
       vfov_angle = 30,
       max_range = 100,
@@ -806,7 +807,7 @@
       parent_link = root,
       sensor_name = "os",
       ouster_model = ouster_model,
-      rot_freq = 10,
+      rotation_freq = rotation_freq,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
       horizontal_samples = horizontal_samples,
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
index b00afb9..8fbe882 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
@@ -833,6 +833,7 @@
       namespace = namespace,
       parent_link = root,
       horizontal_samples = horizontal_samples,
+      rotation_freq = rotation_freq,
       x = 0.0,
       y = 0.0,
       z = 0.136,
@@ -852,7 +853,7 @@
       parent_link = root,
       sensor_name = "os",
       ouster_model = ouster_model,
-      rot_freq = 10,
+      rotation_freq = rotation_freq,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
       horizontal_samples = horizontal_samples,