Port support for computing model bounding box in physics system

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. Added Link::AddWorldWrench function that adds a wrench to a link.
     * [BitBucket pull request 509](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-gazebo/pull-requests/509)
 

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>
@@ -43,6 +44,7 @@
 #include <ignition/physics/FreeGroup.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>
@@ -71,6 +73,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"
@@ -129,6 +132,7 @@ class ignition::gazebo::systems::PhysicsPrivate
           ignition::physics::GetBasicJointState,
           ignition::physics::SetBasicJointState,
           ignition::physics::SetJointVelocityCommandFeature,
+          ignition::physics::GetModelBoundingBox,
           ignition::physics::sdf::ConstructSdfCollision,
           ignition::physics::sdf::ConstructSdfJoint,
           ignition::physics::sdf::ConstructSdfLink,
@@ -248,6 +252,15 @@ 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;
+                     }};
 };
 
 //////////////////////////////////////////////////
@@ -881,6 +894,30 @@ 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;
+        }
+
+        math::AxisAlignedBox bbox =
+            math::eigen3::convert(modelIt->second->GetAxisAlignedBoundingBox());
+        auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ?
+            ComponentState::OneTimeChange :
+            ComponentState::NoChange;
+        _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state);
+
+        return true;
+      });
 }
 
 //////////////////////////////////////////////////

test/integration/physics_system.cc

@@ -34,6 +34,7 @@
 #include "ignition/gazebo/SystemLoader.hh"
 #include "ignition/gazebo/test_config.hh"  // NOLINT(build/include)
 
+#include "ignition/gazebo/components/AxisAlignedBox.hh"
 #include "ignition/gazebo/components/CanonicalLink.hh"
 #include "ignition/gazebo/components/Collision.hh"
 #include "ignition/gazebo/components/Geometry.hh"
@@ -842,3 +843,72 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent)
   // Second velocity should be different, but close
   EXPECT_NEAR(vel0, velocities[1], 0.05);
 }
+
+/////////////////////////////////////////////////
+TEST_F(PhysicsSystemFixture, GetBoundingBox)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/contact.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1ns);
+
+  // a map of model name to its axis aligned box
+  std::map<std::string, ignition::math::AxisAlignedBox> bbox;
+
+  // Create a system that records the bounding box of a model
+  Relay testSystem;
+
+  testSystem.OnPreUpdate(
+    [&](const gazebo::UpdateInfo &,
+    gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Model, components::Name, components::Static>(
+        [&](const ignition::gazebo::Entity &_entity, const components::Model *,
+        const components::Name *_name, const components::Static *)->bool
+        {
+          // create axis aligned box to be filled by physics
+          if (_name->Data() == "box1")
+          {
+            auto bboxComp = _ecm.Component<components::AxisAlignedBox>(_entity);
+            // the test only runs for 1 iteration so the component should be
+            // null in the first iteration.
+            EXPECT_EQ(bboxComp, nullptr);
+            _ecm.CreateComponent(_entity, components::AxisAlignedBox());
+            return true;
+          }
+          return true;
+        });
+    });
+
+  testSystem.OnPostUpdate(
+    [&](const gazebo::UpdateInfo &,
+    const gazebo::EntityComponentManager &_ecm)
+    {
+      // store models that have axis aligned box computed
+      _ecm.Each<components::Model, components::Name, components::Static,
+        components::AxisAlignedBox>(
+        [&](const ignition::gazebo::Entity &, const components::Model *,
+        const components::Name *_name, const components::Static *,
+        const components::AxisAlignedBox *_aabb)->bool
+        {
+          bbox[_name->Data()] = _aabb->Data();
+          return true;
+        });
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+  const size_t iters = 1;
+  server.Run(true, iters, false);
+
+  EXPECT_EQ(1u, bbox.size());
+  EXPECT_EQ("box1", bbox.begin()->first);
+  EXPECT_EQ(ignition::math::AxisAlignedBox(
+      ignition::math::Vector3d(-1.25, -2, 0),
+      ignition::math::Vector3d(-0.25, 2, 1)),
+      bbox.begin()->second);
+}