From 92744ad60dbc1edcf70f16bbf789c80512ace0f4 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 22 Nov 2021 03:34:56 +0100 Subject: [PATCH 01/28] merging obb into ros2 Co-authored-by: Tyler --- CMakeLists.txt | 9 +++ package.xml | 8 +++ src/obb.cpp | 151 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 168 insertions(+) create mode 100644 src/obb.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b6861674..7a1fb3a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,15 @@ find_package(shape_msgs REQUIRED) find_package(visualization_msgs REQUIRED) include(ConfigExtras) +pkg_check_modules(LIBFCL_PC REQUIRED fcl) +# find *absolute* paths to LIBFCL_* paths +find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) +set(LIBFCL_LIBRARIES) +foreach(_lib ${LIBFCL_PC_LIBRARIES}) + find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) + list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) +endforeach() + set(THIS_PACKAGE_EXPORT_DEPENDS eigen3_cmake_module Eigen3 diff --git a/package.xml b/package.xml index f9684a78..19732263 100644 --- a/package.xml +++ b/package.xml @@ -36,6 +36,14 @@ visualization_msgs assimp-dev + boost + eigen + eigen_stl_containers + libconsole-bridge-dev + libfcl-dev + fcl + libqhull + octomap pkg-config libboost-dev libboost-filesystem-dev diff --git a/src/obb.cpp b/src/obb.cpp new file mode 100644 index 00000000..31650863 --- /dev/null +++ b/src/obb.cpp @@ -0,0 +1,151 @@ +#include + +#include + +#if FCL_MINOR_VERSION == 5 +#include +typedef fcl::Vec3f FCL_Vec3; +typedef fcl::OBB FCL_OBB; +#else +#include +typedef fcl::Vector3d FCL_Vec3; +typedef fcl::OBB FCL_OBB; +#endif + +namespace bodies +{ +#if FCL_MINOR_VERSION == 5 +inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) +{ + FCL_Vec3 result; + Eigen::Map(result.data.vs, 3, 1) = eigenVec; + return result; +} +#else +#define toFcl +#endif + +#if FCL_MINOR_VERSION == 5 +Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec) +{ + return Eigen::Map(fclVec.data.vs, 3, 1); +} +#else +#define fromFcl +#endif + +class OBBPrivate : public FCL_OBB +{ +public: + using FCL_OBB::OBB; +}; + +OBB::OBB() +{ + this->obb_.reset(new OBBPrivate); +} + +OBB::OBB(const OBB& other) : OBB() +{ + *obb_ = *other.obb_; +} + +OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB() +{ + setPoseAndExtents(pose, extents); +} + +OBB& OBB::operator=(const OBB& other) +{ + *obb_ = *other.obb_; + return *this; +} + +void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) +{ + const auto rotation = pose.linear(); + +#if 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); +} + +Eigen::Vector3d OBB::getExtents() const +{ + Eigen::Vector3d extents; + getExtents(extents); + return extents; +} + +void OBB::getPose(Eigen::Isometry3d& pose) const +{ + pose = Eigen::Isometry3d::Identity(); + pose.translation() = fromFcl(obb_->To); + // If all axes are zero, we report the rotation as identity + // This happens if OBB is default-constructed +#if FCL_MINOR_VERSION == 5 + if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero()) + { + 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 + if (!obb_->axis.isApprox(fcl::Matrix3d::Zero())) + { + pose.linear() = obb_->axis; + } +#endif +} + +Eigen::Isometry3d OBB::getPose() const +{ + Eigen::Isometry3d pose; + getPose(pose); + return pose; +} + +AABB OBB::toAABB() const +{ + AABB result; + toAABB(result); + return result; +} + +void OBB::toAABB(AABB& aabb) const +{ + aabb.extendWithTransformedBox(getPose(), getExtents()); +} + +OBB* OBB::extendApprox(const OBB& box) +{ + *this->obb_ += *box.obb_; + return this; +} + +bool OBB::contains(const Eigen::Vector3d& point) +{ + return obb_->contain(toFcl(point)); +} + +bool OBB::overlaps(const bodies::OBB& other) +{ + return obb_->overlap(*other.obb_); +} + +OBB::~OBB() = default; + +} // namespace bodies From 2e7de7956a14e1f8cf013cec0b2d8950c5d78103 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 19 Apr 2024 15:10:38 -0400 Subject: [PATCH 02/28] test bounding box updated --- test/test_bounding_box.cpp | 235 ++++++++++++++++++++++++++++++++----- 1 file changed, 207 insertions(+), 28 deletions(-) diff --git a/test/test_bounding_box.cpp b/test/test_bounding_box.cpp index c4b9d5f5..40d174be 100644 --- a/test/test_bounding_box.cpp +++ b/test/test_bounding_box.cpp @@ -1,30 +1,36 @@ -// Copyright 2019 Open Robotics -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Open Robotics nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// 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. +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Open Robotics. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Open Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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 Martin Pecka */ @@ -41,6 +47,8 @@ TEST(SphereBoundingBox, Sphere1) bodies::Sphere body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -48,6 +56,15 @@ TEST(SphereBoundingBox, Sphere1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(SphereBoundingBox, Sphere2) @@ -60,6 +77,8 @@ TEST(SphereBoundingBox, Sphere2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); @@ -68,9 +87,16 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().isApprox(pose, 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); @@ -79,6 +105,14 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4); + + // oriented bounding box doesn't rotate with the sphere + EXPECT_TRUE(obbox.getPose().translation().isApprox(pose.translation(), 1e-4)); + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + // verify the bounding box is rotation-invariant random_numbers::RandomNumberGenerator gen; @@ -95,7 +129,9 @@ TEST(SphereBoundingBox, Sphere2) pose.linear() = quat.toRotationMatrix(); body.setPose(pose); bodies::AABB bbox2; + bodies::OBB obbox2; body.computeBoundingBox(bbox2); + body.computeBoundingBox(obbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); @@ -103,6 +139,12 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); + + EXPECT_NEAR(obbox.getExtents().x(), obbox2.getExtents().x(), 1e-4); + EXPECT_NEAR(obbox.getExtents().y(), obbox2.getExtents().y(), 1e-4); + EXPECT_NEAR(obbox.getExtents().z(), obbox2.getExtents().z(), 1e-4); + + EXPECT_TRUE(obbox2.getPose().isApprox(obbox.getPose(), 1e-4)); } } @@ -112,6 +154,8 @@ TEST(BoxBoundingBox, Box1) bodies::Box body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -119,6 +163,15 @@ TEST(BoxBoundingBox, Box1) EXPECT_NEAR(0.5, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.5, bbox.max().z(), 1e-4); + + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(BoxBoundingBox, Box2) @@ -131,6 +184,8 @@ TEST(BoxBoundingBox, Box2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -139,9 +194,19 @@ TEST(BoxBoundingBox, Box2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); @@ -149,6 +214,15 @@ TEST(BoxBoundingBox, Box2) EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4); EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); + + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); } TEST(CylinderBoundingBox, Cylinder1) @@ -157,6 +231,8 @@ TEST(CylinderBoundingBox, Cylinder1) bodies::Cylinder body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -164,6 +240,15 @@ TEST(CylinderBoundingBox, Cylinder1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(CylinderBoundingBox, Cylinder2) @@ -176,6 +261,8 @@ TEST(CylinderBoundingBox, Cylinder2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.0, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -184,9 +271,19 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.3238, bbox.min().x(), 1e-4); EXPECT_NEAR(0.7862, bbox.min().y(), 1e-4); @@ -195,6 +292,15 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(3.2138, bbox.max().y(), 1e-4); EXPECT_NEAR(4.2761, bbox.max().z(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); + // verify the bounding box is yaw-invariant random_numbers::RandomNumberGenerator gen(0); @@ -206,6 +312,7 @@ TEST(CylinderBoundingBox, Cylinder2) body.computeBoundingBox(bbox); bodies::AABB bbox2; + bodies::OBB obbox2; for (size_t i = 0; i < 10; ++i) { const auto angle = gen.uniformReal(-M_PI, M_PI); @@ -213,6 +320,7 @@ TEST(CylinderBoundingBox, Cylinder2) pose.linear() = (rollPitch * yaw).toRotationMatrix(); body.setPose(pose); body.computeBoundingBox(bbox2); + body.computeBoundingBox(obbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); @@ -220,6 +328,16 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox2.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox2.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox2.getPose().translation().z(), 1e-4); + + // oriented bounding boxes are not yaw-invariant + EXPECT_TRUE(obbox2.getPose().linear().isApprox(pose.linear(), 1e-4)); } } @@ -317,6 +435,8 @@ TEST(MeshBoundingBox, Mesh1) bodies::ConvexMesh body(m); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -324,6 +444,15 @@ TEST(MeshBoundingBox, Mesh1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); delete m; } @@ -338,6 +467,8 @@ TEST(MeshBoundingBox, Mesh2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -346,9 +477,19 @@ TEST(MeshBoundingBox, Mesh2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); @@ -357,6 +498,15 @@ TEST(MeshBoundingBox, Mesh2) EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); + delete m; } @@ -377,8 +527,37 @@ TEST(MergeBoundingBoxes, Merge1) EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } +TEST(MergeBoundingBoxes, OBBApprox1) +{ + std::vector boxes; + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = 0.5 * Eigen::Vector3d::Ones(); + boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1)); + + bodies::OBB bbox; + bbox.setPoseAndExtents(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); + bodies::mergeBoundingBoxesApprox(boxes, bbox); + + // the resulting bounding box is not tight, so we only do some sanity checks + + EXPECT_GE(4.0, bbox.getExtents().x()); + EXPECT_GE(4.0, bbox.getExtents().y()); + EXPECT_GE(4.0, bbox.getExtents().z()); + EXPECT_LE(2.0, bbox.getExtents().x()); + EXPECT_LE(2.0, bbox.getExtents().y()); + EXPECT_LE(2.0, bbox.getExtents().z()); + + EXPECT_TRUE(bbox.contains(boxes[0].getPose().translation())); + EXPECT_TRUE(bbox.contains(boxes[1].getPose().translation())); + EXPECT_TRUE(bbox.overlaps(boxes[0])); + EXPECT_TRUE(bbox.overlaps(boxes[1])); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} +} \ No newline at end of file From f74c0d163d596f62d4d9803c2e2ad0b0995613d0 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:12:36 -0400 Subject: [PATCH 03/28] Added obb.h --- include/geometric_shapes/obb.h | 148 +++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 include/geometric_shapes/obb.h diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h new file mode 100644 index 00000000..79e96e46 --- /dev/null +++ b/include/geometric_shapes/obb.h @@ -0,0 +1,148 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Open Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Martin Pecka */ + +#ifndef GEOMETRIC_SHAPES_OBB_H +#define GEOMETRIC_SHAPES_OBB_H + +#include + +#include +#include + +#include +#include + +namespace bodies +{ +class OBBPrivate; + +/** \brief Represents an oriented bounding box. */ +class OBB +{ +public: + /** \brief Initialize an oriented bounding box at position 0, with 0 extents and + * identity orientation. */ + OBB(); + OBB(const OBB& other); + OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents); + virtual ~OBB(); + + OBB& operator=(const OBB& other); + + /** + * \brief Set both the pose and extents of the OBB. + * \param [in] pose New pose of the OBB. + * \param [in] extents New extents of the OBB. + */ + void setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents); + + /** + * \brief Get the extents of the OBB. + * \return The extents. + */ + Eigen::Vector3d getExtents() const; + + /** + * \brief Get the extents of the OBB. + * \param extents [out] The extents. + */ + void getExtents(Eigen::Vector3d& extents) const; + + /** + * \brief Get the pose of the OBB. + * \return The pose. + */ + Eigen::Isometry3d getPose() const; + + /** + * \brief Get The pose of the OBB. + * \param pose The pose. + */ + void getPose(Eigen::Isometry3d& pose) const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \return The AABB. + */ + AABB toAABB() const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \param aabb The AABB. + */ + void toAABB(AABB& aabb) const; + + /** + * \brief Add the other OBB to this one and compute an approximate enclosing OBB. + * \param box The other box to add. + * \return Pointer to this OBB after the update. + */ + OBB* extendApprox(const OBB& box); + + /** + * \brief Check if this OBB contains the given point. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const Eigen::Vector3d& point) const; + + /** + * \brief Check whether this and the given OBBs have nonempty intersection. + * \param other The other OBB to check. + * \return Whether the OBBs overlap. + */ + bool overlaps(const OBB& other) const; + + /** + * \brief Check if this OBB contains whole other OBB. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const OBB& obb) const; + + /** + * \brief Compute coordinates of the 8 vertices of this OBB. + * \return The vertices. + */ + EigenSTL::vector_Vector3d computeVertices() const; + +protected: + /** \brief PIMPL pointer */ + std::unique_ptr obb_; +}; +} // namespace bodies + +#endif // GEOMETRIC_SHAPES_OBB_H \ No newline at end of file From 517648fe06afca02252910b3508a26f5762ba369 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 19 Apr 2024 15:14:17 -0400 Subject: [PATCH 04/28] updated tests and cpp to 232 --- src/obb.cpp | 103 ++++++++++++++++++++++++++-------- test/test_bounding_box.cpp | 112 +++++++++++++++++++++++++++++++++++-- 2 files changed, 186 insertions(+), 29 deletions(-) diff --git a/src/obb.cpp b/src/obb.cpp index 31650863..83f081c3 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -2,7 +2,7 @@ #include -#if FCL_MINOR_VERSION == 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 #include typedef fcl::Vec3f FCL_Vec3; typedef fcl::OBB FCL_OBB; @@ -14,7 +14,7 @@ typedef fcl::OBB FCL_OBB; namespace bodies { -#if FCL_MINOR_VERSION == 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) { FCL_Vec3 result; @@ -25,7 +25,7 @@ inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) #define toFcl #endif -#if FCL_MINOR_VERSION == 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec) { return Eigen::Map(fclVec.data.vs, 3, 1); @@ -42,16 +42,29 @@ class OBBPrivate : public FCL_OBB OBB::OBB() { - this->obb_.reset(new OBBPrivate); + 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) : OBB() +OBB::OBB(const OBB& other) { - *obb_ = *other.obb_; + obb_.reset(new OBBPrivate(*other.obb_)); } -OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB() +OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) { + obb_.reset(new OBBPrivate); setPoseAndExtents(pose, extents); } @@ -65,7 +78,7 @@ void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d { const auto rotation = pose.linear(); -#if FCL_MINOR_VERSION == 5 +#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)); @@ -94,20 +107,12 @@ void OBB::getPose(Eigen::Isometry3d& pose) const { pose = Eigen::Isometry3d::Identity(); pose.translation() = fromFcl(obb_->To); - // If all axes are zero, we report the rotation as identity - // This happens if OBB is default-constructed -#if FCL_MINOR_VERSION == 5 - if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero()) - { - pose.linear().col(0) = fromFcl(obb_->axis[0]); - pose.linear().col(1) = fromFcl(obb_->axis[1]); - pose.linear().col(2) = fromFcl(obb_->axis[2]); - } +#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 - if (!obb_->axis.isApprox(fcl::Matrix3d::Zero())) - { - pose.linear() = obb_->axis; - } + pose.linear() = obb_->axis; #endif } @@ -132,20 +137,70 @@ void OBB::toAABB(AABB& aabb) const OBB* OBB::extendApprox(const OBB& box) { + if (this->getExtents() == Eigen::Vector3d::Zero()) + { + *obb_ = *box.obb_; + return this; + } + + if (this->contains(box)) + return this; + + if (box.contains(*this)) + { + *obb_ = *box.obb_; + return this; + } + *this->obb_ += *box.obb_; return this; } -bool OBB::contains(const Eigen::Vector3d& point) +bool OBB::contains(const Eigen::Vector3d& point) const { return obb_->contain(toFcl(point)); } -bool OBB::overlaps(const bodies::OBB& other) +bool OBB::overlaps(const bodies::OBB& other) const { return obb_->overlap(*other.obb_); } +EigenSTL::vector_Vector3d OBB::computeVertices() const +{ + const Eigen::Vector3d e = getExtents() / 2; // do not use auto type, Eigen would be inefficient + // clang-format off + EigenSTL::vector_Vector3d result = { + { -e[0], -e[1], -e[2] }, + { -e[0], -e[1], e[2] }, + { -e[0], e[1], -e[2] }, + { -e[0], e[1], e[2] }, + { e[0], -e[1], -e[2] }, + { e[0], -e[1], e[2] }, + { e[0], e[1], -e[2] }, + { e[0], e[1], e[2] }, + }; + // clang-format on + + const auto pose = getPose(); + for (auto& v : result) + { + v = pose * v; + } + + return result; +} + +bool OBB::contains(const OBB& obb) const +{ + for (const auto& v : obb.computeVertices()) + { + if (!contains(v)) + return false; + } + return true; +} + OBB::~OBB() = default; -} // namespace bodies +} // namespace bodies \ No newline at end of file diff --git a/test/test_bounding_box.cpp b/test/test_bounding_box.cpp index 40d174be..046ec1c6 100644 --- a/test/test_bounding_box.cpp +++ b/test/test_bounding_box.cpp @@ -527,6 +527,94 @@ TEST(MergeBoundingBoxes, Merge1) EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } +TEST(MergeBoundingBoxes, OBBInvalid) +{ + auto pose = Eigen::Isometry3d::Identity(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + + bodies::OBB b1; // uninitialized OBB, extents == 0 and invalid rotation + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + // invalid b1 extended with valid b2 should equal b2 + b1.extendApprox(b2); + + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + EXPECT_NEAR(0.1, b1.getExtents().x(), 1e-12); + EXPECT_NEAR(0.1, b1.getExtents().y(), 1e-12); + EXPECT_NEAR(0.1, b1.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b1.getPose().linear().isApprox(pose.linear(), 1e-12)); +} + +TEST(MergeBoundingBoxes, OBBContains1) +{ + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + // b1 contains whole b2, so the extended b1 should be equal to the original b1 + b1.extendApprox(b2); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + auto u = b1.computeVertices(); + auto v = b2.computeVertices(); + EXPECT_NEAR(1, b1.getExtents().x(), 1e-12); + EXPECT_NEAR(1, b1.getExtents().y(), 1e-12); + EXPECT_NEAR(1, b1.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b1.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12)); +} + +TEST(MergeBoundingBoxes, OBBContains2) +{ + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + // b1 contains whole b2, so the extended b2 should be equal to the original b1 + b2.extendApprox(b1); + + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + EXPECT_NEAR(1, b2.getExtents().x(), 1e-12); + EXPECT_NEAR(1, b2.getExtents().y(), 1e-12); + EXPECT_NEAR(1, b2.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b2.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12)); +} + TEST(MergeBoundingBoxes, OBBApprox1) { std::vector boxes; @@ -538,17 +626,31 @@ TEST(MergeBoundingBoxes, OBBApprox1) boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1)); bodies::OBB bbox; - bbox.setPoseAndExtents(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); + // check that the empty constructor constructs a valid empty OBB + EXPECT_EQ(0.0, bbox.getPose().translation().x()); + EXPECT_EQ(0.0, bbox.getPose().translation().y()); + EXPECT_EQ(0.0, bbox.getPose().translation().z()); + EXPECT_EQ(0.0, bbox.getExtents().x()); + EXPECT_EQ(0.0, bbox.getExtents().y()); + EXPECT_EQ(0.0, bbox.getExtents().z()); + EXPECT_TRUE(bbox.getPose().rotation().isApprox(Eigen::Matrix3d::Identity())); + bodies::mergeBoundingBoxesApprox(boxes, bbox); - // the resulting bounding box is not tight, so we only do some sanity checks + // the resulting bounding box might not be tight, so we only do some sanity checks - EXPECT_GE(4.0, bbox.getExtents().x()); - EXPECT_GE(4.0, bbox.getExtents().y()); - EXPECT_GE(4.0, bbox.getExtents().z()); + EXPECT_GE(2.1, bbox.getExtents().x()); + EXPECT_GE(2.1, bbox.getExtents().y()); + EXPECT_GE(2.1, bbox.getExtents().z()); + EXPECT_GE(0.1, bbox.getPose().translation().x()); + EXPECT_GE(0.1, bbox.getPose().translation().y()); + EXPECT_GE(0.1, bbox.getPose().translation().z()); EXPECT_LE(2.0, bbox.getExtents().x()); EXPECT_LE(2.0, bbox.getExtents().y()); EXPECT_LE(2.0, bbox.getExtents().z()); + EXPECT_LE(-0.1, bbox.getPose().translation().x()); + EXPECT_LE(-0.1, bbox.getPose().translation().y()); + EXPECT_LE(-0.1, bbox.getPose().translation().z()); EXPECT_TRUE(bbox.contains(boxes[0].getPose().translation())); EXPECT_TRUE(bbox.contains(boxes[1].getPose().translation())); From d6a80af799577bdb2498f3772ff251271b038d85 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:19:55 -0400 Subject: [PATCH 05/28] Fixed build --- CMakeLists.txt | 16 ++++++++-------- include/geometric_shapes/bodies.h | 1 + package.xml | 16 ++++++++-------- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7a1fb3a4..f7a001b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,14 +52,14 @@ find_package(shape_msgs REQUIRED) find_package(visualization_msgs REQUIRED) include(ConfigExtras) -pkg_check_modules(LIBFCL_PC REQUIRED fcl) -# find *absolute* paths to LIBFCL_* paths -find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) -set(LIBFCL_LIBRARIES) -foreach(_lib ${LIBFCL_PC_LIBRARIES}) - find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) - list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) -endforeach() +# pkg_check_modules(LIBFCL_PC REQUIRED fcl) +# # find *absolute* paths to LIBFCL_* paths +# find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) +# set(LIBFCL_LIBRARIES) +# foreach(_lib ${LIBFCL_PC_LIBRARIES}) +# find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) +# list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) +# endforeach() set(THIS_PACKAGE_EXPORT_DEPENDS eigen3_cmake_module diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index c8bf1ec0..6351fde4 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -33,6 +33,7 @@ #define _USE_MATH_DEFINES #include "geometric_shapes/aabb.h" +#include "geometric_shapes/obb.h" #include "geometric_shapes/shapes.h" #include #include diff --git a/package.xml b/package.xml index 19732263..e171e097 100644 --- a/package.xml +++ b/package.xml @@ -36,14 +36,14 @@ visualization_msgs assimp-dev - boost - eigen - eigen_stl_containers - libconsole-bridge-dev - libfcl-dev - fcl - libqhull - octomap + + + + + pkg-config libboost-dev libboost-filesystem-dev From 483b844a99eca3d4685e581b6e129c431052605b Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:21:53 -0400 Subject: [PATCH 06/28] Fixed license --- test/test_bounding_box.cpp | 60 +++++++++++++++++--------------------- 1 file changed, 27 insertions(+), 33 deletions(-) diff --git a/test/test_bounding_box.cpp b/test/test_bounding_box.cpp index 046ec1c6..ba06aa8c 100644 --- a/test/test_bounding_box.cpp +++ b/test/test_bounding_box.cpp @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Open Robotics. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Open Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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 Martin Pecka */ From c014758631641979d12475a542295cc8cf705145 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 19 Apr 2024 15:22:09 -0400 Subject: [PATCH 07/28] added obb to bodies header --- include/geometric_shapes/bodies.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index 6351fde4..da1263b9 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -241,6 +241,10 @@ class Body pose. Scaling and padding are accounted for. */ virtual void computeBoundingBox(AABB& bbox) const = 0; + /** \brief Compute the oriented bounding box for the body, in its current + pose. Scaling and padding are accounted for. */ + virtual void computeBoundingBox(OBB& bbox) const = 0; + /** \brief Get a clone of this body, but one that is located at the pose \e pose */ inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const { @@ -308,6 +312,7 @@ class Sphere : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -360,6 +365,7 @@ class Cylinder : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -422,6 +428,7 @@ class Box : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -484,6 +491,7 @@ class ConvexMesh : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; From 169d33bfc7fd39c970b5ea6f7699624169e8996f Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:28:50 -0400 Subject: [PATCH 08/28] Fixed FCL dependency --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f7a001b9..59917e37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,6 +50,7 @@ find_package(rclcpp REQUIRED) find_package(resource_retriever REQUIRED) find_package(shape_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(fcl REQUIRED) include(ConfigExtras) # pkg_check_modules(LIBFCL_PC REQUIRED fcl) @@ -75,6 +76,7 @@ set(THIS_PACKAGE_EXPORT_DEPENDS resource_retriever shape_msgs visualization_msgs + fcl ) # Set VERSION from package.xml @@ -87,6 +89,7 @@ add_library(${PROJECT_NAME} SHARED src/bodies.cpp src/body_operations.cpp src/mesh_operations.cpp + src/obb.cpp src/shape_extents.cpp src/shape_operations.cpp src/shape_to_marker.cpp @@ -94,6 +97,7 @@ add_library(${PROJECT_NAME} SHARED ) set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION} WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_compile_options(${PROJECT_NAME} PRIVATE ${PROJECT_COMPILE_OPTIONS}) +target_link_libraries(${PROJECT_NAME} fcl) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_EXPORT_DEPENDS} ) From e055eed7647e45794804c3518d36974080ca52a2 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:29:20 -0400 Subject: [PATCH 09/28] . --- CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 59917e37..c1891f2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,15 +53,6 @@ find_package(visualization_msgs REQUIRED) find_package(fcl REQUIRED) include(ConfigExtras) -# pkg_check_modules(LIBFCL_PC REQUIRED fcl) -# # find *absolute* paths to LIBFCL_* paths -# find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) -# set(LIBFCL_LIBRARIES) -# foreach(_lib ${LIBFCL_PC_LIBRARIES}) -# find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) -# list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) -# endforeach() - set(THIS_PACKAGE_EXPORT_DEPENDS eigen3_cmake_module Eigen3 From a108625315527c99287d5541a2a0b51d76ca34d8 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 19 Apr 2024 15:30:35 -0400 Subject: [PATCH 10/28] merged changes from 232 --- include/geometric_shapes/body_operations.h | 64 ++++++++------- src/aabb.cpp | 77 +++++++++++------- src/bodies.cpp | 86 +++++++++++++------- src/body_operations.cpp | 66 +++++++++------- src/obb.cpp | 91 +++++----------------- 5 files changed, 198 insertions(+), 186 deletions(-) diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index 5e1cf984..28faa961 100644 --- a/include/geometric_shapes/body_operations.h +++ b/include/geometric_shapes/body_operations.h @@ -1,31 +1,36 @@ -// Copyright 2008 Willow Garage, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// 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. - +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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_ @@ -67,6 +72,9 @@ void mergeBoundingSpheres(const std::vector& spheres, BoundingSp /** \brief Compute an axis-aligned bounding box to enclose a set of bounding boxes. */ void mergeBoundingBoxes(const std::vector& boxes, AABB& mergedBox); +/** \brief Compute an approximate oriented bounding box to enclose a set of bounding boxes. */ +void mergeBoundingBoxesApprox(const std::vector& boxes, OBB& mergedBox); + /** \brief Compute the bounding sphere for a set of \e bodies and store the resulting sphere in \e mergedSphere */ void computeBoundingSphere(const std::vector& bodies, BoundingSphere& mergedSphere); } // namespace bodies diff --git a/src/aabb.cpp b/src/aabb.cpp index c323e5ff..860c5d45 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -1,43 +1,54 @@ -// Copyright 2019 Open Robotics -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Open Robotics nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// 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. - +@@ -1,49 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Open Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Open Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Martin Pecka */ #include +#include +#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 +#include +#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(...) (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. + // We don't call their code because it would need creating temporary objects, and their method is in floats in FCL 0.5 // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ - const Eigen::Matrix3d& r = transform.rotation(); const Eigen::Vector3d& t = transform.translation(); @@ -48,4 +59,10 @@ 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 } diff --git a/src/bodies.cpp b/src/bodies.cpp index 0742001a..6de52c7e 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -1,30 +1,36 @@ -// Copyright 2008 Willow Garage, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// 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. +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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 */ @@ -206,6 +212,15 @@ void bodies::Sphere::computeBoundingBox(bodies::AABB& bbox) const bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_)); } +void bodies::Sphere::computeBoundingBox(bodies::OBB& bbox) const +{ + // it's a sphere, so we do not rotate the bounding box + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + transform.translation() = getPose().translation(); + + bbox.setPoseAndExtents(transform, 2 * Eigen::Vector3d(radiusU_, radiusU_, radiusU_)); +} + bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const { @@ -427,6 +442,11 @@ void bodies::Cylinder::computeBoundingBox(bodies::AABB& bbox) const bbox.extend(pb + e); } +void bodies::Cylinder::computeBoundingBox(bodies::OBB& bbox) const +{ + bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(radiusU_, radiusU_, length2_)); +} + bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { @@ -663,6 +683,11 @@ void bodies::Box::computeBoundingBox(bodies::AABB& bbox) const bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); } +void bodies::Box::computeBoundingBox(bodies::OBB& bbox) const +{ + bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); +} + bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { @@ -1155,6 +1180,11 @@ void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& bbox) const bounding_box_.computeBoundingBox(bbox); } +void bodies::ConvexMesh::computeBoundingBox(bodies::OBB& bbox) const +{ + bounding_box_.computeBoundingBox(bbox); +} + bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const { unsigned int numplanes = mesh_data_->planes_.size(); @@ -1393,4 +1423,4 @@ bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eige return true; } return false; -} +} \ No newline at end of file diff --git a/src/body_operations.cpp b/src/body_operations.cpp index 559259a6..c84881bf 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -1,30 +1,36 @@ -// Copyright 2008 Willow Garage, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// 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. +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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 */ @@ -259,3 +265,9 @@ void bodies::mergeBoundingBoxes(const std::vector& boxes, bodies:: for (const auto& box : boxes) mergedBox.extend(box); } + +void bodies::mergeBoundingBoxesApprox(const std::vector& boxes, bodies::OBB& mergedBox) +{ + for (const auto& box : boxes) + mergedBox.extendApprox(box); +} \ No newline at end of file diff --git a/src/obb.cpp b/src/obb.cpp index 83f081c3..37091d58 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -42,29 +42,16 @@ class OBBPrivate : public FCL_OBB 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 + this->obb_.reset(new OBBPrivate); } -OBB::OBB(const OBB& other) +OBB::OBB(const OBB& other) : OBB() { - obb_.reset(new OBBPrivate(*other.obb_)); + *obb_ = *other.obb_; } -OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) +OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB() { - obb_.reset(new OBBPrivate); setPoseAndExtents(pose, extents); } @@ -107,12 +94,20 @@ void OBB::getPose(Eigen::Isometry3d& pose) const { pose = Eigen::Isometry3d::Identity(); pose.translation() = fromFcl(obb_->To); + // If all axes are zero, we report the rotation as identity + // This happens if OBB is default-constructed #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]); + if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero()) + { + 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.linear() = obb_->axis; + if (!obb_->axis.isApprox(fcl::Matrix3d::Zero())) + { + pose.linear() = obb_->axis; + } #endif } @@ -137,70 +132,20 @@ void OBB::toAABB(AABB& aabb) const OBB* OBB::extendApprox(const OBB& box) { - if (this->getExtents() == Eigen::Vector3d::Zero()) - { - *obb_ = *box.obb_; - return this; - } - - if (this->contains(box)) - return this; - - if (box.contains(*this)) - { - *obb_ = *box.obb_; - return this; - } - *this->obb_ += *box.obb_; return this; } -bool OBB::contains(const Eigen::Vector3d& point) const +bool OBB::contains(const Eigen::Vector3d& point) { return obb_->contain(toFcl(point)); } -bool OBB::overlaps(const bodies::OBB& other) const +bool OBB::overlaps(const bodies::OBB& other) { return obb_->overlap(*other.obb_); } -EigenSTL::vector_Vector3d OBB::computeVertices() const -{ - const Eigen::Vector3d e = getExtents() / 2; // do not use auto type, Eigen would be inefficient - // clang-format off - EigenSTL::vector_Vector3d result = { - { -e[0], -e[1], -e[2] }, - { -e[0], -e[1], e[2] }, - { -e[0], e[1], -e[2] }, - { -e[0], e[1], e[2] }, - { e[0], -e[1], -e[2] }, - { e[0], -e[1], e[2] }, - { e[0], e[1], -e[2] }, - { e[0], e[1], e[2] }, - }; - // clang-format on - - const auto pose = getPose(); - for (auto& v : result) - { - v = pose * v; - } - - return result; -} - -bool OBB::contains(const OBB& obb) const -{ - for (const auto& v : obb.computeVertices()) - { - if (!contains(v)) - return false; - } - return true; -} - OBB::~OBB() = default; } // namespace bodies \ No newline at end of file From 8aa8ca30b52a5f0ced27f21dd82137f6b86da2d9 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:38:35 -0400 Subject: [PATCH 11/28] Builds tests pass (copyright fails though) --- src/aabb.cpp | 1 - src/obb.cpp | 91 +++++++++++++++++++++++++++++++++++++++++----------- 2 files changed, 73 insertions(+), 19 deletions(-) diff --git a/src/aabb.cpp b/src/aabb.cpp index 860c5d45..a11daa01 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -1,4 +1,3 @@ -@@ -1,49 +1,54 @@ /********************************************************************* * Software License Agreement (BSD License) * diff --git a/src/obb.cpp b/src/obb.cpp index 37091d58..83f081c3 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -42,16 +42,29 @@ class OBBPrivate : public FCL_OBB OBB::OBB() { - this->obb_.reset(new OBBPrivate); + 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) : OBB() +OBB::OBB(const OBB& other) { - *obb_ = *other.obb_; + obb_.reset(new OBBPrivate(*other.obb_)); } -OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB() +OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) { + obb_.reset(new OBBPrivate); setPoseAndExtents(pose, extents); } @@ -94,20 +107,12 @@ void OBB::getPose(Eigen::Isometry3d& pose) const { pose = Eigen::Isometry3d::Identity(); pose.translation() = fromFcl(obb_->To); - // If all axes are zero, we report the rotation as identity - // This happens if OBB is default-constructed #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 - if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero()) - { - pose.linear().col(0) = fromFcl(obb_->axis[0]); - pose.linear().col(1) = fromFcl(obb_->axis[1]); - pose.linear().col(2) = fromFcl(obb_->axis[2]); - } + 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 - if (!obb_->axis.isApprox(fcl::Matrix3d::Zero())) - { - pose.linear() = obb_->axis; - } + pose.linear() = obb_->axis; #endif } @@ -132,20 +137,70 @@ void OBB::toAABB(AABB& aabb) const OBB* OBB::extendApprox(const OBB& box) { + if (this->getExtents() == Eigen::Vector3d::Zero()) + { + *obb_ = *box.obb_; + return this; + } + + if (this->contains(box)) + return this; + + if (box.contains(*this)) + { + *obb_ = *box.obb_; + return this; + } + *this->obb_ += *box.obb_; return this; } -bool OBB::contains(const Eigen::Vector3d& point) +bool OBB::contains(const Eigen::Vector3d& point) const { return obb_->contain(toFcl(point)); } -bool OBB::overlaps(const bodies::OBB& other) +bool OBB::overlaps(const bodies::OBB& other) const { return obb_->overlap(*other.obb_); } +EigenSTL::vector_Vector3d OBB::computeVertices() const +{ + const Eigen::Vector3d e = getExtents() / 2; // do not use auto type, Eigen would be inefficient + // clang-format off + EigenSTL::vector_Vector3d result = { + { -e[0], -e[1], -e[2] }, + { -e[0], -e[1], e[2] }, + { -e[0], e[1], -e[2] }, + { -e[0], e[1], e[2] }, + { e[0], -e[1], -e[2] }, + { e[0], -e[1], e[2] }, + { e[0], e[1], -e[2] }, + { e[0], e[1], e[2] }, + }; + // clang-format on + + const auto pose = getPose(); + for (auto& v : result) + { + v = pose * v; + } + + return result; +} + +bool OBB::contains(const OBB& obb) const +{ + for (const auto& v : obb.computeVertices()) + { + if (!contains(v)) + return false; + } + return true; +} + OBB::~OBB() = default; } // namespace bodies \ No newline at end of file From f49fdf68a5821fdffd707cae121de2e828e5abe2 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:41:37 -0400 Subject: [PATCH 12/28] Reverted copyright notice changes --- include/geometric_shapes/obb.h | 60 +++++++++++++++------------------- src/aabb.cpp | 60 +++++++++++++++------------------- src/bodies.cpp | 60 +++++++++++++++------------------- src/body_operations.cpp | 60 +++++++++++++++------------------- 4 files changed, 108 insertions(+), 132 deletions(-) diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h index 79e96e46..7046d275 100644 --- a/include/geometric_shapes/obb.h +++ b/include/geometric_shapes/obb.h @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Open Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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: Martin Pecka */ diff --git a/src/aabb.cpp b/src/aabb.cpp index a11daa01..44072995 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Open Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Open Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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: Martin Pecka */ #include diff --git a/src/bodies.cpp b/src/bodies.cpp index 6de52c7e..7da333ce 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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 */ diff --git a/src/body_operations.cpp b/src/body_operations.cpp index c84881bf..b31a0d6a 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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 */ From fdad9d3dff19da7dcce31e3fd74e238baab5256c Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:43:51 -0400 Subject: [PATCH 13/28] Fixed copyright notices --- include/geometric_shapes/body_operations.h | 60 ++++++++++------------ src/aabb.cpp | 1 + src/bodies.cpp | 6 +-- src/body_operations.cpp | 2 +- 4 files changed, 32 insertions(+), 37 deletions(-) diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index 28faa961..57b9af89 100644 --- a/include/geometric_shapes/body_operations.h +++ b/include/geometric_shapes/body_operations.h @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// Copyright 2019 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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_ diff --git a/src/aabb.cpp b/src/aabb.cpp index 44072995..45c09200 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -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: Martin Pecka */ #include diff --git a/src/bodies.cpp b/src/bodies.cpp index 7da333ce..98a1e78b 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Robotics +// Copyright 2019 2008 Willow Garage, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of the Open Robotics nor the names of its +// * Neither the name of the Willow Garage, Inc. nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // @@ -1417,4 +1417,4 @@ bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eige return true; } return false; -} \ No newline at end of file +} diff --git a/src/body_operations.cpp b/src/body_operations.cpp index b31a0d6a..af6f95f0 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of the Open Robotics nor the names of its +// * Neither the name of the Willow Garage, Inc. nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // From 43939d45bd2e70f5bc3610fc3a8e5f19ede1969f Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:45:09 -0400 Subject: [PATCH 14/28] ^ --- include/geometric_shapes/body_operations.h | 4 ++-- src/bodies.cpp | 2 +- src/body_operations.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index 57b9af89..cc6c8810 100644 --- a/include/geometric_shapes/body_operations.h +++ b/include/geometric_shapes/body_operations.h @@ -1,4 +1,4 @@ -// Copyright 2019 Open Robotics +// Copyright 2008 Willow Garage, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of the Open Robotics nor the names of its +// * Neither the name of the Willow Garage, Inc. nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // diff --git a/src/bodies.cpp b/src/bodies.cpp index 98a1e78b..64d22751 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 2008 Willow Garage, Inc. +// Copyright 2008 Willow Garage, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: diff --git a/src/body_operations.cpp b/src/body_operations.cpp index af6f95f0..62ef0079 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Robotics +// Copyright 2008 Willow Garage, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: From 48bc778baf236195b77c65403fa7170154c83250 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 15:57:25 -0400 Subject: [PATCH 15/28] Ran pre-commit --- include/geometric_shapes/bodies.h | 4 ++-- include/geometric_shapes/obb.h | 2 +- src/body_operations.cpp | 2 +- src/obb.cpp | 2 +- test/test_bounding_box.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index da1263b9..c08a363f 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -241,8 +241,8 @@ class Body pose. Scaling and padding are accounted for. */ virtual void computeBoundingBox(AABB& bbox) const = 0; - /** \brief Compute the oriented bounding box for the body, in its current - pose. Scaling and padding are accounted for. */ + /** \brief Compute the oriented bounding box for the body, in its current + pose. Scaling and padding are accounted for. */ virtual void computeBoundingBox(OBB& bbox) const = 0; /** \brief Get a clone of this body, but one that is located at the pose \e pose */ diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h index 7046d275..070ae0a5 100644 --- a/include/geometric_shapes/obb.h +++ b/include/geometric_shapes/obb.h @@ -139,4 +139,4 @@ class OBB }; } // namespace bodies -#endif // GEOMETRIC_SHAPES_OBB_H \ No newline at end of file +#endif // GEOMETRIC_SHAPES_OBB_H diff --git a/src/body_operations.cpp b/src/body_operations.cpp index 62ef0079..75f00715 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -264,4 +264,4 @@ void bodies::mergeBoundingBoxesApprox(const std::vector& boxes, bod { for (const auto& box : boxes) mergedBox.extendApprox(box); -} \ No newline at end of file +} diff --git a/src/obb.cpp b/src/obb.cpp index 83f081c3..885e48e1 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -203,4 +203,4 @@ bool OBB::contains(const OBB& obb) const OBB::~OBB() = default; -} // namespace bodies \ No newline at end of file +} // namespace bodies diff --git a/test/test_bounding_box.cpp b/test/test_bounding_box.cpp index ba06aa8c..6d85f8fa 100644 --- a/test/test_bounding_box.cpp +++ b/test/test_bounding_box.cpp @@ -656,4 +656,4 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 5f351f413d6a9da80c83ff8386756acfa386c3b2 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 16:15:43 -0400 Subject: [PATCH 16/28] added pcl to package.xml --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index e171e097..4152d9e9 100644 --- a/package.xml +++ b/package.xml @@ -40,10 +40,11 @@ - + libfcl-dev pkg-config libboost-dev libboost-filesystem-dev From e15c6aff587f76ca0e5e4757679746476a594517 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 16:16:55 -0400 Subject: [PATCH 17/28] format --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 4152d9e9..19b226ac 100644 --- a/package.xml +++ b/package.xml @@ -40,7 +40,7 @@ - From 520d5a5cffd71456323216615594898044b7e709 Mon Sep 17 00:00:00 2001 From: Sebastian Pelletier <37712986+spelletier1996@users.noreply.github.com> Date: Mon, 22 Apr 2024 17:24:35 -0400 Subject: [PATCH 18/28] Copyright year Co-authored-by: Martin Pecka --- include/geometric_shapes/obb.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h index 070ae0a5..f7b700c0 100644 --- a/include/geometric_shapes/obb.h +++ b/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: From 126955b1d32ea7c9170e2868844ae5fa35c0ed61 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 13:47:26 -0400 Subject: [PATCH 19/28] reversed white space change --- include/geometric_shapes/body_operations.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index cc6c8810..877c362f 100644 --- a/include/geometric_shapes/body_operations.h +++ b/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_ From 94ea0df99b6ef93c79efcf56e3dccae2f9ed722e Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 17:56:33 +0000 Subject: [PATCH 20/28] removed commented depends --- package.xml | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/package.xml b/package.xml index 19b226ac..0df31ecd 100644 --- a/package.xml +++ b/package.xml @@ -36,14 +36,7 @@ visualization_msgs assimp-dev - - - - - + eigen libfcl-dev pkg-config libboost-dev From 48bba6f1a4fed830a89caeb22d0196b5d0501fb0 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 18:38:23 +0000 Subject: [PATCH 21/28] toFCL fromFCL removed, FCL 0.5 checks removed --- src/aabb.cpp | 9 --------- src/obb.cpp | 52 +++------------------------------------------------- 2 files changed, 3 insertions(+), 58 deletions(-) diff --git a/src/aabb.cpp b/src/aabb.cpp index 45c09200..5c31a62b 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -31,13 +31,10 @@ #include #include -#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 #include -#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(...) (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 } diff --git a/src/obb.cpp b/src/obb.cpp index 885e48e1..3afebcea 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -2,37 +2,12 @@ #include -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -#include -typedef fcl::Vec3f FCL_Vec3; -typedef fcl::OBB FCL_OBB; -#else #include typedef fcl::Vector3d FCL_Vec3; typedef fcl::OBB 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(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(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 From b36a5d7fbe627b85ea15bfb61280177369f074eb Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 7 Jun 2024 21:16:25 +0000 Subject: [PATCH 22/28] addressing header and xml comments --- package.xml | 1 + src/aabb.cpp | 3 --- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/package.xml b/package.xml index 0df31ecd..19ab2672 100644 --- a/package.xml +++ b/package.xml @@ -47,6 +47,7 @@ assimp libboost-filesystem rosidl_default_runtime + libfcl-dev ament_cmake_gtest ament_lint_auto diff --git a/src/aabb.cpp b/src/aabb.cpp index 5c31a62b..a14ccae0 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -30,9 +30,6 @@ #include -#include -#include - void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV(...) (BSD-licensed code): From 6c321ec82c26d29b1371042a53e5ee6b8ad34b33 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 7 Jun 2024 21:23:16 +0000 Subject: [PATCH 23/28] re-added accidental removal of translation line --- src/obb.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/obb.cpp b/src/obb.cpp index 3afebcea..3dfb32f6 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -48,6 +48,8 @@ void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d obb_->axis = rotation; + obb_->To = pose.translation(); + obb_->extent = { extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0 }; } From 1818146ae9f304dc6872a55e2e7e7075eb1d327e Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 7 Jun 2024 21:26:53 +0000 Subject: [PATCH 24/28] correct ifdef --- src/aabb.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/aabb.cpp b/src/aabb.cpp index a14ccae0..2956186f 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -30,6 +30,8 @@ #include +#include + void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV(...) (BSD-licensed code): @@ -44,7 +46,8 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, double y_range = 0.5 * (fabs(r(1, 0) * box[0]) + fabs(r(1, 1) * box[1]) + fabs(r(1, 2) * box[2])); double z_range = 0.5 * (fabs(r(2, 0) * box[0]) + fabs(r(2, 1) * box[1]) + fabs(r(2, 2) * box[2])); - const Eigen::Vector3d v_delta(x_range, y_range, z_range); - extend(t + v_delta); - extend(t - v_delta); + fcl::AABBd aabb; + fcl::computeBV(fcl::Boxd(box), transform, aabb); + extend(aabb.min_); + extend(aabb.max_); } From 5bd19db4778142c3972b8aefaab9e8ce2565db1e Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Mon, 15 Jul 2024 15:04:06 -0400 Subject: [PATCH 25/28] fix redundant depends --- package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/package.xml b/package.xml index 19ab2672..0df31ecd 100644 --- a/package.xml +++ b/package.xml @@ -47,7 +47,6 @@ assimp libboost-filesystem rosidl_default_runtime - libfcl-dev ament_cmake_gtest ament_lint_auto From 998a998ee968fa90c6747c0f83ef42b4cd0c4efe Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Mon, 15 Jul 2024 15:13:19 -0400 Subject: [PATCH 26/28] added copyright notice --- src/obb.cpp | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/src/obb.cpp b/src/obb.cpp index 3dfb32f6..31f57418 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -1,3 +1,37 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Open Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. + *********************************************************************/ + #include #include From 0d04ba5d59873d468a9e5fdae39fdb7c667f39a9 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Mon, 15 Jul 2024 15:19:29 -0400 Subject: [PATCH 27/28] copied the copyright from the obb.h file --- src/obb.cpp | 60 ++++++++++++++++++++++++----------------------------- 1 file changed, 27 insertions(+), 33 deletions(-) diff --git a/src/obb.cpp b/src/obb.cpp index 31f57418..10841194 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -1,36 +1,30 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Open Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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. - *********************************************************************/ +// 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: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// 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. #include From 7f76e3b6c22c58469d53f57189d66b31202069b3 Mon Sep 17 00:00:00 2001 From: Tyler Date: Thu, 18 Jul 2024 14:42:27 -0400 Subject: [PATCH 28/28] remove fcl export --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 85e4973e..aa7505e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,7 +69,6 @@ set(THIS_PACKAGE_EXPORT_DEPENDS resource_retriever shape_msgs visualization_msgs - fcl ) # Set VERSION from package.xml