diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index a2b67392272..21dc4e338ba 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -50,6 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -399,6 +400,10 @@ std::shared_ptr createShapePrimitive(const CollisionShapeConst shape->setMargin(BULLET_MARGIN); break; } + case tesseract_geometry::GeometryType::COMPOUND_MESH: + { + throw std::runtime_error("CompundMesh type should not be passed to this function!"); + } // LCOV_EXCL_START default: { @@ -445,7 +450,8 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter; m_collisionFilterMask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter; - if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity()) + if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity() && + m_shapes[0]->getType() != tesseract_geometry::GeometryType::COMPOUND_MESH) { std::shared_ptr shape = createShapePrimitive(m_shapes[0], this, 0); manage(shape); @@ -461,14 +467,32 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, // effect when negative setCollisionShape(compound.get()); + int shape_id{ 0 }; for (std::size_t j = 0; j < m_shapes.size(); ++j) { - std::shared_ptr subshape = createShapePrimitive(m_shapes[j], this, static_cast(j)); - if (subshape != nullptr) + if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH) { - manage(subshape); - btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); - compound->addChildShape(geomTrans, subshape.get()); + const auto& meshes = std::static_pointer_cast(m_shapes[j])->getMeshes(); + for (const auto& mesh : meshes) + { + std::shared_ptr subshape = createShapePrimitive(mesh, this, shape_id++); + if (subshape != nullptr) + { + manage(subshape); + btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); + compound->addChildShape(geomTrans, subshape.get()); + } + } + } + else + { + std::shared_ptr subshape = createShapePrimitive(m_shapes[j], this, shape_id++); + if (subshape != nullptr) + { + manage(subshape); + btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); + compound->addChildShape(geomTrans, subshape.get()); + } } } } diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index 1daac09ebf7..b2cac88505d 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -50,6 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -190,6 +191,10 @@ CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) { return createShapePrimitive(std::static_pointer_cast(geom)); } + case tesseract_geometry::GeometryType::COMPOUND_MESH: + { + throw std::runtime_error("CompundMesh type should not be passed to this function!"); + } default: { CONSOLE_BRIDGE_logError("This geometric shape type (%d) is not supported using fcl yet", @@ -349,16 +354,37 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, collision_objects_raw_.reserve(shapes_.size()); for (std::size_t i = 0; i < shapes_.size(); ++i) // NOLINT { - CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]); - if (subshape != nullptr) + if (shapes_[i]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH) + { + const auto& meshes = std::static_pointer_cast(shapes_[i])->getMeshes(); + for (const auto& mesh : meshes) + { + CollisionGeometryPtr subshape = createShapePrimitive(mesh); + if (subshape != nullptr) + { + collision_geometries_.push_back(subshape); + auto co = std::make_shared(subshape); + co->setUserData(this); + co->setTransform(shape_poses_[i]); + co->updateAABB(); + collision_objects_.push_back(co); + collision_objects_raw_.push_back(co.get()); + } + } + } + else { - collision_geometries_.push_back(subshape); - auto co = std::make_shared(subshape); - co->setUserData(this); - co->setTransform(shape_poses_[i]); - co->updateAABB(); - collision_objects_.push_back(co); - collision_objects_raw_.push_back(co.get()); + CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]); + if (subshape != nullptr) + { + collision_geometries_.push_back(subshape); + auto co = std::make_shared(subshape); + co->setUserData(this); + co->setTransform(shape_poses_[i]); + co->updateAABB(); + collision_objects_.push_back(co); + collision_objects_raw_.push_back(co.get()); + } } } } diff --git a/tesseract_collision/package.xml b/tesseract_collision/package.xml index 34729f051e2..3520eda0f38 100644 --- a/tesseract_collision/package.xml +++ b/tesseract_collision/package.xml @@ -35,6 +35,7 @@ benchmark gtest tesseract_scene_graph + tesseract_support cmake diff --git a/tesseract_collision/test/CMakeLists.txt b/tesseract_collision/test/CMakeLists.txt index 2198304736e..14ffced0fc3 100644 --- a/tesseract_collision/test/CMakeLists.txt +++ b/tesseract_collision/test/CMakeLists.txt @@ -65,6 +65,7 @@ add_gtest(${PROJECT_NAME}_octomap_mesh_unit collision_octomap_mesh_unit.cpp) add_gtest(${PROJECT_NAME}_clone_unit collision_clone_unit.cpp) add_gtest(${PROJECT_NAME}_box_box_cast_unit collision_box_box_cast_unit.cpp) add_gtest(${PROJECT_NAME}_compound_compound_unit collision_compound_compound_unit.cpp) +add_gtest(${PROJECT_NAME}_compound_mesh_sphere_unit collision_compound_mesh_sphere_unit.cpp) add_gtest(${PROJECT_NAME}_sphere_sphere_cast_unit collision_sphere_sphere_cast_unit.cpp) add_gtest(${PROJECT_NAME}_octomap_octomap_unit collision_octomap_octomap_unit.cpp) add_gtest(${PROJECT_NAME}_collision_margin_data_unit collision_margin_data_unit.cpp) diff --git a/tesseract_collision/test/collision_compound_mesh_sphere_unit.cpp b/tesseract_collision/test/collision_compound_mesh_sphere_unit.cpp new file mode 100644 index 00000000000..beb888ff005 --- /dev/null +++ b/tesseract_collision/test/collision_compound_mesh_sphere_unit.cpp @@ -0,0 +1,36 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +using namespace tesseract_collision; + +TEST(TesseractCollisionUnit, BulletDiscreteSimpleCollisionCompoundMeshSphereUnit) // NOLINT +{ + tesseract_collision_bullet::BulletDiscreteSimpleManager checker; + test_suite::runTest(checker); +} + +TEST(TesseractCollisionUnit, BulletDiscreteBVHCollisionCompoundMeshSphereUnit) // NOLINT +{ + tesseract_collision_bullet::BulletDiscreteBVHManager checker; + test_suite::runTest(checker); +} + +TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionCompoundMeshSphereUnit) // NOLINT +{ + tesseract_collision_fcl::FCLDiscreteBVHManager checker; + test_suite::runTest(checker); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp new file mode 100644 index 00000000000..bd562845084 --- /dev/null +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp @@ -0,0 +1,288 @@ +#ifndef TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP +#define TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP + +#include +#include +#include +#include +#include +#include + +namespace tesseract_collision::test_suite +{ +namespace detail +{ +inline void addCollisionObjects(DiscreteContactManager& checker) +{ + ////////////////////// + // Add box to checker + ////////////////////// + auto resource_locator = std::make_shared(); + auto compound_mesh_resource = resource_locator->locateResource("package://tesseract_support/meshes/box_box.dae"); + auto meshes = + tesseract_geometry::createMeshFromPath(compound_mesh_resource->getFilePath()); + CollisionShapePtr box = std::make_shared(meshes); + Eigen::Isometry3d box_pose; + box_pose.setIdentity(); + + CollisionShapesConst obj1_shapes; + tesseract_common::VectorIsometry3d obj1_poses; + obj1_shapes.push_back(box); + obj1_poses.push_back(box_pose); + + checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false); + checker.enableCollisionObject("box_link"); + + ///////////////////////////////////////////// + // Add thin box to checker which is disabled + ///////////////////////////////////////////// + CollisionShapePtr thin_box = std::make_shared(0.1, 1, 1); + Eigen::Isometry3d thin_box_pose; + thin_box_pose.setIdentity(); + + CollisionShapesConst obj2_shapes; + tesseract_common::VectorIsometry3d obj2_poses; + obj2_shapes.push_back(thin_box); + obj2_poses.push_back(thin_box_pose); + + checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses); + checker.disableCollisionObject("thin_box_link"); + + ///////////////////////////////////////////////////////////////// + // Add sphere to checker. If use_convex_mesh = true then this + // sphere will be added as a convex hull mesh. + ///////////////////////////////////////////////////////////////// + CollisionShapePtr sphere = std::make_shared(0.25); + + Eigen::Isometry3d sphere_pose; + sphere_pose.setIdentity(); + + CollisionShapesConst obj3_shapes; + tesseract_common::VectorIsometry3d obj3_poses; + obj3_shapes.push_back(sphere); + obj3_poses.push_back(sphere_pose); + + checker.addCollisionObject("sphere_link", 0, obj3_shapes, obj3_poses); + + ///////////////////////////////////////////// + // Add box and remove + ///////////////////////////////////////////// + CollisionShapePtr remove_box = std::make_shared(0.1, 1, 1); + Eigen::Isometry3d remove_box_pose; + thin_box_pose.setIdentity(); + + CollisionShapesConst obj4_shapes; + tesseract_common::VectorIsometry3d obj4_poses; + obj4_shapes.push_back(remove_box); + obj4_poses.push_back(remove_box_pose); + + checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses); + EXPECT_TRUE(checker.getCollisionObjects().size() == 4); + EXPECT_TRUE(checker.hasCollisionObject("remove_box_link")); + checker.removeCollisionObject("remove_box_link"); + EXPECT_FALSE(checker.hasCollisionObject("remove_box_link")); + + ///////////////////////////////////////////// + // Try functions on a link that does not exist + ///////////////////////////////////////////// + EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist")); + EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist")); + EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist")); + + ///////////////////////////////////////////// + // Try to add empty Collision Object + ///////////////////////////////////////////// + EXPECT_FALSE( + checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d())); + + ///////////////////////////////////////////// + // Check sizes + ///////////////////////////////////////////// + EXPECT_TRUE(checker.getCollisionObjects().size() == 3); + for (const auto& co : checker.getCollisionObjects()) + { + EXPECT_TRUE(checker.getCollisionObjectGeometries(co).size() == 1); + EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co).size() == 1); + for (const auto& cgt : checker.getCollisionObjectGeometriesTransforms(co)) + { + EXPECT_TRUE(cgt.isApprox(Eigen::Isometry3d::Identity(), 1e-5)); + } + } +} + +} // namespace detail + +inline void runTest(DiscreteContactManager& checker) +{ + // Add collision objects + detail::addCollisionObjects(checker); + + // Call it again to test adding same object + detail::addCollisionObjects(checker); + + ////////////////////////////////////// + // Test when object is in collision + ////////////////////////////////////// + std::vector active_links{ "box_link", "sphere_link" }; + checker.setActiveCollisionObjects(active_links); + std::vector check_active_links = checker.getActiveCollisionObjects(); + EXPECT_TRUE(tesseract_common::isIdentical(active_links, check_active_links, false)); + + EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr); + + checker.setDefaultCollisionMarginData(0.1); + EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); + + // Set the collision object transforms + tesseract_common::TransformMap location; + location["box_link"] = Eigen::Isometry3d::Identity(); + location["sphere_link"] = Eigen::Isometry3d::Identity(); + location["sphere_link"].translation()(0) = 0.2; + checker.setCollisionObjectsTransform(location); + + // Perform collision check + ContactResultMap result; + checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); + + ContactResultVector result_vector; + result.flattenMoveResults(result_vector); + + EXPECT_TRUE(!result_vector.empty()); + EXPECT_NEAR(result_vector[0].distance, -0.19053635, 0.001); + + std::vector idx = { 0, 1, 1 }; + if (result_vector[0].link_names[0] != "box_link") + idx = { 1, 0, -1 }; + + if (result_vector[0].single_contact_point) + { + EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001); + EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 && + std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001); + } + else + { + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][0], 0.2, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][1], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][2], 0.0594636, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][0], 0.2, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][1], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][2], 0.25, 0.001); + } + + EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.0, 0.001); + EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001); + EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -1.0, 0.001); + + // Compound mesh so check shape id + EXPECT_EQ(result_vector[0].shape_id[static_cast(idx[0])], 1); + + //////////////////////////////////////////////// + // Test object is out side the contact distance + //////////////////////////////////////////////// + location["sphere_link"].translation() = Eigen::Vector3d(0, 0, -0.3); + result.clear(); + result_vector.clear(); + + // Use different method for setting transforms + checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]); + checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); + result.flattenCopyResults(result_vector); + + EXPECT_TRUE(result_vector.empty()); + + ///////////////////////////////////////////// + // Test object inside the contact distance + ///////////////////////////////////////////// + result.clear(); + result_vector.clear(); + + checker.setCollisionMarginData(CollisionMarginData(0.251)); + EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5); + checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); + result.flattenMoveResults(result_vector); + + EXPECT_TRUE(!result_vector.empty()); + EXPECT_NEAR(result_vector[0].distance, 0.1094636, 0.001); + + idx = { 0, 1, 1 }; + if (result_vector[0].link_names[0] != "box_link") + idx = { 1, 0, -1 }; + + if (result_vector[0].single_contact_point) + { + EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001); + EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 && + std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001); + } + else + { + // Increased tolernace because of FCL from 0.001 to 0.002 + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][0], 0.0, 0.002); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][1], 0.0, 0.002); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][2], 0.0594636, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][0], 0.0, 0.002); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][1], 0.0, 0.002); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][2], -0.05, 0.001); + } + + EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.0, 0.001); + EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001); + EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -1.0, 0.001); + + // Compound mesh so check shape id + EXPECT_EQ(result_vector[0].shape_id[static_cast(idx[0])], 1); + + ///////////////////////////////////////////// + // Test collision against second shape + ///////////////////////////////////////////// + result.clear(); + result_vector.clear(); + location["sphere_link"].translation() = Eigen::Vector3d(0, 2.75, 0); + + checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]); + checker.setCollisionMarginData(CollisionMarginData(0.1)); + EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); + checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); + result.flattenMoveResults(result_vector); + + EXPECT_TRUE(!result_vector.empty()); + EXPECT_NEAR(result_vector[0].distance, 0.03130736, 0.001); + + idx = { 0, 1, 1 }; + if (result_vector[0].link_names[0] != "box_link") + idx = { 1, 0, -1 }; + + if (result_vector[0].single_contact_point) + { + EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001); + EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 && + std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001); + } + else + { + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][0], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][1], 2.4702682, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[0])][2], 0.0297318, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][0], 0.0, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][1], 2.5014002, 0.001); + EXPECT_NEAR(result_vector[0].nearest_points[static_cast(idx[1])][2], 0.0264228, 0.001); + } + + EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.0, 0.001); + EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.9943989, 0.001); + EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -0.1056915, 0.001); + + // Compound mesh so check shape id + EXPECT_EQ(result_vector[0].shape_id[static_cast(idx[0])], 0); +} + +} // namespace tesseract_collision::test_suite + +#endif // TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP diff --git a/tesseract_geometry/CMakeLists.txt b/tesseract_geometry/CMakeLists.txt index d6bb7cbb44e..12b02703661 100644 --- a/tesseract_geometry/CMakeLists.txt +++ b/tesseract_geometry/CMakeLists.txt @@ -67,6 +67,7 @@ add_library( src/utils.cpp src/geometries/box.cpp src/geometries/capsule.cpp + src/geometries/compound_mesh.cpp src/geometries/cone.cpp src/geometries/convex_mesh.cpp src/geometries/cylinder.cpp diff --git a/tesseract_geometry/include/tesseract_geometry/geometries.h b/tesseract_geometry/include/tesseract_geometry/geometries.h index 6c20dd63592..cddaa1d64c4 100644 --- a/tesseract_geometry/include/tesseract_geometry/geometries.h +++ b/tesseract_geometry/include/tesseract_geometry/geometries.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include diff --git a/tesseract_geometry/include/tesseract_geometry/geometry.h b/tesseract_geometry/include/tesseract_geometry/geometry.h index ed988ee547c..0e42fb529f0 100644 --- a/tesseract_geometry/include/tesseract_geometry/geometry.h +++ b/tesseract_geometry/include/tesseract_geometry/geometry.h @@ -54,11 +54,13 @@ enum class GeometryType CONVEX_MESH, SDF_MESH, OCTREE, - POLYGON_MESH + POLYGON_MESH, + COMPOUND_MESH }; static const std::vector GeometryTypeStrings = { "UNINITIALIZED", "SPHERE", "CYLINDER", "CAPSULE", "CONE", "BOX", "PLANE", "MESH", - "CONVEX_MESH", "SDF_MESH", "OCTREE", "POLYGON_MESH" }; + "CONVEX_MESH", "SDF_MESH", "OCTREE", "POLYGON_MESH", + "COMPOUND_MESH" }; class Geometry { diff --git a/tesseract_geometry/include/tesseract_geometry/impl/compound_mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/compound_mesh.h new file mode 100644 index 00000000000..771740a87cf --- /dev/null +++ b/tesseract_geometry/include/tesseract_geometry/impl/compound_mesh.h @@ -0,0 +1,99 @@ +/** + * @file compound_mesh.h + * @brief Tesseract Compound Mesh Geometry + * + * @author Levi Armstrong + * @date September 14, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_GEOMETRY_COMPOUND_MESH_H +#define TESSERACT_GEOMETRY_COMPOUND_MESH_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace boost::serialization +{ +class access; +} + +namespace tesseract_geometry +{ +class ConvexMesh; +class Mesh; +class SDFMesh; + +/** @brief This is store meshes that are associated with as single resource */ +class CompoundMesh : public Geometry +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CompoundMesh() = default; + CompoundMesh(std::vector> meshes); + CompoundMesh(std::vector> meshes); + CompoundMesh(std::vector> meshes); + CompoundMesh(std::vector> meshes); + + /** + * @brief Get the meshes + * @return The meshes + */ + const std::vector>& getMeshes() const; + + /** + * @brief Get the path to file used to generate the meshs + * + * Note: If empty, assume it was manually generated. + * + * @return Absolute path to the mesh file + */ + std::shared_ptr getResource() const; + + /** + * @brief Get the scale applied to file used to generate the meshs + * @return The scale x, y, z + */ + const Eigen::Vector3d& getScale() const; + + Geometry::Ptr clone() const override final; + + bool operator==(const CompoundMesh& rhs) const; + bool operator!=(const CompoundMesh& rhs) const; + +private: + std::vector> meshes_; + + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_geometry + +BOOST_CLASS_EXPORT_KEY(tesseract_geometry::CompoundMesh) + +#endif // TESSERACT_GEOMETRY_COMPOUND_MESH_H diff --git a/tesseract_geometry/src/geometries/compound_mesh.cpp b/tesseract_geometry/src/geometries/compound_mesh.cpp new file mode 100644 index 00000000000..6996a4c0413 --- /dev/null +++ b/tesseract_geometry/src/geometries/compound_mesh.cpp @@ -0,0 +1,119 @@ +/** + * @file compund_mesh.cpp + * @brief Tesseract Compound Mesh Geometry + * + * @author Levi Armstrong + * @date March 16, 2022 + * @version TODO + * @bug No known bugs + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include + +template +std::vector> convert(std::vector>& meshes) +{ + std::vector> polygon_meshes; + polygon_meshes.reserve(meshes.size()); + for (const auto& mesh : meshes) + polygon_meshes.push_back(mesh); + + return polygon_meshes; +} + +namespace tesseract_geometry +{ +CompoundMesh::CompoundMesh(std::vector> meshes) + : Geometry(GeometryType::COMPOUND_MESH), meshes_(std::move(meshes)) +{ + if (meshes_.size() <= 1) + throw std::runtime_error("Meshes must contain more than one mesh"); + +#ifndef NDEBUG + for (const auto& mesh : meshes_) + { + assert(tesseract_common::pointersEqual(meshes_[0]->getResource(), + mesh->getResource())); + assert(tesseract_common::almostEqualRelativeAndAbs(meshes_[0]->getScale(), mesh->getScale())); + } +#endif +} + +CompoundMesh::CompoundMesh(std::vector> meshes) : CompoundMesh(convert(meshes)) {} + +CompoundMesh::CompoundMesh(std::vector> meshes) : CompoundMesh(convert(meshes)) {} + +CompoundMesh::CompoundMesh(std::vector> meshes) : CompoundMesh(convert(meshes)) {} + +const std::vector>& CompoundMesh::getMeshes() const { return meshes_; } + +std::shared_ptr CompoundMesh::getResource() const +{ + return meshes_.front()->getResource(); +} + +const Eigen::Vector3d& CompoundMesh::getScale() const { return meshes_.front()->getScale(); } + +Geometry::Ptr CompoundMesh::clone() const +{ + std::vector> meshes; + meshes.reserve(meshes_.size()); + for (const auto& mesh : meshes_) + meshes.emplace_back(std::dynamic_pointer_cast(mesh->clone())); + + return std::make_shared(meshes); +} + +bool CompoundMesh::operator==(const CompoundMesh& rhs) const +{ + if (meshes_.size() != rhs.meshes_.size()) + return false; + + bool equal = true; + equal &= Geometry::operator==(rhs); + for (std::size_t i = 0; i < meshes_.size(); ++i) + equal &= (*meshes_.at(i) == *rhs.meshes_.at(i)); + + return equal; +} +bool CompoundMesh::operator!=(const CompoundMesh& rhs) const { return !operator==(rhs); } + +template +void CompoundMesh::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Geometry); + ar& BOOST_SERIALIZATION_NVP(meshes_); +} +} // namespace tesseract_geometry + +#include +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_geometry::CompoundMesh) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_geometry::CompoundMesh) diff --git a/tesseract_geometry/src/utils.cpp b/tesseract_geometry/src/utils.cpp index fd6087a0298..5ecfe448b25 100644 --- a/tesseract_geometry/src/utils.cpp +++ b/tesseract_geometry/src/utils.cpp @@ -137,6 +137,12 @@ bool isIdentical(const Geometry& geom1, const Geometry& geom2) if (s1.getFaceCount() != s2.getFaceCount()) return false; + if (s1.getFaces() != s2.getFaces()) + return false; + + if (s1.getVertices() != s2.getVertices()) + return false; + break; } case GeometryType::CONVEX_MESH: @@ -150,6 +156,12 @@ bool isIdentical(const Geometry& geom1, const Geometry& geom2) if (s1.getFaceCount() != s2.getFaceCount()) return false; + if (s1.getFaces() != s2.getFaces()) + return false; + + if (s1.getVertices() != s2.getVertices()) + return false; + break; } case GeometryType::SDF_MESH: @@ -163,6 +175,12 @@ bool isIdentical(const Geometry& geom1, const Geometry& geom2) if (s1.getFaceCount() != s2.getFaceCount()) return false; + if (s1.getFaces() != s2.getFaces()) + return false; + + if (s1.getVertices() != s2.getVertices()) + return false; + break; } case GeometryType::OCTREE: @@ -201,6 +219,28 @@ bool isIdentical(const Geometry& geom1, const Geometry& geom2) if (s1.getFaceCount() != s2.getFaceCount()) return false; + if (s1.getFaces() != s2.getFaces()) + return false; + + if (s1.getVertices() != s2.getVertices()) + return false; + + break; + } + case GeometryType::COMPOUND_MESH: + { + const auto& s1 = static_cast(geom1); + const auto& s2 = static_cast(geom2); + + if (s1.getMeshes().size() != s2.getMeshes().size()) + return false; + + for (std::size_t i = 0; i < s1.getMeshes().size(); ++i) + { + if (!isIdentical(*s1.getMeshes()[i], *s2.getMeshes()[i])) + return false; + } + break; } default: diff --git a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp index dc61c93ccad..75f00529c7a 100644 --- a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp @@ -71,6 +71,22 @@ TEST(TesseractGeometrySerializeUnit, ConvexMesh) // NOLINT tesseract_common::testSerializationDerivedClass(object.front(), "ConvexMesh"); } +TEST(TesseractGeometrySerializeUnit, CompoundConvexMesh) // NOLINT +{ + std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; + tesseract_common::TesseractSupportResourceLocator locator; + auto object = tesseract_geometry::createMeshFromResource( + locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); + std::vector meshes; + meshes.push_back(object.front()); + meshes.push_back(object.front()); + meshes.push_back(object.front()); + + auto compound_object = std::make_shared(meshes); + tesseract_common::testSerialization(*compound_object, "CompundConvexMesh"); + tesseract_common::testSerializationDerivedClass(compound_object, "CompundConvexMesh"); +} + TEST(TesseractGeometrySerializeUnit, Cylinder) // NOLINT { auto object = std::make_shared(3.3, 4.4); @@ -89,6 +105,22 @@ TEST(TesseractGeometrySerializeUnit, Mesh) // NOLINT tesseract_common::testSerializationDerivedClass(object.front(), "Mesh"); } +TEST(TesseractGeometrySerializeUnit, CompoundMesh) // NOLINT +{ + std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.stl"; + tesseract_common::TesseractSupportResourceLocator locator; + auto object = tesseract_geometry::createMeshFromResource( + locator.locateResource(path), Eigen::Vector3d(.1, .2, .3), true, true, true, true, true); + std::vector meshes; + meshes.push_back(object.front()); + meshes.push_back(object.front()); + meshes.push_back(object.front()); + + auto compound_object = std::make_shared(meshes); + tesseract_common::testSerialization(*compound_object, "CompoundMesh"); + tesseract_common::testSerializationDerivedClass(compound_object, "CompoundMesh"); +} + TEST(TesseractGeometrySerializeUnit, Octree) // NOLINT { struct TestPointCloud diff --git a/tesseract_geometry/test/tesseract_geometry_unit.cpp b/tesseract_geometry/test/tesseract_geometry_unit.cpp index e78d54212a0..e3a7a348954 100644 --- a/tesseract_geometry/test/tesseract_geometry_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_unit.cpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include TEST(TesseractGeometryUnit, Instantiation) // NOLINT { @@ -26,6 +27,10 @@ TEST(TesseractGeometryUnit, Instantiation) // NOLINT auto mesh = std::make_shared(vertices, faces); auto sdf_mesh = std::make_shared(vertices, faces); auto octree = std::make_shared(nullptr, tesseract_geometry::OctreeSubType::BOX); + auto compound_mesh = + std::make_shared(std::vector>{ + std::make_shared(vertices, faces), + std::make_shared(vertices, faces) }); // Instead making this depend on pcl it expects the structure to have a member called points which is a vector // of another object with has float members x, y and z. @@ -246,6 +251,53 @@ TEST(TesseractGeometryUnit, ConvexMesh) // NOLINT std::make_shared()))); } +TEST(TesseractGeometryUnit, CompoundConvexMesh) // NOLINT +{ + auto vertices = std::make_shared(); + vertices->emplace_back(1, 1, 0); + vertices->emplace_back(1, -1, 0); + vertices->emplace_back(-1, -1, 0); + vertices->emplace_back(1, -1, 0); + + auto faces = std::make_shared(); + faces->resize(5); + (*faces)(0) = 4; + (*faces)(1) = 0; + (*faces)(2) = 1; + (*faces)(3) = 2; + (*faces)(4) = 3; + + using T = tesseract_geometry::ConvexMesh; + auto sub_geom = std::make_shared(vertices, faces); + EXPECT_TRUE(sub_geom->getVertices() != nullptr); + EXPECT_TRUE(sub_geom->getFaces() != nullptr); + EXPECT_TRUE(sub_geom->getVertexCount() == 4); + EXPECT_TRUE(sub_geom->getFaceCount() == 1); + EXPECT_EQ(sub_geom->getType(), tesseract_geometry::GeometryType::CONVEX_MESH); + + std::vector meshes; + meshes.push_back(sub_geom); + meshes.push_back(sub_geom); + meshes.push_back(sub_geom); + + auto geom = std::make_shared(meshes); + EXPECT_EQ(geom->getMeshes().size(), 3); + EXPECT_EQ(geom->getResource(), geom->getMeshes().front()->getResource()); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(geom->getScale(), geom->getMeshes().front()->getScale())); + EXPECT_EQ(geom->getType(), tesseract_geometry::GeometryType::COMPOUND_MESH); + + auto geom_clone = std::static_pointer_cast(geom->clone()); + EXPECT_EQ(geom_clone->getMeshes().size(), 3); + EXPECT_EQ(geom_clone->getResource(), geom->getMeshes().front()->getResource()); + EXPECT_TRUE( + tesseract_common::almostEqualRelativeAndAbs(geom_clone->getScale(), geom->getMeshes().front()->getScale())); + EXPECT_EQ(geom_clone->getType(), tesseract_geometry::GeometryType::COMPOUND_MESH); + + // Test isIdentical + EXPECT_TRUE(tesseract_geometry::isIdentical(*geom, *geom_clone)); + EXPECT_FALSE(tesseract_geometry::isIdentical(*geom, tesseract_geometry::CompoundMesh())); +} + TEST(TesseractGeometryUnit, Mesh) // NOLINT { auto vertices = std::make_shared(); @@ -320,6 +372,59 @@ TEST(TesseractGeometryUnit, Mesh) // NOLINT } } +TEST(TesseractGeometryUnit, CompoundMesh) // NOLINT +{ + auto vertices = std::make_shared(); + vertices->emplace_back(1, 1, 0); + vertices->emplace_back(1, -1, 0); + vertices->emplace_back(-1, -1, 0); + vertices->emplace_back(1, -1, 0); + + auto faces = std::make_shared(); + faces->resize(8); + (*faces)(0) = 3; + (*faces)(1) = 0; + (*faces)(2) = 1; + (*faces)(3) = 2; + + (*faces)(4) = 3; + (*faces)(5) = 0; + (*faces)(6) = 2; + (*faces)(7) = 3; + + using T = tesseract_geometry::Mesh; + + auto sub_geom = std::make_shared(vertices, faces); + EXPECT_TRUE(sub_geom->getVertices() != nullptr); + EXPECT_TRUE(sub_geom->getFaces() != nullptr); + EXPECT_TRUE(sub_geom->getVertexCount() == 4); + EXPECT_TRUE(sub_geom->getFaceCount() == 2); + EXPECT_TRUE(sub_geom->getMaterial() == nullptr); + EXPECT_EQ(sub_geom->getType(), tesseract_geometry::GeometryType::MESH); + + std::vector meshes; + meshes.push_back(sub_geom); + meshes.push_back(sub_geom); + meshes.push_back(sub_geom); + + auto geom = std::make_shared(meshes); + EXPECT_EQ(geom->getMeshes().size(), 3); + EXPECT_EQ(geom->getResource(), geom->getMeshes().front()->getResource()); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(geom->getScale(), geom->getMeshes().front()->getScale())); + EXPECT_EQ(geom->getType(), tesseract_geometry::GeometryType::COMPOUND_MESH); + + auto geom_clone = std::static_pointer_cast(geom->clone()); + EXPECT_EQ(geom_clone->getMeshes().size(), 3); + EXPECT_EQ(geom_clone->getResource(), geom->getMeshes().front()->getResource()); + EXPECT_TRUE( + tesseract_common::almostEqualRelativeAndAbs(geom_clone->getScale(), geom->getMeshes().front()->getScale())); + EXPECT_EQ(geom_clone->getType(), tesseract_geometry::GeometryType::COMPOUND_MESH); + + // Test isIdentical + EXPECT_TRUE(tesseract_geometry::isIdentical(*geom, *geom_clone)); + EXPECT_FALSE(tesseract_geometry::isIdentical(*geom, tesseract_geometry::CompoundMesh())); +} + TEST(TesseractGeometryUnit, SDFMesh) // NOLINT { auto vertices = std::make_shared(); diff --git a/tesseract_urdf/include/tesseract_urdf/collision.h b/tesseract_urdf/include/tesseract_urdf/collision.h index 6d5a06431c2..66d2888e6ea 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -48,10 +48,11 @@ namespace tesseract_urdf * @param xml_element The xml element * @param locator The Tesseract resource locator * @param version The version number - * @return A vector tesseract_scene_graph Collision objects + * @return A Collision object */ -std::vector> -parseCollision(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, int version); +std::shared_ptr parseCollision(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + int version); /** * @brief writeCollision Write collision object to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/geometry.h b/tesseract_urdf/include/tesseract_urdf/geometry.h index daf91b7584e..1cedd772602 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -51,11 +51,10 @@ namespace tesseract_urdf * @param version The version number * @return A Tesseract Geometry */ -std::vector> -parseGeometry(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version); +std::shared_ptr parseGeometry(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual, + int version); /** * @brief writeGeometry Write geometry to URDF XML diff --git a/tesseract_urdf/include/tesseract_urdf/visual.h b/tesseract_urdf/include/tesseract_urdf/visual.h index bca2a188a41..d599d2d2ccf 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -49,9 +49,9 @@ namespace tesseract_urdf * @param xml_element The xml element * @param locator The Tesseract resource locator * @param version The version number - * @return A vector tesseract_scene_graph Visual objects + * @return A Visual object */ -std::vector> +std::shared_ptr parseVisual(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, std::unordered_map>& available_materials, diff --git a/tesseract_urdf/src/collision.cpp b/tesseract_urdf/src/collision.cpp index 7232b8ac95c..2cca47d0690 100644 --- a/tesseract_urdf/src/collision.cpp +++ b/tesseract_urdf/src/collision.cpp @@ -40,13 +40,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -std::vector -tesseract_urdf::parseCollision(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - int version) +tesseract_scene_graph::Collision::Ptr tesseract_urdf::parseCollision(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + int version) { - std::vector collisions; - // get name std::string collision_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -70,43 +67,22 @@ tesseract_urdf::parseCollision(const tinyxml2::XMLElement* xml_element, if (geometry == nullptr) std::throw_with_nested(std::runtime_error("Collision: Error missing 'geometry' element!")); - std::vector geometries; + tesseract_geometry::Geometry::Ptr geom; try { - geometries = parseGeometry(geometry, locator, false, version); + geom = parseGeometry(geometry, locator, false, version); } catch (...) { std::throw_with_nested(std::runtime_error("Collision: Error parsing 'geometry' element!")); } - if (geometries.size() == 1) - { - auto collision = std::make_shared(); - collision->name = collision_name; - collision->origin = collision_origin; - collision->geometry = geometries[0]; - collisions.push_back(collision); - } - else - { - int i = 0; - for (const auto& g : geometries) - { - auto collision = std::make_shared(); - - if (collision_name.empty()) - collision->name = collision_name; - else - collision->name = collision_name + "_" + std::to_string(i); - - collision->origin = collision_origin; - collision->geometry = g; - collisions.push_back(collision); - } - } + auto collision = std::make_shared(); + collision->name = collision_name; + collision->origin = collision_origin; + collision->geometry = geom; - return collisions; + return collision; } tinyxml2::XMLElement* diff --git a/tesseract_urdf/src/geometry.cpp b/tesseract_urdf/src/geometry.cpp index ad49fafb4f6..7a82b5b9365 100644 --- a/tesseract_urdf/src/geometry.cpp +++ b/tesseract_urdf/src/geometry.cpp @@ -47,14 +47,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -std::vector -tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, - const tesseract_common::ResourceLocator& locator, - bool visual, - int version) +tesseract_geometry::Geometry::Ptr tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, + const tesseract_common::ResourceLocator& locator, + bool visual, + int version) { - std::vector geometries; - const tinyxml2::XMLElement* geometry = xml_element->FirstChildElement(); if (geometry == nullptr) std::throw_with_nested(std::runtime_error("Geometry: Error missing 'geometry' element!")); @@ -76,9 +73,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'sphere'!")); } - geometries = { sphere }; + return sphere; } - else if (geometry_type == "box") + + if (geometry_type == "box") { tesseract_geometry::Box::Ptr box; try @@ -90,9 +88,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'box'!")); } - geometries = { box }; + return box; } - else if (geometry_type == "cylinder") + + if (geometry_type == "cylinder") { tesseract_geometry::Cylinder::Ptr cylinder; try @@ -104,9 +103,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'cylinder'!")); } - geometries = { cylinder }; + return cylinder; } - else if (geometry_type == "cone") + + if (geometry_type == "cone") { tesseract_geometry::Cone::Ptr cone; try @@ -118,9 +118,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'cone'!")); } - geometries = { cone }; + return cone; } - else if (geometry_type == "capsule") + + if (geometry_type == "capsule") { tesseract_geometry::Capsule::Ptr capsule; try @@ -132,9 +133,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'capsule'!")); } - geometries = { capsule }; + return capsule; } - else if (geometry_type == "octomap") + + if (geometry_type == "octomap") { tesseract_geometry::Octree::Ptr octree; try @@ -146,9 +148,10 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'octomap'!")); } - geometries = { octree }; + return octree; } - else if (geometry_type == "mesh") + + if (geometry_type == "mesh") { std::vector meshes; try @@ -160,17 +163,31 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'mesh'!")); } - if (version < 2 && !visual) + if (meshes.size() > 1) { - for (const auto& mesh : meshes) - geometries.push_back(tesseract_collision::makeConvexMesh(*mesh)); - } - else - { - geometries = std::vector(meshes.begin(), meshes.end()); + std::vector> poly_meshes; + poly_meshes.reserve(meshes.size()); + + if (version < 2 && !visual) + { + for (const auto& mesh : meshes) + poly_meshes.push_back(tesseract_collision::makeConvexMesh(*mesh)); + } + else + { + for (const auto& mesh : meshes) + poly_meshes.push_back(mesh); + } + return std::make_shared(poly_meshes); } + + if (version < 2 && !visual) + return tesseract_collision::makeConvexMesh(*meshes.front()); + + return meshes.front(); } - else if (geometry_type == "convex_mesh") + + if (geometry_type == "convex_mesh") { std::vector meshes; try @@ -182,9 +199,13 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'convex_mesh'!")); } - geometries = std::vector(meshes.begin(), meshes.end()); + if (meshes.size() > 1) + return std::make_shared(meshes); + + return meshes.front(); } - else if (geometry_type == "sdf_mesh") + + if (geometry_type == "sdf_mesh") { std::vector meshes; try @@ -196,14 +217,13 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Failed parsing geometry type 'sdf_mesh'!")); } - geometries = std::vector(meshes.begin(), meshes.end()); - } - else - { - std::throw_with_nested(std::runtime_error("Geometry: Invalid geometry type '" + geometry_type + "'!")); + if (meshes.size() > 1) + return std::make_shared(meshes); + + return meshes.front(); } - return geometries; + std::throw_with_nested(std::runtime_error("Geometry: Invalid geometry type '" + geometry_type + "'!")); } tinyxml2::XMLElement* tesseract_urdf::writeGeometry(const std::shared_ptr& geometry, diff --git a/tesseract_urdf/src/link.cpp b/tesseract_urdf/src/link.cpp index a151d100a7a..0241e9d3639 100644 --- a/tesseract_urdf/src/link.cpp +++ b/tesseract_urdf/src/link.cpp @@ -72,7 +72,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, for (const tinyxml2::XMLElement* visual = xml_element->FirstChildElement("visual"); visual != nullptr; visual = visual->NextSiblingElement("visual")) { - std::vector temp_visual; + tesseract_scene_graph::Visual::Ptr temp_visual; try { temp_visual = parseVisual(visual, locator, available_materials, version); @@ -82,14 +82,14 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Link: Error parsing 'visual' element for link '" + link_name + "'!")); } - l->visual.insert(l->visual.end(), temp_visual.begin(), temp_visual.end()); + l->visual.push_back(temp_visual); } // get collision if exists for (const tinyxml2::XMLElement* collision = xml_element->FirstChildElement("collision"); collision != nullptr; collision = collision->NextSiblingElement("collision")) { - std::vector temp_collision; + tesseract_scene_graph::Collision::Ptr temp_collision; try { temp_collision = parseCollision(collision, locator, version); @@ -100,7 +100,7 @@ tesseract_urdf::parseLink(const tinyxml2::XMLElement* xml_element, std::runtime_error("Link: Error parsing 'collision' element for link '" + link_name + "'!")); } - l->collision.insert(l->collision.end(), temp_collision.begin(), temp_collision.end()); + l->collision.push_back(temp_collision); } return l; diff --git a/tesseract_urdf/src/visual.cpp b/tesseract_urdf/src/visual.cpp index d9034db23c7..1f7b0e6540a 100644 --- a/tesseract_urdf/src/visual.cpp +++ b/tesseract_urdf/src/visual.cpp @@ -41,14 +41,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -std::vector +tesseract_scene_graph::Visual::Ptr tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, std::unordered_map& available_materials, int version) { - std::vector visuals; - // get name std::string visual_name = tesseract_common::StringAttribute(xml_element, "name", ""); @@ -87,45 +85,23 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, if (geometry == nullptr) std::throw_with_nested(std::runtime_error("Visual: Error missing 'geometry' element!")); - std::vector geometries; + tesseract_geometry::Geometry::Ptr geom; try { - geometries = parseGeometry(geometry, locator, true, version); + geom = parseGeometry(geometry, locator, true, version); } catch (...) { std::throw_with_nested(std::runtime_error("Visual: Error parsing 'geometry' element!")); } - if (geometries.size() == 1) - { - auto visual = std::make_shared(); - visual->name = visual_name; - visual->origin = visual_origin; - visual->geometry = geometries[0]; - visual->material = visual_material; - visuals.push_back(visual); - } - else - { - int i = 0; - for (const auto& g : geometries) - { - auto visual = std::make_shared(); - - if (visual_name.empty()) - visual->name = visual_name; - else - visual->name = visual_name + "_" + std::to_string(i); - - visual->origin = visual_origin; - visual->geometry = g; - visual->material = visual_material; - visuals.push_back(visual); - } - } + auto visual = std::make_shared(); + visual->name = visual_name; + visual->origin = visual_origin; + visual->geometry = geom; + visual->material = visual_material; - return visuals; + return visual; } tinyxml2::XMLElement* tesseract_urdf::writeVisual(const std::shared_ptr& visual, diff --git a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp index 636e07ada3a..ac025f429c8 100644 --- a/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_collision_unit.cpp @@ -6,6 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include "tesseract_urdf_common_unit.h" @@ -20,12 +21,11 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_FALSE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_FALSE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { @@ -34,12 +34,11 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT " )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_TRUE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { @@ -48,14 +47,13 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT " )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.size() == 2); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_TRUE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); - EXPECT_TRUE(elem[1]->geometry != nullptr); - EXPECT_TRUE(elem[1]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH); + EXPECT_EQ(std::dynamic_pointer_cast(elem->geometry)->getMeshes().size(), 2); } { @@ -65,10 +63,10 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { @@ -77,19 +75,19 @@ TEST(TesseractURDFUnit, parse_collision) // NOLINT " )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_scene_graph::Collision::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseCollision, str, "collision", resource_locator, 2)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } } diff --git a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp index 07d9c391b6c..3cce1a7cbcc 100644 --- a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp @@ -17,55 +17,50 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::BOX); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::BOX); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::SPHERE); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SPHERE); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::CYLINDER); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CYLINDER); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::CONE); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CONE); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::CAPSULE); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CAPSULE); } { @@ -74,113 +69,109 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::OCTREE); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::OCTREE); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::CONVEX_MESH); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::CONVEX_MESH); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::MESH); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::MESH); } { std::string str = R"( )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->getType() == tesseract_geometry::GeometryType::SDF_MESH); + EXPECT_TRUE(elem->getType() == tesseract_geometry::GeometryType::SDF_MESH); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { @@ -189,40 +180,40 @@ TEST(TesseractURDFUnit, parse_geometry) // NOLINT )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } { std::string str = R"( )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_geometry::Geometry::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseGeometry, str, "geometry", resource_locator, 2, true)); - EXPECT_TRUE(elem.empty()); + EXPECT_TRUE(elem == nullptr); } } diff --git a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp index c240413e5eb..7d8fd3af662 100644 --- a/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_visual_unit.cpp @@ -5,6 +5,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include #include @@ -24,13 +25,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Visual::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_TRUE(elem[0]->material != nullptr); - EXPECT_FALSE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_TRUE(elem->material != nullptr); + EXPECT_FALSE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { @@ -43,13 +43,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Visual::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_TRUE(elem[0]->material != nullptr); - EXPECT_TRUE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_TRUE(elem->material != nullptr); + EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { @@ -59,13 +58,12 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; - std::vector elem; - EXPECT_TRUE(runTest>( + tesseract_scene_graph::Visual::Ptr elem; + EXPECT_TRUE(runTest( elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); - EXPECT_TRUE(elem.size() == 1); - EXPECT_TRUE(elem[0]->geometry != nullptr); - EXPECT_TRUE(elem[0]->material != nullptr); - EXPECT_TRUE(elem[0]->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); + EXPECT_TRUE(elem->geometry != nullptr); + EXPECT_TRUE(elem->material != nullptr); + EXPECT_TRUE(elem->origin.isApprox(Eigen::Isometry3d::Identity(), 1e-8)); } { @@ -75,8 +73,8 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_scene_graph::Visual::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); } @@ -87,8 +85,8 @@ TEST(TesseractURDFUnit, parse_visual) // NOLINT )"; - std::vector elem; - EXPECT_FALSE(runTest>( + tesseract_scene_graph::Visual::Ptr elem; + EXPECT_FALSE(runTest( elem, &tesseract_urdf::parseVisual, str, "visual", resource_locator, empty_available_materials, 2)); } }