Add parachute plugin (#36)

* Parachute: initial version 1. Add parachute model from px4 gazebo 2. Initial parachute support 3. gitignore upd 4. cmakelists add ParachuteLib 5. cmake add debug config 6. fix debug options 7. create parachute on release Author: Alex Molchanov <[email protected]> ardupilot parachute initial support * Parachute: migrate to gz-sim, add tests and prepare for PR 1. Parachute: namespace migration - Migrate from ignition to gz. - Fix for cpplint. Signed-off-by: Rhys Mainwaring <[email protected]> 2. Parachute: move implementation details to private class. Signed-off-by: Rhys Mainwaring <[email protected]> 3. Parachute: update plugin - Update variable name to Gazebo style. - Remove unused variables. - Only create parachute model if not already created. Signed-off-by: Rhys Mainwaring <[email protected]> 4. Parachute: update plugin - Fix linter. Signed-off-by: Rhys Mainwaring <[email protected]> 5. Parachute: update model and world files - Add notes re. servo config for parachute to zephyr model. - Add review notes to zephyr runway world and param settings. Signed-off-by: Rhys Mainwaring <[email protected]> 6. Parachute: apply formatting changes to parachute model Signed-off-by: Rhys Mainwaring <[email protected]> 7. Parachute: revert changes to ArduPilotPlugin - Revert changes to ArduPilotPlugin and use the COMMAND control type to forward parachute servo commands. - Update ParachutePlugin to subscribe to a servo command (double). - Update control xml for parachute in zephyr_with_ardupilot model. Signed-off-by: Rhys Mainwaring <[email protected]> 8. Parachute: update ParachutePlugin - Remove unused code - simplify plugin to minimal functionality (parachute release only). Signed-off-by: Rhys Mainwaring <[email protected]> 9. Parachute: add test world - Add box with parachute attached to test deployment and pose. Signed-off-by: Rhys Mainwaring <[email protected]> 10. Parachute: update test world Models - Attach plugin to model rather than world. - Add usage notes Plugin - Verify plugin attached to model. - Use Link to wrap link entities. - Resolve parent link in configure rather than at update time. - Enable velocity checks so we can get parent link pose. Signed-off-by: Rhys Mainwaring <[email protected]> 11. Parachute: update parachute model - Change collision to a sphere matching the parachute canopy. - Update lift-drag plugin to gz::sim. Signed-off-by: Rhys Mainwaring <[email protected]> 12. Parachute: update zephyr model and world Models - Add 'attachment_link' to connect to parachute when it is released. - Move parachute plugin into model and update sdf definitions. Plugin - Change pose parameter name to 'child_pose'. - Update pose variable names to use Kane notation and simplify calculation. Signed-off-by: Rhys Mainwaring <[email protected]> 13. Parachute: update parachute model - Update inertial and move CoM to centre of collision sphere. - Update pose notation. - Add version where only the position of the parachute may be adjusted (no rotation). Signed-off-by: Rhys Mainwaring <[email protected]> 14. Parachute: update parachute model - Fix for cpplint. Signed-off-by: Rhys Mainwaring <[email protected]> 15. Parachute: update zephyr model - Alter angle of parachute on release (towards rear of aircraft) Signed-off-by: Rhys Mainwaring <[email protected]> 16. Parachute: add separate models and worlds for the parachute example. Signed-off-by: Rhys Mainwaring <[email protected]> 17. Parachute: revert changes to original zephyr models and worlds. Signed-off-by: Rhys Mainwaring <[email protected]> 18. Parachute: update class documentation - Fix name of zephyr_with_parachute in model. - Refer to correct model in zephyr_parachute world. - Document header with parameter descriptions. - Document private class in ParachutePlugin.cc - Fix MAVProxy commands in zephyr example help. Signed-off-by: Rhys Mainwaring <[email protected]> * Parachute: standardise topic names and relocate test worlds - Update plugin documentation. - Update parachute release command topic to match other Gazebo system plugins. - Update example models and worlds to use new topic name. - Move the test world under ./tests/worlds/ Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: Alex Molchanov <[email protected]>
ArduPilot · Dec 22, 2022 · 321371c · 321371c
1 parent 446396c
commit 321371c
Show file tree

Hide file tree

Showing 12 changed files with 1,557 additions and 2 deletions.
diff --git a/.gitignore b/.gitignore
@@ -26,6 +26,10 @@ msg/_*.py
 .project
 .cproject
 
+# vscode
+.vscode
+.devcontainer
+
 # qcreator stuff
 CMakeLists.txt.user
 

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -39,12 +39,23 @@ add_library(ArduPilotPlugin
     src/Socket.cpp
     src/Util.cc
 )
-
 target_include_directories(ArduPilotPlugin PUBLIC
   include
 	${GZ-SIM_INCLUDE_DIRS}
 )
-
 target_link_libraries(ArduPilotPlugin PUBLIC
   ${GZ-SIM_LIBRARIES}
 )
+
+add_library(ParachutePlugin
+  SHARED
+  src/ParachutePlugin.cc
+)
+target_include_directories(ParachutePlugin PUBLIC
+  include
+  ${GZ-SIM_INCLUDE_DIRS}
+)
+
+target_link_libraries(ParachutePlugin PUBLIC
+  ${GZ-SIM_LIBRARIES}
+)
diff --git a/include/ParachutePlugin.hh b/include/ParachutePlugin.hh
@@ -0,0 +1,80 @@
+/*
+   Copyright (C) 2022 ardupilot.org
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef PARACHUTEPLUGIN_HH_
+#define PARACHUTEPLUGIN_HH_
+
+#include <memory>
+
+#include <gz/sim/System.hh>
+
+namespace gz {
+namespace sim {
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+/// \brief Parachute releaase plugin which may be attached to a model.
+///
+/// ## System Parameters:
+///
+///   `<parent_link>` The link in the target model to attach the parachute.
+///   Required.
+///
+///   `<child_model>` The name of the parachute model.
+///   Required.
+///
+///   `<child_link>` The base link of the parachute model (bridle point).
+///   Required.
+///
+///   `<child_pose>` The relative pose of parent link to the child link.
+///   The default value is: `0, 0, 0, 0, 0, 0`.
+///
+///   `<cmd_topic>` The topic to receive  the parachute release command.
+///   The default value is: `/model/<model_name>/parachute/cmd_release`.
+///
+class ParachutePlugin :
+    public System,
+    public ISystemPreUpdate,
+    public ISystemConfigure
+{
+  /// \brief Destructor
+  public: virtual ~ParachutePlugin();
+
+  /// \brief Constructor
+  public: ParachutePlugin();
+
+  // Documentation inherited
+  public: void PreUpdate(const gz::sim::UpdateInfo &_info,
+                         gz::sim::EntityComponentManager &_ecm) final;
+
+  // Documentation inherited
+  public: void Configure(const Entity &_entity,
+                         const std::shared_ptr<const sdf::Element> &_sdf,
+                         EntityComponentManager &_ecm,
+                         EventManager &) final;
+
+  /// \internal
+  /// \brief Private implementation
+  private: class Impl;
+  private: std::unique_ptr<Impl> impl;
+};
+
+}  // namespace systems
+}
+}  // namespace sim
+}  // namespace gz
+
+#endif  // PARACHUTEPLUGIN_HH_
diff --git a/models/parachute_small/meshes/parachute_small.dae b/models/parachute_small/meshes/parachute_small.dae
diff --git a/models/parachute_small/model.config b/models/parachute_small/model.config
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Parachute small</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <license>Apache License, Version 2.0</license>
+  <author>
+    <name>Aurelien Roy</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A parachute. It avoids hitting the ground too hard ;-)
+    This model has been imported from github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin.git
+  </description>
+
+</model>
diff --git a/models/parachute_small/model.sdf b/models/parachute_small/model.sdf
@@ -0,0 +1,66 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <model name="parachute_small">
+    <pose>0 0 0 0 0 0</pose>
+    <link name="chute">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 2.3 0 0 0</pose>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.06250</ixx>
+          <ixy>0.00</ixy>
+          <ixz>0.00</ixz>
+          <iyy>0.06250</iyy>
+          <iyz>0.00</iyz>
+          <izz>0.06250</izz>
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <pose>0 0 2.3  0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>1.25</radius>
+          </sphere>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>2 2 2</scale>
+            <uri>meshes/parachute_small.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <!-- viual at bridle (located at the link origin) -->
+      <visual name="bridle_visual">
+        <geometry>
+          <sphere>
+            <radius>0.025</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <ambient>0.0 0.3 0.7</ambient>
+          <diffuse>0.0 0.3 0.7</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+    </link>
+    <plugin filename="libgz-sim-lift-drag-system.so"
+        name="gz::sim::systems::LiftDrag">
+      <a0>0</a0>
+      <cla>0</cla>
+      <cda>1.75</cda>  <!-- Such that Cd = 1.75 at alpha = 90deg (= upright parachute) -->
+      <alpha_stall>1.5708</alpha_stall> <!-- = upright parachute -->
+      <cla_stall>0</cla_stall>
+      <cda_stall>0.01</cda_stall>
+      <cp>0 0 1.25</cp>   <!-- About the vertical center of the chute -->
+      <area>3</area>   <!-- Area of a circle of diameter 1m = 0.79, 3m2 is for a better visualization -->
+      <air_density>1.204</air_density>
+      <forward>0 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>chute</link_name>
+    </plugin>
+  </model>
+</sdf>
diff --git a/models/zephyr/meshes/parachute_small.dae b/models/zephyr/meshes/parachute_small.dae
diff --git a/models/zephyr_with_parachute/model.config b/models/zephyr_with_parachute/model.config
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Zephyr Delta Wing With Parachute</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+
+  <author>
+    <name>Cole Biesemeyer</name>
+    <email>[email protected]</email>
+  </author>
+
+  <author>
+    <name>Nate Koenig</name>
+    <email>[email protected]</email>
+  </author>
+
+  <author>
+    <name>Rhys Mainwaring</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A model of a Zephyr delta wing with Ardupilot and Parachute plugins.
+  </description>
+
+</model>