diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 2d5b5245..f1ff8668 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -48,6 +49,8 @@ #include #include #include +#include +#include #include #include @@ -198,6 +201,9 @@ class subt::GameLogicPluginPrivate public: bool OnFinishCall(const ignition::msgs::Boolean &_req, ignition::msgs::Boolean &_res); + /// \brief Checks if a robot has flipped. + public: void CheckRobotFlip(); + /// \brief Ignition Transport node. public: transport::Node node; @@ -291,6 +297,11 @@ class subt::GameLogicPluginPrivate /// \brief A map of robot name and its previous pose public: std::map robotPrevPose; + /// \brief A map of robot name to its flip information. + /// The value is a pair stating the sim time where the most recent flip started, + /// and if the robot is currently flipped. + public: std::map> robotFlipInfo; + /// \brief Robot name with the max velocity public: std::pair maxRobotVel = {"", 0}; @@ -1002,6 +1013,8 @@ void GameLogicPlugin::PostUpdate( this->dataPtr->lastStatusPubTime = currentTime; } + this->dataPtr->CheckRobotFlip(); + // Periodically update the score file. if (!this->dataPtr->finished && currentTime - this->dataPtr->lastUpdateScoresTime > std::chrono::seconds(30)) @@ -1865,3 +1878,46 @@ std::ofstream &GameLogicPluginPrivate::Log() << " " << this->simTime.nsec() << " "; return this->logStream; } + +///////////////////////////////////////////////// +void GameLogicPluginPrivate::CheckRobotFlip() +{ + for (const auto &posePair : this->robotPrevPose) + { + auto name = posePair.first; + auto pose = posePair.second; + + if (this->robotFlipInfo.find(name) == this->robotFlipInfo.end()) + { + this->robotFlipInfo[name] = {this->simTime.sec(), false}; + continue; + } + + // Get cos(theta) between the world's z-axis and the robot's z-axis + // If they are in opposite directions (cos(theta) close to -1), robot is flipped + ignition::math::Vector3d a = pose.Rot() * ignition::math::Vector3d(0,0,1); + auto cos_theta = a.Z(); + if (std::abs(-1 - cos_theta) <= 0.1 ) + { + // make sure the robot has been flipped for a few seconds before logging a flip + // (avoid false positives) + auto simElapsed = this->simTime.sec() - this->robotFlipInfo[name].first; + if (!this->robotFlipInfo[name].second && (simElapsed >= 3)) + { + this->robotFlipInfo[name].second = true; + + std::ostringstream stream; + stream + << "- event:\n" + << " type: flip\n" + << " time_sec: " << this->simTime.sec() << "\n" + << " robot:" << name << "\n"; + this->LogEvent(stream.str()); + } + } + else + { + this->robotFlipInfo[name] = {this->simTime.sec(), false}; + } + } +}