From 43330fa4864b8b175d33ec967932fd8387fd983b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 19 Sep 2024 15:26:00 -0500 Subject: [PATCH] Update to support Geometry Type CompoundMesh --- tesseract_msgs/msg/Geometry.msg | 5 + tesseract_rosutils/src/utils.cpp | 544 ++++++++++++----------------- tesseract_rviz/src/conversions.cpp | 139 ++++---- 3 files changed, 311 insertions(+), 377 deletions(-) diff --git a/tesseract_msgs/msg/Geometry.msg b/tesseract_msgs/msg/Geometry.msg index b59280f3..ee790797 100644 --- a/tesseract_msgs/msg/Geometry.msg +++ b/tesseract_msgs/msg/Geometry.msg @@ -10,6 +10,7 @@ uint8 MESH=6 uint8 CONVEX_MESH=7 uint8 SDF_MESH=8 uint8 OCTREE=9 +uint8 COMPOUND_MESH=11 # The type of the geometry uint8 type @@ -38,3 +39,7 @@ tesseract_msgs/Mesh mesh # OCTREE octomap_msgs/Octomap octomap tesseract_msgs/OctreeSubType octomap_sub_type + +# COMPOUND_MESH (MESH, CONVEX_MESH, SDF_MESH) +tesseract_msgs/Mesh[] compound_mesh +uint8 compound_mesh_type #(MESH, CONVEX_MESH, SDF_MESH) diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp index 41e71bb1..94e082bf 100644 --- a/tesseract_rosutils/src/utils.cpp +++ b/tesseract_rosutils/src/utils.cpp @@ -92,6 +92,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -145,152 +146,6 @@ bool isMsgEmpty(const sensor_msgs::JointState& msg) return msg.name.empty() && msg.position.empty() && msg.velocity.empty() && msg.effort.empty(); } -bool isIdentical(const tesseract_geometry::Geometry& shape1, const tesseract_geometry::Geometry& shape2) -{ - if (shape1.getType() != shape2.getType()) - return false; - - switch (shape1.getType()) - { - case tesseract_geometry::GeometryType::BOX: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (std::abs(s1.getX() - s2.getX()) > std::numeric_limits::epsilon()) - return false; - - if (std::abs(s1.getY() - s2.getY()) > std::numeric_limits::epsilon()) - return false; - - if (std::abs(s1.getZ() - s2.getZ()) > std::numeric_limits::epsilon()) - return false; - - break; - } - case tesseract_geometry::GeometryType::SPHERE: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (std::abs(s1.getRadius() - s2.getRadius()) > std::numeric_limits::epsilon()) - return false; - - break; - } - case tesseract_geometry::GeometryType::CYLINDER: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (std::abs(s1.getRadius() - s2.getRadius()) > std::numeric_limits::epsilon()) - return false; - - if (std::abs(s1.getLength() - s2.getLength()) > std::numeric_limits::epsilon()) - return false; - - break; - } - case tesseract_geometry::GeometryType::CONE: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (std::abs(s1.getRadius() - s2.getRadius()) > std::numeric_limits::epsilon()) - return false; - - if (std::abs(s1.getLength() - s2.getLength()) > std::numeric_limits::epsilon()) - return false; - - break; - } - case tesseract_geometry::GeometryType::MESH: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (s1.getVertexCount() != s2.getVertexCount()) - return false; - - if (s1.getFaceCount() != s2.getFaceCount()) - return false; - - if (s1.getFaces() != s2.getFaces()) - return false; - - if (s1.getVertices() != s2.getVertices()) - return false; - - break; - } - case tesseract_geometry::GeometryType::CONVEX_MESH: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (s1.getVertexCount() != s2.getVertexCount()) - return false; - - if (s1.getFaceCount() != s2.getFaceCount()) - return false; - - if (s1.getFaces() != s2.getFaces()) - return false; - - if (s1.getVertices() != s2.getVertices()) - return false; - - break; - } - case tesseract_geometry::GeometryType::SDF_MESH: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (s1.getVertexCount() != s2.getVertexCount()) - return false; - - if (s1.getFaceCount() != s2.getFaceCount()) - return false; - - if (s1.getFaces() != s2.getFaces()) - return false; - - if (s1.getVertices() != s2.getVertices()) - return false; - - break; - } - case tesseract_geometry::GeometryType::OCTREE: - { - const auto& s1 = static_cast(shape1); - const auto& s2 = static_cast(shape2); - - if (s1.getOctree()->getTreeType() != s2.getOctree()->getTreeType()) - return false; - - if (s1.getOctree()->size() != s2.getOctree()->size()) - return false; - - if (s1.getOctree()->getTreeDepth() != s2.getOctree()->getTreeDepth()) - return false; - - if (s1.getOctree()->memoryUsage() != s2.getOctree()->memoryUsage()) - return false; - - if (s1.getOctree()->memoryFullGrid() != s2.getOctree()->memoryFullGrid()) - return false; - - break; - } - default: - ROS_ERROR("This geometric shape type (%d) is not supported", static_cast(shape1.getType())); - return false; - } - - return true; -} - bool isIdentical(const tesseract_scene_graph::Visual& /*visual1*/, const tesseract_scene_graph::Visual& /*visual2*/) { assert(false); @@ -343,6 +198,134 @@ bool toMsg(geometry_msgs::Pose& pose_msg, const Eigen::Isometry3d& pose) return true; } +inline bool toMsg(tesseract_msgs::Mesh& mesh_msgs, const tesseract_geometry::Geometry& geometry) +{ + switch (geometry.getType()) + { + case tesseract_geometry::GeometryType::MESH: + { + const auto& mesh = static_cast(geometry); + const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); + mesh_msgs.vertices.resize(vertices.size()); + for (size_t i = 0; i < vertices.size(); ++i) + { + mesh_msgs.vertices[i].x = vertices[i](0); + mesh_msgs.vertices[i].y = vertices[i](1); + mesh_msgs.vertices[i].z = vertices[i](2); + } + + const Eigen::VectorXi& faces = *(mesh.getFaces()); + mesh_msgs.faces.resize(static_cast(faces.size())); + for (size_t i = 0; i < static_cast(faces.size()); ++i) + mesh_msgs.faces[i] = static_cast(faces[static_cast(i)]); + + if (mesh.getResource() && mesh.getResource()->isFile()) + { + mesh_msgs.file_path = mesh.getResource()->getFilePath(); + } + if (mesh_msgs.file_path.empty()) + { + mesh_msgs.scale[0] = 1; + mesh_msgs.scale[1] = 1; + mesh_msgs.scale[2] = 1; + } + else + { + const Eigen::Vector3f& scale = mesh.getScale().cast(); + mesh_msgs.scale[0] = scale.x(); + mesh_msgs.scale[1] = scale.y(); + mesh_msgs.scale[2] = scale.z(); + } + + break; + } + case tesseract_geometry::GeometryType::CONVEX_MESH: + { + const auto& mesh = static_cast(geometry); + + const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); + mesh_msgs.vertices.resize(vertices.size()); + for (size_t i = 0; i < vertices.size(); ++i) + { + mesh_msgs.vertices[i].x = vertices[i](0); + mesh_msgs.vertices[i].y = vertices[i](1); + mesh_msgs.vertices[i].z = vertices[i](2); + } + + const Eigen::VectorXi& faces = *(mesh.getFaces()); + mesh_msgs.faces.resize(static_cast(faces.size())); + for (size_t i = 0; i < static_cast(faces.size()); ++i) + mesh_msgs.faces[i] = static_cast(faces[static_cast(i)]); + + if (mesh.getResource() && mesh.getResource()->isFile()) + { + mesh_msgs.file_path = mesh.getResource()->getFilePath(); + } + if (mesh_msgs.file_path.empty()) + { + mesh_msgs.scale[0] = 1; + mesh_msgs.scale[1] = 1; + mesh_msgs.scale[2] = 1; + } + else + { + const Eigen::Vector3f& scale = mesh.getScale().cast(); + mesh_msgs.scale[0] = scale.x(); + mesh_msgs.scale[1] = scale.y(); + mesh_msgs.scale[2] = scale.z(); + } + + break; + } + case tesseract_geometry::GeometryType::SDF_MESH: + { + const auto& mesh = static_cast(geometry); + + const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); + mesh_msgs.vertices.resize(vertices.size()); + for (size_t i = 0; i < vertices.size(); ++i) + { + mesh_msgs.vertices[i].x = vertices[i](0); + mesh_msgs.vertices[i].y = vertices[i](1); + mesh_msgs.vertices[i].z = vertices[i](2); + } + + const Eigen::VectorXi& faces = *(mesh.getFaces()); + mesh_msgs.faces.resize(static_cast(faces.size())); + for (size_t i = 0; i < static_cast(faces.size()); ++i) + mesh_msgs.faces[i] = static_cast(faces[static_cast(i)]); + + if (mesh.getResource() && mesh.getResource()->isFile()) + { + mesh_msgs.file_path = mesh.getResource()->getFilePath(); + } + if (mesh_msgs.file_path.empty()) + { + mesh_msgs.scale[0] = 1; + mesh_msgs.scale[1] = 1; + mesh_msgs.scale[2] = 1; + } + else + { + const Eigen::Vector3f& scale = mesh.getScale().cast(); + mesh_msgs.scale[0] = scale.x(); + mesh_msgs.scale[1] = scale.y(); + mesh_msgs.scale[2] = scale.z(); + } + + break; + } + default: + { + ROS_ERROR("Unable to construct primitive shape message for shape of type %d", + static_cast(geometry.getType())); + return false; + } + } + + return true; +} + /** \brief Construct the message that corresponds to the shape. Return false on failure. */ bool toMsg(tesseract_msgs::Geometry& geometry_msgs, const tesseract_geometry::Geometry& geometry) { @@ -415,121 +398,42 @@ bool toMsg(tesseract_msgs::Geometry& geometry_msgs, const tesseract_geometry::Ge } case tesseract_geometry::GeometryType::MESH: { - const auto& mesh = static_cast(geometry); - geometry_msgs.type = tesseract_msgs::Geometry::MESH; - - const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); - geometry_msgs.mesh.vertices.resize(vertices.size()); - for (size_t i = 0; i < vertices.size(); ++i) - { - geometry_msgs.mesh.vertices[i].x = vertices[i](0); - geometry_msgs.mesh.vertices[i].y = vertices[i](1); - geometry_msgs.mesh.vertices[i].z = vertices[i](2); - } - - const Eigen::VectorXi& faces = *(mesh.getFaces()); - geometry_msgs.mesh.faces.resize(static_cast(faces.size())); - for (size_t i = 0; i < static_cast(faces.size()); ++i) - geometry_msgs.mesh.faces[i] = static_cast(faces[static_cast(i)]); - - if (mesh.getResource() && mesh.getResource()->isFile()) - { - geometry_msgs.mesh.file_path = mesh.getResource()->getFilePath(); - } - if (geometry_msgs.mesh.file_path.empty()) - { - geometry_msgs.mesh.scale[0] = 1; - geometry_msgs.mesh.scale[1] = 1; - geometry_msgs.mesh.scale[2] = 1; - } - else - { - const Eigen::Vector3f& scale = mesh.getScale().cast(); - geometry_msgs.mesh.scale[0] = scale.x(); - geometry_msgs.mesh.scale[1] = scale.y(); - geometry_msgs.mesh.scale[2] = scale.z(); - } - + toMsg(geometry_msgs.mesh, geometry); break; } case tesseract_geometry::GeometryType::CONVEX_MESH: { - const auto& mesh = static_cast(geometry); - geometry_msgs.type = tesseract_msgs::Geometry::CONVEX_MESH; - - const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); - geometry_msgs.mesh.vertices.resize(vertices.size()); - for (size_t i = 0; i < vertices.size(); ++i) - { - geometry_msgs.mesh.vertices[i].x = vertices[i](0); - geometry_msgs.mesh.vertices[i].y = vertices[i](1); - geometry_msgs.mesh.vertices[i].z = vertices[i](2); - } - - const Eigen::VectorXi& faces = *(mesh.getFaces()); - geometry_msgs.mesh.faces.resize(static_cast(faces.size())); - for (size_t i = 0; i < static_cast(faces.size()); ++i) - geometry_msgs.mesh.faces[i] = static_cast(faces[static_cast(i)]); - - if (mesh.getResource() && mesh.getResource()->isFile()) - { - geometry_msgs.mesh.file_path = mesh.getResource()->getFilePath(); - } - if (geometry_msgs.mesh.file_path.empty()) - { - geometry_msgs.mesh.scale[0] = 1; - geometry_msgs.mesh.scale[1] = 1; - geometry_msgs.mesh.scale[2] = 1; - } - else - { - const Eigen::Vector3f& scale = mesh.getScale().cast(); - geometry_msgs.mesh.scale[0] = scale.x(); - geometry_msgs.mesh.scale[1] = scale.y(); - geometry_msgs.mesh.scale[2] = scale.z(); - } - + toMsg(geometry_msgs.mesh, geometry); break; } case tesseract_geometry::GeometryType::SDF_MESH: { - const auto& mesh = static_cast(geometry); - geometry_msgs.type = tesseract_msgs::Geometry::SDF_MESH; - - const tesseract_common::VectorVector3d& vertices = *(mesh.getVertices()); - geometry_msgs.mesh.vertices.resize(vertices.size()); - for (size_t i = 0; i < vertices.size(); ++i) + toMsg(geometry_msgs.mesh, geometry); + break; + } + case tesseract_geometry::GeometryType::COMPOUND_MESH: + { + geometry_msgs.type = tesseract_msgs::Geometry::COMPOUND_MESH; + const auto& compound_mesh = static_cast(geometry); + for (const auto& mesh : compound_mesh.getMeshes()) { - geometry_msgs.mesh.vertices[i].x = vertices[i](0); - geometry_msgs.mesh.vertices[i].y = vertices[i](1); - geometry_msgs.mesh.vertices[i].z = vertices[i](2); + tesseract_msgs::Mesh mesh_msg; + toMsg(mesh_msg, *mesh); + geometry_msgs.compound_mesh.push_back(mesh_msg); } - const Eigen::VectorXi& faces = *(mesh.getFaces()); - geometry_msgs.mesh.faces.resize(static_cast(faces.size())); - for (size_t i = 0; i < static_cast(faces.size()); ++i) - geometry_msgs.mesh.faces[i] = static_cast(faces[static_cast(i)]); - - if (mesh.getResource() && mesh.getResource()->isFile()) - { - geometry_msgs.mesh.file_path = mesh.getResource()->getFilePath(); - } - if (geometry_msgs.mesh.file_path.empty()) - { - geometry_msgs.mesh.scale[0] = 1; - geometry_msgs.mesh.scale[1] = 1; - geometry_msgs.mesh.scale[2] = 1; - } + const auto compound_mesh_type = compound_mesh.getMeshes().front()->getType(); + if (compound_mesh_type == tesseract_geometry::GeometryType::MESH) + geometry_msgs.compound_mesh_type = tesseract_msgs::Geometry::MESH; + else if (compound_mesh_type == tesseract_geometry::GeometryType::CONVEX_MESH) + geometry_msgs.compound_mesh_type = tesseract_msgs::Geometry::CONVEX_MESH; + else if (compound_mesh_type == tesseract_geometry::GeometryType::SDF_MESH) + geometry_msgs.compound_mesh_type = tesseract_msgs::Geometry::SDF_MESH; else - { - const Eigen::Vector3f& scale = mesh.getScale().cast(); - geometry_msgs.mesh.scale[0] = scale.x(); - geometry_msgs.mesh.scale[1] = scale.y(); - geometry_msgs.mesh.scale[2] = scale.z(); - } + throw std::runtime_error("Invalid compound mesh type!"); break; } @@ -544,6 +448,74 @@ bool toMsg(tesseract_msgs::Geometry& geometry_msgs, const tesseract_geometry::Ge return true; } +inline std::shared_ptr fromMsg(uint8_t type, const tesseract_msgs::Mesh& mesh_msg) +{ + if (type == tesseract_msgs::Geometry::MESH) + { + auto vertices = std::make_shared(mesh_msg.vertices.size()); + auto faces = std::make_shared(mesh_msg.faces.size()); + + for (unsigned int i = 0; i < mesh_msg.vertices.size(); ++i) + (*vertices)[i] = Eigen::Vector3d(mesh_msg.vertices[i].x, mesh_msg.vertices[i].y, mesh_msg.vertices[i].z); + + for (unsigned int i = 0; i < mesh_msg.faces.size(); ++i) + (*faces)[static_cast(i)] = static_cast(mesh_msg.faces[i]); + + if (!mesh_msg.file_path.empty()) + return std::make_shared( + vertices, + faces, + std::make_shared(mesh_msg.file_path, mesh_msg.file_path), + Eigen::Vector3f(mesh_msg.scale[0], mesh_msg.scale[1], mesh_msg.scale[2]).cast()); + + return std::make_shared(vertices, faces); + } + + if (type == tesseract_msgs::Geometry::CONVEX_MESH) + { + auto vertices = std::make_shared(mesh_msg.vertices.size()); + auto faces = std::make_shared(mesh_msg.faces.size()); + + for (unsigned int i = 0; i < mesh_msg.vertices.size(); ++i) + (*vertices)[i] = Eigen::Vector3d(mesh_msg.vertices[i].x, mesh_msg.vertices[i].y, mesh_msg.vertices[i].z); + + for (unsigned int i = 0; i < mesh_msg.faces.size(); ++i) + (*faces)[static_cast(i)] = static_cast(mesh_msg.faces[i]); + + if (!mesh_msg.file_path.empty()) + return std::make_shared( + vertices, + faces, + std::make_shared(mesh_msg.file_path, mesh_msg.file_path), + Eigen::Vector3f(mesh_msg.scale[0], mesh_msg.scale[1], mesh_msg.scale[2]).cast()); + + return std::make_shared(vertices, faces); + } + + if (type == tesseract_msgs::Geometry::SDF_MESH) + { + auto vertices = std::make_shared(mesh_msg.vertices.size()); + auto faces = std::make_shared(mesh_msg.faces.size()); + + for (unsigned int i = 0; i < mesh_msg.vertices.size(); ++i) + (*vertices)[i] = Eigen::Vector3d(mesh_msg.vertices[i].x, mesh_msg.vertices[i].y, mesh_msg.vertices[i].z); + + for (unsigned int i = 0; i < mesh_msg.faces.size(); ++i) + (*faces)[static_cast(i)] = static_cast(mesh_msg.faces[i]); + + if (!mesh_msg.file_path.empty()) + return std::make_shared( + vertices, + faces, + std::make_shared(mesh_msg.file_path, mesh_msg.file_path), + Eigen::Vector3f(mesh_msg.scale[0], mesh_msg.scale[1], mesh_msg.scale[2]).cast()); + + return std::make_shared(vertices, faces); + } + + return nullptr; +} + bool fromMsg(std::shared_ptr& geometry, const tesseract_msgs::Geometry& geometry_msg) { geometry = nullptr; @@ -578,74 +550,11 @@ bool fromMsg(std::shared_ptr& geometry, const tess geometry_msg.plane_coeff[2], geometry_msg.plane_coeff[3]); } - else if (geometry_msg.type == tesseract_msgs::Geometry::MESH) - { - auto vertices = std::make_shared(geometry_msg.mesh.vertices.size()); - auto faces = std::make_shared(geometry_msg.mesh.faces.size()); - - for (unsigned int i = 0; i < geometry_msg.mesh.vertices.size(); ++i) - (*vertices)[i] = Eigen::Vector3d( - geometry_msg.mesh.vertices[i].x, geometry_msg.mesh.vertices[i].y, geometry_msg.mesh.vertices[i].z); - - for (unsigned int i = 0; i < geometry_msg.mesh.faces.size(); ++i) - (*faces)[static_cast(i)] = static_cast(geometry_msg.mesh.faces[i]); - - if (!geometry_msg.mesh.file_path.empty()) - geometry = std::make_shared( - vertices, - faces, - std::make_shared(geometry_msg.mesh.file_path, - geometry_msg.mesh.file_path), - Eigen::Vector3f(geometry_msg.mesh.scale[0], geometry_msg.mesh.scale[1], geometry_msg.mesh.scale[2]) - .cast()); - else - geometry = std::make_shared(vertices, faces); - } - else if (geometry_msg.type == tesseract_msgs::Geometry::CONVEX_MESH) + else if (geometry_msg.type == tesseract_msgs::Geometry::MESH || + geometry_msg.type == tesseract_msgs::Geometry::CONVEX_MESH || + geometry_msg.type == tesseract_msgs::Geometry::SDF_MESH) { - auto vertices = std::make_shared(geometry_msg.mesh.vertices.size()); - auto faces = std::make_shared(geometry_msg.mesh.faces.size()); - - for (unsigned int i = 0; i < geometry_msg.mesh.vertices.size(); ++i) - (*vertices)[i] = Eigen::Vector3d( - geometry_msg.mesh.vertices[i].x, geometry_msg.mesh.vertices[i].y, geometry_msg.mesh.vertices[i].z); - - for (unsigned int i = 0; i < geometry_msg.mesh.faces.size(); ++i) - (*faces)[static_cast(i)] = static_cast(geometry_msg.mesh.faces[i]); - - if (!geometry_msg.mesh.file_path.empty()) - geometry = std::make_shared( - vertices, - faces, - std::make_shared(geometry_msg.mesh.file_path, - geometry_msg.mesh.file_path), - Eigen::Vector3f(geometry_msg.mesh.scale[0], geometry_msg.mesh.scale[1], geometry_msg.mesh.scale[2]) - .cast()); - else - geometry = std::make_shared(vertices, faces); - } - else if (geometry_msg.type == tesseract_msgs::Geometry::SDF_MESH) - { - auto vertices = std::make_shared(geometry_msg.mesh.vertices.size()); - auto faces = std::make_shared(geometry_msg.mesh.faces.size()); - - for (unsigned int i = 0; i < geometry_msg.mesh.vertices.size(); ++i) - (*vertices)[i] = Eigen::Vector3d( - geometry_msg.mesh.vertices[i].x, geometry_msg.mesh.vertices[i].y, geometry_msg.mesh.vertices[i].z); - - for (unsigned int i = 0; i < geometry_msg.mesh.faces.size(); ++i) - (*faces)[static_cast(i)] = static_cast(geometry_msg.mesh.faces[i]); - - if (!geometry_msg.mesh.file_path.empty()) - geometry = std::make_shared( - vertices, - faces, - std::make_shared(geometry_msg.mesh.file_path, - geometry_msg.mesh.file_path), - Eigen::Vector3f(geometry_msg.mesh.scale[0], geometry_msg.mesh.scale[1], geometry_msg.mesh.scale[2]) - .cast()); - else - geometry = std::make_shared(vertices, faces); + geometry = fromMsg(geometry_msg.type, geometry_msg.mesh); } else if (geometry_msg.type == tesseract_msgs::Geometry::OCTREE) { @@ -653,6 +562,15 @@ bool fromMsg(std::shared_ptr& geometry, const tess auto sub_type = static_cast(geometry_msg.octomap_sub_type.type); geometry = std::make_shared(om, sub_type); } + else if (geometry_msg.type == tesseract_msgs::Geometry::COMPOUND_MESH) + { + std::vector> meshes; + meshes.reserve(geometry_msg.compound_mesh.size()); + for (const auto& mesh : geometry_msg.compound_mesh) + meshes.push_back(fromMsg(geometry_msg.compound_mesh_type, mesh)); + + geometry = std::make_shared(meshes); + } if (geometry == nullptr) { diff --git a/tesseract_rviz/src/conversions.cpp b/tesseract_rviz/src/conversions.cpp index 698741aa..3b68e19e 100644 --- a/tesseract_rviz/src/conversions.cpp +++ b/tesseract_rviz/src/conversions.cpp @@ -464,6 +464,41 @@ Ogre::SceneNode* loadLinkWireBox(Ogre::SceneManager& scene, return wire_box_node; } +Ogre::Entity* loadMesh(Ogre::SceneManager& scene, + Ogre::Vector3& ogre_scale, + tesseract_gui::EntityContainer& entity_container, + const tesseract_geometry::PolygonMesh& mesh, + bool is_visual) +{ + if (mesh.getResource() && mesh.getResource()->isFile() && is_visual) + { + std::string model_name = "file://" + mesh.getResource()->getFilePath(); + + const Eigen::Vector3d& mesh_scale = mesh.getScale(); + ogre_scale = Ogre::Vector3( + static_cast(mesh_scale.x()), static_cast(mesh_scale.y()), static_cast(mesh_scale.z())); + + try + { + rviz::loadMeshFromResource(model_name); + auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); + return scene.createEntity(entity.unique_name, model_name); + } + catch (Ogre::InvalidParametersException& e) + { + ROS_ERROR("Could not convert mesh resource '%s'. It might be an empty mesh: %s", model_name.c_str(), e.what()); + } + catch (Ogre::Exception& e) + { + ROS_ERROR("Could not load model '%s': %s", model_name.c_str(), e.what()); + } + + return nullptr; + } + + return createEntityForMeshData(scene, entity_container, mesh.getVertices(), mesh.getFaces()); +} + Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, tesseract_gui::EntityContainer& entity_container, const tesseract_geometry::Geometry& geometry, @@ -472,7 +507,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, const Ogre::MaterialPtr& material, bool is_visual) { - Ogre::Entity* ogre_entity = nullptr; // default in case nothing works. + std::vector ogre_entity; Ogre::Vector3 ogre_scale(Ogre::Vector3::UNIT_SCALE); Ogre::Vector3 offset_position(Ogre::Vector3::ZERO); Ogre::Quaternion offset_orientation(Ogre::Quaternion::IDENTITY); @@ -498,7 +533,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { const auto& sphere = static_cast(geometry); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, "tesseract_sphere.mesh"); + ogre_entity.push_back(scene.createEntity(entity.unique_name, "tesseract_sphere.mesh")); float diameter = static_cast(sphere.getRadius()) * 2.0f; ogre_scale = Ogre::Vector3(diameter, diameter, diameter); break; @@ -507,7 +542,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { const auto& box = static_cast(geometry); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, "tesseract_cube.mesh"); + ogre_entity.push_back(scene.createEntity(entity.unique_name, "tesseract_cube.mesh")); ogre_scale = Ogre::Vector3(static_cast(box.getX()), static_cast(box.getY()), static_cast(box.getZ())); break; @@ -516,7 +551,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { const auto& cylinder = static_cast(geometry); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, "tesseract_cylinder.mesh"); + ogre_entity.push_back(scene.createEntity(entity.unique_name, "tesseract_cylinder.mesh")); ogre_scale = Ogre::Vector3(static_cast(cylinder.getRadius() * 2), static_cast(cylinder.getRadius() * 2), static_cast(cylinder.getLength())); @@ -526,7 +561,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { const auto& cone = static_cast(geometry); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, "tesseract_cone.mesh"); + ogre_entity.push_back(scene.createEntity(entity.unique_name, "tesseract_cone.mesh")); ogre_scale = Ogre::Vector3(static_cast(cone.getRadius() * 2), static_cast(cone.getRadius() * 2), static_cast(cone.getLength())); @@ -536,56 +571,27 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { const auto& capsule = static_cast(geometry); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, "tesseract_capsule.mesh"); + ogre_entity.push_back(scene.createEntity(entity.unique_name, "tesseract_capsule.mesh")); ogre_scale = Ogre::Vector3(static_cast(capsule.getRadius() * 2), static_cast(capsule.getRadius() * 2), static_cast((0.5 * capsule.getLength()) + capsule.getRadius())); break; } case tesseract_geometry::GeometryType::MESH: + case tesseract_geometry::GeometryType::CONVEX_MESH: { - const auto& mesh = static_cast(geometry); - - if (mesh.getResource() && mesh.getResource()->isFile() && is_visual) - { - std::string model_name = "file://" + mesh.getResource()->getFilePath(); - - const Eigen::Vector3d& mesh_scale = mesh.getScale(); - ogre_scale = Ogre::Vector3( - static_cast(mesh_scale.x()), static_cast(mesh_scale.y()), static_cast(mesh_scale.z())); - - try - { - rviz::loadMeshFromResource(model_name); - auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, model_name); - } - catch (Ogre::InvalidParametersException& e) - { - ROS_ERROR( - "Could not convert mesh resource '%s'. It might be an empty mesh: %s", model_name.c_str(), e.what()); - } - catch (Ogre::Exception& e) - { - ROS_ERROR("Could not load model '%s': %s", model_name.c_str(), e.what()); - } - } - else - { - ogre_entity = createEntityForMeshData(scene, entity_container, mesh.getVertices(), mesh.getFaces()); - } - + const auto& mesh = static_cast(geometry); + ogre_entity.push_back(loadMesh(scene, ogre_scale, entity_container, mesh, is_visual)); break; } - case tesseract_geometry::GeometryType::CONVEX_MESH: + case tesseract_geometry::GeometryType::COMPOUND_MESH: { - const auto& mesh = static_cast(geometry); - - if (mesh.getResource() && mesh.getResource()->isFile() && is_visual) + const auto& compound_mesh = static_cast(geometry); + if (compound_mesh.getResource() && compound_mesh.getResource()->isFile() && is_visual) { - std::string model_name = "file://" + mesh.getResource()->getFilePath(); + std::string model_name = "file://" + compound_mesh.getResource()->getFilePath(); - const Eigen::Vector3d& mesh_scale = mesh.getScale(); + const Eigen::Vector3d& mesh_scale = compound_mesh.getScale(); ogre_scale = Ogre::Vector3( static_cast(mesh_scale.x()), static_cast(mesh_scale.y()), static_cast(mesh_scale.z())); @@ -593,7 +599,7 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, { rviz::loadMeshFromResource(model_name); auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::RESOURCE_NS); - ogre_entity = scene.createEntity(entity.unique_name, model_name); + ogre_entity.push_back(scene.createEntity(entity.unique_name, model_name)); } catch (Ogre::InvalidParametersException& e) { @@ -607,7 +613,9 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, } else { - ogre_entity = createEntityForMeshData(scene, entity_container, mesh.getVertices(), mesh.getFaces()); + for (const auto& mesh : compound_mesh.getMeshes()) + ogre_entity.push_back( + createEntityForMeshData(scene, entity_container, mesh->getVertices(), mesh->getFaces())); } break; } @@ -749,38 +757,41 @@ Ogre::SceneNode* loadLinkGeometry(Ogre::SceneManager& scene, break; } - if (ogre_entity) + if (!ogre_entity.empty()) { auto entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::VISUAL_NS); Ogre::SceneNode* offset_node = scene.createSceneNode(entity.unique_name); - // std::vector* meshes; - offset_node->attachObject(ogre_entity); + for (auto* obj : ogre_entity) + offset_node->attachObject(obj); + offset_node->setScale(ogre_scale); offset_node->setPosition(offset_position); offset_node->setOrientation(offset_orientation); - for (uint32_t i = 0; i < ogre_entity->getNumSubEntities(); ++i) + for (auto* obj : ogre_entity) { - // Assign materials only if the submesh does not have one already - Ogre::SubEntity* sub = ogre_entity->getSubEntity(i); - const std::string& material_name = sub->getMaterialName(); - - if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting") + for (uint32_t i = 0; i < obj->getNumSubEntities(); ++i) { - std::string cloned_name = material_name_generator.generate(); - material->clone(cloned_name); - sub->setMaterialName(cloned_name); - } - else - { - std::string cloned_name = material_name_generator.generate(); - sub->getMaterial()->clone(cloned_name); - sub->setMaterialName(cloned_name); + // Assign materials only if the submesh does not have one already + Ogre::SubEntity* sub = obj->getSubEntity(i); + const std::string& material_name = sub->getMaterialName(); + + if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting") + { + std::string cloned_name = material_name_generator.generate(); + material->clone(cloned_name); + sub->setMaterialName(cloned_name); + } + else + { + std::string cloned_name = material_name_generator.generate(); + sub->getMaterial()->clone(cloned_name); + sub->setMaterialName(cloned_name); + } } } - // meshes->push_back(ogre_entity); return offset_node; }