KineticEnergy and HaltMotion added to models

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;

subt_ign/launch/competition.ign

@@ -606,6 +606,12 @@
               <wheel_radius>0.1651</wheel_radius>
             </wheel>
           </plugin>
+          <plugin
+            filename="ignition-gazebo-kinetic-energy-monitor-system"
+            name="ignition::gazebo::systems::KineticEnergyMonitor">
+            <link_name>base_link</link_name>
+            <kinetic_energy_threshold>100</kinetic_energy_threshold>
+          </plugin>
         </include>
         </sdf>
       </spawn>
@@ -749,6 +755,12 @@
               <wheel_radius>0.098</wheel_radius>
             </wheel>
           </plugin>
+          <plugin
+            filename="ignition-gazebo-kinetic-energy-monitor-system"
+            name="ignition::gazebo::systems::KineticEnergyMonitor">
+            <link_name>base_link</link_name>
+            <kinetic_energy_threshold>100</kinetic_energy_threshold>
+          </plugin>
         </include>
         </sdf>
       </spawn>
@@ -951,6 +963,12 @@
           <update_rate>10</update_rate>
           <type>gas</type>
         </plugin>
+        <plugin
+          filename="ignition-gazebo-kinetic-energy-monitor-system"
+          name="ignition::gazebo::systems::KineticEnergyMonitor">
+          <link_name>base_link</link_name>
+          <kinetic_energy_threshold>100</kinetic_energy_threshold>
+        </plugin>
       </include>
       </sdf>
     </spawn>
@@ -1206,6 +1224,12 @@
           <update_rate>10</update_rate>
           <type>gas</type>
         </plugin>
+        <plugin
+          filename="ignition-gazebo-kinetic-energy-monitor-system"
+          name="ignition::gazebo::systems::KineticEnergyMonitor">
+          <link_name>base_link</link_name>
+          <kinetic_energy_threshold>100</kinetic_energy_threshold>
+        </plugin>
         </include>
       </sdf>
     </spawn>

subt_ign/src/GameLogicPlugin.cc

@@ -45,6 +45,7 @@
 #include <ignition/gazebo/components/Static.hh>
 #include <ignition/gazebo/components/SourceFilePath.hh>
 #include <ignition/gazebo/components/World.hh>
+#include <ignition/gazebo/components/HaltMotion.hh>
 #include <ignition/gazebo/Conversions.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
 #include <ignition/gazebo/Events.hh>
@@ -76,6 +77,7 @@ IGNITION_ADD_PLUGIN(
     subt::GameLogicPlugin,
     ignition::gazebo::System,
     subt::GameLogicPlugin::ISystemConfigure,
+    subt::GameLogicPlugin::ISystemPreUpdate,
     subt::GameLogicPlugin::ISystemPostUpdate)
 
 using namespace ignition;
@@ -248,6 +250,13 @@ class subt::GameLogicPluginPrivate
   public: ignition::math::Pose3d ConvertToArtifactOrigin(
     const ignition::math::Pose3d &_pose) const;
 
+  /// \brief Kinetic energy callback triggered
+  /// when the energy goes above a set threshold.
+  /// \param[in] _msg Kinetic energy message.
+  /// \param[in] _info Message information.
+  public: void OnKineticEnergyEvent(const ignition::msgs::Double &_msg,
+    const transport::MessageInfo &_info);
+
   /// \brief Ignition Transport node.
   public: transport::Node node;
 
@@ -525,6 +534,9 @@ class subt::GameLogicPluginPrivate
 
   /// \brief Mutex to protect the eventCounter.
   public: std::mutex eventCounterMutex;
+
+  public: std::map<std::string, ignition::gazebo::Entity> robotEntityMap;
+  public: std::set<std::string> haltedRobots;
 };
 
 //////////////////////////////////////////////////
@@ -1012,6 +1024,60 @@ void GameLogicPluginPrivate::OnEvent(const ignition::msgs::Pose &_msg)
   }
 }
 
+//////////////////////////////////////////////////
+void GameLogicPlugin::PreUpdate(const UpdateInfo &_info,
+                                 EntityComponentManager &_ecm)
+{
+  if (_info.dt < std::chrono::steady_clock::duration::zero())
+  {
+    ignwarn << "Detected jump back in time ["
+        << std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
+        << "s]. System may not work properly." << std::endl;
+  }
+
+  if (_info.paused)
+    return;
+
+  if (!this->dataPtr->started)
+  {
+    _ecm.Each<gazebo::components::Sensor,
+              gazebo::components::ParentEntity>(
+        [&](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<gazebo::components::ParentEntity>(
+              _parent->Data());
+
+          if (model)
+          {
+            if (!_ecm.Component<components::HaltMotion>(model->Data()))
+            {
+              _ecm.CreateComponent(model->Data(),components::HaltMotion(false));
+            }
+          }
+          return true;
+        });
+  }
+  else
+  {
+    for (std::string robotName : this->dataPtr->haltedRobots)
+    {
+      if (this->dataPtr->robotEntityMap.find(robotName) ==
+          this->dataPtr->robotEntityMap.end())
+          continue;
+      gazebo::Entity robotEntity = this->dataPtr->robotEntityMap.at(robotName);
+      auto *haltMotionComp = _ecm.Component<components::HaltMotion>(robotEntity);
+      if (haltMotionComp && !haltMotionComp->Data())
+      {
+        haltMotionComp->Data() = true;
+      }
+    }
+  }
+}
+
 //////////////////////////////////////////////////
 void GameLogicPlugin::PostUpdate(
     const ignition::gazebo::UpdateInfo &_info,
@@ -1151,11 +1217,32 @@ void GameLogicPlugin::PostUpdate(
                 mName->Data() + "/battery/linear_battery/state";
               this->dataPtr->node.Subscribe(batteryTopic,
                   &GameLogicPluginPrivate::OnBatteryMsg, this->dataPtr.get());
+
+              // Subscribe to Kinetic energy events to evaluate robot state.
+              std::string kineticTopic = std::string("/model/") +
+                mName->Data() + "/kinetic_energy";
+              this->dataPtr->node.Subscribe(kineticTopic,
+                  &GameLogicPluginPrivate::OnKineticEnergyEvent,
+                  this->dataPtr.get());
             }
           }
           return true;
         });
 
+
+    _ecm.Each<gazebo::components::HaltMotion>(
+        [&](const gazebo::Entity &_entity,
+            const gazebo::components::HaltMotion *) -> bool
+        {
+          auto robotName = _ecm.Component<gazebo::components::Name>(_entity);
+          if (robotName)
+          {
+            this->dataPtr->robotEntityMap[robotName->Data()] = _entity;
+          }
+
+          return true;
+        });
+
     // Start automatically if warmup time has elapsed.
     if (this->dataPtr->simTime.sec() >= this->dataPtr->warmupTimeSec)
     {
@@ -2748,3 +2835,36 @@ double GameLogicPluginPrivate::FloorMultiple(double _n, double _m)
     out -= _m;
   return out;
 }
+
+/////////////////////////////////////////////////
+void GameLogicPluginPrivate::OnKineticEnergyEvent(
+    const ignition::msgs::Double &/*_msg*/,
+    const transport::MessageInfo &_info)
+{
+  if (!this->started)
+    return;
+  ignition::msgs::Time localSimTime(this->simTime);
+  std::vector<std::string> topicParts = common::split(_info.Topic(), "/");
+  std::string name = "_unknown_";
+
+  // Get the name of the model from the topic name, where the topic name
+  // look like '/model/{model_name}/kinetic_energy'.
+  if (topicParts.size() > 1)
+    name = topicParts[1];
+
+  {
+    std::lock_guard<std::mutex> lock(this->eventCounterMutex);
+    std::ostringstream stream;
+    stream
+      << "- event:\n"
+      << "  id: " << this->eventCounter << "\n"
+      << "  type: collision\n"
+      << "  time_sec: " << localSimTime.sec() << "\n"
+      << "  robot: " << name << std::endl;
+
+    this->LogEvent(stream.str());
+    this->PublishRobotEvent(localSimTime, "kinetic", name, this->eventCounter);
+    this->eventCounter++;
+    this->haltedRobots.emplace(name);
+  }
+}