From 899f3161563d2a0875894beb796d944b9c4e67fb Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Mon, 14 Aug 2023 17:40:35 +0200
Subject: [PATCH 01/12] removed schedulled workflow

---
 .github/workflows/noetic.yml | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml
index de7a43e..0151fcc 100644
--- a/.github/workflows/noetic.yml
+++ b/.github/workflows/noetic.yml
@@ -10,9 +10,6 @@ on:
   pull_request:
     branches: [ master ]
 
-  schedule:
-    - cron:  '0 0 * * *' # at the end of every day
-
   # Allows you to run this workflow manually from the Actions tab
   workflow_dispatch:
 

From 98d7da99093905e5ff1fa1f123383610b80b568a Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Wed, 13 Sep 2023 16:03:19 +0200
Subject: [PATCH 02/12] Added linear fluid resistance plugin.

---
 CMakeLists.txt                                |  12 +
 .../fluid_resistance_plugin.cpp               | 288 ++++++++++++++++++
 2 files changed, 300 insertions(+)
 create mode 100644 src/physics_plugins/fluid_resistance_plugin.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 03bee06..ce138a3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -94,6 +94,7 @@ set(LIBRARIES
   MrsGazeboCommonResources_ServoCameraPlugin
   MrsGazeboCommonResources_DynamicModelPlugin
   MrsGazeboCommonResources_SafetyLedPlugin
+  MrsGazeboCommonResources_FluidResistancePlugin
   )
 
 catkin_package(
@@ -326,6 +327,17 @@ target_link_libraries(MrsGazeboCommonResources_SafetyLedPlugin
   ${Boost_LIBRARIES}
   )
 
+# FluidResistancePlugin
+
+add_library(MrsGazeboCommonResources_FluidResistancePlugin SHARED
+  src/physics_plugins/fluid_resistance_plugin.cpp
+  )
+
+target_link_libraries(MrsGazeboCommonResources_FluidResistancePlugin
+  ${GAZEBO_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  )
 ## --------------------------------------------------------------
 ## |                           Install                          |
 ## --------------------------------------------------------------
diff --git a/src/physics_plugins/fluid_resistance_plugin.cpp b/src/physics_plugins/fluid_resistance_plugin.cpp
new file mode 100644
index 0000000..bc03b32
--- /dev/null
+++ b/src/physics_plugins/fluid_resistance_plugin.cpp
@@ -0,0 +1,288 @@
+/**  \file
+     \brief Defines class that applies linear drag force to the uav body.
+     \inspiration https://www.theconstructsim.com/q-a-190-air-drag-in-gazebo/
+     \author Ondrej Prochazka - prochon4@fel.cvut.cz
+ */
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/callback_queue.h>
+#include <ros/subscribe_options.h>
+
+#include <gazebo/common/common.hh>
+
+#include <gazebo/gazebo.hh>
+
+#include <gazebo/msgs/msgs.hh>
+
+#include <gazebo/physics/physics.hh>
+
+#include <gazebo/transport/transport.hh>
+
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Vector3.h>
+
+#include <ignition/math/Vector3.hh>
+
+#include <thread>
+
+#include <functional>
+
+namespace gazebo
+{
+
+/* Class definition //{ */
+class FluidResistancePlugin : public ModelPlugin {
+public:
+  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+  void OnUpdate();
+  void UpdateLinearVel();
+  void OnRosMsg(const geometry_msgs::Vector3ConstPtr &_msg);
+
+  void ApplyResistance();
+
+private:
+  void QueueThread();
+
+private:
+  physics::ModelPtr model;  // Pointer to the model
+
+private:
+  event::ConnectionPtr updateConnection;  // Pointer to the update event connection
+
+  // TODO: remove this variable and exchange it for x_res, ... to be abel update by topic
+  double fluid_resistance_index = 1.0;
+
+private:
+  std::unique_ptr<ros::NodeHandle> rosNode;  // node use for ROS transport
+
+private:
+  ros::Subscriber rosSub;  // ROS subscriber
+private:
+  ros::CallbackQueue rosQueue;  // ROS callbackqueue that helps process messages
+private:
+  std::thread rosQueueThread;  // thread the keeps running the rosQueue
+
+private:
+  physics::LinkPtr link_to_apply_resistance;  // ROS subscriber
+
+private:
+  std::string fluid_resistanceTopicName = "fluid_resistance";
+
+private:
+  std::string NameLinkToApplyResistance = "base_link";
+
+private:
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  ignition::math::Vector3d now_lin_vel;
+#else
+  math::Vector3 now_lin_vel;
+#endif
+
+private:
+  double rate  = 1.0;
+  double res_x = 1.0;
+  double res_y = 1.0;
+  double res_z = 1.0;
+
+private:
+  float last_time = 0.0;
+
+private:
+  physics::WorldPtr world;  // The parent World
+};
+
+//}
+
+/* Load() //{ */
+void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
+
+  std::cout << "Using Fluid Resistance PLugin" << std::endl;
+
+  /* load elements //{ */
+  if (_sdf->HasElement("fluid_resistanceTopicName")) {
+    fluid_resistanceTopicName = _sdf->Get<std::string>("fluid_resistanceTopicName");
+  } else {
+    ROS_WARN("No Topic Given name given, setting default name %s", fluid_resistanceTopicName.c_str());
+  }
+
+  if (_sdf->HasElement("NameLinkToApplyResistance")) {
+    NameLinkToApplyResistance = _sdf->Get<std::string>("NameLinkToApplyResistance");
+  } else {
+    ROS_WARN(
+        "No NameLinkToApplyResistance Given name given, setting default "
+        "name %s",
+        NameLinkToApplyResistance.c_str());
+  }
+
+  if (_sdf->HasElement("rate")) {
+    rate = _sdf->Get<double>("rate");
+  } else {
+    ROS_WARN(
+        "No rate Given name given, setting default "
+        "name %f",
+        rate);
+  }
+
+  if (_sdf->HasElement("res_x")) {
+    res_x = _sdf->Get<double>("res_x");
+  } else {
+    ROS_WARN(
+        "No res_x Given name given, setting default "
+        "name %f",
+        res_x);
+  }
+
+  if (_sdf->HasElement("res_y")) {
+    res_y = _sdf->Get<double>("res_y");
+  } else {
+    ROS_WARN(
+        "No res_y Given name given, setting default "
+        "name %f",
+        res_y);
+  }
+
+  if (_sdf->HasElement("res_z")) {
+    res_z = _sdf->Get<double>("res_z");
+  } else {
+    ROS_WARN(
+        "No res_z Given name given, setting default "
+        "name %f",
+        res_z);
+  }
+
+  ROS_WARN("All elements loaded successfully!");
+
+  //}
+
+  // Store the pointer to the model
+  model                    = _parent;
+  world                    = model->GetWorld();
+  link_to_apply_resistance = model->GetLink(NameLinkToApplyResistance);
+
+  // Listen to the update event. This event is broadcast every
+  // simulation iteration.
+  updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&FluidResistancePlugin::OnUpdate, this));
+
+  // Create a topic name
+  // std::string fluid_resistance_index_topicName = "/fluid_resistance_index";
+
+  // Initialize ros, if it has not already bee initialized.
+  if (!ros::isInitialized()) {
+    int    argc = 0;
+    char **argv = NULL;
+    ros::init(argc, argv, "fluid_resistance_node", ros::init_options::NoSigintHandler);
+  }
+
+  // Create our ROS node. This acts in a similar manner to
+  // the Gazebo node
+  rosNode.reset(new ros::NodeHandle("fluid_resistance_node"));
+
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  last_time = world->SimTime().Float();
+#else
+  last_time          = world->GetSimTime().Float();
+#endif
+
+  // Freq
+  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Vector3>(
+      fluid_resistanceTopicName, 1, boost::bind(&FluidResistancePlugin::OnRosMsg, this, _1), ros::VoidPtr(), &rosQueue);
+  rosSub = rosNode->subscribe(so);
+
+  // Spin up the queue helper thread.
+  rosQueueThread = std::thread(std::bind(&FluidResistancePlugin::QueueThread, this));
+
+  ROS_WARN(
+      "Loaded FluidResistance Plugin with parent...%s, With Fluid "
+      "Resistance = %f "
+      "Started ",
+      model->GetName().c_str(), fluid_resistance_index);
+}
+//}
+
+/* OnUpdate() //{ */
+// Called by the world update start event
+void FluidResistancePlugin::OnUpdate() {
+  float period = 1.0 / rate;
+
+  // Get simulator time
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  float current_time = world->SimTime().Float();
+#else
+  float current_time = world->GetSimTime().Float();
+#endif
+  float dt = current_time - last_time;
+
+  if (dt <= period) {
+    ROS_DEBUG(">>>>>>>>>>TimePassed = %f, TimePeriod =%f ", dt, period);
+    return;
+  } else {
+    last_time = current_time;
+    ApplyResistance();
+  }
+}
+//}
+
+/* UpdateLinearVel() //{ */
+void FluidResistancePlugin::UpdateLinearVel() {
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  now_lin_vel = model->RelativeLinearVel();
+#else
+  now_lin_vel        = model->GetRelativeLinearVel();
+#endif
+}
+//}
+
+/* ApplyResistance() //{ */
+void FluidResistancePlugin::ApplyResistance() {
+
+  UpdateLinearVel();
+
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  ignition::math::Vector3d force, torque;
+#else
+  math::Vector3 force, torque;
+#endif
+
+  ROS_WARN("LinearSpeed = [%f,%f,%f] ", now_lin_vel.X(), now_lin_vel.Y(), now_lin_vel.Z());
+
+  force.X(-1.0 * res_x * now_lin_vel.X());
+  force.Y(-1.0 * res_y * now_lin_vel.Y());
+  force.Z(-1.0 * res_z * now_lin_vel.Z());
+
+  // Applying force to uav
+  link_to_apply_resistance->AddRelativeForce(force);
+#if (GAZEBO_MAJOR_VERSION >= 8)
+  link_to_apply_resistance->AddRelativeTorque(torque - link_to_apply_resistance->GetInertial()->CoG().Cross(force));
+#else
+  link_to_apply_resistance->AddRelativeTorque(torque - link_to_apply_resistance->GetInertial()->GetCoG().Cross(force));
+#endif
+
+  ROS_WARN("FluidResistanceApplying = [%f,%f,%f] ", force.X(), force.Y(), force.Z());
+}
+//}
+
+/* OnRosMsg() //{ */
+void FluidResistancePlugin::OnRosMsg(const geometry_msgs::Vector3ConstPtr &_msg) {
+  /* UpdateResistanceValues(_msg->data); */
+  res_x = _msg->x;
+  res_y = _msg->y;
+  res_z = _msg->z;
+  ROS_WARN("fluid resistance changed!!");
+}
+//}
+
+/* QueueThread() //{ */
+/// \brief ROS helper function that processes messages
+void FluidResistancePlugin::QueueThread() {
+  static const double timeout = 0.01;
+  while (rosNode->ok()) {
+    rosQueue.callAvailable(ros::WallDuration(timeout));
+  }
+}
+//}
+
+// Register this plugin with the simulator
+GZ_REGISTER_MODEL_PLUGIN(FluidResistancePlugin)
+
+}  // namespace gazebo

From dbbbcf7c4d72f17c6f779191dcc663e00755fd50 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Thu, 14 Sep 2023 13:16:48 +0200
Subject: [PATCH 03/12] Added nicer prints into fluid plugin and updated
 README.

---
 src/physics_plugins/README.md                 | 19 +++++++++
 .../fluid_resistance_plugin.cpp               | 39 +++++++++----------
 2 files changed, 37 insertions(+), 21 deletions(-)
 create mode 100644 src/physics_plugins/README.md

diff --git a/src/physics_plugins/README.md b/src/physics_plugins/README.md
new file mode 100644
index 0000000..1b17757
--- /dev/null
+++ b/src/physics_plugins/README.md
@@ -0,0 +1,19 @@
+# Physics plugins for Gazebo
+
+This directory includes plugins that somehow modify the physics behavior of the UAV or its parts.
+
+## Plugins
+* fluid_resistace_plugin
+
+### Fluid Resistance
+This plugin models linear resistance of the air (Fd = resistance_constant * linear_body_velocity).
+You can set resistance coefficientsa (for every axis in body frame) in model definition using fluid resistance macro.
+It is also possible to change fluid resistance while simulation is running using topic */fluid_resistance*.
+```
+rostopic pub /fluid_resistance geometry_msgs/Vector3 "x: 0.0 
+y: 0.0
+z: 0.0" 
+```
+
+
+
diff --git a/src/physics_plugins/fluid_resistance_plugin.cpp b/src/physics_plugins/fluid_resistance_plugin.cpp
index bc03b32..00d2f56 100644
--- a/src/physics_plugins/fluid_resistance_plugin.cpp
+++ b/src/physics_plugins/fluid_resistance_plugin.cpp
@@ -50,9 +50,6 @@ class FluidResistancePlugin : public ModelPlugin {
 private:
   event::ConnectionPtr updateConnection;  // Pointer to the update event connection
 
-  // TODO: remove this variable and exchange it for x_res, ... to be abel update by topic
-  double fluid_resistance_index = 1.0;
-
 private:
   std::unique_ptr<ros::NodeHandle> rosNode;  // node use for ROS transport
 
@@ -67,7 +64,7 @@ class FluidResistancePlugin : public ModelPlugin {
   physics::LinkPtr link_to_apply_resistance;  // ROS subscriber
 
 private:
-  std::string fluid_resistanceTopicName = "fluid_resistance";
+  std::string FluidResistanceTopicName = "fluid_resistance";
 
 private:
   std::string NameLinkToApplyResistance = "base_link";
@@ -100,17 +97,17 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
   std::cout << "Using Fluid Resistance PLugin" << std::endl;
 
   /* load elements //{ */
-  if (_sdf->HasElement("fluid_resistanceTopicName")) {
-    fluid_resistanceTopicName = _sdf->Get<std::string>("fluid_resistanceTopicName");
+  if (_sdf->HasElement("FluidResistanceTopicName")) {
+    FluidResistanceTopicName = _sdf->Get<std::string>("FluidResistanceTopicName");
   } else {
-    ROS_WARN("No Topic Given name given, setting default name %s", fluid_resistanceTopicName.c_str());
+    ROS_WARN("[FluidResistancePlugin]: No Topic Given name given, setting default name %s", FluidResistanceTopicName.c_str());
   }
 
   if (_sdf->HasElement("NameLinkToApplyResistance")) {
     NameLinkToApplyResistance = _sdf->Get<std::string>("NameLinkToApplyResistance");
   } else {
     ROS_WARN(
-        "No NameLinkToApplyResistance Given name given, setting default "
+        "[FluidResistancePlugin]: No NameLinkToApplyResistance Given name given, setting default "
         "name %s",
         NameLinkToApplyResistance.c_str());
   }
@@ -119,7 +116,7 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     rate = _sdf->Get<double>("rate");
   } else {
     ROS_WARN(
-        "No rate Given name given, setting default "
+        "[FluidResistancePlugin]: No rate Given name given, setting default "
         "name %f",
         rate);
   }
@@ -128,7 +125,7 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     res_x = _sdf->Get<double>("res_x");
   } else {
     ROS_WARN(
-        "No res_x Given name given, setting default "
+        "[FluidResistancePlugin]: No res_x Given name given, setting default "
         "name %f",
         res_x);
   }
@@ -137,7 +134,7 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     res_y = _sdf->Get<double>("res_y");
   } else {
     ROS_WARN(
-        "No res_y Given name given, setting default "
+        "[FluidResistancePlugin]: No res_y Given name given, setting default "
         "name %f",
         res_y);
   }
@@ -146,12 +143,12 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     res_z = _sdf->Get<double>("res_z");
   } else {
     ROS_WARN(
-        "No res_z Given name given, setting default "
+        "[FluidResistancePlugin]: No res_z Given name given, setting default "
         "name %f",
         res_z);
   }
 
-  ROS_WARN("All elements loaded successfully!");
+  ROS_INFO("[FluidResistancePlugin]: All elements loaded successfully!");
 
   //}
 
@@ -186,17 +183,17 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
 
   // Freq
   ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Vector3>(
-      fluid_resistanceTopicName, 1, boost::bind(&FluidResistancePlugin::OnRosMsg, this, _1), ros::VoidPtr(), &rosQueue);
+      FluidResistanceTopicName, 1, boost::bind(&FluidResistancePlugin::OnRosMsg, this, _1), ros::VoidPtr(), &rosQueue);
   rosSub = rosNode->subscribe(so);
 
   // Spin up the queue helper thread.
   rosQueueThread = std::thread(std::bind(&FluidResistancePlugin::QueueThread, this));
 
   ROS_WARN(
-      "Loaded FluidResistance Plugin with parent...%s, With Fluid "
-      "Resistance = %f "
+      "[FluidResistancePlugin]: Loaded FluidResistance Plugin with parent...%s, With Fluid "
+      "Resistance = ( %f, %f, %f) "
       "Started ",
-      model->GetName().c_str(), fluid_resistance_index);
+      model->GetName().c_str(), res_x, res_y, res_z);
 }
 //}
 
@@ -214,7 +211,7 @@ void FluidResistancePlugin::OnUpdate() {
   float dt = current_time - last_time;
 
   if (dt <= period) {
-    ROS_DEBUG(">>>>>>>>>>TimePassed = %f, TimePeriod =%f ", dt, period);
+    ROS_DEBUG("[FluidResistancePlugin]: >>>>>>>>>>TimePassed = %f, TimePeriod =%f ", dt, period);
     return;
   } else {
     last_time = current_time;
@@ -244,7 +241,7 @@ void FluidResistancePlugin::ApplyResistance() {
   math::Vector3 force, torque;
 #endif
 
-  ROS_WARN("LinearSpeed = [%f,%f,%f] ", now_lin_vel.X(), now_lin_vel.Y(), now_lin_vel.Z());
+  /* ROS_WARN("[FluidResistancePlugin]: LinearSpeed = [%f,%f,%f] ", now_lin_vel.X(), now_lin_vel.Y(), now_lin_vel.Z()); */
 
   force.X(-1.0 * res_x * now_lin_vel.X());
   force.Y(-1.0 * res_y * now_lin_vel.Y());
@@ -258,7 +255,7 @@ void FluidResistancePlugin::ApplyResistance() {
   link_to_apply_resistance->AddRelativeTorque(torque - link_to_apply_resistance->GetInertial()->GetCoG().Cross(force));
 #endif
 
-  ROS_WARN("FluidResistanceApplying = [%f,%f,%f] ", force.X(), force.Y(), force.Z());
+  /* ROS_WARN("[FluidResistancePlugin]: FluidResistanceApplying = [%f,%f,%f] ", force.X(), force.Y(), force.Z()); */
 }
 //}
 
@@ -268,7 +265,7 @@ void FluidResistancePlugin::OnRosMsg(const geometry_msgs::Vector3ConstPtr &_msg)
   res_x = _msg->x;
   res_y = _msg->y;
   res_z = _msg->z;
-  ROS_WARN("fluid resistance changed!!");
+  ROS_WARN("[FluidResistancePlugin]: Fluid resistance changed to (%f, %f, %f)!!", res_x, res_y, res_z);
 }
 //}
 

From 62e4ed002217ba0d93959a1ab40af77030393f73 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Thu, 14 Sep 2023 13:26:22 +0200
Subject: [PATCH 04/12] Moved motor model from mavlink_sitl_gazebo and modified
 for a300 drone.

---
 CMakeLists.txt                                |  36 +-
 include/motor_prop_model.h                    | 157 ++++++++
 include/rotors_model/motor_model.hpp          |  60 ++++
 msgs/CommandMotorSpeed.proto                  |   7 +
 msgs/MotorSpeed.proto                         |   7 +
 msgs/Wind.proto                               |  10 +
 package.xml                                   |   1 +
 src/physics_plugins/README.md                 |   8 +
 .../motor_prop_model_plugin.cpp               | 334 ++++++++++++++++++
 9 files changed, 597 insertions(+), 23 deletions(-)
 create mode 100644 include/motor_prop_model.h
 create mode 100644 include/rotors_model/motor_model.hpp
 create mode 100644 msgs/CommandMotorSpeed.proto
 create mode 100644 msgs/MotorSpeed.proto
 create mode 100644 msgs/Wind.proto
 create mode 100644 src/physics_plugins/motor_prop_model_plugin.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 990b9d0..f78df94 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,25 +60,17 @@ endforeach()
 # PROTOBUF_IMPORT_DIRS has to be set before
 find_package(Protobuf REQUIRED)
 
-# set(mav_msgs
-#   msgs/CommandMotorSpeed.proto
-#   msgs/MotorSpeed.proto
-#   )
-
-# set(physics_msgs
-#     msgs/Wind.proto
-#     )
-
 set(sensor_msgs
   msgs/SITLGps.proto
   msgs/Groundtruth.proto
   msgs/MagneticField.proto
   msgs/Float.proto
+  msgs/CommandMotorSpeed.proto
+  msgs/MotorSpeed.proto
+  msgs/Wind.proto
   )
 
 PROTOBUF_GENERATE_CPP(SEN_PROTO_SRCS SEN_PROTO_HDRS ${sensor_msgs})
-# PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs})
-# PROTOBUF_GENERATE_CPP(PHY_PROTO_SRCS PHY_PROTO_HDRS ${physics_msgs})
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -108,7 +100,7 @@ set(LIBRARIES
   MrsGazeboCommonResources_SafetyLedPlugin
   MrsGazeboCommonResources_FluidResistancePlugin
   MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
-  # MrsGazeboCommonResources_MotorPropModelPlugin
+  MrsGazeboCommonResources_MotorPropModelPlugin
   )
 
 catkin_package(
@@ -367,18 +359,16 @@ target_link_libraries(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
 
 # MotorPropModelPlugin
 
-# add_library(mav_msgs SHARED ${MAV_PROTO_SRCS})
-# add_library(physics_msgs SHARED ${PHY_PROTO_SRCS})
-
-# add_library(MrsGazeboCommonResources_MotorPropModelPlugin SHARED
-#   src/physics_plugins/motor_prop_model_plugin.cpp
-#   )
+add_library(MrsGazeboCommonResources_MotorPropModelPlugin SHARED
+  src/physics_plugins/motor_prop_model_plugin.cpp
+  ${SEN_PROTO_SRCS}
+  )
 
-# target_link_libraries(MrsGazeboCommonResources_MotorPropModelPlugin
-#   ${GAZEBO_LIBRARIES}
-#   ${catkin_LIBRARIES}
-#   ${Boost_LIBRARIES}
-#   )
+target_link_libraries(MrsGazeboCommonResources_MotorPropModelPlugin
+  ${GAZEBO_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  )
 
 ## --------------------------------------------------------------
 ## |                           Install                          |
diff --git a/include/motor_prop_model.h b/include/motor_prop_model.h
new file mode 100644
index 0000000..05f7c67
--- /dev/null
+++ b/include/motor_prop_model.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+
+#include <stdio.h>
+
+#include <boost/bind.hpp>
+#include <Eigen/Eigen>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+#include <rotors_model/motor_model.hpp>
+#include "CommandMotorSpeed.pb.h"
+#include "gazebo/transport/transport.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "MotorSpeed.pb.h"
+#include "Float.pb.h"
+#include "Wind.pb.h"
+
+#include "common.h"
+
+
+namespace turning_direction {
+const static int CCW = 1;
+const static int CW = -1;
+}
+
+namespace gazebo {
+// Default values
+static const std::string kDefaultNamespace = "";
+static const std::string kDefaultCommandSubTopic = "/gazebo/command/motor_speed";
+static const std::string kDefaultMotorFailureNumSubTopic = "/gazebo/motor_failure_num";
+static const std::string kDefaultMotorVelocityPubTopic = "/motor_speed";
+std::string wind_sub_topic_ = "/world_wind";
+
+typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
+typedef const boost::shared_ptr<const physics_msgs::msgs::Wind> WindPtr;
+/*
+// Protobuf test
+typedef const boost::shared_ptr<const mav_msgs::msgs::MotorSpeed> MotorSpeedPtr;
+static const std::string kDefaultMotorTestSubTopic = "motors";
+*/
+
+// Set the max_force_ to the max double value. The limitations get handled by the FirstOrderFilter.
+static constexpr double kDefaultMaxForce = std::numeric_limits<double>::max();
+static constexpr double kDefaultMotorConstant = 8.54858e-06;
+static constexpr double kDefaultMomentConstant = 0.016;
+static constexpr double kDefaultTimeConstantUp = 1.0 / 80.0;
+static constexpr double kDefaultTimeConstantDown = 1.0 / 40.0;
+static constexpr double kDefaulMaxRotVelocity = 838.0;
+static constexpr double kDefaultRotorDragCoefficient = 1.0e-4;
+static constexpr double kDefaultRollingMomentCoefficient = 1.0e-6;
+static constexpr double kDefaultRotorVelocitySlowdownSim = 10.0;
+
+class GazeboMotorPropModel : public MotorPropModel, public ModelPlugin {
+ public:
+  GazeboMotorPropModel()
+      : MotorPropModel(), 
+        ModelPlugin(){
+  }
+
+  virtual ~GazeboMotorPropModel();
+
+  virtual void InitializeParams();
+  virtual void Publish();
+  //void testProto(MotorSpeedPtr &msg);
+ protected:
+  virtual void UpdateForcesAndMoments();
+  /// \brief A function to check the motor_Failure_Number_ and stimulate motor fail
+  /// \details Doing joint_->SetVelocity(0,0) for the flagged motor to fail
+  virtual void UpdateMotorFail();
+  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+  virtual void OnUpdate(const common::UpdateInfo & /*_info*/);
+
+ private:
+  std::string command_sub_topic_{kDefaultCommandSubTopic};
+  std::string motor_failure_sub_topic_{kDefaultMotorFailureNumSubTopic};
+  std::string joint_name_;
+  std::string link_name_;
+  std::string motor_speed_pub_topic_{kDefaultMotorVelocityPubTopic};
+  std::string namespace_;
+
+  int motor_number_{0};
+  int turning_direction_{turning_direction::CW};
+
+  int motor_Failure_Number_{0}; /*!< motor_Failure_Number is (motor_number_ + 1) as (0) is considered no_fail. Publish accordingly */
+  int tmp_motor_num; // A temporary variable used to print msg
+
+  int screen_msg_flag = 1;
+
+  bool motor_ok_flag_ = true;
+  bool enable_motor_crash_ = true;
+  double last_motor_speed_ = 0.0;
+
+  double max_force_{kDefaultMaxForce};
+  double max_rot_velocity_{kDefaulMaxRotVelocity};
+  double moment_constant_{kDefaultMomentConstant};
+  double motor_constant_{kDefaultMotorConstant};
+  double ref_motor_rot_vel_{0.0};
+  double rolling_moment_coefficient_{kDefaultRollingMomentCoefficient};
+  double rotor_drag_coefficient_{kDefaultRotorDragCoefficient};
+  double rotor_velocity_slowdown_sim_{kDefaultRotorVelocitySlowdownSim};
+  double time_constant_down_{kDefaultTimeConstantDown};
+  double time_constant_up_{kDefaultTimeConstantUp};
+
+  bool reversible_{false};
+
+  transport::NodePtr node_handle_;
+  transport::PublisherPtr motor_velocity_pub_;
+  transport::SubscriberPtr command_sub_;
+  transport::SubscriberPtr motor_failure_sub_; /*!< Subscribing to motor_failure_sub_topic_; receiving motor number to fail, as an integer */
+  transport::SubscriberPtr wind_sub_;
+
+  ignition::math::Vector3d wind_vel_;
+
+  physics::ModelPtr model_;
+  physics::JointPtr joint_;
+  common::PID pid_;
+  bool use_pid_;
+  physics::LinkPtr link_;
+  /// \brief Pointer to the update event connection.
+  event::ConnectionPtr updateConnection_;
+
+  boost::thread callback_queue_thread_;
+  void QueueThread();
+  std_msgs::msgs::Float turning_velocity_msg_;
+  void VelocityCallback(CommandMotorSpeedPtr &rot_velocities);
+  void MotorFailureCallback(const boost::shared_ptr<const msgs::Int> &fail_msg);  /*!< Callback for the motor_failure_sub_ subscriber */
+  void WindVelocityCallback(const boost::shared_ptr<const physics_msgs::msgs::Wind> &msg);
+
+  std::unique_ptr<FirstOrderFilter<double>>  rotor_velocity_filter_;
+/*
+  // Protobuf test
+  std::string motor_test_sub_topic_;
+  transport::SubscriberPtr motor_sub_;
+*/
+};
+}
diff --git a/include/rotors_model/motor_model.hpp b/include/rotors_model/motor_model.hpp
new file mode 100644
index 0000000..56fe226
--- /dev/null
+++ b/include/rotors_model/motor_model.hpp
@@ -0,0 +1,60 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef ROTORS_MODEL_MOTOR_MODEL_H
+#define ROTORS_MODEL_MOTOR_MODEL_H
+
+#include <Eigen/Eigen>
+
+class MotorPropModel
+{
+  public:
+    MotorPropModel()
+        : motor_rot_vel_(0),
+          ref_motor_rot_vel_(0),
+          prev_sim_time_(0),
+          sampling_time_(0.01) {}
+    virtual ~MotorPropModel() {}
+
+    void GetMotorVelocity(double &result) const {
+      result = motor_rot_vel_;
+    }
+
+    void SetReferenceMotorVelocity(double ref_motor_rot_vel) {
+      ref_motor_rot_vel_ = ref_motor_rot_vel;
+    }
+
+    virtual void InitializeParams() = 0;
+
+    virtual void Publish() = 0;
+
+  protected:
+    double motor_rot_vel_;
+    double ref_motor_rot_vel_;
+    double prev_ref_motor_rot_vel_;
+    double prev_sim_time_;
+    double sampling_time_;
+
+
+    virtual void UpdateForcesAndMoments() = 0;
+};
+
+#endif // ROTORS_MODEL_MOTOR_MODEL_H
diff --git a/msgs/CommandMotorSpeed.proto b/msgs/CommandMotorSpeed.proto
new file mode 100644
index 0000000..29ec4f2
--- /dev/null
+++ b/msgs/CommandMotorSpeed.proto
@@ -0,0 +1,7 @@
+syntax = "proto2";
+package mav_msgs.msgs;
+
+message CommandMotorSpeed
+{
+  repeated float   motor_speed  = 1 [packed=true];
+}
diff --git a/msgs/MotorSpeed.proto b/msgs/MotorSpeed.proto
new file mode 100644
index 0000000..35009ac
--- /dev/null
+++ b/msgs/MotorSpeed.proto
@@ -0,0 +1,7 @@
+syntax = "proto2";
+package mav_msgs.msgs;
+
+message MotorSpeed
+{
+  repeated float   motor_speed  = 1 [packed=true];
+}
diff --git a/msgs/Wind.proto b/msgs/Wind.proto
new file mode 100644
index 0000000..9cd1216
--- /dev/null
+++ b/msgs/Wind.proto
@@ -0,0 +1,10 @@
+syntax = "proto2";
+package physics_msgs.msgs;
+import "vector3d.proto";
+
+message Wind
+{
+  required string frame_id = 1;
+  required int64 time_usec = 2;
+  required gazebo.msgs.Vector3d velocity = 3;
+}
diff --git a/package.xml b/package.xml
index 95128dc..5bc650f 100644
--- a/package.xml
+++ b/package.xml
@@ -24,6 +24,7 @@
   <depend>gazebo</depend>
   <depend>boost</depend>
   <depend>libqt5-widgets</depend>
+  <depend>mav_msgs</depend>
 
   <export>
     <!-- gazebo_ros_paths_plugin automatically adds these to
diff --git a/src/physics_plugins/README.md b/src/physics_plugins/README.md
index 1b17757..6531e74 100644
--- a/src/physics_plugins/README.md
+++ b/src/physics_plugins/README.md
@@ -4,6 +4,7 @@ This directory includes plugins that somehow modify the physics behavior of the
 
 ## Plugins
 * fluid_resistace_plugin
+* motor_prop_model_plugin
 
 ### Fluid Resistance
 This plugin models linear resistance of the air (Fd = resistance_constant * linear_body_velocity).
@@ -15,5 +16,12 @@ y: 0.0
 z: 0.0" 
 ```
 
+### Motor Propeller Model 
+This plugin models the motor with propeller. 
+Currently it is just modificaiton of the Fadri Furre's work, which was slightly modified for our needs.
 
+*Changes*
+
+- Motor speed republisher was added.
+- Linear limitation of the motor's generated force was removed (it was modeled such as the motor's force was zero at 25 m/s).
 
diff --git a/src/physics_plugins/motor_prop_model_plugin.cpp b/src/physics_plugins/motor_prop_model_plugin.cpp
new file mode 100644
index 0000000..5a50385
--- /dev/null
+++ b/src/physics_plugins/motor_prop_model_plugin.cpp
@@ -0,0 +1,334 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "motor_prop_model.h"
+#include <ignition/math.hh>
+
+namespace gazebo {
+
+GazeboMotorPropModel::~GazeboMotorPropModel() {
+  updateConnection_->~Connection();
+  use_pid_ = false;
+}
+
+void GazeboMotorPropModel::InitializeParams() {}
+
+void GazeboMotorPropModel::Publish() {
+  turning_velocity_msg_.set_data(joint_->GetVelocity(0));
+  // FIXME: Commented out to prevent warnings about queue limit reached.
+  motor_velocity_pub_->Publish(turning_velocity_msg_);
+}
+
+/* Load() //{ */
+void GazeboMotorPropModel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+  model_ = _model;
+
+  namespace_.clear();
+
+  if (_sdf->HasElement("robotNamespace"))
+    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+  else
+    gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
+  node_handle_ = transport::NodePtr(new transport::Node());
+  node_handle_->Init(namespace_);
+
+  if (_sdf->HasElement("jointName"))
+    joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
+  else
+    gzerr << "[gazebo_motor_model] Please specify a jointName, where the rotor is attached.\n";
+  // Get the pointer to the joint.
+  joint_ = model_->GetJoint(joint_name_);
+  if (joint_ == NULL)
+    gzthrow("[gazebo_motor_model] Couldn't find specified joint \"" << joint_name_ << "\".");
+
+  // setup joint control pid to control joint
+  if (_sdf->HasElement("joint_control_pid"))
+  {
+    sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid");
+    double p = 0.1;
+    if (pid->HasElement("p"))
+      p = pid->Get<double>("p");
+    double i = 0;
+    if (pid->HasElement("i"))
+      i = pid->Get<double>("i");
+    double d = 0;
+    if (pid->HasElement("d"))
+      d = pid->Get<double>("d");
+    double iMax = 0;
+    if (pid->HasElement("iMax"))
+      iMax = pid->Get<double>("iMax");
+    double iMin = 0;
+    if (pid->HasElement("iMin"))
+      iMin = pid->Get<double>("iMin");
+    double cmdMax = 3;
+    if (pid->HasElement("cmdMax"))
+      cmdMax = pid->Get<double>("cmdMax");
+    double cmdMin = -3;
+    if (pid->HasElement("cmdMin"))
+      cmdMin = pid->Get<double>("cmdMin");
+    pid_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
+    use_pid_ = true;
+  }
+  else
+  {
+    use_pid_ = false;
+  }
+
+  if (_sdf->HasElement("linkName"))
+    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
+  else
+    gzerr << "[gazebo_motor_model] Please specify a linkName of the rotor.\n";
+  link_ = model_->GetLink(link_name_);
+  if (link_ == NULL)
+    gzthrow("[gazebo_motor_model] Couldn't find specified link \"" << link_name_ << "\".");
+
+
+  if (_sdf->HasElement("motorNumber"))
+    motor_number_ = _sdf->GetElement("motorNumber")->Get<int>();
+  else
+    gzerr << "[gazebo_motor_model] Please specify a motorNumber.\n";
+
+  if (_sdf->HasElement("turningDirection")) {
+    std::string turning_direction = _sdf->GetElement("turningDirection")->Get<std::string>();
+    if (turning_direction == "cw")
+      turning_direction_ = turning_direction::CW;
+    else if (turning_direction == "ccw")
+      turning_direction_ = turning_direction::CCW;
+    else
+      gzerr << "[gazebo_motor_model] Please only use 'cw' or 'ccw' as turningDirection.\n";
+  }
+  else
+    gzerr << "[gazebo_motor_model] Please specify a turning direction ('cw' or 'ccw').\n";
+
+  if(_sdf->HasElement("reversible")) {
+    reversible_ = _sdf->GetElement("reversible")->Get<bool>();
+  }
+
+  getSdfParam<std::string>(_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_);
+  getSdfParam<std::string>(_sdf, "motorSpeedPubTopic", motor_speed_pub_topic_,
+                           motor_speed_pub_topic_);
+
+  getSdfParam<double>(_sdf, "rotorDragCoefficient", rotor_drag_coefficient_, rotor_drag_coefficient_);
+  getSdfParam<double>(_sdf, "rollingMomentCoefficient", rolling_moment_coefficient_,
+                      rolling_moment_coefficient_);
+  getSdfParam<double>(_sdf, "maxRotVelocity", max_rot_velocity_, max_rot_velocity_);
+  getSdfParam<double>(_sdf, "motorConstant", motor_constant_, motor_constant_);
+  getSdfParam<double>(_sdf, "momentConstant", moment_constant_, moment_constant_);
+
+  getSdfParam<double>(_sdf, "timeConstantUp", time_constant_up_, time_constant_up_);
+  getSdfParam<double>(_sdf, "timeConstantDown", time_constant_down_, time_constant_down_);
+  getSdfParam<double>(_sdf, "rotorVelocitySlowdownSim", rotor_velocity_slowdown_sim_, 10);
+
+  getSdfParam<bool>(_sdf, "enableMotorCrash", enable_motor_crash_, true);
+
+  /*
+  std::cout << "Subscribing to: " << motor_test_sub_topic_ << std::endl;
+  motor_sub_ = node_handle_->Subscribe<mav_msgs::msgs::MotorSpeed>("~/" + model_->GetName() + motor_test_sub_topic_, &GazeboMotorPropModel::testProto, this);
+  */
+
+  // Set the maximumForce on the joint. This is deprecated from V5 on, and the joint won't move.
+#if GAZEBO_MAJOR_VERSION < 5
+  joint_->SetMaxForce(0, max_force_);
+#endif
+  // Listen to the update event. This event is broadcast every
+  // simulation iteration.
+  updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboMotorPropModel::OnUpdate, this, _1));
+
+  command_sub_ = node_handle_->Subscribe<mav_msgs::msgs::CommandMotorSpeed>("~/" + model_->GetName() + command_sub_topic_, &GazeboMotorPropModel::VelocityCallback, this);
+  //std::cout << "[gazebo_motor_model]: Subscribe to gz topic: "<< motor_failure_sub_topic_ << std::endl;
+  motor_failure_sub_ = node_handle_->Subscribe<msgs::Int>(motor_failure_sub_topic_, &GazeboMotorPropModel::MotorFailureCallback, this);
+  // FIXME: Commented out to prevent warnings about queue limit reached.
+  motor_velocity_pub_ = node_handle_->Advertise<std_msgs::msgs::Float>("~/" + model_->GetName() + motor_speed_pub_topic_, 1);
+  wind_sub_ = node_handle_->Subscribe("~/" + wind_sub_topic_, &GazeboMotorPropModel::WindVelocityCallback, this);
+
+  // Create the first order filter.
+  rotor_velocity_filter_.reset(new FirstOrderFilter<double>(time_constant_up_, time_constant_down_, ref_motor_rot_vel_));
+}
+//}
+
+// Protobuf test
+/*
+void GazeboMotorPropModel::testProto(MotorSpeedPtr &msg) {
+  std::cout << "Received message" << std::endl;
+  std::cout << msg->motor_speed_size()<< std::endl;
+  for(int i; i < msg->motor_speed_size(); i++){
+    std::cout << msg->motor_speed(i) <<" ";
+  }
+  std::cout << std::endl;
+}
+*/
+
+/* OnUpdate() //{ */
+// This gets called by the world update start event.
+void GazeboMotorPropModel::OnUpdate(const common::UpdateInfo& _info) {
+  sampling_time_ = _info.simTime.Double() - prev_sim_time_;
+  prev_sim_time_ = _info.simTime.Double();
+  UpdateForcesAndMoments();
+  UpdateMotorFail();
+  Publish();
+}
+//}
+
+/* VelocityCallback() //{  */
+void GazeboMotorPropModel::VelocityCallback(CommandMotorSpeedPtr &rot_velocities) {
+  if(rot_velocities->motor_speed_size() < motor_number_) {
+    std::cout  << "You tried to access index " << motor_number_
+      << " of the MotorSpeed message array which is of size " << rot_velocities->motor_speed_size() << "." << std::endl;
+  } else ref_motor_rot_vel_ = std::min(static_cast<double>(rot_velocities->motor_speed(motor_number_)), static_cast<double>(max_rot_velocity_));
+}
+//}
+
+void GazeboMotorPropModel::MotorFailureCallback(const boost::shared_ptr<const msgs::Int> &fail_msg) {
+  motor_Failure_Number_ = fail_msg->data();
+}
+
+/* UpdateForcesAndMoments() //{ */
+void GazeboMotorPropModel::UpdateForcesAndMoments() {
+  motor_rot_vel_ = joint_->GetVelocity(0);
+  if (motor_rot_vel_ / (2 * M_PI) > 1 / (2 * sampling_time_)) {
+    gzerr << "Aliasing on motor [" << motor_number_ << "] might occur. Consider making smaller simulation time steps or raising the rotor_velocity_slowdown_sim_ param.\n";
+  }
+  double real_motor_velocity = motor_rot_vel_ * rotor_velocity_slowdown_sim_;
+  double force = real_motor_velocity * std::abs(real_motor_velocity) * motor_constant_;
+  if(!reversible_) {
+    // Not allowed to have negative thrust.
+    force = std::abs(force);
+  }
+
+  // scale down force linearly with forward speed
+  // XXX this has to be modelled better
+  
+  double speed_diff = last_motor_speed_ - std::fabs(real_motor_velocity);
+
+  if (speed_diff > 0.15 && motor_ok_flag_ && enable_motor_crash_) {
+    std::cout << "Motor  " << motor_number_ << " crashed!" << enable_motor_crash_ <<std::endl;
+    motor_ok_flag_ = false;
+  }
+
+  last_motor_speed_ = std::fabs(real_motor_velocity);
+
+#if GAZEBO_MAJOR_VERSION >= 9
+  ignition::math::Vector3d body_velocity = link_->WorldLinearVel();
+  ignition::math::Vector3d joint_axis = joint_->GlobalAxis(0);
+#else
+  ignition::math::Vector3d body_velocity = ignitionFromGazeboMath(link_->GetWorldLinearVel());
+  ignition::math::Vector3d joint_axis = ignitionFromGazeboMath(joint_->GetGlobalAxis(0));
+#endif
+
+  ignition::math::Vector3d relative_wind_velocity = body_velocity - wind_vel_;
+  ignition::math::Vector3d velocity_parallel_to_rotor_axis = (relative_wind_velocity.Dot(joint_axis)) * joint_axis;
+  // TODO: modify calculation of the reduction of the generated motor's force  according to the airspeed  
+  /* double vel = velocity_parallel_to_rotor_axis.Length(); */
+  double scalar = 1 ; // - vel / 25.0; // at 25 m/s the rotor will not produce any force anymore
+  scalar = ignition::math::clamp(scalar, 0.0, 1.0);
+  // Apply a force to the link.
+  link_->AddRelativeForce(ignition::math::Vector3d(0, 0, force * scalar));
+
+  // Forces from Philppe Martin's and Erwan Salaün's
+  // 2010 IEEE Conference on Robotics and Automation paper
+  // The True Role of Accelerometer Feedback in Quadrotor Control
+  // - \omega * \lambda_1 * V_A^{\perp}
+  ignition::math::Vector3d velocity_perpendicular_to_rotor_axis = relative_wind_velocity - (relative_wind_velocity.Dot(joint_axis)) * joint_axis;
+  ignition::math::Vector3d air_drag = -std::abs(real_motor_velocity) * rotor_drag_coefficient_ * velocity_perpendicular_to_rotor_axis;
+  // Apply air_drag to link.
+  link_->AddForce(air_drag);
+  // Moments
+  // Getting the parent link, such that the resulting torques can be applied to it.
+  physics::Link_V parent_links = link_->GetParentJointsLinks();
+  // The tansformation from the parent_link to the link_.
+#if GAZEBO_MAJOR_VERSION >= 9
+  ignition::math::Pose3d pose_difference = link_->WorldCoGPose() - parent_links.at(0)->WorldCoGPose();
+#else
+  ignition::math::Pose3d pose_difference = ignitionFromGazeboMath(link_->GetWorldCoGPose() - parent_links.at(0)->GetWorldCoGPose());
+#endif
+  ignition::math::Vector3d drag_torque(0, 0, -turning_direction_ * force * moment_constant_);
+  // Transforming the drag torque into the parent frame to handle arbitrary rotor orientations.
+  ignition::math::Vector3d drag_torque_parent_frame = pose_difference.Rot().RotateVector(drag_torque);
+  parent_links.at(0)->AddRelativeTorque(drag_torque_parent_frame);
+
+  ignition::math::Vector3d rolling_moment;
+  // - \omega * \mu_1 * V_A^{\perp}
+  rolling_moment = -std::abs(real_motor_velocity) * turning_direction_ * rolling_moment_coefficient_ * velocity_perpendicular_to_rotor_axis;
+  parent_links.at(0)->AddTorque(rolling_moment);
+  // Apply the filter on the motor's velocity.
+  double ref_motor_rot_vel;
+  ref_motor_rot_vel = rotor_velocity_filter_->updateFilter(ref_motor_rot_vel_, sampling_time_);
+
+#if 0 //FIXME: disable PID for now, it does not play nice with the PX4 CI system.
+  if (use_pid_)
+  {
+    double err = joint_->GetVelocity(0) - turning_direction_ * ref_motor_rot_vel / rotor_velocity_slowdown_sim_;
+    double rotorForce = pid_.Update(err, sampling_time_);
+    joint_->SetForce(0, rotorForce);
+    // gzerr << "rotor " << joint_->GetName() << " : " << rotorForce << "\n";
+  }
+  else
+  {
+#if GAZEBO_MAJOR_VERSION >= 7
+    // Not desirable to use SetVelocity for parts of a moving model
+    // impact on rest of the dynamic system is non-physical.
+    joint_->SetVelocity(0, turning_direction_ * ref_motor_rot_vel / rotor_velocity_slowdown_sim_);
+#elif GAZEBO_MAJOR_VERSION >= 6
+    // Not ideal as the approach could result in unrealistic impulses, and
+    // is only available in ODE
+    joint_->SetParam("fmax", 0, 2.0);
+    joint_->SetParam("vel", 0, turning_direction_ * ref_motor_rot_vel / rotor_velocity_slowdown_sim_);
+#endif
+  }
+#else
+  if (motor_ok_flag_) {
+    joint_->SetVelocity(0, turning_direction_ * ref_motor_rot_vel / rotor_velocity_slowdown_sim_);
+  } else {
+    joint_->SetVelocity(0, 0);
+  }
+#endif /* if 0 */
+}
+//}
+
+/* UpdateMotorFail() //{ */
+void GazeboMotorPropModel::UpdateMotorFail() {
+  if (motor_number_ == motor_Failure_Number_ - 1){
+    // motor_constant_ = 0.0;
+    joint_->SetVelocity(0,0);
+    if (screen_msg_flag){
+      std::cout << "Motor number [" << motor_Failure_Number_ <<"] failed!  [Motor thrust = 0]" << std::endl;
+      tmp_motor_num = motor_Failure_Number_;
+
+      screen_msg_flag = 0;
+    }
+  }else if (motor_Failure_Number_ == 0 && motor_number_ ==  tmp_motor_num - 1){
+     if (!screen_msg_flag){
+       //motor_constant_ = kDefaultMotorConstant;
+       std::cout << "Motor number [" << tmp_motor_num <<"] running! [Motor thrust = (default)]" << std::endl;
+       screen_msg_flag = 1;
+     }
+  }
+}
+//}
+
+void GazeboMotorPropModel::WindVelocityCallback(WindPtr& msg) {
+  wind_vel_ = ignition::math::Vector3d(msg->velocity().x(),
+            msg->velocity().y(),
+            msg->velocity().z());
+}
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboMotorPropModel);
+}

From 3dc5195ee19dd41a179d5f054d317a9bf0f3cf08 Mon Sep 17 00:00:00 2001
From: ondra <prochon4@fel.cvut.cz>
Date: Thu, 14 Sep 2023 16:40:48 +0200
Subject: [PATCH 05/12] Modified calculation of the drag force.

---
 src/physics_plugins/README.md                 |  2 +-
 .../fluid_resistance_plugin.cpp               | 55 ++++++++++++++-----
 2 files changed, 41 insertions(+), 16 deletions(-)

diff --git a/src/physics_plugins/README.md b/src/physics_plugins/README.md
index 6531e74..46ab769 100644
--- a/src/physics_plugins/README.md
+++ b/src/physics_plugins/README.md
@@ -7,7 +7,7 @@ This directory includes plugins that somehow modify the physics behavior of the
 * motor_prop_model_plugin
 
 ### Fluid Resistance
-This plugin models linear resistance of the air (Fd = resistance_constant * linear_body_velocity).
+This plugin models linear resistance of the air (Fd = model_mass * resistance_constant * linear_body_velocity).
 You can set resistance coefficientsa (for every axis in body frame) in model definition using fluid resistance macro.
 It is also possible to change fluid resistance while simulation is running using topic */fluid_resistance*.
 ```
diff --git a/src/physics_plugins/fluid_resistance_plugin.cpp b/src/physics_plugins/fluid_resistance_plugin.cpp
index 00d2f56..c56abd4 100644
--- a/src/physics_plugins/fluid_resistance_plugin.cpp
+++ b/src/physics_plugins/fluid_resistance_plugin.cpp
@@ -77,10 +77,12 @@ class FluidResistancePlugin : public ModelPlugin {
 #endif
 
 private:
-  double rate  = 1.0;
-  double res_x = 1.0;
-  double res_y = 1.0;
-  double res_z = 1.0;
+  double rate       = 1.0;
+  double res_x      = 1.0;
+  double res_y      = 1.0;
+  double res_z      = 1.0;
+  double model_mass = 1.13;
+  bool   verbose    = false;
 
 private:
   float last_time = 0.0;
@@ -116,16 +118,25 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     rate = _sdf->Get<double>("rate");
   } else {
     ROS_WARN(
-        "[FluidResistancePlugin]: No rate Given name given, setting default "
+        "[FluidResistancePlugin]: No rate Given, setting default "
         "name %f",
         rate);
   }
 
+  if (_sdf->HasElement("model_mass")) {
+    model_mass = _sdf->Get<double>("model_mass");
+  } else {
+    ROS_WARN(
+        "[FluidResistancePlugin]: No model_mass Given, setting default "
+        "name %f",
+        model_mass);
+  }
+
   if (_sdf->HasElement("res_x")) {
     res_x = _sdf->Get<double>("res_x");
   } else {
     ROS_WARN(
-        "[FluidResistancePlugin]: No res_x Given name given, setting default "
+        "[FluidResistancePlugin]: No res_x Given, setting default "
         "name %f",
         res_x);
   }
@@ -134,7 +145,7 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     res_y = _sdf->Get<double>("res_y");
   } else {
     ROS_WARN(
-        "[FluidResistancePlugin]: No res_y Given name given, setting default "
+        "[FluidResistancePlugin]: No res_y Given, setting default "
         "name %f",
         res_y);
   }
@@ -143,12 +154,23 @@ void FluidResistancePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf
     res_z = _sdf->Get<double>("res_z");
   } else {
     ROS_WARN(
-        "[FluidResistancePlugin]: No res_z Given name given, setting default "
+        "[FluidResistancePlugin]: No res_z Given, setting default "
         "name %f",
         res_z);
   }
 
-  ROS_INFO("[FluidResistancePlugin]: All elements loaded successfully!");
+  if (_sdf->HasElement("verbose")) {
+    verbose = _sdf->Get<double>("verbose");
+  } else {
+    ROS_WARN(
+        "[FluidResistancePlugin]: No verbose Given, setting default "
+        "name %d",
+        verbose);
+  }
+
+  if (verbose) {
+    ROS_INFO("[FluidResistancePlugin]: All elements loaded successfully!");
+  }
 
   //}
 
@@ -241,11 +263,13 @@ void FluidResistancePlugin::ApplyResistance() {
   math::Vector3 force, torque;
 #endif
 
-  /* ROS_WARN("[FluidResistancePlugin]: LinearSpeed = [%f,%f,%f] ", now_lin_vel.X(), now_lin_vel.Y(), now_lin_vel.Z()); */
+  if (verbose) {
+    ROS_WARN("[FluidResistancePlugin]: LinearSpeed = [%f,%f,%f] ", now_lin_vel.X(), now_lin_vel.Y(), now_lin_vel.Z());
+  }
 
-  force.X(-1.0 * res_x * now_lin_vel.X());
-  force.Y(-1.0 * res_y * now_lin_vel.Y());
-  force.Z(-1.0 * res_z * now_lin_vel.Z());
+  force.X(-1.0 * (res_x * model_mass) * now_lin_vel.X());
+  force.Y(-1.0 * (res_y * model_mass) * now_lin_vel.Y());
+  force.Z(-1.0 * (res_z * model_mass) * now_lin_vel.Z());
 
   // Applying force to uav
   link_to_apply_resistance->AddRelativeForce(force);
@@ -254,8 +278,9 @@ void FluidResistancePlugin::ApplyResistance() {
 #else
   link_to_apply_resistance->AddRelativeTorque(torque - link_to_apply_resistance->GetInertial()->GetCoG().Cross(force));
 #endif
-
-  /* ROS_WARN("[FluidResistancePlugin]: FluidResistanceApplying = [%f,%f,%f] ", force.X(), force.Y(), force.Z()); */
+  if (verbose) {
+    ROS_WARN("[FluidResistancePlugin]: FluidResistanceApplying = [%f,%f,%f] ", force.X(), force.Y(), force.Z());
+  }
 }
 //}
 

From 129d1326e303c14d0cd66e1020f079ccec8f6bb5 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Sat, 16 Sep 2023 14:39:57 +0200
Subject: [PATCH 06/12] updated workflow

---
 .github/workflows/ros_package_build.yml | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/.github/workflows/ros_package_build.yml b/.github/workflows/ros_package_build.yml
index a7670c8..100c48e 100644
--- a/.github/workflows/ros_package_build.yml
+++ b/.github/workflows/ros_package_build.yml
@@ -5,9 +5,6 @@ on:
   push:
     branches: [ hw_api ]
 
-  pull_request:
-    branches: [ hw_api ]
-
   workflow_dispatch:
 
 concurrency:

From cddcd84056ff2f48b62b0a3044e00d7912ff9d13 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Tue, 3 Oct 2023 14:23:54 +0200
Subject: [PATCH 07/12] fixing compilation on arm

---
 .github/workflows/ros_build_test.yml          | 19 +++++++++++++++++++
 CMakeLists.txt                                |  5 +++--
 .../motor_prop_model_plugin.cpp               |  2 +-
 3 files changed, 23 insertions(+), 3 deletions(-)
 create mode 100644 .github/workflows/ros_build_test.yml

diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml
new file mode 100644
index 0000000..994cbbf
--- /dev/null
+++ b/.github/workflows/ros_build_test.yml
@@ -0,0 +1,19 @@
+name: ros_package_build
+
+on:
+
+  push:
+    branches: [ testing ]
+
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+
+  build:
+    uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master
+    secrets:
+      PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f78df94..1cb7828 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -347,8 +347,9 @@ target_link_libraries(MrsGazeboCommonResources_FluidResistancePlugin
 
 # MotorSpeedRepublisherPlugin
 
-add_library(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
-  SHARED src/sensor_and_model_plugins/motor_speed_republisher.cpp
+add_library(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin SHARED
+  src/sensor_and_model_plugins/motor_speed_republisher.cpp
+  ${SEN_PROTO_SRCS}
   )
 
 target_link_libraries(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
diff --git a/src/physics_plugins/motor_prop_model_plugin.cpp b/src/physics_plugins/motor_prop_model_plugin.cpp
index 5a50385..44edab1 100644
--- a/src/physics_plugins/motor_prop_model_plugin.cpp
+++ b/src/physics_plugins/motor_prop_model_plugin.cpp
@@ -19,7 +19,7 @@
  */
 
 
-#include "motor_prop_model.h"
+#include <motor_prop_model.h>
 #include <ignition/math.hh>
 
 namespace gazebo {

From f99e8f01c56b9cbede97cb605eee1aa3690665d0 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Tue, 3 Oct 2023 14:28:15 +0200
Subject: [PATCH 08/12] changed to devel branch

---
 .github/workflows/ros_build_test.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml
index 994cbbf..e407143 100644
--- a/.github/workflows/ros_build_test.yml
+++ b/.github/workflows/ros_build_test.yml
@@ -3,7 +3,7 @@ name: ros_package_build
 on:
 
   push:
-    branches: [ testing ]
+    branches: [ devel ]
 
   workflow_dispatch:
 

From a741e2649816d00bcf5a8e7335f1af0cdae73233 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Tue, 3 Oct 2023 14:30:01 +0200
Subject: [PATCH 09/12] renamed workflow

---
 .github/workflows/ros_build_test.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml
index e407143..b01a8fa 100644
--- a/.github/workflows/ros_build_test.yml
+++ b/.github/workflows/ros_build_test.yml
@@ -1,4 +1,4 @@
-name: ros_package_build
+name: ros_build_test
 
 on:
 

From 8151e2c539e3d5bd40eff77beb1684c17eee2c96 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Fri, 6 Oct 2023 09:44:00 +0200
Subject: [PATCH 10/12] updated readme

---
 README.md | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/README.md b/README.md
index 2830500..5c15b98 100644
--- a/README.md
+++ b/README.md
@@ -1,16 +1,15 @@
 # MRS resources for Gazebo
 
-| Build status | [![Build Status](https://github.com/ctu-mrs/mrs_gazebo_common_resources/workflows/Noetic/badge.svg)](https://github.com/ctu-mrs/mrs_gazebo_common_resources/actions) |
-|--------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-
-This package contains common gazebo files (worlds, models and plugins) for the MRS system.
+This package contains common gazebo files (worlds, models and plugins) for the [MRS UAV system](https://github.com/ctu-mrs/mrs_uav_system).
 
 ## Worlds
+
 - [forest.world](worlds/forest.world)
 - [grass_plane.world](worlds/grass_plane.world)
 - [uneven_plane.world](worlds/uneven_plane.world)
 
 ## Models
+
 - [grass_plane](models/grass_plane)
 - [grey_wall](models/grey_wall)
 - [static_camera](models/static_camera)
@@ -24,6 +23,7 @@ This package contains common gazebo files (worlds, models and plugins) for the M
 - [safety_led](models/safety_led)
 
 ## Plugins
+
 - [world_plugins](src/world_plugins) - Detail descriptions in [README](src/world_plugins/README.md)
 - [gui_plugins](src/gui_plugins) - Detail descriptions in [README](src/gui_plugins/README.md)
 - [sensor_and_model_plugins](src/sensor_and_model_plugins) - Detail descriptions in [README](src/sensor_and_model_plugins/README.md)

From d7d5b151731f913b3472373d8f9a31710fd9f2d0 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Fri, 6 Oct 2023 09:49:33 +0200
Subject: [PATCH 11/12] updated actions

---
 .github/workflows/ros_build_test.yml    | 25 +++++++++++++++++++++++++
 .github/workflows/ros_package_build.yml |  3 +++
 2 files changed, 28 insertions(+)
 create mode 100644 .github/workflows/ros_build_test.yml

diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml
new file mode 100644
index 0000000..f6f91cf
--- /dev/null
+++ b/.github/workflows/ros_build_test.yml
@@ -0,0 +1,25 @@
+name: ros_build_test
+
+on:
+
+  push:
+    branches: [ devel ]
+
+    paths-ignore:
+      - '**/README.md'
+
+    pull_request:
+      branches: [ hw_api ]
+
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+
+  build:
+    uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master
+    secrets:
+      PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }}
diff --git a/.github/workflows/ros_package_build.yml b/.github/workflows/ros_package_build.yml
index 100c48e..d3bb22c 100644
--- a/.github/workflows/ros_package_build.yml
+++ b/.github/workflows/ros_package_build.yml
@@ -5,6 +5,9 @@ on:
   push:
     branches: [ hw_api ]
 
+    paths-ignore:
+      - '**/README.md'
+
   workflow_dispatch:
 
 concurrency:

From f9ae5d286e395cb4aa6c092a92c61c6c58b9a098 Mon Sep 17 00:00:00 2001
From: devel <klaxalk@gmail.com>
Date: Tue, 10 Oct 2023 13:37:38 +0200
Subject: [PATCH 12/12] updated workflows, updated readme

---
 .ci/build.sh                            | 12 -------
 .ci/install.sh                          | 46 -------------------------
 .github/workflows/ros_build_test.yml    |  2 +-
 .github/workflows/ros_package_build.yml |  2 +-
 README.md                               |  4 +++
 5 files changed, 6 insertions(+), 60 deletions(-)
 delete mode 100755 .ci/build.sh
 delete mode 100755 .ci/install.sh

diff --git a/.ci/build.sh b/.ci/build.sh
deleted file mode 100755
index 16e7dfa..0000000
--- a/.ci/build.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-set -e
-
-distro=`lsb_release -r | awk '{ print $2 }'`
-[ "$distro" = "18.04" ] && ROS_DISTRO="melodic"
-[ "$distro" = "20.04" ] && ROS_DISTRO="noetic"
-
-echo "Starting build"
-cd ~/mrs_workspace
-source /opt/ros/$ROS_DISTRO/setup.bash
-catkin build --limit-status-rate 0.2 --summarize
-echo "Ended build"
diff --git a/.ci/install.sh b/.ci/install.sh
deleted file mode 100755
index 746a8fa..0000000
--- a/.ci/install.sh
+++ /dev/null
@@ -1,46 +0,0 @@
-#!/bin/bash
-set -e
-
-distro=`lsb_release -r | awk '{ print $2 }'`
-[ "$distro" = "18.04" ] && ROS_DISTRO="melodic"
-[ "$distro" = "20.04" ] && ROS_DISTRO="noetic"
-
-# get the path to this script
-MY_PATH=`pwd`
-
-echo "Starting install"
-
-# get the current package name
-PACKAGE_NAME=${PWD##*/}
-
-sudo apt-get -y install git
-
-echo "installing uav_core"
-cd
-git clone https://github.com/ctu-mrs/uav_core
-cd uav_core
-./installation/install.sh
-gitman update
-
-echo "installing simulation"
-cd
-git clone https://github.com/ctu-mrs/simulation
-cd simulation
-./installation/install.sh
-gitman update
-
-# link the up-to-date version of this package
-rm -rf ~/simulation/.gitman/$PACKAGE_NAME
-ln -s "$MY_PATH" ~/simulation/.gitman/$PACKAGE_NAME
-
-echo "creating workspace"
-mkdir -p ~/mrs_workspace/src
-cd ~/mrs_workspace/src
-ln -s ~/simulation
-ln -s ~/uav_core/ros_packages/mavros
-ln -s ~/uav_core/ros_packages/mrs_msgs
-source /opt/ros/$ROS_DISTRO/setup.bash
-cd ~/mrs_workspace
-catkin init
-
-echo "install part ended"
diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml
index f6f91cf..28768ce 100644
--- a/.github/workflows/ros_build_test.yml
+++ b/.github/workflows/ros_build_test.yml
@@ -9,7 +9,7 @@ on:
       - '**/README.md'
 
     pull_request:
-      branches: [ hw_api ]
+      branches: [ master ]
 
   workflow_dispatch:
 
diff --git a/.github/workflows/ros_package_build.yml b/.github/workflows/ros_package_build.yml
index d3bb22c..d17da2a 100644
--- a/.github/workflows/ros_package_build.yml
+++ b/.github/workflows/ros_package_build.yml
@@ -3,7 +3,7 @@ name: ros_package_build
 on:
 
   push:
-    branches: [ hw_api ]
+    branches: [ master ]
 
     paths-ignore:
       - '**/README.md'
diff --git a/README.md b/README.md
index 5c15b98..8455d4b 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,9 @@
 # MRS resources for Gazebo
 
+> :warning: **Attention please: This README is outdated.**
+>
+> The MRS UAV System 1.5 is being released and this page needs updating. Please, keep in mind that the information on this page might not be valid.
+
 This package contains common gazebo files (worlds, models and plugins) for the [MRS UAV system](https://github.com/ctu-mrs/mrs_uav_system).
 
 ## Worlds