Add support for nested models (#32)

1. Add support for nested models - Search for links, joints and sensors in nested models - Add a custom version of Model::JointByName implemented as a lambda - Search for entities given a scoped name and then check that those found support the required components (Joint and JointVelocity) - Rewrite the search for the IMU sensor component to use scoped names - In the `ApplyForce` function add a check that the JointVelocity and JointPosition components are valid before dereferencing them - Rename modeLink to imuLink (it is the pose of the link containing the IMU that is passed to the flight controller). - Fix initialisation of imuLink after rebase. - Replace references to ignition with gz replacements. Signed-off-by: Rhys Mainwaring <[email protected]> 2. Add support for nested models - Add utility to search for all descendent entities matching an un-scoped name. - Ensure initialisation of IMU sensor is backwards compatible. Signed-off-by: Rhys Mainwaring <[email protected]> 3. Add support for nested models - Move JointByName into Utils.hh Signed-off-by: Rhys Mainwaring <[email protected]> 4. Add support for nested models - Update Iris models. The basic model now has no plugins. Signed-off-by: Rhys Mainwaring <[email protected]> 5. Add support for nested models - Review feedback - remove commented code. Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Dec 16, 2022 · 28829af · 28829af
1 parent 8686cc1
commit 28829af
Show file tree

Hide file tree

Showing 9 changed files with 374 additions and 441 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -37,6 +37,7 @@ add_library(ArduPilotPlugin
     SHARED
     src/ArduPilotPlugin.cc
     src/Socket.cpp
+    src/Util.cc
 )
 
 target_include_directories(ArduPilotPlugin PUBLIC

diff --git a/include/Util.hh b/include/Util.hh
@@ -0,0 +1,60 @@
+/*
+   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/>.
+ */
+
+#pragma once
+
+#include <string>
+#include <unordered_set>
+#include <vector>
+
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/Export.hh>
+
+namespace gz
+{
+namespace sim
+{
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+/// \brief Helper function to get an entity given its unscoped name.
+///
+/// \param[in] _name Entity's unscoped name.
+/// \param[in] _ecm Immutable reference to ECM.
+/// \param[in] _relativeTo Entity that the unscoped name is relative to.
+/// If not provided, the unscoped name could be relative to any entity.
+/// \return All entities that match the unscoped name and relative to
+/// requirements, or an empty set otherwise.
+std::unordered_set<Entity> EntitiesFromUnscopedName(
+    const std::string &_name, const EntityComponentManager &_ecm,
+    Entity _relativeTo = kNullEntity);
+
+/// \brief Get the ID of a joint entity which is a descendent of this model.
+///
+/// A replacement for gz::sim::Model::JointByName which does not resolve
+/// joints for nested models.
+/// \param[in] _ecm Entity-component manager.
+/// \param[in] _entity Model entity.
+/// \param[in] _name Scoped joint name.
+/// \return Joint entity.
+Entity JointByName(EntityComponentManager &_ecm,
+    Entity _modelEntity,
+    const std::string &_name);
+
+}
+}  // namespace sim
+}  // namespace gz
diff --git a/models/iris_with_ardupilot/model.config b/models/iris_with_ardupilot/model.config
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <model>
-  <name>Iris with Standoffs and Camera LiftDrag ArduCopter Plugins</name>
+  <name>Iris with ArduCopter</name>
   <version>1.0</version>
   <sdf version="1.6">model.sdf</sdf>
 
@@ -24,9 +24,8 @@
 
   <maintainer email="[email protected]">john hsu</maintainer>
 
-
   <description>
-    starting with iris_with_standoffs
+    Starting with iris_with_standoffs
     add LiftDragPlugin
     add ArduCopterPlugin
     attach gimbal_small_2d model with GimbalSmall2dPlugin

diff --git a/models/iris_with_ardupilot/model.sdf b/models/iris_with_ardupilot/model.sdf
@@ -1,10 +1,11 @@
 <?xml version='1.0'?>
 <sdf version="1.6">
-  <model name="iris_demo">
+  <model name="iris_with_ardupilot">
     <include>
       <uri>model://iris_with_standoffs</uri>
     </include>
 
+    <!-- TODO: migrate gimbal model to Gazebo Sim and add camera.
     <include>
       <uri>model://gimbal_small_2d</uri>
       <pose>0 -0.01 0.070 1.57 0 1.57</pose>
@@ -22,6 +23,8 @@
         <use_parent_model_frame>true</use_parent_model_frame>
       </axis>
     </joint>
+    -->
+
     <!-- visual markers for debugging
     <link name="rotor_0_blade_1_cp">
       <gravity>0</gravity>
@@ -381,7 +384,11 @@
     -->
 
     <!-- plugins -->
-    <plugin name="rotor_0_blade_1" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-joint-state-publisher-system"
+      name="gz::sim::systems::JointStatePublisher">
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -395,9 +402,10 @@
       <cp>0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_0</link_name>
+      <link_name>iris_with_standoffs::rotor_0</link_name>
     </plugin>
-    <plugin name="rotor_0_blade_2" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -411,10 +419,11 @@
       <cp>-0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_0</link_name>
+      <link_name>iris_with_standoffs::rotor_0</link_name>
     </plugin>
 
-    <plugin name="rotor_1_blade_1" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -428,9 +437,10 @@
       <cp>0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_1</link_name>
+      <link_name>iris_with_standoffs::rotor_1</link_name>
     </plugin>
-    <plugin name="rotor_1_blade_2" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -444,10 +454,11 @@
       <cp>-0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_1</link_name>
+      <link_name>iris_with_standoffs::rotor_1</link_name>
     </plugin>
 
-    <plugin name="rotor_2_blade_1" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -461,9 +472,10 @@
       <cp>0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_2</link_name>
+      <link_name>iris_with_standoffs::rotor_2</link_name>
     </plugin>
-    <plugin name="rotor_2_blade_2" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -477,10 +489,11 @@
       <cp>-0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_2</link_name>
+      <link_name>iris_with_standoffs::rotor_2</link_name>
     </plugin>
 
-    <plugin name="rotor_3_blade_1" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -494,9 +507,10 @@
       <cp>0.084 0 0</cp>
       <forward>0 -1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_3</link_name>
+      <link_name>iris_with_standoffs::rotor_3</link_name>
     </plugin>
-    <plugin name="rotor_3_blade_2" filename="libLiftDragPlugin">
+    <plugin filename="gz-sim-lift-drag-system"
+        name="gz::sim::systems::LiftDrag">
       <a0>0.3</a0>
       <alpha_stall>1.4</alpha_stall>
       <cla>4.2500</cla>
@@ -510,81 +524,118 @@
       <cp>-0.084 0 0</cp>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
-      <link_name>iris::rotor_3</link_name>
+      <link_name>iris_with_standoffs::rotor_3</link_name>
+    </plugin>
+
+    <plugin filename="gz-sim-apply-joint-force-system"
+      name="gz::sim::systems::ApplyJointForce">
+      <joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
     </plugin>
-    <plugin name="arducopter_plugin" filename="libArduPilotPlugin">
+    <plugin filename="gz-sim-apply-joint-force-system"
+      name="gz::sim::systems::ApplyJointForce">
+      <joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
+    </plugin>
+    <plugin filename="gz-sim-apply-joint-force-system"
+      name="gz::sim::systems::ApplyJointForce">
+      <joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
+    </plugin>
+    <plugin filename="gz-sim-apply-joint-force-system"
+      name="gz::sim::systems::ApplyJointForce">
+      <joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
+    </plugin>
+
+    <plugin name="ArduPilotPlugin" filename="libArduPilotPlugin">
+      <!-- Port settings -->
       <fdm_addr>127.0.0.1</fdm_addr>
       <fdm_port_in>9002</fdm_port_in>
-      <fdm_port_out>9003</fdm_port_out>
-      <!--
-          Require by APM :
-          Only change model and gazebo from XYZ to XY-Z coordinates
+      <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
+      <lock_step>1</lock_step>
+
+      <!-- Frame conventions
+        Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
       -->
       <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
       <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
-      <imuName>iris_demo::iris::iris/imu_link::imu_sensor</imuName>
-      <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
-      <control channel="0">
+
+      <!-- Sensors -->
+      <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
+
       <!--
           incoming control command [0, 1]
           so offset it by 0 to get [0, 1]
           and divide max target by 1.
           offset = 0
           multiplier = 838 max rpm / 1 = 838
         -->
-        <type>VELOCITY</type>
+      <control channel="0">
+        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
+        <useForce>1</useForce>
+        <multiplier>838</multiplier>
         <offset>0</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>VELOCITY</type>
         <p_gain>0.20</p_gain>
         <i_gain>0</i_gain>
         <d_gain>0</d_gain>
         <i_max>0</i_max>
         <i_min>0</i_min>
         <cmd_max>2.5</cmd_max>
         <cmd_min>-2.5</cmd_min>
-        <jointName>iris::rotor_0_joint</jointName>
-        <multiplier>838</multiplier>
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
+
       <control channel="1">
-        <type>VELOCITY</type>
+        <jointName>iris_with_standoffs::rotor_1_joint</jointName>
+        <useForce>1</useForce>
+        <multiplier>838</multiplier>
         <offset>0</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>VELOCITY</type>
         <p_gain>0.20</p_gain>
         <i_gain>0</i_gain>
         <d_gain>0</d_gain>
         <i_max>0</i_max>
         <i_min>0</i_min>
         <cmd_max>2.5</cmd_max>
         <cmd_min>-2.5</cmd_min>
-        <jointName>iris::rotor_1_joint</jointName>
-        <multiplier>838</multiplier>
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
+
       <control channel="2">
-        <type>VELOCITY</type>
+        <jointName>iris_with_standoffs::rotor_2_joint</jointName>
+        <useForce>1</useForce>
+        <multiplier>-838</multiplier>
         <offset>0</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>VELOCITY</type>
         <p_gain>0.20</p_gain>
         <i_gain>0</i_gain>
         <d_gain>0</d_gain>
         <i_max>0</i_max>
         <i_min>0</i_min>
         <cmd_max>2.5</cmd_max>
         <cmd_min>-2.5</cmd_min>
-        <jointName>iris::rotor_2_joint</jointName>
-        <multiplier>-838</multiplier>
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
+
       <control channel="3">
-        <type>VELOCITY</type>
+        <jointName>iris_with_standoffs::rotor_3_joint</jointName>
+        <useForce>1</useForce>
+        <multiplier>-838</multiplier>
         <offset>0</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>VELOCITY</type>
         <p_gain>0.20</p_gain>
         <i_gain>0</i_gain>
         <d_gain>0</d_gain>
         <i_max>0</i_max>
         <i_min>0</i_min>
         <cmd_max>2.5</cmd_max>
         <cmd_min>-2.5</cmd_min>
-        <jointName>iris::rotor_3_joint</jointName>
-        <multiplier>-838</multiplier>
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
     </plugin>

diff --git a/models/iris_with_standoffs/model.config b/models/iris_with_standoffs/model.config
@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-
 <model>
   <name>Iris with Standoffs</name>
   <version>1.0</version>
@@ -24,7 +23,6 @@
 
   <maintainer email="[email protected]">john hsu</maintainer>
 
-
   <description>
     A copy of the 3DR Iris model taken from
     https://github.com/PX4/sitl_gazebo/tree/master/models