Port support for computing model bounding box in physics system (#127)

* port support for computing model bounding box in physics system

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

Changelog.md

@@ -2,6 +2,9 @@
 
 ### Ignition Gazebo 3.X.X
 
+1. Port support for computing model bounding box in physics system
+    * [pull request 127](https://github.com/ignitionrobotics/ign-gazebo/pull/127)
+
 1.  Add DetachableJoint: A system that initially attaches two models via a fixed joint and allows for the models to get detached during simulation via a topic.
     * [BitBucket pull request 440](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-gazebo/pull-requests/440)
 

include/ignition/gazebo/Conversions.hh

@@ -552,6 +552,41 @@ namespace ignition
     /// \return Entity_Type.
     template<>
     msgs::Entity_Type convert(const std::string &_in);
+
+    /// \brief Generic conversion from axis aligned box object to another type.
+    /// \param[in] _in Axis aligned box object.
+    /// \return Conversion result.
+    /// \tparam Out Output type.
+    template<class Out>
+    Out convert(const math::AxisAlignedBox &/*_in*/)
+    {
+      Out::ConversionNotImplemented;
+    }
+
+    /// \brief Specialized conversion from a math axis aligned box object to an
+    /// axis aligned box message
+    /// \param[in] _in Axis aligned box message
+    /// \return Axis aligned box message.
+    template<>
+    msgs::AxisAlignedBox convert(const math::AxisAlignedBox &_in);
+
+    /// \brief Generic conversion from an axis aligned box message to another
+    /// type.
+    /// \param[in] _in Axis aligned box message
+    /// \return Conversion result.
+    /// \tparam Out Output type.
+    template<class Out>
+    Out convert(const msgs::AxisAlignedBox &/*_in*/)
+    {
+      Out::ConversionNotImplemented;
+    }
+
+    /// \brief Specialized conversion from an math axis aligned box message to
+    /// an axis aligned box object.
+    /// \param[in] _in Axis aligned box object
+    /// \return Axis aligned box object.
+    template<>
+    math::AxisAlignedBox convert(const msgs::AxisAlignedBox &_in);
     }
   }
 }

include/ignition/gazebo/components/AxisAlignedBox.hh

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_AxisAlignedBox_HH_
+#define IGNITION_GAZEBO_COMPONENTS_AxisAlignedBox_HH_
+
+#include <ignition/msgs/axis_aligned_box.pb.h>
+#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Conversions.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace serializers
+{
+  using AxisAlignedBoxSerializer =
+      serializers::ComponentToMsgSerializer<math::AxisAlignedBox,
+      msgs::AxisAlignedBox>;
+}
+
+namespace components
+{
+  /// \brief A component type that contains axis aligned box,
+  /// ignition::math::AxisAlignedBox, information.
+  /// The axis aligned box is created from collisions in the entity
+  using AxisAlignedBox = Component<ignition::math::AxisAlignedBox,
+      class AxisAlignedBoxTag, serializers::AxisAlignedBoxSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.AxisAlignedBox",
+      AxisAlignedBox)
+}
+}
+}
+}
+
+#endif

src/Conversions.cc

@@ -16,6 +16,7 @@
 */
 
 #include <ignition/msgs/atmosphere.pb.h>
+#include <ignition/msgs/axis_aligned_box.pb.h>
 #include <ignition/msgs/boxgeom.pb.h>
 #include <ignition/msgs/cylindergeom.pb.h>
 #include <ignition/msgs/entity.pb.h>
@@ -32,6 +33,7 @@
 #include <ignition/msgs/Utility.hh>
 
 #include <ignition/math/Angle.hh>
+#include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Helpers.hh>
 #include <ignition/math/Temperature.hh>
 
@@ -1242,3 +1244,23 @@ gazebo::UpdateInfo ignition::gazebo::convert(const msgs::WorldStatistics &_in)
   out.dt = convert<std::chrono::steady_clock::duration>(_in.step_size());
   return out;
 }
+
+//////////////////////////////////////////////////
+template<>
+msgs::AxisAlignedBox ignition::gazebo::convert(const math::AxisAlignedBox &_in)
+{
+  msgs::AxisAlignedBox out;
+  msgs::Set(out.mutable_min_corner(), _in.Min());
+  msgs::Set(out.mutable_max_corner(), _in.Max());
+  return out;
+}
+
+//////////////////////////////////////////////////
+template<>
+math::AxisAlignedBox ignition::gazebo::convert(const msgs::AxisAlignedBox &_in)
+{
+  math::AxisAlignedBox out;
+  out.Min() = msgs::Convert(_in.min_corner());
+  out.Max() = msgs::Convert(_in.max_corner());
+  return out;
+}

src/Conversions_TEST.cc

@@ -626,6 +626,28 @@ TEST(Conversions, UpdateInfo)
   EXPECT_TRUE(newInfo.paused);
 }
 
+/////////////////////////////////////////////////
+TEST(Conversions, AxisAlignedbox)
+{
+  math::AxisAlignedBox aabb;
+  aabb.Min() = math::Vector3d(-1, -2, -3);
+  aabb.Max() = math::Vector3d(1, 2, 3);
+
+  auto aabbMsg = convert<msgs::AxisAlignedBox>(aabb);
+  auto min = msgs::Convert(aabbMsg.min_corner());
+  auto max = msgs::Convert(aabbMsg.max_corner());
+  EXPECT_EQ(math::Vector3d(-1, -2, -3), min);
+  EXPECT_EQ(math::Vector3d(1, 2, 3), max);
+
+  msgs::AxisAlignedBox aabbMsg2;
+  msgs::Set(aabbMsg2.mutable_min_corner(), math::Vector3d(2, 3, 4));
+  msgs::Set(aabbMsg2.mutable_max_corner(), math::Vector3d(20, 30, 40));
+
+  auto aabb2 = convert<math::AxisAlignedBox>(aabbMsg2);
+  EXPECT_EQ(math::Vector3d(2, 3, 4), aabb2.Min());
+  EXPECT_EQ(math::Vector3d(20, 30, 40), aabb2.Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Conversions, Actor)
 {

src/systems/physics/Physics.cc

@@ -26,6 +26,7 @@
 
 #include <ignition/common/Profiler.hh>
 #include <ignition/common/MeshManager.hh>
+#include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/eigen3/Conversions.hh>
 #include <ignition/physics/FeatureList.hh>
 #include <ignition/physics/FeaturePolicy.hh>
@@ -44,6 +45,7 @@
 #include <ignition/physics/FixedJoint.hh>
 #include <ignition/physics/GetContacts.hh>
 #include <ignition/physics/GetEntities.hh>
+#include <ignition/physics/GetBoundingBox.hh>
 #include <ignition/physics/Joint.hh>
 #include <ignition/physics/Link.hh>
 #include <ignition/physics/RemoveEntities.hh>
@@ -72,6 +74,7 @@
 // Components
 #include "ignition/gazebo/components/AngularAcceleration.hh"
 #include "ignition/gazebo/components/AngularVelocity.hh"
+#include "ignition/gazebo/components/AxisAlignedBox.hh"
 #include "ignition/gazebo/components/BatterySoC.hh"
 #include "ignition/gazebo/components/CanonicalLink.hh"
 #include "ignition/gazebo/components/ChildLinkName.hh"
@@ -254,6 +257,34 @@ class ignition::gazebo::systems::PhysicsPrivate
                          math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
                          math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
                      }};
+
+  /// \brief AxisAlignedBox equality comparison function.
+  public: std::function<bool(const math::AxisAlignedBox &,
+          const math::AxisAlignedBox&)>
+          axisAlignedBoxEql { [](const math::AxisAlignedBox &_a,
+                                 const math::AxisAlignedBox &_b)
+                     {
+                       return _a == _b;
+                     }};
+
+  //////////////////////////////////////////////////
+  // Bounding box
+
+  /// \brief Feature list for model bounding box.
+  public: using BoundingBoxFeatureList = ignition::physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetModelBoundingBox>;
+
+  /// \brief Model type with bounding box feature.
+  public: using ModelBoundingBoxPtrType = ignition::physics::ModelPtr<
+            ignition::physics::FeaturePolicy3d, BoundingBoxFeatureList>;
+
+  /// \brief A map between model entity ids in the ECM to Model Entities in
+  /// ign-physics, with bounding box feature.
+  /// All models on this map are also in `entityModelMap`. The difference is
+  /// that here they've been casted for `BoundingBoxFeatureList`.
+  public: std::unordered_map<Entity, ModelBoundingBoxPtrType>
+      entityModelBoundingBoxMap;
 };
 
 //////////////////////////////////////////////////
@@ -971,6 +1002,39 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
   {
     _ecm.RemoveComponent<components::WorldPoseCmd>(entity);
   }
+
+  // Populate bounding box info
+  // Only compute bounding box if component exists to avoid unnecessary
+  // computations
+  _ecm.Each<components::Model, components::AxisAlignedBox>(
+      [&](const Entity &_entity, const components::Model *,
+          components::AxisAlignedBox *_bbox)
+      {
+        auto modelIt = this->entityModelMap.find(_entity);
+        if (modelIt == this->entityModelMap.end())
+        {
+          ignwarn << "Failed to find model [" << _entity << "]." << std::endl;
+          return true;
+        }
+
+        auto bbModel = entityCast(_entity, modelIt->second,
+            this->entityModelBoundingBoxMap);
+        if (!bbModel)
+        {
+          ignwarn << "Can't process AxisAlignedBox component, physics engine "
+                  << "missing GetModelBoundingBox" << std::endl;
+          return true;
+        }
+
+        math::AxisAlignedBox bbox =
+            math::eigen3::convert(bbModel->GetAxisAlignedBoundingBox());
+        auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ?
+            ComponentState::OneTimeChange :
+            ComponentState::NoChange;
+        _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state);
+
+        return true;
+      });
 }
 
 //////////////////////////////////////////////////

src/systems/physics/Physics.hh

@@ -18,6 +18,10 @@
 #define IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_
 
 #include <memory>
+#include <unordered_map>
+#include <utility>
+#include <ignition/physics/RequestFeatures.hh>
+
 #include <ignition/gazebo/config.hh>
 #include <ignition/gazebo/Export.hh>
 #include <ignition/gazebo/System.hh>
@@ -52,8 +56,59 @@ namespace systems
     /// \brief Private data pointer.
     private: std::unique_ptr<PhysicsPrivate> dataPtr;
   };
+
+  /// \brief Helper function to cast from an entity type with minimum features
+  /// to an entity with a different set of features. When the entity is cast
+  /// successfully, it is added to _castMap so that subsequent casts will
+  /// use the entity from the map.
+  /// \tparam PolicyT The feature policy, such as
+  /// `ignition::physics::FeaturePolicy3d`.
+  /// \tparam ToFeatureList The list of features of the resulting entity.
+  /// \tparam MinimumFeatureList The minimum list of features.
+  /// \tparam ToEntity Type of entities with ToFeatureList
+  /// \tparam MinimumEntity Type of entities with MinimumFeatureList
+  /// \param[in] _entity Entity ID.
+  /// \param[in] _minimumEntity Entity pointer with minimum features.
+  /// \param[in] _castMap Map to store entities that have already been cast.
+  template <
+      typename PolicyT,
+      typename ToFeatureList,
+      typename MinimumFeatureList,
+      template <typename, typename> class ToEntity,
+      template <typename, typename> class MinimumEntity>
+  physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> entityCast(
+      Entity _entity,
+      const physics::EntityPtr<MinimumEntity<PolicyT, MinimumFeatureList>>
+        &_minimumEntity,
+      std::unordered_map<Entity, physics::EntityPtr<
+        ToEntity<PolicyT, ToFeatureList>>> &_castMap)
+  {
+    // Has already been cast
+    auto castIt = _castMap.find(_entity);
+    if (castIt != _castMap.end())
+    {
+      return castIt->second;
+    }
+
+    physics::EntityPtr<ToEntity<PolicyT, ToFeatureList>> castEntity;
+
+    // Cast
+    castEntity =
+        physics::RequestFeatures<ToFeatureList>::From(_minimumEntity);
+
+    if (!castEntity)
+    {
+      ignwarn << "Physics engine missing requested feature." << std::endl;
+    }
+    else
+    {
+      _castMap.insert(std::make_pair(_entity, castEntity));
+    }
+
+    return castEntity;
   }
 }
 }
 }
+}
 #endif