Make breadcrumb static after specified time

Changelog.md

@@ -2,6 +2,9 @@
 
 ### Ignition Gazebo 2.xx.xx (20XX-XX-XX)
 
+1. Make breadcrumb static after specified time
+    * [Pull Request 90](https://github.com/ignitionrobotics/ign-gazebo/pull/90)
+
 1. Disable breadcrumbs if the `max_deployments` == 0.
     * [Pull Request 88](https://github.com/ignitionrobotics/ign-gazebo/pull/88)
 

examples/worlds/breadcrumbs.sdf

@@ -464,6 +464,7 @@
       </plugin>
       <plugin filename="libignition-gazebo-breadcrumbs-system.so" name="ignition::gazebo::systems::Breadcrumbs">
        <topic>/B2/deploy</topic>
+       <disable_physics_time>2.0</disable_physics_time>
        <max_deployments>3</max_deployments>
         <breadcrumb>
           <sdf version="1.6">

src/systems/breadcrumbs/Breadcrumbs.cc

@@ -24,9 +24,13 @@
 #include <ignition/math/Quaternion.hh>
 #include <ignition/plugin/Register.hh>
 
+#include <sdf/parser.hh>
 #include <sdf/Geometry.hh>
 
+#include "ignition/gazebo/components/CanonicalLink.hh"
+#include "ignition/gazebo/components/DetachableJoint.hh"
 #include "ignition/gazebo/components/Geometry.hh"
+#include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Performer.hh"
@@ -56,6 +60,12 @@ void Breadcrumbs::Configure(const Entity &_entity,
     return;
   }
 
+  double period =
+      _sdf->Get<double>("disable_physics_time", 0.0).first;
+  this->disablePhysicsTime =
+      std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+      std::chrono::duration<double>(period));
+
   this->model = Model(_entity);
 
   if (!_sdf->HasElement("breadcrumb"))
@@ -128,7 +138,7 @@ void Breadcrumbs::Configure(const Entity &_entity,
 }
 
 //////////////////////////////////////////////////
-void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &,
+void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
     ignition::gazebo::EntityComponentManager &_ecm)
 {
   IGN_PROFILE("Breadcrumbs::PreUpdate");
@@ -159,6 +169,14 @@ void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &,
         Entity entity = this->creator->CreateEntities(&modelToSpawn);
         this->creator->SetParent(entity, this->worldEntity);
 
+        // keep track of entities that are set to auto disable
+        if (!modelToSpawn.Static() &&
+            this->disablePhysicsTime >
+            std::chrono::steady_clock::duration::zero())
+        {
+          this->autoStaticEntities[entity] = _info.simTime;
+        }
+
         if (this->isPerformer)
         {
           auto worldName =
@@ -218,9 +236,88 @@ void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &,
     {
       this->pendingGeometryUpdate.erase(e);
     }
+
+    // make entities static when auto disable period is reached.
+    for (auto it = this->autoStaticEntities.begin();
+        it != this->autoStaticEntities.end();)
+    {
+      auto td = _info.simTime - it->second;
+      if (td > this->disablePhysicsTime)
+      {
+        auto nameComp = _ecm.Component<components::Name>(it->first);
+        if (!this->MakeStatic(it->first, _ecm))
+        {
+          ignerr << "Failed to make breadcrumb '" << nameComp->Data()
+                 << "' static." << std::endl;
+        }
+        else
+        {
+          ignmsg << "Breadcrumb '" << nameComp->Data()
+                 << "' is now static." << std::endl;
+
+        }
+        this->autoStaticEntities.erase(it++);
+      }
+      else
+      {
+        ++it;
+      }
+    }
   }
 }
 
+//////////////////////////////////////////////////
+bool Breadcrumbs::MakeStatic(Entity _entity, EntityComponentManager &_ecm)
+{
+  // make breadcrumb static by spawning a static model and attaching the
+  // breadcrumb to the static model
+  // todo(anyone) Add a feature in ign-physics to support making a model
+  // static
+  if (this->staticModelToSpawn.LinkCount() == 0u)
+  {
+    sdf::ElementPtr staticModelSDF(new sdf::Element);
+    sdf::initFile("model.sdf", staticModelSDF);
+    staticModelSDF->GetAttribute("name")->Set("static_model");
+    staticModelSDF->GetElement("static")->Set(true);
+    sdf::ElementPtr linkElem = staticModelSDF->AddElement("link");
+    linkElem->GetAttribute("name")->Set("static_link");
+    this->staticModelToSpawn.Load(staticModelSDF);
+  }
+
+  auto bcPoseComp = _ecm.Component<components::Pose>(_entity);
+  if (!bcPoseComp)
+    return false;
+  math::Pose3d p = bcPoseComp->Data();
+  this->staticModelToSpawn.SetPose(p);
+
+  auto nameComp = _ecm.Component<components::Name>(_entity);
+  this->staticModelToSpawn.SetName(nameComp->Data() + "__static__");
+
+  Entity staticEntity = this->creator->CreateEntities(&staticModelToSpawn);
+  this->creator->SetParent(staticEntity, this->worldEntity);
+
+  Entity parentLinkEntity = _ecm.EntityByComponents(
+      components::Link(), components::ParentEntity(staticEntity),
+      components::Name("static_link"));
+
+  if (parentLinkEntity == kNullEntity)
+    return false;
+
+  Entity childLinkEntity = _ecm.EntityByComponents(
+      components::CanonicalLink(), components::ParentEntity(_entity));
+
+  if (childLinkEntity == kNullEntity)
+    return false;
+
+  Entity detachableJointEntity = _ecm.CreateEntity();
+  _ecm.CreateComponent(detachableJointEntity,
+      components::DetachableJoint(
+      {parentLinkEntity, childLinkEntity, "fixed"}));
+
+  return true;
+}
+
+
 //////////////////////////////////////////////////
 void Breadcrumbs::OnDeploy(const msgs::Empty &)
 {

src/systems/breadcrumbs/Breadcrumbs.hh

@@ -63,6 +63,10 @@ namespace systems
   /// be unbounded. If a value of zero is used, then the breadcrumb system will
   /// be disabled. A zero value is useful for situations where SDF files are
   /// programmatically created.
+  /// `<disable_physics_time>`: The time in which the breadcrumb entity's
+  /// dynamics remain enabled. After his specified time, the breadcrumb will
+  /// be made static. If this value is <= 0 or the param is not specified, the
+  /// breadcrumb model's dynamics will not be modified.
   /// `<performer_volume>`: Geometry that represents the bounding volume of
   /// the performer. Only `<geometry><box>` is supported currently. When this
   /// parameter is present, the deployed models will be performers.
@@ -90,6 +94,12 @@ namespace systems
     /// \brief Callback to deployment topic
     private: void OnDeploy(const msgs::Empty &_msg);
 
+    /// \brief Make an entity static
+    /// \param[in] _entity Entity to make static
+    /// \param[in] _ecm Entity component manager
+    /// \return True if operation is successful, false otherwise
+    public: bool MakeStatic(Entity _entity, EntityComponentManager &_ecm);
+
     /// \brief Set to true after initialization with valid parameters
     private: bool initialized{false};
 
@@ -128,6 +138,17 @@ namespace systems
 
     /// \brief Mutex to protect pending commands
     private: std::mutex pendingCmdsMutex;
+
+    /// \brief Time when the entity should be made static after they are spawned
+    private: std::chrono::steady_clock::duration disablePhysicsTime =
+        std::chrono::steady_clock::duration::zero();
+
+    /// \brief A map of auto static entities and time when they are spawned.
+    private: std::unordered_map<Entity, std::chrono::steady_clock::duration>
+        autoStaticEntities;
+
+    /// \brief SDF DOM of a static model with empty link
+    private: sdf::Model staticModelToSpawn;
   };
   }
 }

test/integration/breadcrumbs.cc

@@ -17,6 +17,9 @@
 
 #include <gtest/gtest.h>
 
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
 #include <ignition/msgs/empty.pb.h>
 #include <ignition/msgs/twist.pb.h>
 
@@ -439,3 +442,90 @@ TEST_F(BreadcrumbsTest, PerformerSetVolume)
   server.AddSystem(testSystem.systemPtr);
   server.Run(true, nIters, false);
 }
+
+/////////////////////////////////////////////////
+// The test verifies breadcrumbs physics is disabled using disable_physics_time
+TEST_F(BreadcrumbsTest, DeployDisablePhysics)
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/breadcrumbs.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  using namespace std::chrono_literals;
+  server.SetUpdatePeriod(1ns);
+
+  Relay testSystem;
+  transport::Node node;
+  auto cmdVel = node.Advertise<msgs::Twist>("/model/vehicle_blue/cmd_vel");
+  auto deployB2 =
+      node.Advertise<msgs::Empty>("/model/vehicle_blue/breadcrumbs/B2/deploy");
+
+  std::size_t iterTestStart = 1000;
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
+                              const gazebo::EntityComponentManager &_ecm)
+  {
+    // Start moving the vehicle
+    // After 1000 iterations, stop the vehicle, spawn a breadcrumb
+    // Check the pose of the breadcrumb
+    if (_info.iterations == iterTestStart)
+    {
+      msgs::Twist msg;
+      msg.mutable_linear()->set_x(0.5);
+      cmdVel.Publish(msg);
+    }
+    else if (_info.iterations == iterTestStart + 1000)
+    {
+      // Stop vehicle
+      cmdVel.Publish(msgs::Twist());
+    }
+    else if (_info.iterations == iterTestStart + 1200)
+    {
+      // Deploy
+      deployB2.Publish(msgs::Empty());
+    }
+    else if (_info.iterations == iterTestStart + 2000)
+    {
+      // Check pose
+      Entity vehicleBlue = _ecm.EntityByComponents(
+          components::Model(), components::Name("vehicle_blue"));
+      ASSERT_NE(vehicleBlue, kNullEntity);
+
+      auto poseVehicle = _ecm.Component<components::Pose>(vehicleBlue);
+      ASSERT_NE(poseVehicle , nullptr);
+
+      EXPECT_GT(poseVehicle->Data().Pos().X(), 0.4);
+
+      // The first breadcrumb
+      Entity b2 = _ecm.EntityByComponents(components::Model(),
+                                          components::Name("B2_0"));
+      ASSERT_NE(b2, kNullEntity);
+      auto poseB2 = _ecm.Component<components::Pose>(b2);
+
+      ASSERT_NE(poseB2, nullptr);
+
+      // check pose of breadcrumb
+      auto poseDiff = poseVehicle->Data().Inverse() * poseB2->Data();
+      EXPECT_NEAR(-2.2, poseDiff.Pos().X(), 1e-2);
+      EXPECT_NEAR(0.0, poseDiff.Pos().Y(), 1e-2);
+
+      // Verify that the breadcrumb stopped falling after 0.5s.
+      sdf::Root root;
+      root.Load(sdfFile);
+      const sdf::World *world = root.WorldByIndex(0);
+      double gz = world->Gravity().Z();
+      double z0 = 2.0;
+      double t = 0.5;
+      double z = z0 + gz/2*t*t;
+      EXPECT_NEAR(z, poseDiff.Pos().Z(), 1e-2);
+    }
+  });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, iterTestStart + 2001, false);
+}

test/worlds/breadcrumbs.sdf

@@ -496,6 +496,50 @@
         </sdf>
        </breadcrumb>
       </plugin>
+      <plugin filename="libignition-gazebo-breadcrumbs-system.so" name="ignition::gazebo::systems::Breadcrumbs">
+       <max_deployments>3</max_deployments>
+       <disable_physics_time>0.5</disable_physics_time>
+       <breadcrumb>
+         <sdf version="1.6">
+          <model name="B2">
+            <pose>-2.2 0 2.0 0 0 0</pose>
+            <link name='body'>
+              <pose>0 0 0.25 0 0 0</pose>
+              <inertial>
+                <mass>0.6</mass>
+                <inertia>
+                  <ixx>0.017</ixx>
+                  <ixy>0</ixy>
+                  <ixz>0</ixz>
+                  <iyy>0.017</iyy>
+                  <iyz>0</iyz>
+                  <izz>0.017</izz>
+                </inertia>
+              </inertial>
+              <visual name='visual'>
+                <geometry>
+                  <box>
+                    <size>0.5 0.5 0.5</size>
+                  </box>
+                </geometry>
+                <material>
+                  <ambient>0.0 1.0 1.0 1</ambient>
+                  <diffuse>0.0 1.0 1.0 1</diffuse>
+                  <specular>0.5 0.5 0.5 1</specular>
+                </material>
+              </visual>
+              <collision name='collision'>
+                <geometry>
+                  <box>
+                    <size>0.5 0.5 0.5</size>
+                  </box>
+                </geometry>
+              </collision>
+            </link>
+          </model>
+        </sdf>
+       </breadcrumb>
+      </plugin>
       <plugin filename="libignition-gazebo-breadcrumbs-system.so" name="ignition::gazebo::systems::Breadcrumbs">
         <max_deployments>-1</max_deployments>
         <topic>/fuel_deploy</topic>