diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 458f3bc..e1018ee 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -35,6 +35,7 @@ add_library( src/hydra_ros_pipeline.cpp src/active_window/reconstruction_visualizer.cpp src/active_window/tsdf_occupancy_publisher.cpp + src/active_window/tsdf_gradient_occupancy_publisher.cpp src/backend/ros_backend_publisher.cpp src/backend/gt_room_publisher.cpp src/frontend/gvd_occupancy_publisher.cpp diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h new file mode 100644 index 0000000..b2fc137 --- /dev/null +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -0,0 +1,151 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace hydra { + +using Index2D = Eigen::Vector2i; + +struct Index2DHash { + inline static const auto s = Index2D(1, 1290); + int operator()(const Index2D& index) const { return index.dot(s); } +}; + +template +using Index2DMap = + std::unordered_map, + Eigen::aligned_allocator>>; + +struct GradientInfo { + float gradient = 0.0f; // mean gradient magnitude + float confidence = 0.0f; // num_neighbors / 8.0 +}; + +class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { + public: + struct Config { + // Base occupancy config fields + std::string ns = "~/tsdf_gradient"; + bool collate = false; + bool use_relative_height = true; + double slice_height = -1.0; + size_t num_slices = 20; // if if 10 cm, we want 2 m from -1 m to +1 m + bool add_robot_footprint = false; + Eigen::Vector3f footprint_min = Eigen::Vector3f::Zero(); + Eigen::Vector3f footprint_max = Eigen::Vector3f::Zero(); + + // Gradient-specific parameters + float gradient_threshold = 0.5f; // m/m - max traversable gradient + float min_weight = 1.0e-6f; // min TSDF weight for observed voxel + float min_confidence = 0.5f; // min confidence (neighbors/8) for valid cell + bool smoothing = true; // apply box filter to reduce TSDF ripple + bool probabilistic = false; // continuous vs binary occupancy + bool filter_disjoint = false; // remove free space not connected to robot + } const config; + + explicit TsdfGradientOccupancyPublisher(const Config& config); + + virtual ~TsdfGradientOccupancyPublisher() = default; + + std::string printInfo() const override; + + void call(uint64_t timestamp_ns, + const VolumetricMap& map, + const ActiveWindowOutput& output) const override; + + private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr height_map_pub_; + rclcpp::Publisher::SharedPtr gradient_map_pub_; + + // Helper functions + std::optional extractSurfaceHeight(const TsdfLayer& layer, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, + float min_z, + float max_z) const; + + void buildHeightMap(const TsdfLayer& layer, + float min_z, + float max_z, + Index2DMap& height_map) const; + + void computeGradientMap(const Index2DMap& height_map, + float voxel_size, + Index2DMap& gradient_map) const; + + void fillOccupancyGrid(const Index2DMap& gradient_map, + const Eigen::Isometry3d& world_T_sensor, + const TsdfLayer& layer, + nav_msgs::msg::OccupancyGrid& msg) const; + + void filterDisjointFreeSpace(nav_msgs::msg::OccupancyGrid& msg, + const Eigen::Isometry3d& world_T_body) const; + + void publishHeightMapViz(const Index2DMap& height_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const; + + void publishGradientMapViz(const Index2DMap& gradient_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const; + + float computeHorizontalDistance(const Index2D& offset, float voxel_size) const; + + float computeTraversabilityFromGradient(float gradient) const; + + int8_t gradientToOccupancy(float gradient, float confidence) const; + + // 8-way neighbor offsets + static const std::array kNeighborOffsets; +}; + +void declare_config(TsdfGradientOccupancyPublisher::Config& config); + +} // namespace hydra diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp new file mode 100644 index 0000000..8835bbb --- /dev/null +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -0,0 +1,637 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace hydra { + +namespace { + +// 8-way neighbor offsets (cardinal + diagonal) +const std::array kNeighborOffsetsArray = {{ + {0, -1}, // bottom + {-1, 0}, // left + {0, 1}, // top + {1, 0}, // right + {-1, -1}, // bottom-left + {-1, 1}, // top-left + {1, 1}, // top-right + {1, -1} // bottom-right +}}; + +} // namespace + +const std::array TsdfGradientOccupancyPublisher::kNeighborOffsets = + kNeighborOffsetsArray; + +void declare_config(TsdfGradientOccupancyPublisher::Config& config) { + using namespace config; + name("TsdfGradientOccupancyPublisher::Config"); + field(config.ns, "ns"); + field(config.collate, "collate"); + field(config.use_relative_height, "use_relative_height"); + field(config.slice_height, "slice_height", "m"); + field(config.num_slices, "num_slices"); + field(config.add_robot_footprint, "add_robot_footprint"); + field(config.footprint_min, "footprint_min"); + field(config.footprint_max, "footprint_max"); + field(config.gradient_threshold, "gradient_threshold"); + field(config.min_weight, "min_weight"); + field(config.min_confidence, "min_confidence"); + field(config.smoothing, "smoothing"); + field(config.probabilistic, "probabilistic"); + field(config.filter_disjoint, "filter_disjoint"); + + checkCondition(config.gradient_threshold > 0.0f, + "gradient_threshold must be positive"); + checkInRange(config.min_confidence, 0.0f, 1.0f, "min_confidence"); +} + +TsdfGradientOccupancyPublisher::TsdfGradientOccupancyPublisher(const Config& config) + : config(config::checkValid(config)), + pub_(ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "occupancy", rclcpp::QoS(1).transient_local())), + height_map_pub_(ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "height_map_debug", rclcpp::QoS(1).transient_local())), + gradient_map_pub_( + ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "gradient_map_debug", rclcpp::QoS(1).transient_local())) {} + +std::string TsdfGradientOccupancyPublisher::printInfo() const { + return config::toString(config); +} + +void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, + const VolumetricMap& map, + const ActiveWindowOutput& output) const { + if (!pub_->get_subscription_count() && !height_map_pub_->get_subscription_count() && + !gradient_map_pub_->get_subscription_count()) { + return; + } + + const auto& tsdf_layer = map.getTsdfLayer(); + const auto& world_T_body = output.world_T_body(); + + // Compute vertical range + double base_z = config.slice_height; + if (config.use_relative_height) { + base_z += world_T_body.translation().z(); + } + + const float voxel_size = tsdf_layer.voxel_size; + const float min_z = static_cast(base_z); + const float max_z = static_cast(base_z + config.num_slices * voxel_size); + + // Build height map (Pass 1) + Index2DMap height_map; + buildHeightMap(tsdf_layer, min_z, max_z, height_map); + publishHeightMapViz(height_map, tsdf_layer, timestamp_ns); + + // Compute gradient map (Pass 2) + Index2DMap gradient_map; + computeGradientMap(height_map, voxel_size, gradient_map); + publishGradientMapViz(gradient_map, tsdf_layer, timestamp_ns); + + // Fill and publish occupancy grid (Pass 3) + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + + fillOccupancyGrid(gradient_map, world_T_body, tsdf_layer, msg); + if (config.filter_disjoint) { + filterDisjointFreeSpace(msg, world_T_body); + } + pub_->publish(msg); +} + +std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( + const TsdfLayer& layer, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, + float min_z, + float max_z) const { + // Uses block/local index access directly (same pattern as + // GradientTraversabilityEstimator) to avoid the signed/unsigned division bug in + // spatial_hash::blockIndexFromGlobalIndex, which corrupts getVoxelPtr lookups for + // negative world coordinates. + const float voxel_size = layer.voxel_size; + const int vps = static_cast(layer.voxels_per_side); + + // Get vertical range in voxel coordinates + const auto min_key = layer.getVoxelKey(spatial_hash::Point(0, 0, min_z)); + const auto max_key = layer.getVoxelKey(spatial_hash::Point(0, 0, max_z)); + + // Scan from top to bottom to find highest surface + for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { + const auto tsdf_block = + layer.getBlockPtr(BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z)); + if (!tsdf_block) { + continue; + } + + const int min_voxel_z = block_z == min_key.first.z() ? min_key.second.z() : 0; + const int max_voxel_z = block_z == max_key.first.z() ? max_key.second.z() : vps - 1; + + for (int z = max_voxel_z; z >= min_voxel_z; --z) { + const auto& voxel = + tsdf_block->getVoxel(VoxelIndex(local_2d.x(), local_2d.y(), z)); + + if (voxel.weight < config.min_weight) { + continue; + } + + if (voxel.distance < voxel_size) { + const VoxelKey key(BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z), + VoxelIndex(local_2d.x(), local_2d.y(), z)); + return layer.getVoxelPosition(key).z(); + } + } + } + + return std::nullopt; +} + +void TsdfGradientOccupancyPublisher::buildHeightMap( + const TsdfLayer& layer, + float min_z, + float max_z, + Index2DMap& height_map) const { + const int vps = static_cast(layer.voxels_per_side); + + // Deduplicate to unique 2D blocks (exact pattern from + // GradientTraversabilityEstimator:293) + spatial_hash::IndexSet blocks_2d; + for (const auto& block_index : layer.allocatedBlockIndices()) { + blocks_2d.emplace(block_index.x(), block_index.y(), 0); + } + + // Process each 2D column ONCE (matches GradientTraversabilityEstimator:294-307) + for (const auto& block_idx_2d : blocks_2d) { + for (int x = 0; x < vps; ++x) { + for (int y = 0; y < vps; ++y) { + std::optional surface_height = extractSurfaceHeight( + layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z); + + if (surface_height) { + const Index2D key(block_idx_2d.x() * vps + x, block_idx_2d.y() * vps + y); + height_map[key] = *surface_height; + } + } + } + } +} + +void TsdfGradientOccupancyPublisher::computeGradientMap( + const Index2DMap& height_map, + float voxel_size, + Index2DMap& gradient_map) const { + // Optional smoothing pass + Index2DMap smoothed_height_map; + if (config.smoothing) { + smoothed_height_map.reserve(height_map.size()); + for (const auto& [center_idx, center_height] : height_map) { + float height_sum = center_height; + int count = 1; + + for (const auto& offset : kNeighborOffsets) { + const Index2D neighbor_idx(center_idx.x() + offset.x(), + center_idx.y() + offset.y()); + auto it = height_map.find(neighbor_idx); + if (it != height_map.end()) { + height_sum += it->second; + count++; + } + } + + smoothed_height_map[center_idx] = height_sum / count; + } + } + + const auto& grad_height_map = config.smoothing ? smoothed_height_map : height_map; + + // Compute gradients + gradient_map.reserve(grad_height_map.size()); + for (const auto& [center_idx, center_height] : grad_height_map) { + float gradient_sum = 0.0f; + int num_neighbors_observed = 0; + + for (const auto& offset : kNeighborOffsets) { + const Index2D neighbor_idx(center_idx.x() + offset.x(), + center_idx.y() + offset.y()); + + auto neighbor_it = grad_height_map.find(neighbor_idx); + if (neighbor_it == grad_height_map.end()) { + continue; + } + + num_neighbors_observed++; + + const float height_diff = std::abs(neighbor_it->second - center_height); + const float horiz_dist = computeHorizontalDistance(offset, voxel_size); + gradient_sum += height_diff / horiz_dist; + } + + GradientInfo info; + info.gradient = + num_neighbors_observed > 0 ? gradient_sum / num_neighbors_observed : 0.0f; + info.confidence = num_neighbors_observed / 8.0f; + gradient_map[center_idx] = info; + } +} + +void TsdfGradientOccupancyPublisher::fillOccupancyGrid( + const Index2DMap& gradient_map, + const Eigen::Isometry3d& world_T_sensor, + const TsdfLayer& layer, + nav_msgs::msg::OccupancyGrid& msg) const { + if (gradient_map.empty()) { + return; + } + + // Compute bounds using block origins (matches getLayerBounds pattern) + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); + } + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = config.slice_height; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Setup robot footprint if needed + spark_dsg::BoundingBox bbox; + Eigen::Isometry3f sensor_T_world; + if (config.add_robot_footprint) { + bbox = spark_dsg::BoundingBox(config.footprint_min, config.footprint_max); + sensor_T_world = world_T_sensor.inverse().cast(); + } + + // Fill occupancy grid + for (const auto& [global_idx, gradient_info] : gradient_map) { + // Convert global 2D index to 3D, get voxel key, then actual position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Check robot footprint + if (config.add_robot_footprint) { + const Eigen::Vector3f pos_3d(pos.x(), pos.y(), 0.0f); + if (bbox.contains((sensor_T_world * pos_3d).eval())) { + msg.data[index] = 0; + continue; + } + } + + // Map gradient to occupancy + msg.data[index] = + gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); + } +} + +void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( + nav_msgs::msg::OccupancyGrid& msg, const Eigen::Isometry3d& world_T_body) const { + if (msg.data.empty()) { + return; + } + + const int width = static_cast(msg.info.width); + const int height = static_cast(msg.info.height); + + // Convert robot world position to grid coordinates + const auto robot_pos_world = world_T_body.translation(); + const float rel_x = + static_cast(robot_pos_world.x() - msg.info.origin.position.x); + const float rel_y = + static_cast(robot_pos_world.y() - msg.info.origin.position.y); + const int robot_r = static_cast(std::floor(rel_y / msg.info.resolution)); + const int robot_c = static_cast(std::floor(rel_x / msg.info.resolution)); + + // Check if robot is within grid bounds + if (robot_r < 0 || robot_r >= height || robot_c < 0 || robot_c >= width) { + return; // Robot outside grid - skip filtering + } + + const size_t robot_index = robot_r * width + robot_c; + + // Only filter if robot is in free space + if (msg.data[robot_index] != 0) { + return; // Robot not in free space - skip filtering + } + + // BFS flood fill from robot position + std::vector visited(msg.data.size(), false); + std::queue to_visit; + + to_visit.push(robot_index); + visited[robot_index] = true; + + while (!to_visit.empty()) { + const size_t current_index = to_visit.front(); + to_visit.pop(); + + const int r = current_index / width; + const int c = current_index % width; + + // Check 4-connected neighbors (up, down, left, right) + const std::array, 4> neighbors = { + {{r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1}}}; + + for (const auto& [nr, nc] : neighbors) { + // Check bounds + if (nr < 0 || nr >= height || nc < 0 || nc >= width) { + continue; + } + + const size_t neighbor_index = nr * width + nc; + + // Skip if already visited + if (visited[neighbor_index]) { + continue; + } + + // Only expand through free cells (value 0) + if (msg.data[neighbor_index] == 0) { + visited[neighbor_index] = true; + to_visit.push(neighbor_index); + } + } + } + + // Mark unvisited free cells as occupied + for (size_t i = 0; i < msg.data.size(); ++i) { + if (msg.data[i] == 0 && !visited[i]) { + msg.data[i] = 100; // Mark as occupied + } + } +} + +float TsdfGradientOccupancyPublisher::computeHorizontalDistance( + const Index2D& offset, float voxel_size) const { + // Diagonal: sqrt(2) * voxel_size + if (offset.x() != 0 && offset.y() != 0) { + return voxel_size * std::sqrt(2.0f); + } + + // Cardinal: voxel_size + return voxel_size; +} + +float TsdfGradientOccupancyPublisher::computeTraversabilityFromGradient( + float gradient) const { + if (gradient >= config.gradient_threshold) { + return 0.0f; // Intraversable + } + + // Linear interpolation: 1.0 at gradient=0, 0.0 at gradient=threshold + return 1.0f - (gradient / config.gradient_threshold); +} + +int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, + float confidence) const { + // Check confidence threshold + if (confidence < config.min_confidence) { + return -1; // Unknown + } + + if (config.probabilistic) { + // Continuous mode: map traversability to occupancy + const float traversability = computeTraversabilityFromGradient(gradient); + const float occupancy_float = (1.0f - traversability) * 100.0f; + return static_cast(std::clamp(occupancy_float, 0.0f, 100.0f)); + } else { + // Binary mode: threshold-based + return gradient >= config.gradient_threshold ? 100 : 0; + } +} + +void TsdfGradientOccupancyPublisher::publishHeightMapViz( + const Index2DMap& height_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const { + if (!height_map_pub_->get_subscription_count()) { + return; + } + + if (height_map.empty()) { + return; + } + + // Find min/max heights for normalization + float min_height = std::numeric_limits::max(); + float max_height = std::numeric_limits::lowest(); + for (const auto& [_, height] : height_map) { + min_height = std::min(min_height, height); + max_height = std::max(max_height, height); + } + + // Compute bounds using block origins (matches getLayerBounds pattern) + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); + } + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Fill grid with normalized heights + const float height_range = max_height - min_height; + for (const auto& [global_idx, height] : height_map) { + // Convert global 2D index to actual voxel position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Normalize height to 0-100 range + if (height_range > 1e-6f) { + const float normalized = (height - min_height) / height_range * 100.0f; + msg.data[index] = static_cast(std::clamp(normalized, 0.0f, 100.0f)); + } else { + msg.data[index] = 50; // All same height -> mid-gray + } + } + + height_map_pub_->publish(msg); +} + +void TsdfGradientOccupancyPublisher::publishGradientMapViz( + const Index2DMap& gradient_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const { + if (!gradient_map_pub_->get_subscription_count()) { + return; + } + + if (gradient_map.empty()) { + return; + } + + // Compute bounds using block origins (matches getLayerBounds pattern) + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); + } + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Fill grid with gradient values + for (const auto& [global_idx, gradient_info] : gradient_map) { + // Convert global 2D index to actual voxel position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Map gradient to 0-100 (using gradient_threshold as max) + // Low gradient (flat) -> 0 (free), high gradient (steep) -> 100 (occupied) + const float normalized = + std::min(gradient_info.gradient / config.gradient_threshold, 1.0f) * 100.0f; + msg.data[index] = static_cast(normalized); + } + + gradient_map_pub_->publish(msg); +} + +namespace { + +static const auto registration_ = + config::RegistrationWithConfig( + "TsdfGradientOccupancyPublisher"); + +} // namespace + +} // namespace hydra