Skip to content

Commit

Permalink
Update to support Geometry Type CompoundMesh
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Sep 19, 2024
1 parent abf4b7d commit fbb9024
Show file tree
Hide file tree
Showing 4 changed files with 219 additions and 68 deletions.
53 changes: 27 additions & 26 deletions common/include/tesseract_qt/common/models/standard_item_type.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,32 +64,33 @@ enum class StandardItemType : int
JOINT_TRAJECTORY_SET_END = COMMON_TOOL_PATH_END + 4,

// Scene Graph Types
SG_MATERIAL = JOINT_TRAJECTORY_SET_END + 1,
SG_INERTIAL = JOINT_TRAJECTORY_SET_END + 2,
SG_ORIGIN = JOINT_TRAJECTORY_SET_END + 3,
SG_GEOMETRY = JOINT_TRAJECTORY_SET_END + 4,
SG_VISUAL = JOINT_TRAJECTORY_SET_END + 5,
SG_COLLISION = JOINT_TRAJECTORY_SET_END + 6,
SG_LINK = JOINT_TRAJECTORY_SET_END + 7,
SG_DYNAMICS = JOINT_TRAJECTORY_SET_END + 8,
SG_LIMITS = JOINT_TRAJECTORY_SET_END + 9,
SG_SAFETY = JOINT_TRAJECTORY_SET_END + 10,
SG_CALIBRATION = JOINT_TRAJECTORY_SET_END + 11,
SG_MIMIC = JOINT_TRAJECTORY_SET_END + 12,
SG_JOINT = JOINT_TRAJECTORY_SET_END + 13,
SG_SCENE_GRAPH = JOINT_TRAJECTORY_SET_END + 15,
SG_BOX = JOINT_TRAJECTORY_SET_END + 16,
SG_CAPSULE = JOINT_TRAJECTORY_SET_END + 17,
SG_CONE = JOINT_TRAJECTORY_SET_END + 18,
SG_CYLINDER = JOINT_TRAJECTORY_SET_END + 19,
SG_PLANE = JOINT_TRAJECTORY_SET_END + 20,
SG_SPHERE = JOINT_TRAJECTORY_SET_END + 21,
SG_POLYGON_MESH = JOINT_TRAJECTORY_SET_END + 22,
SG_OCTREE = JOINT_TRAJECTORY_SET_END + 23,
SG_VISUALS = JOINT_TRAJECTORY_SET_END + 24,
SG_COLLISIONS = JOINT_TRAJECTORY_SET_END + 25,
SG_SCENE_STATE = JOINT_TRAJECTORY_SET_END + 26,
SG_TYPES_END = JOINT_TRAJECTORY_SET_END + 27,
SG_MATERIAL = JOINT_TRAJECTORY_SET_END + 1,
SG_INERTIAL = JOINT_TRAJECTORY_SET_END + 2,
SG_ORIGIN = JOINT_TRAJECTORY_SET_END + 3,
SG_GEOMETRY = JOINT_TRAJECTORY_SET_END + 4,
SG_VISUAL = JOINT_TRAJECTORY_SET_END + 5,
SG_COLLISION = JOINT_TRAJECTORY_SET_END + 6,
SG_LINK = JOINT_TRAJECTORY_SET_END + 7,
SG_DYNAMICS = JOINT_TRAJECTORY_SET_END + 8,
SG_LIMITS = JOINT_TRAJECTORY_SET_END + 9,
SG_SAFETY = JOINT_TRAJECTORY_SET_END + 10,
SG_CALIBRATION = JOINT_TRAJECTORY_SET_END + 11,
SG_MIMIC = JOINT_TRAJECTORY_SET_END + 12,
SG_JOINT = JOINT_TRAJECTORY_SET_END + 13,
SG_SCENE_GRAPH = JOINT_TRAJECTORY_SET_END + 15,
SG_BOX = JOINT_TRAJECTORY_SET_END + 16,
SG_CAPSULE = JOINT_TRAJECTORY_SET_END + 17,
SG_CONE = JOINT_TRAJECTORY_SET_END + 18,
SG_CYLINDER = JOINT_TRAJECTORY_SET_END + 19,
SG_PLANE = JOINT_TRAJECTORY_SET_END + 20,
SG_SPHERE = JOINT_TRAJECTORY_SET_END + 21,
SG_POLYGON_MESH = JOINT_TRAJECTORY_SET_END + 22,
SG_OCTREE = JOINT_TRAJECTORY_SET_END + 23,
SG_COMPOUND_MESH = JOINT_TRAJECTORY_SET_END + 24,
SG_VISUALS = JOINT_TRAJECTORY_SET_END + 25,
SG_COLLISIONS = JOINT_TRAJECTORY_SET_END + 26,
SG_SCENE_STATE = JOINT_TRAJECTORY_SET_END + 27,
SG_TYPES_END = JOINT_TRAJECTORY_SET_END + 28,

// SRDF Types
SRDF_OPW_PARAMS = SG_TYPES_END + 1,
Expand Down
117 changes: 117 additions & 0 deletions rendering/src/gazebo_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -584,6 +584,123 @@ loadLinkGeometry(gz::rendering::Scene& scene,
assert(false);
return nullptr;
}
case tesseract_geometry::GeometryType::COMPOUND_MESH:
{
const auto& shape = static_cast<const tesseract_geometry::CompoundMesh&>(geometry);
auto resource = shape.getResource();

auto gv_entity = entity_container.addUntrackedEntity(tesseract_gui::EntityContainer::VISUAL_NS);
std::shared_ptr<gz::rendering::Visual> mesh = scene.CreateVisual(gv_entity.id, gv_entity.unique_name);
mesh->SetLocalPose(gz::math::eigen3::convert(local_pose));
gz::common::MeshManager* mesh_manager = gz::common::MeshManager::Instance();

const auto& sub_meshes = shape.getMeshes();
if (sub_meshes.front()->getType() == tesseract_geometry::GeometryType::CONVEX_MESH)
{
if (resource && static_cast<const tesseract_geometry::ConvexMesh&>(*sub_meshes.front()).getCreationMethod() !=
tesseract_geometry::ConvexMesh::CreationMethod::CONVERTED)
{
gz::rendering::MeshDescriptor descriptor;
descriptor.meshName = resource->getFilePath();
descriptor.mesh = mesh_manager->Load(descriptor.meshName);
gz::rendering::MeshPtr mesh_geom = scene.CreateMesh(descriptor);

if (!isMeshWithColor(resource->getFilePath()))
mesh_geom->SetMaterial(ign_material);

mesh->AddGeometry(mesh_geom);
mesh->SetLocalScale(shape.getScale().x(), shape.getScale().y(), shape.getScale().z());
return mesh;
}

for (const auto& sub_mesh : sub_meshes)
{
const std::string geom_hash_str =
std::to_string(std::hash<const tesseract_geometry::Geometry*>{}(sub_mesh.get()));
std::string name{ geom_hash_str };
if (resource)
{
name.append(resource->getFilePath());
name.append("::");
name.append(geom_hash_str);
name.append("::CONVERTED_CONVEX_HULL");
}

const gz::common::Mesh* gz_mesh{ nullptr };
if (!mesh_manager->HasMesh(name))
{
auto* nm = new gz::common::Mesh();
nm->SetName(name);
nm->AddSubMesh(convert(*sub_mesh));
mesh_manager->AddMesh(nm);
gz_mesh = nm;
}
else
{
gz_mesh = mesh_manager->MeshByName(name);
}

gz::rendering::MeshDescriptor descriptor;
descriptor.meshName = name;
descriptor.mesh = gz_mesh;
gz::rendering::MeshPtr mesh_geom = scene.CreateMesh(descriptor);
mesh_geom->SetMaterial(ign_material);
mesh->AddGeometry(mesh_geom);
}

mesh->SetLocalScale(shape.getScale().x(), shape.getScale().y(), shape.getScale().z());
return mesh;
}

if (sub_meshes.front()->getType() == tesseract_geometry::GeometryType::MESH)
{
if (resource)
{
gz::rendering::MeshDescriptor descriptor;
descriptor.meshName = resource->getFilePath();
descriptor.mesh = mesh_manager->Load(descriptor.meshName);
gz::rendering::MeshPtr mesh_geom = scene.CreateMesh(descriptor);

if (!isMeshWithColor(resource->getFilePath()))
mesh_geom->SetMaterial(ign_material);

mesh->AddGeometry(mesh_geom);
mesh->SetLocalScale(shape.getScale().x(), shape.getScale().y(), shape.getScale().z());
return mesh;
}

for (const auto& sub_mesh : sub_meshes)
{
std::string name = std::to_string(std::hash<const tesseract_geometry::Geometry*>{}(sub_mesh.get()));

const gz::common::Mesh* gz_mesh{ nullptr };
if (!mesh_manager->HasMesh(name))
{
auto* nm = new gz::common::Mesh();
nm->SetName(name);
nm->AddSubMesh(convert(*sub_mesh));
mesh_manager->AddMesh(nm);
gz_mesh = nm;
}
else
{
gz_mesh = mesh_manager->MeshByName(name);
}

gz::rendering::MeshDescriptor descriptor;
descriptor.meshName = name;
descriptor.mesh = gz_mesh;
gz::rendering::MeshPtr mesh_geom = scene.CreateMesh(descriptor);
mesh_geom->SetMaterial(ign_material);
mesh->AddGeometry(mesh_geom);
}
mesh->SetLocalScale(shape.getScale().x(), shape.getScale().y(), shape.getScale().z());
return mesh;
}

if (sub_meshes.front()->getType() == tesseract_geometry::GeometryType::SDF_MESH)
throw std::runtime_error("SDF Mesh, currently not supported!");
}
default:
{
// CONSOLE_BRIDGE_logError("This geometric shape type (%d) is not supported",
Expand Down
58 changes: 37 additions & 21 deletions scene_graph/src/models/collision_standard_item.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <tesseract_qt/scene_graph/models/polygon_mesh_standard_item.h>
#include <tesseract_qt/scene_graph/models/octree_standard_item.h>
#include <tesseract_qt/common/models/transform_standard_item.h>
#include <tesseract_qt/common/models/type_standard_item.h>
#include <tesseract_qt/common/models/standard_item_utils.h>
#include <tesseract_qt/common/models/standard_item_type.h>
#include <tesseract_qt/common/icon_utils.h>
Expand Down Expand Up @@ -62,66 +63,81 @@ CollisionStandardItem::CollisionStandardItem(const QIcon& icon,

int CollisionStandardItem::type() const { return static_cast<int>(StandardItemType::SG_COLLISION); }

void CollisionStandardItem::ctor()
inline QStandardItem* getGeometryItem(const std::shared_ptr<const tesseract_geometry::Geometry>& geometry)
{
appendRow(createStandardItemString("name", collision->name));
appendRow(new TransformStandardItem(collision->origin));

QStandardItem* geometry_item;
switch (collision->geometry->getType())
QStandardItem* geometry_item{ nullptr };
switch (geometry->getType())
{
case tesseract_geometry::GeometryType::BOX:
{
geometry_item = new BoxStandardItem(std::static_pointer_cast<const tesseract_geometry::Box>(collision->geometry));
geometry_item = new BoxStandardItem(std::static_pointer_cast<const tesseract_geometry::Box>(geometry));
break;
}
case tesseract_geometry::GeometryType::CAPSULE:
{
geometry_item =
new CapsuleStandardItem(std::static_pointer_cast<const tesseract_geometry::Capsule>(collision->geometry));
geometry_item = new CapsuleStandardItem(std::static_pointer_cast<const tesseract_geometry::Capsule>(geometry));
break;
}
case tesseract_geometry::GeometryType::CONE:
{
geometry_item =
new ConeStandardItem(std::static_pointer_cast<const tesseract_geometry::Cone>(collision->geometry));
geometry_item = new ConeStandardItem(std::static_pointer_cast<const tesseract_geometry::Cone>(geometry));
break;
}
case tesseract_geometry::GeometryType::CYLINDER:
{
geometry_item =
new CylinderStandardItem(std::static_pointer_cast<const tesseract_geometry::Cylinder>(collision->geometry));
geometry_item = new CylinderStandardItem(std::static_pointer_cast<const tesseract_geometry::Cylinder>(geometry));
break;
}
case tesseract_geometry::GeometryType::PLANE:
{
geometry_item =
new PlaneStandardItem(std::static_pointer_cast<const tesseract_geometry::Plane>(collision->geometry));
geometry_item = new PlaneStandardItem(std::static_pointer_cast<const tesseract_geometry::Plane>(geometry));
break;
}
case tesseract_geometry::GeometryType::SPHERE:
{
geometry_item =
new SphereStandardItem(std::static_pointer_cast<const tesseract_geometry::Sphere>(collision->geometry));
geometry_item = new SphereStandardItem(std::static_pointer_cast<const tesseract_geometry::Sphere>(geometry));
break;
}
case tesseract_geometry::GeometryType::OCTREE:
{
geometry_item =
new OctreeStandardItem(std::static_pointer_cast<const tesseract_geometry::Octree>(collision->geometry));
geometry_item = new OctreeStandardItem(std::static_pointer_cast<const tesseract_geometry::Octree>(geometry));
break;
}
case tesseract_geometry::GeometryType::CONVEX_MESH:
case tesseract_geometry::GeometryType::MESH:
case tesseract_geometry::GeometryType::POLYGON_MESH:
case tesseract_geometry::GeometryType::SDF_MESH:
{
geometry_item = new PolygonMeshStandardItem(
std::static_pointer_cast<const tesseract_geometry::PolygonMesh>(collision->geometry));
geometry_item =
new PolygonMeshStandardItem(std::static_pointer_cast<const tesseract_geometry::PolygonMesh>(geometry));
break;
}
case tesseract_geometry::GeometryType::COMPOUND_MESH:
{
geometry_item = new TypeStandardItem(
icons::getVisualVectorIcon(), "CompoundMesh", static_cast<int>(StandardItemType::SG_COMPOUND_MESH));
auto meshes = std::static_pointer_cast<const tesseract_geometry::CompoundMesh>(geometry)->getMeshes();
for (const auto& mesh : meshes)
geometry_item->appendRow(getGeometryItem(mesh));

if (meshes.front()->getType() == tesseract_geometry::GeometryType::MESH ||
meshes.front()->getType() == tesseract_geometry::GeometryType::SDF_MESH)
geometry_item->setIcon(icons::getMeshIcon());
else if (meshes.front()->getType() == tesseract_geometry::GeometryType::CONVEX_MESH)
geometry_item->setIcon(icons::getConvexMeshIcon());
}
}
return geometry_item;
}

void CollisionStandardItem::ctor()
{
appendRow(createStandardItemString("name", collision->name));
appendRow(new TransformStandardItem(collision->origin));

QStandardItem* geometry_item = getGeometryItem(collision->geometry);
setIcon(geometry_item->icon());

appendRow(geometry_item);
}
} // namespace tesseract_gui
Loading

0 comments on commit fbb9024

Please sign in to comment.