Skip to content

Commit

Permalink
Fix bug in how geometry octree are converted from message and visualized
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 13, 2021
1 parent edd74b9 commit 5c6b683
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 36 deletions.
1 change: 1 addition & 0 deletions tesseract_rosutils/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,7 @@ bool toMsg(tesseract_msgs::Geometry& geometry_msgs, const tesseract_geometry::Ge

geometry_msgs.type = tesseract_msgs::Geometry::OCTREE;
octomap_msgs::fullMapToMsg(*(octree.getOctree()), geometry_msgs.octomap);
geometry_msgs.octomap_sub_type.type = static_cast<uint8_t>(octree.getSubType());
break;
}
case tesseract_geometry::GeometryType::MESH:
Expand Down
76 changes: 40 additions & 36 deletions tesseract_rviz/src/render_tools/link_widget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1065,42 +1065,46 @@ bool LinkWidget::createEntityForGeometryElement(const tesseract_scene_graph::Lin
// voxels
if ((static_cast<unsigned>(octree->isNodeOccupied(*it)) + 1) & render_mode_mask)
{
// check if current voxel has neighbors on all sides -> no need to be
// displayed
bool allNeighborsFound = true;

octomap::OcTreeKey key;
octomap::OcTreeKey nKey = it.getKey();

for (key[2] = static_cast<octomap::key_type>(nKey[2] - 1);
allNeighborsFound && key[2] <= static_cast<octomap::key_type>(nKey[2] + 1);
++key[2])
{
for (key[1] = static_cast<octomap::key_type>(nKey[1] - 1);
allNeighborsFound && key[1] <= static_cast<octomap::key_type>(nKey[1] + 1);
++key[1])
{
for (key[0] = static_cast<octomap::key_type>(nKey[0] - 1);
allNeighborsFound && key[0] <= static_cast<octomap::key_type>(nKey[0] + 1);
++key[0])
{
if (key != nKey)
{
octomap::OcTreeNode* node = octree->search(key);

// the left part evaluates to 1 for free voxels and 2 for
// occupied voxels
if (!(node && (static_cast<unsigned>(octree->isNodeOccupied(node)) + 1) & render_mode_mask))
{
// we do not have a neighbor => break!
allNeighborsFound = false;
}
}
}
}
}

display_voxel |= !allNeighborsFound;
// Disabling because it does not work correctly with a pruned octomap
display_voxel = true;

// // check if current voxel has neighbors on all sides -> no need to be
// // displayed
// bool allNeighborsFound = true;

// octomap::OcTreeKey key;
// octomap::OcTreeKey nKey = it.getKey();

// for (key[2] = static_cast<octomap::key_type>(nKey[2] - 1);
// allNeighborsFound && key[2] <= static_cast<octomap::key_type>(nKey[2] + 1);
// ++key[2])
// {
// for (key[1] = static_cast<octomap::key_type>(nKey[1] - 1);
// allNeighborsFound && key[1] <= static_cast<octomap::key_type>(nKey[1] + 1);
// ++key[1])
// {
// for (key[0] = static_cast<octomap::key_type>(nKey[0] - 1);
// allNeighborsFound && key[0] <= static_cast<octomap::key_type>(nKey[0] + 1);
// ++key[0])
// {
// if (key != nKey)
// {
// octomap::OcTreeNode* node = octree->search(key);

// // the left part evaluates to 1 for free voxels and 2 for
// // occupied voxels
// if (!(node && (static_cast<unsigned>(octree->isNodeOccupied(node)) + 1) &
// render_mode_mask))
// {
// // we do not have a neighbor => break!
// allNeighborsFound = false;
// }
// }
// }
// }
// }

// display_voxel |= !allNeighborsFound;
}

if (display_voxel)
Expand Down

0 comments on commit 5c6b683

Please sign in to comment.