Add a package dropping for drone delivery

This PR adds a package dropping model standard_vtol_drop and the drop_plugin so that a drone delivery scenario can be simulated

.gitignore

@@ -27,3 +27,4 @@ models/tiltrotor/tiltrotor.sdf
 models/boat/boat.sdf
 models/typhoon_h480/typhoon_h480.sdf
 models/depth_camera/depth_camera.sdf
+models/standard_vtol_drop/standard_vtol_drop.sdf

CMakeLists.txt

@@ -357,6 +357,7 @@ add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp)
 add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp)
 add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp)
 add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp)
+add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp)
 
 set(plugins
   gazebo_airspeed_plugin
@@ -383,6 +384,7 @@ set(plugins
   gazebo_usv_dynamics_plugin
   gazebo_parachute_plugin
   gazebo_airship_dynamics_plugin
+  gazebo_drop_plugin
   )
 
 foreach(plugin ${plugins})

include/gazebo_drop_plugin.h

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 Drop Plugin
+ *
+ * This plugin simulates dropping a payload
+ *
+ * @author Jaeyoung Lim <[email protected]>
+ */
+
+#ifndef _GAZEBO_DROP_PLUGIN_HH_
+#define _GAZEBO_DROP_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 DropPlugin : public ModelPlugin
+{
+public:
+  DropPlugin();
+  virtual ~DropPlugin();
+
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  void OnUpdate(const common::UpdateInfo&);
+  physics::ModelPtr GetModelPtr(std::string model_name);
+
+private:
+  /// \brief Loads the package model
+  void LoadPackage();
+
+  /// \brief Callback for subscribing to motor commands
+   /// \param rot_velocities Motor command velocity commanded from firmware
+  void VelocityCallback(CommandMotorSpeedPtr &rot_velocities);
+
+
+  std::string namespace_;
+  physics::ModelPtr model_;
+  physics::WorldPtr world_;
+  event::ConnectionPtr update_connection_;
+
+  bool package_spawned = false;  ///< Check if the package has already been spawned in the world
+  double max_rot_velocity_ = 3500;   ///< Clip maximum motor velocity
+  double ref_motor_rot_vel_ = 0;     ///< Reference motor velocity 
+  double release_rot_vel_ = 300; ///< Motor velocity command to release the package
+  int motor_number_;
+
+  std::string trigger_sub_topic_ = "/gazebo/command/motor_speed"; ///< Parachute trigger signal topic
+  std::string model_name_ = "parachute_package";
+
+  transport::NodePtr node_handle_;
+  transport::SubscriberPtr trigger_sub_;
+
+};     // class GAZEBO_VISIBLE DropPlugin
+}      // namespace gazebo
+#endif // _GAZEBO_DROP_PLUGIN_HH_

models/parachute_package/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Parachute Package</name>
+  <version>1.0</version>
+  <sdf version="1.5">model.sdf</sdf>
+  <author>
+    <name>Jaeyoung Lim</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A package that has a parachute attached, so it can be dropped from a vehicle
+  </description>
+
+</model>

models/parachute_package/model.sdf

@@ -0,0 +1,67 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="parachute_package">
+    <include>
+      <uri>model://parachute_small</uri>
+      <pose>0 0 0.2 0 0 0</pose>
+    </include>
+    <link name="base_link">
+      <pose>0 0 0  0 0 0</pose>
+      <inertial>
+        <pose>0 0 0  0 0 0</pose>
+        <mass>1.00</mass>
+        <inertia>
+          <ixx>0.010000</ixx>
+          <ixy>0.000000</ixy>
+          <ixz>0.000000</ixz>
+          <iyy>0.010000</iyy>
+          <iyz>0.000000</iyz>
+          <izz>0.010000</izz>
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <pose>0 0 0.3  0 0 0</pose>
+        <geometry>
+          <box>
+              <size>0.15 0.1 0.18</size>
+          </box>
+        </geometry>
+	      <surface>
+          <contact>
+            <ode>
+              <max_vel>0.1</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="visual">
+        <pose>0 0 0.3  0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.125 0.125 0.125</scale>
+            <uri>model://big_box/meshes/big_box.dae</uri>
+          </mesh>
+	      </geometry>
+      </visual>
+    </link>
+    <joint name='parachute_joint' type='revolute'>
+      <child>base_link</child>
+      <parent>parachute_small::chute</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0</upper>
+          <effort>0</effort>
+          <velocity>0</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    </model>
+</sdf>

models/standard_vtol_drop/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Standard VTOL drop</name>
+  <version>1.0</version>
+  <sdf version='1.4'>standard_vtol_drop.sdf</sdf>
+
+  <author>
+   <name>Jaeyoung Lim</name>
+   <email>[email protected]</email>
+  </author>
+
+  <description>
+    This is a model of a standard VTOL quad plane.
+  </description>
+</model>