From 742a03f3c49d898fc9fb5e71d4ffb30ecc48a8e9 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Wed, 6 Sep 2023 13:17:48 +0200
Subject: [PATCH 1/4] Modified scripts and configs for alien drone.

---
 .../init.d-posix/airframes/4001_a300          | 21 +++++++++++++++++++
 .../config/spawner_params.yaml                |  6 +++---
 .../scripts/mrs_drone_spawner.py              |  4 ++--
 3 files changed, 26 insertions(+), 5 deletions(-)
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300

diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300 b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300
new file mode 100644
index 0000000..5305429
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300
@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# @name DJI F330 SITL
+#
+# @type Quadcopter x
+#
+# @maintainer Petr Stibinger <stibipet@fel.cvut.cz>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MAV_TYPE 2
+
+param set-default BAT1_N_CELLS 6
+param set-default BAT1_CAPACITY 8000
+param set-default BAT1_V_EMPTY 3.6
+param set-default BAT1_V_CHARGED 4.2
+param set-default BAT1_V_LOAD_DROP 0.4
+
+set MIXER quad_x
+
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 d878d3f..10883ed 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
+++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -1,7 +1,7 @@
 enable_rangefinder: [False, 'Add a laser rangefinder (Garmin) pointing down', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec]]
 enable_rangefinder_up: [False, 'Add a laser rangefinder (Garmin) pointing up', [f450, f550, t650, naki]]
 enable_teraranger: [False, 'Add a laser rangefinder (Teraranger) to the vehicle', [f450, f550, t650]]
-enable_ground_truth: [False, 'Enable topic with ground truth odometry', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec]]
+enable_ground_truth: [False, 'Enable topic with ground truth odometry', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec, a300]]
 enable_mobius_camera_front: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the front', [f450, f550, t650]]
 enable_mobius_camera_down: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the ground', [f450, f550]]
 enable_mobius_camera_back_left: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the back left', [f450, f550]]
@@ -53,5 +53,5 @@ enable_vio: [False, 'Add a forward-looking fisheye camera and a high frequency I
 enable_vio_down: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
 enable_omni_ultrasounds: [False, 'Add omnidirectional ultrasonic sensors (up, down, 4 in horizontal plane)', [naki]]
 enable_safety_led: [False, 'Add a safety LED', [naki]]
-model_package: ["mrs_uav_gazebo_simulation", 'package name for the UAV models', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]]
-disable_motor_crash: [False, 'Disables motor failure after crash with the environment', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]]
+model_package: ["mrs_uav_gazebo_simulation", 'package name for the UAV models', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]]
+disable_motor_crash: [False, 'Disables motor failure after crash with the environment', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]]
diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
index 14b63f5..ac49b1f 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
+++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
@@ -26,7 +26,7 @@
 MAVLINK_UDP_BASE_PORT = 14560
 LAUNCH_BASE_PORT = 14900
 DEFAULT_VEHICLE_TYPE = 't650'
-VEHICLE_TYPES = ['f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec']
+VEHICLE_TYPES = ['a300', 'f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec']
 SPAWNING_DELAY_SECONDS = 6
 
 class MrsDroneSpawner():
@@ -435,7 +435,7 @@ def get_params_dict(self, params_list, vehicle_type):
                     allowed_vehicle_types = self.spawner_params[pname][2]
                 # print('For param ' + str(pname) + ' allowed: ' + str(allowed_vehicle_types))
                     if vehicle_type not in allowed_vehicle_types:
-                        raise Exception('Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\'!')
+                        raise Exception('Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\' ....... !')
 
                 print('\t' + str(pname) + ': ' + str(pval))
             params_dict.update(custom_params)

From 4a4fd399d0e58a3922380dc819084c5c0b7d6d05 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Wed, 6 Sep 2023 13:22:49 +0200
Subject: [PATCH 2/4] Removed accidentally added dots.

---
 .../mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
index ac49b1f..566a2bb 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
+++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py
@@ -435,7 +435,7 @@ def get_params_dict(self, params_list, vehicle_type):
                     allowed_vehicle_types = self.spawner_params[pname][2]
                 # print('For param ' + str(pname) + ' allowed: ' + str(allowed_vehicle_types))
                     if vehicle_type not in allowed_vehicle_types:
-                        raise Exception('Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\' ....... !')
+                        raise Exception('Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\'!')
 
                 print('\t' + str(pname) + ': ' + str(pval))
             params_dict.update(custom_params)

From 56dbaf91c7473b1f27ad8c4846ce92ff0780e090 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Thu, 14 Sep 2023 13:44:52 +0200
Subject: [PATCH 3/4] Modification for fluid resistace and modified motor
 model.

---
 .../sdf/component_snippets.sdf.jinja          | 118 +++++++++++++++++-
 1 file changed, 116 insertions(+), 2 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 8b883ee..b0212db 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
@@ -439,7 +439,7 @@ limitations under the License.
   <enableMotorCrash>{{ enable_motor_crash }}</enableMotorCrash>
 </plugin>
 
-<plugin name="motor{{ motor_number }}_speed_republisher_plugin" filename="libMRSGazeboMotorSpeedRepublisherPlugin.so">
+<plugin name="motor{{ motor_number }}_speed_republisher_plugin" filename="libMrsGazeboCommonResources_MotorSpeedRepublisherPlugin.so.so">
   <robotNamespace/>
   <motorSpeedTopic>/motor_speed/{{ motor_number }}</motorSpeedTopic>
 </plugin>
@@ -539,7 +539,107 @@ limitations under the License.
   <enableMotorCrash>{{ enable_motor_crash }}</enableMotorCrash>
 </plugin>
 
-<plugin name="motor{{ motor_number }}_speed_republisher_plugin" filename="libMRSGazeboMotorSpeedRepublisherPlugin.so">
+<plugin name="motor{{ motor_number }}_speed_republisher_plugin" filename="libMrsGazeboCommonResources_MotorSpeedRepublisherPlugin.so">
+  <robotNamespace/>
+  <motorSpeedTopic>/motor_speed/{{ motor_number }}</motorSpeedTopic>
+</plugin>
+
+<joint name="prop_{{ motor_number }}_joint" type="revolute">
+  <parent>{{ parent }}</parent>
+  <child>prop_{{ motor_number }}_link</child>
+  <axis>
+    <xyz>0 0 1</xyz>
+    <limit>
+      <lower>-1e+16</lower>
+      <upper>1e+16</upper>
+    </limit>
+    <dynamics>
+      <spring_reference>0</spring_reference>
+      <spring_stiffness>0</spring_stiffness>
+    </dynamics>
+    <use_parent_model_frame>1</use_parent_model_frame>
+  </axis>
+</joint>
+{%- endmacro -%}
+<!--}-->
+
+<!-- Prop macro with 2 meshes mrs motor model{-->
+{%- macro prop_macro_2_meshes_mrs_motor_model(direction, rotor_velocity_slowdown_sim, motor_constant, moment_constant, parent, mass, radius, time_constant_up, time_constant_down, max_rot_velocity, motor_number, rotor_drag_coefficient, rolling_moment_coefficient, enable_motor_crash, color, mesh_file_1, mesh_file_2, meshes_z_offset,  mesh_scale, x, y, z, roll, pitch, yaw, ixx, ixy, ixz, iyy, iyz, izz) -%}
+<link name="prop_{{ motor_number }}_link">
+  <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
+  <inertial>
+    <mass>{{ mass }}</mass>
+    <inertia>
+      <ixx>{{ ixx }}</ixx>
+      <ixy>{{ ixy }}</ixy>
+      <ixz>{{ ixz }}</ixz>
+      <iyy>{{ iyy }}</iyy>
+      <iyz>{{ iyz }}</iyz>
+      <izz>{{ izz }}</izz>
+    </inertia>
+  </inertial>
+  <visual name="visual_1">
+    <geometry>
+      <mesh>
+        <uri>{{ mesh_file_1 }}</uri>
+        <scale>{{ mesh_scale }}</scale>
+      </mesh>
+    </geometry>
+  </visual>
+  <visual name="visual_2">
+    <pose>{{ 0 }} {{ 0 }} {{ meshes_z_offset }} {{ 0 }} {{ 0 }} {{ 0 }}</pose>
+    <geometry>
+      <mesh>
+        <uri>{{ mesh_file_2 }}</uri>
+        <scale>{{ mesh_scale }}</scale>
+      </mesh>
+    </geometry>
+    <material>
+      <script>
+        <name>Gazebo/{{ color }}</name>
+        <uri>file://media/materials/scripts/gazebo.material</uri>
+      </script>
+    </material>
+  </visual>
+  <collision name="prop_{{ motor_number }}_link_collision">
+    <pose>0 0 0 0 {{ rad90 }} 0</pose>
+    <geometry>
+      <cylinder>
+        <length>{{ 2*radius }}</length>
+        <radius>0.01</radius>
+      </cylinder>
+    </geometry>
+    <surface>
+      <contact>
+        <ode/>
+      </contact>
+      <friction>
+        <ode/>
+      </friction>
+    </surface>
+  </collision>
+</link>
+
+<plugin name="motor{{ motor_number }}_plugin" filename="libMrsGazeboCommonResources_MotorPropModelPlugin.so">
+  <robotNamespace/>
+  <jointName>prop_{{ motor_number }}_joint</jointName>
+  <linkName>prop_{{ motor_number }}_link</linkName>
+  <turningDirection>{{ direction }}</turningDirection>
+  <timeConstantUp>{{ time_constant_up }}</timeConstantUp>
+  <timeConstantDown>{{ time_constant_down }}</timeConstantDown>
+  <maxRotVelocity>{{ max_rot_velocity }}</maxRotVelocity>
+  <motorConstant>{{ motor_constant }}</motorConstant>
+  <momentConstant>{{ moment_constant }}</momentConstant>
+  <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+  <motorNumber>{{ motor_number }}</motorNumber>
+  <rotorDragCoefficient>{{ rotor_drag_coefficient }}</rotorDragCoefficient>
+  <rollingMomentCoefficient>{{ rolling_moment_coefficient }}</rollingMomentCoefficient>
+  <motorSpeedPubTopic>/motor_speed/{{ motor_number }}</motorSpeedPubTopic>
+  <rotorVelocitySlowdownSim>{{ rotor_velocity_slowdown_sim }}</rotorVelocitySlowdownSim>
+  <enableMotorCrash>{{ enable_motor_crash }}</enableMotorCrash>
+</plugin>
+
+<plugin name="motor{{ motor_number }}_speed_republisher_plugin" filename="libMrsGazeboCommonResources_MotorSpeedRepublisherPlugin.so">
   <robotNamespace/>
   <motorSpeedTopic>/motor_speed/{{ motor_number }}</motorSpeedTopic>
 </plugin>
@@ -610,6 +710,20 @@ limitations under the License.
   <rotorVelocitySlowdownSim>{{ rotor_velocity_slowdown_sim }}</rotorVelocitySlowdownSim>
 </plugin>
 {%- endmacro -%}
+<!--}-->
+
+<!-- Fluid resistance macro {-->
+{%- macro fluid_resistance_plugin_macro(parent_link, update_rate, uav_body_resistance_x, uav_body_resistance_y, uav_body_resistance_z) -%}
+<plugin name="fluid_resistance_plugin" filename="libMrsGazeboCommonResources_FluidResistancePlugin.so">
+  <robotNamespace/>
+  <FluidResistanceTopicName>/fluid_resistance</FluidResistanceTopicName>
+  <NameLinkToApplyResistance>{{ parent_link }}</NameLinkToApplyResistance>
+  <rate>{{ update_rate }}</rate>
+  <res_x>{{ uav_body_resistance_x }}</res_x>
+  <res_y>{{ uav_body_resistance_y }}</res_y>
+  <res_z>{{ uav_body_resistance_z }}</res_z>
+</plugin>
+{%- endmacro -%}
 <!--}-->
 
   <!-- ================================================================== -->

From a2a5d1b5db0686a4b1cb538738072622ccb5b6b2 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Thu, 14 Sep 2023 16:39:57 +0200
Subject: [PATCH 4/4] Modified fluid resistance macro for new computation of
 the drag.

---
 .../mrs_robots_description/sdf/component_snippets.sdf.jinja   | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

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 b0212db..a7225ba 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
@@ -713,15 +713,17 @@ limitations under the License.
 <!--}-->
 
 <!-- Fluid resistance macro {-->
-{%- macro fluid_resistance_plugin_macro(parent_link, update_rate, uav_body_resistance_x, uav_body_resistance_y, uav_body_resistance_z) -%}
+{%- macro fluid_resistance_plugin_macro(verbose, model_mass, parent_link, update_rate, uav_body_resistance_x, uav_body_resistance_y, uav_body_resistance_z) -%}
 <plugin name="fluid_resistance_plugin" filename="libMrsGazeboCommonResources_FluidResistancePlugin.so">
   <robotNamespace/>
+  <model_mass>{{ model_mass }}</model_mass>
   <FluidResistanceTopicName>/fluid_resistance</FluidResistanceTopicName>
   <NameLinkToApplyResistance>{{ parent_link }}</NameLinkToApplyResistance>
   <rate>{{ update_rate }}</rate>
   <res_x>{{ uav_body_resistance_x }}</res_x>
   <res_y>{{ uav_body_resistance_y }}</res_y>
   <res_z>{{ uav_body_resistance_z }}</res_z>
+  <verbose>{{ verbose }}</verbose>
 </plugin>
 {%- endmacro -%}
 <!--}-->