From a7ebecca4393d43393e315d379a876e71820fd96 Mon Sep 17 00:00:00 2001
From: Jennifer Buehler <jennifer.buehler@tomtom.com>
Date: Mon, 7 Dec 2020 16:48:29 +0100
Subject: [PATCH] Add compatibility for gazebo 11

---
 gazebo_state_plugins/src/GazeboObjectInfo.cpp   |  2 +-
 .../GazeboVersionHelpers.h                      | 17 ++++++++++++++---
 .../src/GazeboVersionHelpers.cpp                |  7 +++++--
 3 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/gazebo_state_plugins/src/GazeboObjectInfo.cpp b/gazebo_state_plugins/src/GazeboObjectInfo.cpp
index 30f38db..0c5ba7f 100644
--- a/gazebo_state_plugins/src/GazeboObjectInfo.cpp
+++ b/gazebo_state_plugins/src/GazeboObjectInfo.cpp
@@ -157,7 +157,7 @@ shape_msgs::SolidPrimitive * GazeboObjectInfo::getSolidPrimitive(physics::Collis
     else
     {
         ROS_WARN("shape type %i of collision %s not supported. Using bounding box instead. ", c->GetShapeType(),c->GetName().c_str());
-        gz_math::Box box = GetBoundingBox(*c);
+        GzBox box = GetBoundingBox(*c);
         GzVector3 bb = GetBoundingBoxDimensions(box);
         if ((GetX(bb) < 1e-05) || (GetY(bb) < 1e-05) || (GetZ(bb) < 1e-05)){
             ROS_WARN_ONCE("Skipping coll %s because its bounding box is flat",c->GetName().c_str());
diff --git a/gazebo_version_helpers/include/gazebo_version_helpers/GazeboVersionHelpers.h b/gazebo_version_helpers/include/gazebo_version_helpers/GazeboVersionHelpers.h
index 5a8f359..6deafce 100644
--- a/gazebo_version_helpers/include/gazebo_version_helpers/GazeboVersionHelpers.h
+++ b/gazebo_version_helpers/include/gazebo_version_helpers/GazeboVersionHelpers.h
@@ -7,13 +7,22 @@ namespace gazebo
 {
 
 // typedefs
-#if GAZEBO_MAJOR_VERSION >= 8
+#if GAZEBO_MAJOR_VERSION >= 11
+namespace gz_math = ignition::math;
+typedef gz_math::Pose3d GzPose3;
+typedef gz_math::Vector3d GzVector3;
+typedef gz_math::Quaterniond GzQuaternion;
+typedef gz_math::Matrix4d GzMatrix4;
+typedef gz_math::Matrix3d GzMatrix3;
+typedef gz_math::AxisAlignedBox GzBox;
+#elif GAZEBO_MAJOR_VERSION >= 8
 namespace gz_math = ignition::math;
 typedef gz_math::Pose3d GzPose3;
 typedef gz_math::Vector3d GzVector3;
 typedef gz_math::Quaterniond GzQuaternion;
 typedef gz_math::Matrix4d GzMatrix4;
 typedef gz_math::Matrix3d GzMatrix3;
+typedef gz_math::Box GzBox;
 #else
 namespace gz_math = gazebo::math;
 typedef gz_math::Pose GzPose3;
@@ -21,8 +30,10 @@ typedef gz_math::Vector3 GzVector3;
 typedef gz_math::Quaternion GzQuaternion;
 typedef gz_math::Matrix4 GzMatrix4;
 typedef gz_math::Matrix3 GzMatrix3;
+typedef gz_math::Box GzBox;
 #endif
 
+
 // Helper functions
 // //////////////////////////
 
@@ -116,7 +127,7 @@ std::string GetName(const T& t)
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename T>
-gz_math::Box GetBoundingBox(const T &t)
+GzBox GetBoundingBox(const T &t)
 {
 #if GAZEBO_MAJOR_VERSION >= 8
     return t.BoundingBox();
@@ -126,7 +137,7 @@ gz_math::Box GetBoundingBox(const T &t)
 }
 
 ///////////////////////////////////////////////////////////////////////////////
-GzVector3 GetBoundingBoxDimensions(const gz_math::Box &box);
+GzVector3 GetBoundingBoxDimensions(const GzBox &box);
 
 ///////////////////////////////////////////////////////////////////////////////
 template<typename T>
diff --git a/gazebo_version_helpers/src/GazeboVersionHelpers.cpp b/gazebo_version_helpers/src/GazeboVersionHelpers.cpp
index e0ef898..f724c34 100644
--- a/gazebo_version_helpers/src/GazeboVersionHelpers.cpp
+++ b/gazebo_version_helpers/src/GazeboVersionHelpers.cpp
@@ -314,9 +314,12 @@ gazebo::physics::Model_V gazebo::GetModels(
 }
 
 ///////////////////////////////////////////////////////////////////////////////
-gazebo::GzVector3 gazebo::GetBoundingBoxDimensions(const gz_math::Box &box)
+gazebo::GzVector3 gazebo::GetBoundingBoxDimensions(const GzBox &box)
 {
-#if GAZEBO_MAJOR_VERSION >= 8
+#if GAZEBO_MAJOR_VERSION >= 11
+    auto const size = box.Size();
+    GzVector3 bb(size.X(), size.Y(), size.Z());
+#elif GAZEBO_MAJOR_VERSION >= 8
     GzVector3 bb(box.XLength(), box.YLength(), box.ZLength());
 #else
     GzVector3 bb(box.GetXLength(), box.GetYLength(), box.GetZLength());
-- 
GitLab