diff --git a/subt_ign/include/subt_ign/GameLogicPlugin.hh b/subt_ign/include/subt_ign/GameLogicPlugin.hh index 1cb128fd..386f7456 100644 --- a/subt_ign/include/subt_ign/GameLogicPlugin.hh +++ b/subt_ign/include/subt_ign/GameLogicPlugin.hh @@ -28,6 +28,7 @@ namespace subt class GameLogicPlugin: public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate, public ignition::gazebo::ISystemPostUpdate { /// \brief Constructor @@ -42,6 +43,10 @@ namespace subt ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &_eventMgr) override; + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + // Documentation inherited public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) override; diff --git a/subt_ign/include/subt_ign/RobotPlatformTypes.hh b/subt_ign/include/subt_ign/RobotPlatformTypes.hh index f0315fd1..62bb3e41 100644 --- a/subt_ign/include/subt_ign/RobotPlatformTypes.hh +++ b/subt_ign/include/subt_ign/RobotPlatformTypes.hh @@ -16,43 +16,43 @@ */ #ifndef SUBT_IGN_ROBOTPLATFORMTYPES_HH_ #define SUBT_IGN_ROBOTPLATFORMTYPES_HH_ -/// \brief List of robot platform types. This is used to count unique robot -/// types. -const std::vector robotPlatformTypes = { - "ABSOLEM", - "ALLIE", - "ANYMAL_B", - "ANYMAL_C", - "CRYSTAL", - "DS1", - "DTR", - "FREYJA", - "GAGARIN" - "HD2", - "HOVERMAP", - "HUSKY", - "JEANINE", - "KAREN", - "KLOUBAK", - "LILY", - "M100", - "MARV", - "MIKE", - "OZBOT_ATR", - "PAM", - "QAV500", - "R2", - "R3", - "RMF", - "ROCKY", - "SHAFTER", - "SPOT", - "TEAMBASE", - "X1", - "X2", - "X3", - "X4", - "X500", +#include +/// \brief List of robot platform types and kinetic energy threshold factor. This is used to count unique robot types and determine crashes. +const std::map robotPlatformTypes = { + {"ABSOLEM", 1}, + {"ALLIE", 1}, + {"ANYMAL_B", 1}, + {"ANYMAL_C", 1}, + {"CRYSTAL", 8}, // UAV, no prop guards + {"DS1", 6}, // UAV, prop guards + {"DTR", 1}, + {"FREYJA", 1}, + {"GAGARIN", 1}, // UAV, collision tolerant + {"HD2", 1}, + {"HOVERMAP", 6}, // UAV, prop guards + {"HUSKY", 1}, + {"JEANINE", 1}, + {"KAREN", 1}, + {"KLOUBAK", 1}, + {"LILY", 1}, + {"M100", 8}, // UAV, no prop guards + {"MARV", 1}, + {"MIKE", 1}, + {"OZBOT_ATR", 1}, + {"PAM", 6}, // UAV, prop guards + {"QAV500", 6}, // UAV, prop guards + {"R2", 1}, + {"R3", 1}, + {"RMF", 1}, // UAV, collision tolerant + {"ROCKY", 1}, + {"SHAFTER", 8}, // UAV, no prop guards + {"SPOT", 1}, + {"TEAMBASE", 1}, + {"X1", 1}, + {"X2", 1}, + {"X3", 8}, // UAV, no prop guards + {"X4", 8}, // UAV, no prop guards + {"X500", 8} // UAV, no prop guards }; #endif diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index f1f82230..2f6b5cc1 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -441,6 +441,7 @@ name="subt::GameLogicPlugin"> <%= $worldName %> <%= $ros %> + 0.078 <%= $durationSec %> diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index 7023452b..fa54a95a 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -367,6 +367,7 @@ <%= $worldName %> false + 0.078 <%= $durationSec %> diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 3d679ef9..aefe15f4 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -31,6 +31,13 @@ #include #include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -76,6 +83,7 @@ IGNITION_ADD_PLUGIN( subt::GameLogicPlugin, ignition::gazebo::System, subt::GameLogicPlugin::ISystemConfigure, + subt::GameLogicPlugin::ISystemPreUpdate, subt::GameLogicPlugin::ISystemPostUpdate) using namespace ignition; @@ -83,6 +91,23 @@ using namespace gazebo; using namespace systems; using namespace subt; +/// \brief Kinetic energy information used to determine when a crash occurs. +class KineticEnergyInfo +{ + /// \brief Value of the previous iteration's kinetic energy. + public: double prevKineticEnergy{0.0}; + + /// \brief Threshold past which a crash has happened. Negative number + /// indicates that unlimited kinetic energy is allowed. + public: double kineticEnergyThreshold{-1.0}; + + /// \brief The link used to get kinetic energy. + public: gazebo::Entity link; + + /// \brief Name of the robot. + public: std::string robotName; +}; + class subt::GameLogicPluginPrivate { /// \brief Write a simulation timestamp to a logfile. @@ -525,6 +550,15 @@ class subt::GameLogicPluginPrivate /// \brief Mutex to protect the eventCounter. public: std::mutex eventCounterMutex; + + /// \brief Kinetic energy information for each robot. + public: std::map keInfo; + + // \brief Height used to determine the KE threshold. + // Increasing this value will lower the threshold, meaning a + // less violent collision will disable the robot. This value can be + // adjusted via this plugin's SDF parameters. + public: double keHeight = 0.077; }; ////////////////////////////////////////////////// @@ -600,6 +634,9 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/, } } + this->dataPtr->keHeight = + _sdf->Get("ke_height", this->dataPtr->keHeight).first; + const sdf::ElementPtr rosElem = const_cast(_sdf.get())->GetElement("ros"); if (rosElem) @@ -1012,6 +1049,176 @@ void GameLogicPluginPrivate::OnEvent(const ignition::msgs::Pose &_msg) } } +////////////////////////////////////////////////// +void GameLogicPlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + if (!this->dataPtr->started) + { + _ecm.Each( + [&](const gazebo::Entity &, + const gazebo::components::Sensor *, + const gazebo::components::ParentEntity *_parent) -> bool + { + // Get the model. We are assuming that a sensor is attached to + // a link. + auto model = _ecm.Component( + _parent->Data()); + if (model) + { + // Get the model name + auto mName = + _ecm.Component(model->Data()); + + // Skip if we already have information for this robot. + if (this->dataPtr->keInfo.find(model->Data()) != + this->dataPtr->keInfo.end()) { + return true; + } + + std::vector links = _ecm.EntitiesByComponents( + components::ParentEntity(model->Data()), components::Link()); + + // Get the mass for the robot by summing the mass of each link, + // and use the canonical link for KE computation. + double mass = 0.0; + for (const Entity &link : links) + { + auto inertial = _ecm.Component(link); + auto canonical = _ecm.Component(link); + mass += inertial->Data().MassMatrix().Mass(); + + if (canonical) + { + this->dataPtr->keInfo[model->Data()].link += link; + // Create a world pose component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, components::WorldPose()); + } + + // Create an inertial component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, components::Inertial()); + } + + // Create a world linear velocity component if one is not + // present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, + components::WorldLinearVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component(link)) + { + _ecm.CreateComponent(link, + components::AngularVelocity()); + } + + // Create a world angular velocity component if one is not + // present. + if (!_ecm.Component( + link)) + { + _ecm.CreateComponent(link, + components::WorldAngularVelocity()); + } + } + } + + if (mass > 0) + { + // This sets the kinetic energy threshold for the robot. It is + // based on the kinetic energy formula KE = 0.5 * M * v * v + // + // M is the mass of the robot. + // v is velocity of the robot. We are using the velocity acheived + // by falling from a height. + this->dataPtr->keInfo[model->Data()].kineticEnergyThreshold = + 0.5 * mass * std::pow( + sqrt(2 * this->dataPtr->keHeight * 9.8), 2); + this->dataPtr->keInfo[model->Data()].robotName = mName->Data(); + + // Create a halt motion component if one is not + // present. + if (!_ecm.Component(model->Data())) + { + _ecm.CreateComponent(model->Data(), components::HaltMotion()); + } + } + } + return true; + }); + } + else + { + // Check for crashes + for (auto &ke : this->dataPtr->keInfo) + { + ignition::gazebo::Link link(ke.second.link); + if (std::nullopt != link.WorldKineticEnergy(_ecm)) + { + double currKineticEnergy = *link.WorldKineticEnergy(_ecm); + + // We only care about positive values of this (the links looses energy) + double deltaKE = ke.second.prevKineticEnergy - currKineticEnergy; + + // Debug: Compute the factor needed to hit the threshold. + // if (deltaKE > 0.01) + // { + // double factor = ke.second.kineticEnergyThreshold / deltaKE; + // std::cout << "KE[" << deltaKE << "] Thresh[" + // << ke.second.kineticEnergyThreshold << "] Factor[" + // << factor << "]\n"; + // } + + // Apply KE factor. + deltaKE *= robotPlatformTypes.at( + this->dataPtr->robotFullTypes[ke.second.robotName].first); + ke.second.prevKineticEnergy = currKineticEnergy; + + // Crash if past the threshold. + if (ke.second.kineticEnergyThreshold > 0 && + deltaKE >= ke.second.kineticEnergyThreshold) + { + int64_t sec, nsec; + std::tie(sec, nsec) = + ignition::math::durationToSecNsec(_info.simTime); + ignition::msgs::Time localSimTime; + localSimTime.set_sec(sec); + localSimTime.set_nsec(nsec); + + std::lock_guard lock(this->dataPtr->eventCounterMutex); + std::ostringstream stream; + stream + << "- event:\n" + << " id: " << this->dataPtr->eventCounter << "\n" + << " type: collision\n" + << " time_sec: " << localSimTime.sec() << "\n" + << " robot: " << ke.second.robotName << std::endl; + this->dataPtr->LogEvent(stream.str()); + this->dataPtr->PublishRobotEvent( + localSimTime, "collision", ke.second.robotName, + this->dataPtr->eventCounter); + this->dataPtr->eventCounter++; + + auto *haltMotionComp = + _ecm.Component(ke.first); + if (haltMotionComp && !haltMotionComp->Data()) + { + igndbg << "Robot[" << ke.second.robotName << "] has crashed!\n"; + haltMotionComp->Data() = true; + } + } + } + } + } +} + ////////////////////////////////////////////////// void GameLogicPlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, @@ -1103,22 +1310,23 @@ void GameLogicPlugin::PostUpdate( model->Data()); // Store unique robot platform information. - for (const std::string &type : robotPlatformTypes) + for (const std::pair &typeKE : + robotPlatformTypes) { std::string platformNameUpper = filePath->Data(); std::transform(platformNameUpper.begin(), platformNameUpper.end(), platformNameUpper.begin(), ::toupper); - if (platformNameUpper.find(type) != std::string::npos) + if (platformNameUpper.find(typeKE.first) != std::string::npos) { - this->dataPtr->robotTypes.insert(type); + this->dataPtr->robotTypes.insert(typeKE.first); // The full type is in the directory name, which is third // from the end (.../TYPE/VERSION/model.sdf). std::vector pathParts = ignition::common::split(platformNameUpper, "/"); this->dataPtr->robotFullTypes[mName->Data()] = - {type, pathParts[pathParts.size()-3]}; + {typeKE.first, pathParts[pathParts.size()-3]}; } }