From 8d6b558fedadc6a9e7be838be514fd3cae7a1791 Mon Sep 17 00:00:00 2001
From: stibipet <stibinger@fly4future.com>
Date: Fri, 3 Nov 2023 13:58:30 +0100
Subject: [PATCH 1/4] add option to set horizontal_samples for lidars as a
 spawner param

---
 .../config/spawner_params.yaml                |  1 +
 .../sdf/component_snippets.sdf.jinja          | 36 ++++++++++++++-----
 .../mrs_robots_description/sdf/f450.sdf.jinja |  3 ++
 .../mrs_robots_description/sdf/f550.sdf.jinja |  3 ++
 .../mrs_robots_description/sdf/naki.sdf.jinja |  2 ++
 .../mrs_robots_description/sdf/t650.sdf.jinja | 23 ++++++------
 .../mrs_robots_description/sdf/x500.sdf.jinja |  2 ++
 7 files changed, 51 insertions(+), 19 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 10883ed..14bdd93 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
+++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -28,6 +28,7 @@ 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]]
 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 2b5b28f..7d3b374 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,7 +1501,12 @@ limitations under the License.
 <!--}-->
 
   <!-- Macro to add a RPlidar A3{-->
-{%- macro rplidar_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%}
+{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, x, y, z, roll, pitch, yaw) -%}
+{% if horizontal_samples == 'None' %}
+  {%- set samples = 14400 -%}
+{% else %}
+  {%- set samples = horizontal_samples | int -%}
+{% endif %}
 <link name="rplidar_link">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1541,7 +1546,7 @@ limitations under the License.
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ 14400/20 }}</samples>
+          <samples>{{ samples/20 }}</samples>
           <resolution>1</resolution>
           <min_angle>-3.1241390751</min_angle>
           <max_angle>3.1241390751</max_angle>
@@ -1580,7 +1585,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, x, y, z, roll, pitch, yaw) -%}
+{%- 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) -%}
 
 {# 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. #}
@@ -1612,6 +1617,12 @@ limitations under the License.
   {%- set sensor_type = "ray"  -%}
 {% endif %}
 
+{% if horizontal_samples == 'None' %}
+  {%- set samples = 291200 -%}
+{% else %}
+  {%- set samples = horizontal_samples | int -%}
+{% endif %}
+
 <link name="velodyne_link">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1668,7 +1679,7 @@ limitations under the License.
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ 291200/(rot_freq*lasers) }}</samples>
+          <samples>{{ samples/(rot_freq*lasers) }}</samples>
           <resolution>1</resolution>
           <min_angle>{{ -rad180 }}</min_angle>
           <max_angle>{{ rad180 }}</max_angle>
@@ -1722,7 +1733,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, x, y, z, roll, pitch, yaw) -%}
+{%- 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) -%}
 
 {# 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. #}
@@ -1765,6 +1776,12 @@ limitations under the License.
   {%- set sensor_type = "ray"  -%}
 {% endif %}
 
+{% if horizontal_samples == 'None' %}
+  {%- set samples = 20480 -%}
+{% else %}
+  {%- set samples = horizontal_samples | int -%}
+{% endif %}
+
 <link name="{{ sensor_link }}">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
@@ -1798,7 +1815,7 @@ limitations under the License.
       </script>
     </material>
   </visual>
-  
+
   {# IMU #}
   <sensor name="{{ sensor_name }}_imu_sensor" type="imu">
     <pose>{{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }}</pose>
@@ -1829,7 +1846,7 @@ limitations under the License.
   <!--   <headingGaussianNoise>0.005</headingGaussianNoise> -->
   <!-- </plugin> -->
   </sensor>
-  
+
   {# LIDAR #}
   <sensor name="{{ sensor_name }}_lidar_sensor" type="{{ sensor_type }}">
     <pose>{{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }}</pose>
@@ -1839,7 +1856,7 @@ limitations under the License.
     <ray>
       <scan>
         <horizontal>
-          <samples>{{ 20480/rot_freq }}</samples>
+          <samples>{{ samples/rot_freq }}</samples>
           <resolution>1</resolution>
           <min_angle>0</min_angle>
           <max_angle>{{ 2*pi }}</max_angle>
@@ -1902,7 +1919,7 @@ limitations under the License.
 {%- endmacro -%}
 <!--}-->
 
-{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_freq, noise, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%}
+{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_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 #}
@@ -1999,6 +2016,7 @@ limitations under the License.
   max_range = range,
   noise = noise,
   enable_gpu_ray = enable_gpu_ray,
+  horizontal_samples = horizontal_samples,
   x = x,
   y = y,
   z = z,
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 e596537..4d34495 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
@@ -857,6 +857,7 @@
     {{ components.rplidar_macro(
       namespace = namespace,
       parent_link = root,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.1,
@@ -882,6 +883,7 @@
       max_range = 100,
       noise = 0.01,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.066,
@@ -904,6 +906,7 @@
       rot_freq = 10,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.066,
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 abe26c5..83934df 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
@@ -1006,6 +1006,7 @@
     {{ components.rplidar_macro(
       namespace = namespace,
       parent_link = root,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.1,
@@ -1031,6 +1032,7 @@
       max_range = 100,
       noise = 0.01,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.066,
@@ -1053,6 +1055,7 @@
       rot_freq = 10,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.066,
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 b82c923..b821cb8 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
@@ -782,6 +782,7 @@
     {{ components.rplidar_macro(
       namespace = namespace,
       parent_link = root,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.09,
@@ -804,6 +805,7 @@
       rot_freq = 10,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.0611,
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 ba35a1b..9d0ee68 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
@@ -213,7 +213,7 @@
         yaw = 0)
       }}
       <!--}-->
-      
+
       <!-- pixhawk {-->
       {{ components.visual_mesh_macro(
         name = "pixhawk",
@@ -392,7 +392,7 @@
       }}
       <!--}-->
       {% endif %}
-      
+
       {% if enable_rplidar or fire_challenge_blanket %}
       <!-- RPLidar mount {-->
       {{ components.visual_mesh_macro(
@@ -409,7 +409,7 @@
       }}
       <!--}-->
       {% endif %}
-     
+
       {% if enable_realsense_front %}
       <!-- Realsense mount {-->
       {{ components.visual_mesh_macro(
@@ -426,7 +426,7 @@
       }}
       <!--}-->
       {% endif %}
-     
+
       {% if enable_ouster or enable_velodyne %}
       <!-- Ouster mount {-->
       {{ components.visual_mesh_macro(
@@ -443,7 +443,7 @@
       }}
       <!--}-->
       {% endif %}
-     
+
       <!--}-->
     </link>
 
@@ -760,6 +760,7 @@
     {{ components.rplidar_macro(
       namespace = namespace,
       parent_link = root,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.107,
@@ -785,6 +786,7 @@
       max_range = 100,
       noise = 0.01,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.073,
@@ -807,6 +809,7 @@
       rot_freq = 10,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.073,
@@ -817,7 +820,7 @@
     <!--}-->
     {% endif %}
     {# <!--}--> #}
-    
+
     <!-- ========================= camera sensors ========================= -->
 
     <!-- Bluefox camera placements{-->
@@ -1269,10 +1272,10 @@
       yaw=-2.5665787)
     }}
     <!--}-->
-    
+
     <!--}-->
     {% endif %}
-    
+
     {% if fire_challenge_blanket %}
     <!-- fire_challenge_blanket {-->
 
@@ -1289,7 +1292,7 @@
       yaw = 0)
     }}
     <!--}-->
-    
+
     <!-- rplidar {-->
     {{ components.rplidar_macro(
       namespace = namespace,
@@ -1344,7 +1347,7 @@
       yaw = 0)
     }}
     <!--}-->
-    
+
     <!--}-->
     {% endif %}
     {# <!--}--> #}
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 94dbe4e..b00afb9 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
@@ -832,6 +832,7 @@
     {{ components.rplidar_macro(
       namespace = namespace,
       parent_link = root,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.136,
@@ -854,6 +855,7 @@
       rot_freq = 10,
       noise = 0.03,
       enable_gpu_ray = use_gpu_ray,
+      horizontal_samples = horizontal_samples,
       x = 0.0,
       y = 0.0,
       z = 0.107,

From aed3231b68e16ab09d82c719ce85b333191b2c25 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Fri, 3 Nov 2023 14:41:34 +0100
Subject: [PATCH 2/4] added 3dlidar example session

---
 .../config/custom_config.yaml                 |  26 +++++
 .../config/network_config.yaml                |  15 +++
 .../config/world_config.yaml                  |  34 ++++++
 .../tmux/one_drone_3dlidar/kill.sh            |  14 +++
 .../tmux/one_drone_3dlidar/layout.json        | 105 ++++++++++++++++++
 .../tmux/one_drone_3dlidar/session.yml        |  59 ++++++++++
 .../tmux/one_drone_3dlidar/start.sh           |  27 +++++
 7 files changed, 280 insertions(+)
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml
 create mode 100755 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
 create mode 100755 ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh

diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml
new file mode 100644
index 0000000..249e548
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml
@@ -0,0 +1,26 @@
+mrs_uav_managers:
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    state_estimators: [
+      "gps_garmin",
+      "gps_baro",
+      "ground_truth",
+    ]
+
+    initial_state_estimator: "gps_baro" # will be used as the first state estimator
+    agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback)
+
+  uav_manager:
+
+    takeoff:
+
+      during_takeoff:
+        controller: "MpcController"
+        tracker: "LandoffTracker"
+
+      after_takeoff:
+        controller: "MpcController"
+        # controller: "Se3Controller"
+        tracker: "MpcTracker"
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml
new file mode 100644
index 0000000..08f370d
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml
@@ -0,0 +1,15 @@
+# 1. This list is used by NimbroNetwork for mutual communication of the UAVs
+#    The names of the robots have to match hostnames described in /etc/hosts.
+#
+# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs.
+#    The names should match the true "UAV_NAMES" (the topic prefixes).
+#
+#     network_config:=~/config/network_config.yaml
+#
+#    to the core.launch and nimbro.launch.
+
+network:
+
+  robot_names: [
+    uav1,
+  ]
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml
new file mode 100644
index 0000000..9b55067
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml
@@ -0,0 +1,34 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 47.397743
+  origin_y: 8.545594
+
+safety_area:
+
+  enabled: true
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "world_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      -50, -50,
+      50,  -50,
+      50,  50,
+      -50, 50,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 15.0
+    min_z: 0.5
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh
new file mode 100755
index 0000000..28d50b0
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+# Absolute path to this script. /home/user/bin/foo.sh
+SCRIPT=$(readlink -f $0)
+# Absolute path this script is in. /home/user/bin
+SCRIPTPATH=`dirname $SCRIPT`
+cd "$SCRIPTPATH"
+
+export TMUX_SESSION_NAME=simulation
+export TMUX_SOCKET_NAME=mrs
+
+# just attach to the session
+tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME
+tmux -L $TMUX_SOCKET_NAME send-keys -t $TMUX_SESSION_NAME "sleep 1; tmux list-panes -s -F \"#{pane_pid} #{pane_current_command}\" | grep -v tmux | cut -d\" \" -f1 | while read in; do killp \$in; done; exit" ENTER
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json
new file mode 100644
index 0000000..3b10030
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json
@@ -0,0 +1,105 @@
+[
+	{
+		"border": "normal",
+		"floating": "auto_off",
+		"fullscreen_mode": 0,
+		"layout": "splith",
+		"percent": 0.5,
+		"type": "con",
+		"nodes": [
+			{
+				"border": "normal",
+				"floating": "auto_off",
+				"layout": "splith",
+				"percent": 1,
+				"type": "con",
+				"nodes": [
+					{
+						"border": "normal",
+						"floating": "auto_off",
+						"layout": "splitv",
+						"percent": 1,
+						"type": "con",
+						"nodes": [
+							{
+								"border": "normal",
+								"floating": "auto_off",
+								"layout": "splith",
+								"percent": 0.5,
+								"type": "con",
+								"nodes": [
+									{
+										"border": "pixel",
+										"current_border_width": 3,
+										"floating": "auto_off",
+										"geometry": {
+											"height": 460,
+											"width": 724,
+											"x": 0,
+											"y": 0
+										},
+										"name": "Gazebo",
+										"percent": 0.5,
+										"swallows": [
+											{
+												"instance": "^gazebo$"
+											}
+										],
+										"type": "con"
+									},
+									{
+										"border": "pixel",
+										"current_border_width": 3,
+										"floating": "auto_off",
+										"geometry": {
+											"height": 460,
+											"width": 724,
+											"x": 0,
+											"y": 0
+										},
+										"name": "default_simulation.rviz* - RViz",
+										"percent": 0.5,
+										"swallows": [
+											{
+												"instance": "^rviz$"
+											}
+										],
+										"type": "con"
+									}
+								]
+							},
+							{
+								"border": "normal",
+								"floating": "auto_off",
+								"layout": "splith",
+								"percent": 0.5,
+								"type": "con",
+								"nodes": [
+									{
+										"border": "pixel",
+										"current_border_width": 3,
+										"floating": "auto_off",
+										"geometry": {
+											"height": 460,
+											"width": 724,
+											"x": 0,
+											"y": 0
+										},
+										"name": "./start.sh",
+										"percent": 1,
+										"swallows": [
+											{
+												"instance": "^urxvt$"
+											}
+										],
+										"type": "con"
+									}
+								]
+							}
+						]
+					}
+				]
+			}
+		]
+	}
+]
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
new file mode 100644
index 0000000..90b41b7
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
@@ -0,0 +1,59 @@
+# do not modify these
+root: ./
+name: simulation
+socket_name: mrs
+attach: false
+tmux_options: -f /etc/ctu-mrs/tmux.conf
+# you can modify these
+pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500
+startup_window: status
+windows:
+  - roscore:
+      layout: tiled
+      panes:
+        - roscore
+  - gazebo:
+      layout: tiled
+      panes:
+        - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=forest gui:=true
+        - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME
+  - status:
+      layout: tiled
+      panes:
+        - waitForHw; roslaunch mrs_uav_status status.launch
+  - spawn:
+      layout: tiled
+      panes:
+        # the horizontal-samples decreases the base sample rate of 20480 (=2048 points per linescan) to a user-defined value
+        - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ground-truth --enable-ouster --ouster-model OS0-128 --use-gpu-ray --horizontal-samples 1280"
+  - hw_api:
+      layout: tiled
+      panes:
+        - waitForTime; roslaunch mrs_uav_px4_api api.launch
+  - core:
+      layout: tiled
+      panes:
+        - waitForHw; roslaunch mrs_uav_core core.launch
+          platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml
+          custom_config:=./config/custom_config.yaml
+          world_config:=./config/world_config.yaml
+          network_config:=./config/network_config.yaml
+  - takeoff:
+      layout: tiled
+      panes:
+        - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+        - 'waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard'
+  - goto:
+      layout: tiled
+      panes:
+        - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 10.0, 1.5, 0.0\]\"'
+  - rviz:
+      layout: tiled
+      panes:
+        - waitForControl; roslaunch mrs_uav_core rviz.launch
+        - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch
+        - waitForTime; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch
+  - layout:
+      layout: tiled
+      panes:
+        - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json
diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh
new file mode 100755
index 0000000..5bb2275
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# Absolute path to this script. /home/user/bin/foo.sh
+SCRIPT=$(readlink -f $0)
+# Absolute path this script is in. /home/user/bin
+SCRIPTPATH=`dirname $SCRIPT`
+cd "$SCRIPTPATH"
+
+export TMUX_SESSION_NAME=simulation
+export TMUX_SOCKET_NAME=mrs
+
+# start tmuxinator
+tmuxinator start -p ./session.yml
+
+# if we are not in tmux
+if [ -z $TMUX ]; then
+
+  # just attach to the session
+  tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME
+
+# if we are in tmux
+else
+
+  # switch to the newly-started session
+  tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 
+
+fi

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 3/4] 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,

From 5ba2320e25b5d64f34300bf1f6310d86d896dc02 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Fri, 3 Nov 2023 16:24:38 +0100
Subject: [PATCH 4/4] updated session

---
 .../tmux/one_drone_3dlidar/session.yml                         | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
index 90b41b7..68f2571 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
+++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml
@@ -24,8 +24,7 @@ windows:
   - spawn:
       layout: tiled
       panes:
-        # the horizontal-samples decreases the base sample rate of 20480 (=2048 points per linescan) to a user-defined value
-        - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ground-truth --enable-ouster --ouster-model OS0-128 --use-gpu-ray --horizontal-samples 1280"
+        - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ground-truth --enable-ouster --ouster-model OS0-128 --use-gpu-ray --horizontal-samples 128 --rotation-freq 10"
   - hw_api:
       layout: tiled
       panes: