diff --git a/CMakeLists.txt b/CMakeLists.txt index 409c657..8d69ea0 100644 --- 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 new file mode 100644 index 0000000..cd4a26a --- /dev/null +++ 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 . + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +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 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 index 0a4e0fd..673f5a4 100755 --- a/models/iris_with_ardupilot/model.config +++ b/models/iris_with_ardupilot/model.config @@ -1,7 +1,7 @@ - Iris with Standoffs and Camera LiftDrag ArduCopter Plugins + Iris with ArduCopter 1.0 model.sdf @@ -24,9 +24,8 @@ john hsu - - 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 index 23c62b9..4765ac3 100755 --- a/models/iris_with_ardupilot/model.sdf +++ b/models/iris_with_ardupilot/model.sdf @@ -1,10 +1,11 @@ - + model://iris_with_standoffs + + - + + + 0.3 1.4 4.2500 @@ -395,9 +402,10 @@ 0.084 0 0 0 1 0 0 0 1 - iris::rotor_0 + iris_with_standoffs::rotor_0 - + 0.3 1.4 4.2500 @@ -411,10 +419,11 @@ -0.084 0 0 0 -1 0 0 0 1 - iris::rotor_0 + iris_with_standoffs::rotor_0 - + 0.3 1.4 4.2500 @@ -428,9 +437,10 @@ 0.084 0 0 0 1 0 0 0 1 - iris::rotor_1 + iris_with_standoffs::rotor_1 - + 0.3 1.4 4.2500 @@ -444,10 +454,11 @@ -0.084 0 0 0 -1 0 0 0 1 - iris::rotor_1 + iris_with_standoffs::rotor_1 - + 0.3 1.4 4.2500 @@ -461,9 +472,10 @@ 0.084 0 0 0 -1 0 0 0 1 - iris::rotor_2 + iris_with_standoffs::rotor_2 - + 0.3 1.4 4.2500 @@ -477,10 +489,11 @@ -0.084 0 0 0 1 0 0 0 1 - iris::rotor_2 + iris_with_standoffs::rotor_2 - + 0.3 1.4 4.2500 @@ -494,9 +507,10 @@ 0.084 0 0 0 -1 0 0 0 1 - iris::rotor_3 + iris_with_standoffs::rotor_3 - + 0.3 1.4 4.2500 @@ -510,21 +524,42 @@ -0.084 0 0 0 1 0 0 0 1 - iris::rotor_3 + iris_with_standoffs::rotor_3 + + + + iris_with_standoffs::rotor_0_joint - + + iris_with_standoffs::rotor_1_joint + + + iris_with_standoffs::rotor_2_joint + + + iris_with_standoffs::rotor_3_joint + + + + 127.0.0.1 9002 - 9003 - 0 0 0 3.141593 0 0 0 0 0 3.141593 0 0 - iris_demo::iris::iris/imu_link::imu_sensor - 5 - + + + iris_with_standoffs::imu_link::imu_sensor + - VELOCITY + + iris_with_standoffs::rotor_0_joint + 1 + 838 0 + 1100 + 1900 + VELOCITY 0.20 0 0 @@ -541,13 +582,17 @@ 0 2.5 -2.5 - iris::rotor_0_joint - 838 1 + - VELOCITY + iris_with_standoffs::rotor_1_joint + 1 + 838 0 + 1100 + 1900 + VELOCITY 0.20 0 0 @@ -555,13 +600,17 @@ 0 2.5 -2.5 - iris::rotor_1_joint - 838 1 + - VELOCITY + iris_with_standoffs::rotor_2_joint + 1 + -838 0 + 1100 + 1900 + VELOCITY 0.20 0 0 @@ -569,13 +618,17 @@ 0 2.5 -2.5 - iris::rotor_2_joint - -838 1 + - VELOCITY + iris_with_standoffs::rotor_3_joint + 1 + -838 0 + 1100 + 1900 + VELOCITY 0.20 0 0 @@ -583,8 +636,6 @@ 0 2.5 -2.5 - iris::rotor_3_joint - -838 1 diff --git a/models/iris_with_standoffs/model.config b/models/iris_with_standoffs/model.config index e33118a..d6b7228 100755 --- a/models/iris_with_standoffs/model.config +++ b/models/iris_with_standoffs/model.config @@ -1,5 +1,4 @@ - Iris with Standoffs 1.0 @@ -24,7 +23,6 @@ john hsu - A copy of the 3DR Iris model taken from https://github.com/PX4/sitl_gazebo/tree/master/models diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf index 9e6ed0f..93747b9 100755 --- a/models/iris_with_standoffs/model.sdf +++ b/models/iris_with_standoffs/model.sdf @@ -1,6 +1,6 @@ - - + + 0 0 0.194923 0 0 0 @@ -19,7 +19,7 @@ 0.017 - + 0 0 -0.08 0 0 0 @@ -41,7 +41,7 @@ - + model://iris_with_standoffs/meshes/iris.dae @@ -130,43 +130,6 @@ - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - 0.15 - - 0.0001 - 0 - 0 - 0.0002 - 0 - 0.0002 - - - - - iris/ground_truth/odometry_sensorgt_link - base_link - - 0 0 1 - - 0 - 0 - 0 - 0 - - - 1.0 - - 1 - - - - 1 - - - 0 0 0 0 0 0 @@ -186,7 +149,7 @@ 1000.0 - + imu_link base_link @@ -200,49 +163,8 @@ 1.0 - 1 - - - - 1 - - - - 0.13 -0.22 0.023 0 -0 0 @@ -535,264 +457,5 @@ - 0 - - - - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - 0.084 0 0 - 0 1 0 - 0 0 1 - rotor_0 - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - -0.084 0 0 - 0 -1 0 - 0 0 1 - rotor_0 - - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - 0.084 0 0 - 0 1 0 - 0 0 1 - rotor_1 - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - -0.084 0 0 - 0 -1 0 - 0 0 1 - rotor_1 - - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - 0.084 0 0 - 0 -1 0 - 0 0 1 - rotor_2 - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - -0.084 0 0 - 0 1 0 - 0 0 1 - rotor_2 - - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - 0.084 0 0 - 0 -1 0 - 0 0 1 - rotor_3 - - - 0.3 - 1.4 - 4.2500 - 0.10 - 0.00 - -0.025 - 0.0 - 0.0 - 0.002 - 1.2041 - -0.084 0 0 - 0 1 0 - 0 0 1 - rotor_3 - - - - rotor_0_joint - - - rotor_1_joint - - - rotor_2_joint - - - rotor_3_joint - - - - - 127.0.0.1 - 9002 - 5 - 1 - - - 0 0 0 3.141593 0 0 - 0 0 0 3.141593 0 0 - - - imu_sensor - - - - rotor_0_joint - 1 - 838 - 0 - 1100 - 1900 - VELOCITY - 0.20 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - 1 - - - - rotor_1_joint - 1 - 838 - 0 - 1100 - 1900 - VELOCITY - 0.20 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - 1 - - - - rotor_2_joint - 1 - -838 - 0 - 1100 - 1900 - VELOCITY - 0.20 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - 1 - - - - rotor_3_joint - 1 - -838 - 0 - 1100 - 1900 - VELOCITY - 0.20 - 0 - 0 - 0 - 0 - 2.5 - -2.5 - 1 - - - diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index e4ba4f5..c6956c9 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -28,13 +28,16 @@ #include #include +#include #include #include #include #include #include +#include #include #include +#include #include #include #include @@ -50,6 +53,7 @@ #include #include "Socket.h" +#include "Util.hh" #define DEBUG_JSON_IO 0 @@ -157,8 +161,8 @@ class gz::sim::systems::ArduPilotPluginPrivate /// \brief The model public: gz::sim::Model model{gz::sim::kNullEntity}; - /// \brief The entity representing the base or root link of the model - public: gz::sim::Entity modelLink{gz::sim::kNullEntity}; + /// \brief The entity representing the link containing the imu sensor. + public: gz::sim::Entity imuLink{gz::sim::kNullEntity}; /// \brief The model name; public: std::string modelName; @@ -293,16 +297,16 @@ gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin() void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink, components::WorldPose::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, + _ecm.CreateComponent(this->dataPtr->imuLink, gz::sim::components::WorldPose()); } - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink, components::WorldLinearVelocity::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, + _ecm.CreateComponent(this->dataPtr->imuLink, gz::sim::components::WorldLinearVelocity()); } @@ -503,7 +507,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( } // Get the pointer to the joint. - control.joint = this->dataPtr->model.JointByName(_ecm, control.jointName); + control.joint = JointByName(_ecm, + this->dataPtr->model.Entity(), control.jointName); if (control.joint == gz::sim::kNullEntity) { gzerr << "[" << this->dataPtr->modelName << "] " @@ -865,43 +870,78 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( // Set unconditionally because we're only going to try this once. this->dataPtr->imuInitialized = true; std::string imuTopicName; - _ecm.Each( - [&](const gz::sim::Entity &_imu_entity, - const gz::sim::components::Imu * /*_imu*/, - const gz::sim::components::Name *_name)->bool + + // The model must contain an imu sensor element: + // + // + // Extract the following: + // - Sensor topic name: to subscribe to the imu data + // - Link containing the sensor: to get the pose to transform to + // the correct frame for ArduPilot + + // try scoped names first + auto entities = entitiesFromScopedName( + this->dataPtr->imuName, _ecm, this->dataPtr->model.Entity()); + + // fall-back to unscoped name + if (entities.empty()) + { + entities = EntitiesFromUnscopedName( + this->dataPtr->imuName, _ecm, this->dataPtr->model.Entity()); + } + + if (!entities.empty()) { - if (_name->Data() == this->dataPtr->imuName) + if (entities.size() > 1) + { + gzwarn << "Multiple IMU sensors with name [" + << this->dataPtr->imuName << "] found. " + << "Using the first one.\n"; + } + + // select first entity + gz::sim::Entity imuEntity = *entities.begin(); + + // validate + if (!_ecm.EntityHasComponentType(imuEntity, + gz::sim::components::Imu::typeId)) + { + gzerr << "Entity with name [" + << this->dataPtr->imuName + << "] is not an IMU sensor\n"; + } + else + { + gzmsg << "Found IMU sensor with name [" + << this->dataPtr->imuName + << "]\n"; + + // verify the parent of the imu sensor is a link. + gz::sim::Entity parent = _ecm.ParentEntity(imuEntity); + if (_ecm.EntityHasComponentType(parent, + gz::sim::components::Link::typeId)) { - // The parent of the imu is imu_link - gz::sim::Entity parent = _ecm.ParentEntity(_imu_entity); - if (parent != gz::sim::kNullEntity) - { - // The grandparent of the imu is the quad itself, - // which is where this plugin is attached - gz::sim::Entity gparent = _ecm.ParentEntity(parent); - if (gparent != gz::sim::kNullEntity) - { - gz::sim::Model gparent_model(gparent); - if (gparent_model.Name(_ecm) == - this->dataPtr->modelName) - { - this->dataPtr->modelLink = parent; - imuTopicName = gz::sim::scopedName( - _imu_entity, _ecm) + "/imu"; - gzdbg << "Computed IMU topic to be: " - << imuTopicName << std::endl; - } - } - } - } - return true; - }); + this->dataPtr->imuLink = parent; + + imuTopicName = gz::sim::scopedName( + imuEntity, _ecm) + "/imu"; - if (imuTopicName.empty()) + gzdbg << "Computed IMU topic to be: " + << imuTopicName << std::endl; + } + else + { + gzerr << "Parent of IMU sensor [" + << this->dataPtr->imuName + << "] is not a link\n"; + } + } + } + else { gzerr << "[" << this->dataPtr->modelName << "] " - << "imu_sensor [" << this->dataPtr->imuName - << "] not found, abort ArduPilot plugin." << "\n"; + << "imu_sensor [" << this->dataPtr->imuName + << "] not found, abort ArduPilot plugin." << "\n"; return; } @@ -909,18 +949,18 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( &gz::sim::systems::ArduPilotPluginPrivate::imuCb, this->dataPtr.get()); - // Make sure that the "imu_link" entity has WorldPose + // Make sure that the 'imuLink' entity has WorldPose // and WorldLinearVelocity components, which we'll need later. - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink, components::WorldPose::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, + _ecm.CreateComponent(this->dataPtr->imuLink, gz::sim::components::WorldPose()); } - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink, components::WorldLinearVelocity::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, + _ecm.CreateComponent(this->dataPtr->imuLink, gz::sim::components::WorldLinearVelocity()); } } @@ -1082,11 +1122,14 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces( gz::sim::components::JointVelocity* vComp = _ecm.Component( this->dataPtr->controls[i].joint); - const double vel = vComp->Data()[0]; - const double error = vel - velTarget; - const double force = this->dataPtr->controls[i].pid.Update( - error, std::chrono::duration(_dt)); - jfcComp->Data()[0] = force; + if (vComp && !vComp->Data().empty()) + { + const double vel = vComp->Data()[0]; + const double error = vel - velTarget; + const double force = this->dataPtr->controls[i].pid.Update( + error, std::chrono::duration(_dt)); + jfcComp->Data()[0] = force; + } } else if (this->dataPtr->controls[i].type == "POSITION") { @@ -1094,11 +1137,14 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces( gz::sim::components::JointPosition* pComp = _ecm.Component( this->dataPtr->controls[i].joint); - const double pos = pComp->Data()[0]; - const double error = pos - posTarget; - const double force = this->dataPtr->controls[i].pid.Update( - error, std::chrono::duration(_dt)); - jfcComp->Data()[0] = force; + if (pComp && !pComp->Data().empty()) + { + const double pos = pComp->Data()[0]; + const double error = pos - posTarget; + const double force = this->dataPtr->controls[i].pid.Update( + error, std::chrono::duration(_dt)); + jfcComp->Data()[0] = force; + } } else if (this->dataPtr->controls[i].type == "EFFORT") { @@ -1478,11 +1524,11 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( // get pose and velocity in Gazebo world frame const gz::sim::components::WorldPose* worldPose = _ecm.Component( - this->dataPtr->modelLink); + this->dataPtr->imuLink); const gz::sim::components::WorldLinearVelocity* worldLinearVel = _ecm.Component( - this->dataPtr->modelLink); + this->dataPtr->imuLink); // position and orientation transform (Aircraft world to Aircraft body) gz::math::Pose3d bdyAToBdyG = diff --git a/src/Util.cc b/src/Util.cc new file mode 100644 index 0000000..894e0bd --- /dev/null +++ b/src/Util.cc @@ -0,0 +1,115 @@ +/* + 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 . + */ + +#include "Util.hh" + +#include +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE { + +////////////////////////////////////////////////// +std::unordered_set EntitiesFromUnscopedName( + const std::string &_name, const EntityComponentManager &_ecm, + Entity _relativeTo) +{ + // holds entities that match + std::vector entities; + + if (_relativeTo == kNullEntity) + { + // search everything + entities = _ecm.EntitiesByComponents(components::Name(_name)); + } + else + { + // search all descendents + auto descendents = _ecm.Descendants(_relativeTo); + for (const auto& descendent : descendents) + { + if (_ecm.EntityHasComponentType(descendent, + gz::sim::components::Name::typeId)) + { + auto nameComp = _ecm.Component(descendent); + if (nameComp->Data() == _name) + { + entities.push_back(descendent); + } + } + } + + } + if (entities.empty()) + return {}; + + return std::unordered_set(entities.begin(), entities.end()); +} + +////////////////////////////////////////////////// +Entity JointByName(EntityComponentManager &_ecm, + Entity _modelEntity, + const std::string &_name) +{ + // Retrieve entities from a scoped name. + // See for example: + // https://github.com/gazebosim/ign-gazebo/pull/955 + // which applies to the LiftDrag plugin + auto entities = entitiesFromScopedName(_name, _ecm, _modelEntity); + + if (entities.empty()) + { + gzerr << "Joint with name [" << _name << "] not found. " + << "The joint will not respond to ArduPilot commands\n"; + return kNullEntity; + } + else if (entities.size() > 1) + { + gzwarn << "Multiple joint entities with name[" << _name << "] found. " + << "Using the first one.\n"; + } + + Entity joint = *entities.begin(); + + // Validate + if (!_ecm.EntityHasComponentType(joint, + components::Joint::typeId)) + { + gzerr << "Entity with name[" << _name << "] is not a joint\n"; + return kNullEntity; + } + + // Ensure the joint has a velocity component + if (!_ecm.EntityHasComponentType(joint, + components::JointVelocity::typeId)) + { + _ecm.CreateComponent(joint, + components::JointVelocity()); + } + + return joint; +}; + +} +} // namespace sim +} // namespace gz diff --git a/worlds/iris_arducopter_runway.world b/worlds/iris_arducopter_runway.world index 3149f17..b621fd0 100755 --- a/worlds/iris_arducopter_runway.world +++ b/worlds/iris_arducopter_runway.world @@ -39,7 +39,7 @@ - model://iris_with_standoffs + model://iris_with_ardupilot