Merge remote-tracking branch 'origin/fluid_resistance_plus_motor_mode…

…l' into hw_api

CMakeLists.txt

@@ -19,7 +19,7 @@ set(CATKIN_DEPENDENCIES
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   ${CATKIN_DEPENDENCIES}
-)
+  )
 
 # include Gazebo
 find_package(gazebo REQUIRED)
@@ -64,6 +64,10 @@ 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})
@@ -94,14 +98,17 @@ set(LIBRARIES
   MrsGazeboCommonResources_ServoCameraPlugin
   MrsGazeboCommonResources_DynamicModelPlugin
   MrsGazeboCommonResources_SafetyLedPlugin
+  MrsGazeboCommonResources_FluidResistancePlugin
+  MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
+  MrsGazeboCommonResources_MotorPropModelPlugin
   )
 
 catkin_package(
-   INCLUDE_DIRS include
-   CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}
-   DEPENDS GAZEBO Qt5Widgets Boost Eigen OpenCV
-   LIBRARIES ${LIBRARIES}
-   )
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}
+  DEPENDS GAZEBO Qt5Widgets Boost Eigen OpenCV
+  LIBRARIES ${LIBRARIES}
+  )
 
 ###########
 ## Build ##
@@ -326,6 +333,43 @@ 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}
+  )
+
+# MotorSpeedRepublisherPlugin
+
+add_library(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
+  SHARED src/sensor_and_model_plugins/motor_speed_republisher.cpp
+  )
+
+target_link_libraries(MrsGazeboCommonResources_MotorSpeedRepublisherPlugin
+  ${GAZEBO_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  )
+
+# MotorPropModelPlugin
+
+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}
+  )
+
 ## --------------------------------------------------------------
 ## |                           Install                          |
 ## --------------------------------------------------------------
@@ -338,8 +382,8 @@ install(TARGETS ${LIBRARIES}
 
 install(DIRECTORY models/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models
-)
+  )
 
 install(DIRECTORY worlds/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/worlds
-)
+  )

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_;
+*/
+};
+}

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

msgs/CommandMotorSpeed.proto

@@ -0,0 +1,7 @@
+syntax = "proto2";
+package mav_msgs.msgs;
+
+message CommandMotorSpeed
+{
+  repeated float   motor_speed  = 1 [packed=true];
+}

msgs/Float.proto

@@ -0,0 +1,7 @@
+syntax = "proto2";
+package std_msgs.msgs;
+
+message Float
+{
+  required float   data  = 1;
+}

msgs/MotorSpeed.proto

@@ -0,0 +1,7 @@
+syntax = "proto2";
+package mav_msgs.msgs;
+
+message MotorSpeed
+{
+  repeated float   motor_speed  = 1 [packed=true];
+}

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;
+}

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

src/physics_plugins/README.md

@@ -0,0 +1,27 @@
+# Physics plugins for Gazebo
+
+This directory includes plugins that somehow modify the physics behavior of the UAV or its parts.
+
+## Plugins
+* fluid_resistace_plugin
+* motor_prop_model_plugin
+
+### Fluid Resistance
+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*.
+```
+rostopic pub /fluid_resistance geometry_msgs/Vector3 "x: 0.0 
+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).
+