From 5c6b683f8de3fae81e62471f780f35490e18d990 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 12 Dec 2021 19:41:53 -0700 Subject: [PATCH] Fix bug in how geometry octree are converted from message and visualized --- tesseract_rosutils/src/utils.cpp | 1 + .../src/render_tools/link_widget.cpp | 76 ++++++++++--------- 2 files changed, 41 insertions(+), 36 deletions(-) diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp index 0defbcde0..df7705b84 100644 --- a/tesseract_rosutils/src/utils.cpp +++ b/tesseract_rosutils/src/utils.cpp @@ -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(octree.getSubType()); break; } case tesseract_geometry::GeometryType::MESH: diff --git a/tesseract_rviz/src/render_tools/link_widget.cpp b/tesseract_rviz/src/render_tools/link_widget.cpp index c6385bdc3..9b1bcbf25 100644 --- a/tesseract_rviz/src/render_tools/link_widget.cpp +++ b/tesseract_rviz/src/render_tools/link_widget.cpp @@ -1065,42 +1065,46 @@ bool LinkWidget::createEntityForGeometryElement(const tesseract_scene_graph::Lin // voxels if ((static_cast(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(nKey[2] - 1); - allNeighborsFound && key[2] <= static_cast(nKey[2] + 1); - ++key[2]) - { - for (key[1] = static_cast(nKey[1] - 1); - allNeighborsFound && key[1] <= static_cast(nKey[1] + 1); - ++key[1]) - { - for (key[0] = static_cast(nKey[0] - 1); - allNeighborsFound && key[0] <= static_cast(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(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(nKey[2] - 1); + // allNeighborsFound && key[2] <= static_cast(nKey[2] + 1); + // ++key[2]) + // { + // for (key[1] = static_cast(nKey[1] - 1); + // allNeighborsFound && key[1] <= static_cast(nKey[1] + 1); + // ++key[1]) + // { + // for (key[0] = static_cast(nKey[0] - 1); + // allNeighborsFound && key[0] <= static_cast(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(octree->isNodeOccupied(node)) + 1) & + // render_mode_mask)) + // { + // // we do not have a neighbor => break! + // allNeighborsFound = false; + // } + // } + // } + // } + // } + + // display_voxel |= !allNeighborsFound; } if (display_voxel)