[ROS2] Porting bodies::Body::computeBoundingBox changes from noetic to ROS2

include/geometric_shapes/body_operations.h

@@ -25,6 +25,7 @@
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
+
 /* Author: Ioan Sucan, E. Gil Jones */
 
 #ifndef GEOMETRIC_SHAPES_BODY_OPERATIONS_

include/geometric_shapes/obb.h

@@ -1,4 +1,4 @@
-// Copyright 2019 Open Robotics
+// Copyright 2024 Open Robotics
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:

package.xml

@@ -36,14 +36,7 @@
   <depend>visualization_msgs</depend>
 
   <build_depend>assimp-dev</build_depend>
-  <!-- <build_depend>boost</build_depend> -->
-  <!-- <build_depend>eigen</build_depend>
-  <build_depend>eigen_stl_containers</build_depend> -->
-  <!-- <build_depend>libconsole-bridge-dev</build_depend> -->
-  <!--
-  <build_depend condition="$ROS_DISTRO == noetic">fcl</build_depend> -->
-  <!-- <build_depend>libqhull</build_depend>
-  <build_depend>octomap</build_depend> -->
+  <build_depend>eigen</build_depend>
   <build_depend>libfcl-dev</build_depend>
   <build_depend>pkg-config</build_depend>
   <build_depend>libboost-dev</build_depend>

src/aabb.cpp

@@ -31,13 +31,10 @@
 #include <geometric_shapes/aabb.h>
 
 #include <fcl/config.h>
-#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5
 #include <fcl/geometry/shape/utility.h>
-#endif
 
 void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box)
 {
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
   // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...) (BSD-licensed code):
   // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
   // We don't call their code because it would need creating temporary objects, and their method is in floats in FCL 0.5
@@ -53,10 +50,4 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform,
   const Eigen::Vector3d v_delta(x_range, y_range, z_range);
   extend(t + v_delta);
   extend(t - v_delta);
-#else
-  fcl::AABBd aabb;
-  fcl::computeBV(fcl::Boxd(box), transform, aabb);
-  extend(aabb.min_);
-  extend(aabb.max_);
-#endif
 }

src/obb.cpp

@@ -2,37 +2,12 @@
 
 #include <fcl/config.h>
 
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-#include <fcl/BV/OBB.h>
-typedef fcl::Vec3f FCL_Vec3;
-typedef fcl::OBB FCL_OBB;
-#else
 #include <fcl/math/bv/OBB.h>
 typedef fcl::Vector3d FCL_Vec3;
 typedef fcl::OBB<double> FCL_OBB;
-#endif
 
 namespace bodies
 {
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec)
-{
-  FCL_Vec3 result;
-  Eigen::Map<Eigen::MatrixXd>(result.data.vs, 3, 1) = eigenVec;
-  return result;
-}
-#else
-#define toFcl
-#endif
-
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec)
-{
-  return Eigen::Map<const Eigen::MatrixXd>(fclVec.data.vs, 3, 1);
-}
-#else
-#define fromFcl
-#endif
 
 class OBBPrivate : public FCL_OBB
 {
@@ -44,17 +19,10 @@ OBB::OBB()
 {
   obb_.reset(new OBBPrivate);
   // Initialize the OBB to position 0, with 0 extents and identity rotation
-#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5
   // FCL 0.6+ does not zero-initialize the OBB.
   obb_->extent.setZero();
   obb_->To.setZero();
   obb_->axis.setIdentity();
-#else
-  // FCL 0.5 zero-initializes the OBB, so we just put the identity into orientation.
-  obb_->axis[0][0] = 1.0;
-  obb_->axis[1][1] = 1.0;
-  obb_->axis[2][2] = 1.0;
-#endif
 }
 
 OBB::OBB(const OBB& other)
@@ -78,22 +46,14 @@ void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d
 {
   const auto rotation = pose.linear();
 
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-  obb_->axis[0] = toFcl(rotation.col(0));
-  obb_->axis[1] = toFcl(rotation.col(1));
-  obb_->axis[2] = toFcl(rotation.col(2));
-#else
   obb_->axis = rotation;
-#endif
-
-  obb_->To = toFcl(pose.translation());
 
   obb_->extent = { extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0 };
 }
 
 void OBB::getExtents(Eigen::Vector3d& extents) const
 {
-  extents = 2 * fromFcl(obb_->extent);
+  extents = 2 * obb_->extent;
 }
 
 Eigen::Vector3d OBB::getExtents() const
@@ -106,14 +66,8 @@ Eigen::Vector3d OBB::getExtents() const
 void OBB::getPose(Eigen::Isometry3d& pose) const
 {
   pose = Eigen::Isometry3d::Identity();
-  pose.translation() = fromFcl(obb_->To);
-#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-  pose.linear().col(0) = fromFcl(obb_->axis[0]);
-  pose.linear().col(1) = fromFcl(obb_->axis[1]);
-  pose.linear().col(2) = fromFcl(obb_->axis[2]);
-#else
+  pose.translation() = obb_->To;
   pose.linear() = obb_->axis;
-#endif
 }
 
 Eigen::Isometry3d OBB::getPose() const
@@ -158,7 +112,7 @@ OBB* OBB::extendApprox(const OBB& box)
 
 bool OBB::contains(const Eigen::Vector3d& point) const
 {
-  return obb_->contain(toFcl(point));
+  return obb_->contain(point);
 }
 
 bool OBB::overlaps(const bodies::OBB& other) const