Add "ChangedWorldPose" to ForwardStep::Output's expected data (#238)

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>
gazebosim · Mar 24, 2021 · 78b8ec0 · 78b8ec0
1 parent 246909b
commit 78b8ec0
Show file tree

Hide file tree

Showing 7 changed files with 30 additions and 17 deletions.
diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc
@@ -59,15 +59,15 @@ void SimulationFeatures::WorldForwardStep(
 
   // TODO(MXG): Parse input
   world->step();
-  this->WriteRequiredData(_h);
+  this->Write(_h.Get<ChangedWorldPoses>());
   // TODO(MXG): Fill in state
 }
 
-void SimulationFeatures::Write(WorldPoses &_poses) const
+void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
 {
   // remove link poses from the previous iteration
-  _poses.entries.clear();
-  _poses.entries.reserve(this->links.size());
+  _changedPoses.entries.clear();
+  _changedPoses.entries.reserve(this->links.size());
 
   std::unordered_map<std::size_t, math::Pose3d> newPoses;
 
@@ -88,7 +88,7 @@ void SimulationFeatures::Write(WorldPoses &_poses) const
           !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) ||
           !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6))
       {
-        _poses.entries.push_back(wp);
+        _changedPoses.entries.push_back(wp);
         newPoses[id] = wp.pose;
       }
       else

diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh
@@ -26,6 +26,7 @@
 #include <ignition/physics/CanWriteData.hh>
 #include <ignition/physics/ForwardStep.hh>
 #include <ignition/physics/GetContacts.hh>
+#include <ignition/physics/SpecifyData.hh>
 
 #include "Base.hh"
 
@@ -39,7 +40,8 @@ struct SimulationFeatureList : FeatureList<
 > { };
 
 class SimulationFeatures :
-    public CanWriteRequiredData<SimulationFeatures, ForwardStep::Output>,
+    public CanWriteExpectedData<SimulationFeatures,
+      ExpectData<ChangedWorldPoses>>,
     public virtual Base,
     public virtual Implements3d<SimulationFeatureList>
 {
@@ -49,7 +51,7 @@ class SimulationFeatures :
       ForwardStep::State &_x,
       const ForwardStep::Input &_u) override;
 
-  public: void Write(WorldPoses &_poses) const;
+  public: void Write(ChangedWorldPoses &_changedPoses) const;
 
   public: std::vector<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;

diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc
@@ -117,7 +117,7 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime,
     if (_firstTime && (i == 0))
     {
       EXPECT_FALSE(
-          output.Get<ignition::physics::WorldPoses>().entries.empty());
+          output.Get<ignition::physics::ChangedWorldPoses>().entries.empty());
       checkedOutput = true;
     }
   }

diff --git a/include/ignition/physics/ForwardStep.hh b/include/ignition/physics/ForwardStep.hh
@@ -49,6 +49,15 @@ namespace ignition
       std::string annotation;
     };
 
+    /// \brief ChangedWorldPoses has the same definition as WorldPoses.
+    /// This type provides a way to keep track of which poses have changed in a
+    /// simulation step.
+    struct ChangedWorldPoses
+    {
+      std::vector<WorldPose> entries;
+      std::string annotation;
+    };
+
     struct Point
     {
       ignition::math::Vector3d point;
@@ -150,7 +159,7 @@ namespace ignition
 
       public: using Output = SpecifyData<
           RequireData<WorldPoses>,
-          ExpectData<Contacts, JointPositions> >;
+          ExpectData<ChangedWorldPoses, Contacts, JointPositions> >;
 
       public: using State = CompositeData;
 

diff --git a/tpe/plugin/src/SimulationFeatures.cc b/tpe/plugin/src/SimulationFeatures.cc
@@ -62,14 +62,14 @@ void SimulationFeatures::WorldForwardStep(
     }
   }
   world->Step();
-  this->WriteRequiredData(_h);
+  this->Write(_h.Get<ChangedWorldPoses>());
 }
 
-void SimulationFeatures::Write(WorldPoses &_poses) const
+void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
 {
   // remove link poses from the previous iteration
-  _poses.entries.clear();
-  _poses.entries.reserve(this->links.size());
+  _changedPoses.entries.clear();
+  _changedPoses.entries.reserve(this->links.size());
 
   std::unordered_map<std::size_t, math::Pose3d> newPoses;
 
@@ -90,7 +90,7 @@ void SimulationFeatures::Write(WorldPoses &_poses) const
         WorldPose wp;
         wp.pose = nextPose;
         wp.body = id;
-        _poses.entries.push_back(wp);
+        _changedPoses.entries.push_back(wp);
         newPoses[id] = nextPose;
       }
       else

diff --git a/tpe/plugin/src/SimulationFeatures.hh b/tpe/plugin/src/SimulationFeatures.hh
@@ -26,6 +26,7 @@
 #include <ignition/physics/CanWriteData.hh>
 #include <ignition/physics/ForwardStep.hh>
 #include <ignition/physics/GetContacts.hh>
+#include <ignition/physics/SpecifyData.hh>
 
 #include "Base.hh"
 
@@ -39,7 +40,8 @@ struct SimulationFeatureList : FeatureList<
 > { };
 
 class SimulationFeatures :
-  public CanWriteRequiredData<SimulationFeatures, ForwardStep::Output>,
+  public CanWriteExpectedData<SimulationFeatures,
+    ExpectData<ChangedWorldPoses>>,
   public virtual Base,
   public virtual Implements3d<SimulationFeatureList>
 {
@@ -49,7 +51,7 @@ class SimulationFeatures :
     ForwardStep::State &_x,
     const ForwardStep::Input &_u) override;
 
-  public: void Write(WorldPoses &_poses) const;
+  public: void Write(ChangedWorldPoses &_changedPoses) const;
 
   public: std::vector<ContactInternal> GetContactsFromLastStep(
     const Identity &_worldID) const override;

diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc
@@ -116,7 +116,7 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime,
     if (_firstTime && (i == 0))
     {
       EXPECT_FALSE(
-          output.Get<ignition::physics::WorldPoses>().entries.empty());
+          output.Get<ignition::physics::ChangedWorldPoses>().entries.empty());
       checkedOutput = true;
     }
   }