diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index 41ca0d023..4dc337ebb 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -14,9 +14,9 @@ namespace wavemap::rviz_plugin { struct CellSelectionMode : public TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kSurface, kBand }; + enum Id : TypeId { kSurface, kBoundary, kBand }; - static constexpr std::array names = {"Surface", "Band"}; + static constexpr std::array names = {"Surface", "Boundary", "Band"}; }; class CellSelector : public QObject { @@ -35,6 +35,7 @@ class CellSelector : public QObject { return std::abs(log_odds) < unknown_occupancy_threshold_; } bool hasFreeNeighbor(const OctreeIndex& cell_index) const; + bool hasFreeOrUnknownNeighbor(const OctreeIndex& cell_index) const; private Q_SLOTS: // NOLINT // These Qt slots get connected to signals indicating changes in the diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 9188a7e8e..37c506c65 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -71,17 +71,29 @@ void CellSelector::setMap(const MapBase::ConstPtr& map) { bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const { switch (cell_selection_mode_) { + default: case CellSelectionMode::kSurface: // Skip free cells if (cell_log_odds < surface_occupancy_threshold_ || isUnknown(cell_log_odds)) { return false; } - // Skip cells that are occluded by neighbors on all sides + // Skip cells that would not be visible in real life if (!hasFreeNeighbor(cell_index)) { return false; } break; + case CellSelectionMode::kBoundary: + // Skip free cells + if (cell_log_odds < surface_occupancy_threshold_ || + isUnknown(cell_log_odds)) { + return false; + } + // Skip cells that are occluded by neighbors on all sides in Rviz + if (!hasFreeOrUnknownNeighbor(cell_index)) { + return false; + } + break; case CellSelectionMode::kBand: // Skip cells that don't meet the occupancy threshold if (cell_log_odds < band_min_occupancy_threshold_ || @@ -109,6 +121,21 @@ bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { return false; } +bool CellSelector::hasFreeOrUnknownNeighbor( + const OctreeIndex& cell_index) const { + for (const auto& offset : kNeighborOffsets) { // NOLINT + const OctreeIndex neighbor_index = {cell_index.height, + cell_index.position + offset}; + const FloatingPoint neighbor_log_odds = + query_accelerator_->getCellValue(neighbor_index); + // Check if the neighbor is free and observed + if (neighbor_log_odds < surface_occupancy_threshold_) { + return true; + } + } + return false; +} + void CellSelector::cellSelectionModeUpdateCallback() { // Update the cached cell selection mode value const CellSelectionMode old_cell_selection_mode = cell_selection_mode_;