[WIP] Add support for Monocopters

.gitignore

@@ -31,3 +31,4 @@ models/standard_vtol_drop/standard_vtol_drop.sdf
 models/tailsitter/tailsitter.sdf
 models/advanced_plane/advanced_plane.sdf
 models/quadtailsitter/quadtailsitter.sdf
+models/monocopter/monocopter.sdf

CMakeLists.txt

@@ -373,6 +373,7 @@ add_library(gazebo_motor_model SHARED src/gazebo_motor_model.cpp)
 add_library(gazebo_multirotor_base_plugin SHARED src/gazebo_multirotor_base_plugin.cpp)
 add_library(gazebo_wind_plugin SHARED src/gazebo_wind_plugin.cpp)
 add_library(gazebo_magnetometer_plugin SHARED src/gazebo_magnetometer_plugin.cpp src/geo_mag_declination.cpp)
+add_library(gazebo_monocopter_dynamics_plugin SHARED src/gazebo_monocopter_dynamics_plugin.cpp)
 add_library(gazebo_barometer_plugin SHARED src/gazebo_barometer_plugin.cpp)
 add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp)
 add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp)

include/gazebo_monocopter_dynamics_plugin.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @brief Catapult Plugin
+ *
+ * This plugin simulates the monocopter dynamics
+ *
+ * @author Jaeyoung Lim <[email protected]>
+ */
+
+#ifndef _GAZEBO_MONOCOPTER_DYNAMICS_PLUGIN_HH_
+#define _GAZEBO_MONOCOPTER_DYNAMICS_PLUGIN_HH_
+
+#include <math.h>
+#include <common.h>
+#include <sdf/sdf.hh>
+
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/util/system.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/physics/physics.hh>
+#include <ignition/math.hh>
+#include "CommandMotorSpeed.pb.h"
+
+
+#include <Odometry.pb.h>
+
+namespace gazebo
+{
+
+typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
+
+class GAZEBO_VISIBLE MonocopterDynamicsPlugin : public ModelPlugin
+{
+public:
+  MonocopterDynamicsPlugin();
+  virtual ~MonocopterDynamicsPlugin();
+
+protected:
+  virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  virtual void OnUpdate(const common::UpdateInfo&);
+
+private:
+  void TriggerCallback(const boost::shared_ptr<const msgs::Int> &_msg);
+  void VelocityCallback(CommandMotorSpeedPtr &rot_velocities);
+
+  std::string namespace_;
+  physics::ModelPtr model_;
+  physics::WorldPtr world_;
+  physics::LinkPtr link_;
+
+  event::ConnectionPtr _updateConnection;
+
+  common::Time trigger_time_;
+
+  double max_rot_velocity_ = 3500;
+  double ref_motor_rot_vel_ = 0.0;
+  double arm_rot_vel_ = 100;
+  double launch_duration_ = 0.01;
+  double force_magnitude_ = 1.0;
+  int motor_number_;
+
+  transport::NodePtr node_handle_;
+
+};     // class GAZEBO_VISIBLE MonocopterDynamicsPlugin
+}      // namespace gazebo
+#endif // _GAZEBO_MONOCOPTER_DYNAMICS_PLUGIN_HH_

models/monocopter/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Monocopter</name>
+  <version>1.0</version>
+  <sdf version='1.5'>monocopter.sdf</sdf>
+
+  <author>
+   <name>Jaeyoung Lim</name>
+   <email>[email protected]</email>
+  </author>
+
+  <description>
+    This is a model of a standard plane.
+  </description>
+</model>