From f71fdae747f6e2b40c6e7affdd7fd8558d006489 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Sat, 7 Mar 2026 09:52:18 -0500 Subject: [PATCH] feat: Depth-to-costmap plugin for RealSense D435i (Issue #532) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add saltybot_depth_costmap — a Nav2 costmap2d plugin that converts D435i depth images directly into obstacle markings on both local and global costmaps. Pipeline: 1. Subscribe to /camera/depth/image_rect_raw (16UC1 mm) + camera_info 2. Back-project depth pixels to 3D using pinhole camera intrinsics 3. Transform points to costmap global_frame via TF2 4. Apply configurable height filter (min_height..max_height above ground) 5. Mark obstacle cells as LETHAL_OBSTACLE 6. Inflate neighbours within inflation_radius as INSCRIBED_INFLATED_OBSTACLE Parameters: min_height: 0.05 m — floor clearance (ignores ground returns) max_height: 0.80 m — ceiling cutoff (ignores lights/ceiling) obstacle_range: 3.5 m — max marking distance from camera clearing_range: 4.0 m — max distance processed at all inflation_radius: 0.10 m — in-layer inflation (works before inflation_layer) downsample_factor: 4 — process 1 of N rows+cols (~19k pts @ 640×480) Integration (#478): - Added depth_costmap_layer to local_costmap plugins list - Added depth_costmap_layer to global_costmap plugins list - Plugin registered via pluginlib (plugin.xml) Files: jetson/ros2_ws/src/saltybot_depth_costmap/ CMakeLists.txt, package.xml, plugin.xml include/saltybot_depth_costmap/depth_costmap_layer.hpp src/depth_costmap_layer.cpp jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml (updated) Co-Authored-By: Claude Sonnet 4.6 --- .../saltybot_bringup/config/nav2_params.yaml | 39 ++- .../src/saltybot_depth_costmap/CMakeLists.txt | 78 +++++ .../depth_costmap_layer.hpp | 113 +++++++ .../src/saltybot_depth_costmap/package.xml | 30 ++ .../src/saltybot_depth_costmap/plugin.xml | 15 + .../src/depth_costmap_layer.cpp | 306 ++++++++++++++++++ 6 files changed, 576 insertions(+), 5 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt create mode 100644 jetson/ros2_ws/src/saltybot_depth_costmap/include/saltybot_depth_costmap/depth_costmap_layer.hpp create mode 100644 jetson/ros2_ws/src/saltybot_depth_costmap/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml create mode 100644 jetson/ros2_ws/src/saltybot_depth_costmap/src/depth_costmap_layer.cpp diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml index cd3ae04..e6d4cfa 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml @@ -10,12 +10,14 @@ # → No AMCL, no map_server needed. # # Sensors (Issue #478 — Costmap configuration): -# /scan — RPLIDAR A1M8 360° LaserScan (obstacle) -# /depth_scan — RealSense D435i depth_to_laserscan (obstacle) -# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel) +# /scan — RPLIDAR A1M8 360° LaserScan (obstacle) +# /depth_scan — RealSense D435i depth_to_laserscan (obstacle) +# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel) +# /camera/depth/image_rect_raw — RealSense D435i depth image (Issue #532 DepthCostmapLayer) # # Inflation: # inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding) +# DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer) # # Output: /cmd_vel (Twist) — STM32 bridge consumes this topic. @@ -256,7 +258,21 @@ local_costmap: resolution: 0.05 robot_radius: 0.15 footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m - plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] + plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"] + + # Issue #532: RealSense D435i depth-to-costmap plugin + # Point cloud projection with height filter + in-layer inflation + depth_costmap_layer: + plugin: "saltybot_depth_costmap::DepthCostmapLayer" + enabled: true + depth_topic: /camera/depth/image_rect_raw + camera_info_topic: /camera/depth/camera_info + min_height: 0.05 # m — ignore floor reflections + max_height: 0.80 # m — ignore ceiling/lights + obstacle_range: 3.5 # m — max range to mark obstacles + clearing_range: 4.0 # m — max range for clearing + inflation_radius: 0.10 # m — in-layer inflation (before inflation_layer) + downsample_factor: 4 # process 1 of 4 rows/cols (~19k pts @ 640x480) obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -327,7 +343,20 @@ global_costmap: footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + plugins: ["static_layer", "obstacle_layer", "depth_costmap_layer", "inflation_layer"] + + # Issue #532: RealSense D435i depth-to-costmap plugin (global costmap) + depth_costmap_layer: + plugin: "saltybot_depth_costmap::DepthCostmapLayer" + enabled: true + depth_topic: /camera/depth/image_rect_raw + camera_info_topic: /camera/depth/camera_info + min_height: 0.05 + max_height: 0.80 + obstacle_range: 3.5 + clearing_range: 4.0 + inflation_radius: 0.10 + downsample_factor: 4 static_layer: plugin: "nav2_costmap_2d::StaticLayer" diff --git a/jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt new file mode 100644 index 0000000..fb83e24 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_depth_costmap) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# ── Dependencies ────────────────────────────────────────────────────────────── +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(pluginlib REQUIRED) + +# ── Shared library (the plugin) ─────────────────────────────────────────────── +add_library(${PROJECT_NAME} SHARED + src/depth_costmap_layer.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + nav2_costmap_2d + sensor_msgs + geometry_msgs + tf2_ros + tf2 + pluginlib +) + +# Export plugin XML so pluginlib can discover it +pluginlib_export_plugin_description_file(nav2_costmap_2d plugin.xml) + +# ── Install ─────────────────────────────────────────────────────────────────── +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES plugin.xml + DESTINATION share/${PROJECT_NAME} +) + +# ── Tests ───────────────────────────────────────────────────────────────────── +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + rclcpp + nav2_costmap_2d + sensor_msgs + geometry_msgs + tf2_ros + tf2 + pluginlib +) + +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_depth_costmap/include/saltybot_depth_costmap/depth_costmap_layer.hpp b/jetson/ros2_ws/src/saltybot_depth_costmap/include/saltybot_depth_costmap/depth_costmap_layer.hpp new file mode 100644 index 0000000..ff82a5b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_depth_costmap/include/saltybot_depth_costmap/depth_costmap_layer.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace saltybot_depth_costmap +{ + +struct ObstaclePoint +{ + double wx; // world x (m) + double wy; // world y (m) +}; + +/** + * DepthCostmapLayer — Nav2 costmap2d plugin (Issue #532) + * + * Converts RealSense D435i depth images to a Nav2 costmap layer. + * + * Pipeline: + * 1. Subscribe to /camera/depth/image_rect_raw (16UC1, mm) + camera_info + * 2. Back-project depth pixels to 3D using camera intrinsics + * 3. Transform points to costmap global_frame via TF2 + * 4. Apply height filter (min_height .. max_height above ground) + * 5. Mark obstacle cells as LETHAL_OBSTACLE + * 6. Inflate nearby cells as INSCRIBED_INFLATED_OBSTACLE (in-layer inflation) + * + * Nav2 yaml usage: + * local_costmap: + * plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"] + * depth_costmap_layer: + * plugin: "saltybot_depth_costmap::DepthCostmapLayer" + * enabled: true + * depth_topic: /camera/depth/image_rect_raw + * camera_info_topic: /camera/depth/camera_info + * min_height: 0.05 # m — floor clearance + * max_height: 0.80 # m — ceiling cutoff + * obstacle_range: 3.5 # m — max marking range + * clearing_range: 4.0 # m — max clearing range + * inflation_radius: 0.10 # m — in-layer inflation radius + * downsample_factor: 4 # process 1 of N rows/cols + */ +class DepthCostmapLayer : public nav2_costmap_2d::Layer +{ +public: + DepthCostmapLayer(); + ~DepthCostmapLayer() override = default; + + void onInitialize() override; + + void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, double * max_y) override; + + void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) override; + + void reset() override; + bool isClearable() override { return true; } + +private: + void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg); + void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void processDepthImage(const sensor_msgs::msg::Image::SharedPtr & img); + + // ROS interfaces + rclcpp::Subscription::SharedPtr depth_sub_; + rclcpp::Subscription::SharedPtr info_sub_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Camera intrinsics (from camera_info) + double fx_{0.0}, fy_{0.0}, cx_{0.0}, cy_{0.0}; + bool have_camera_info_{false}; + std::string camera_frame_; + + // Plugin parameters + std::string depth_topic_; + std::string camera_info_topic_; + double min_height_{0.05}; // m — filter floor + double max_height_{0.80}; // m — filter ceiling + double obstacle_range_{3.5}; // m — max marking range (from camera) + double clearing_range_{4.0}; // m — max clearing range + double inflation_radius_{0.10}; // m — in-layer inflation + int downsample_factor_{4}; // skip N-1 of every N pixels + + // Latest processed obstacle points in world frame + std::vector obstacle_points_; + std::mutex points_mutex_; + bool need_update_{false}; + + // Dirty bounds for updateBounds() + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + // For throttled logging outside onInitialize + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace saltybot_depth_costmap diff --git a/jetson/ros2_ws/src/saltybot_depth_costmap/package.xml b/jetson/ros2_ws/src/saltybot_depth_costmap/package.xml new file mode 100644 index 0000000..8ead4c2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_depth_costmap/package.xml @@ -0,0 +1,30 @@ + + + + saltybot_depth_costmap + 0.1.0 + + Nav2 costmap2d plugin: DepthCostmapLayer (Issue #532). + Converts RealSense D435i depth images to a Nav2 costmap layer with + configurable height filtering and in-layer obstacle inflation. + + seb + MIT + + ament_cmake + + rclcpp + nav2_costmap_2d + sensor_msgs + geometry_msgs + tf2_ros + tf2 + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml b/jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml new file mode 100644 index 0000000..fb380ef --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml @@ -0,0 +1,15 @@ + + + + Nav2 costmap layer that converts RealSense D435i depth images into + obstacle markings. Back-projects depth pixels to 3D, transforms to the + costmap global frame via TF2, applies a configurable height filter + (min_height .. max_height), marks lethal obstacles, and inflates them + by inflation_radius within the layer. Integrates with existing Nav2 + costmap config (Issue #478). + + + diff --git a/jetson/ros2_ws/src/saltybot_depth_costmap/src/depth_costmap_layer.cpp b/jetson/ros2_ws/src/saltybot_depth_costmap/src/depth_costmap_layer.cpp new file mode 100644 index 0000000..cb24c56 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_depth_costmap/src/depth_costmap_layer.cpp @@ -0,0 +1,306 @@ +#include "saltybot_depth_costmap/depth_costmap_layer.hpp" + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "tf2/exceptions.h" + +PLUGINLIB_EXPORT_CLASS( + saltybot_depth_costmap::DepthCostmapLayer, + nav2_costmap_2d::Layer) + +namespace saltybot_depth_costmap +{ + +using nav2_costmap_2d::FREE_SPACE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +DepthCostmapLayer::DepthCostmapLayer() +: last_min_x_(-1e9), last_min_y_(-1e9), + last_max_x_(1e9), last_max_y_(1e9), + logger_(rclcpp::get_logger("DepthCostmapLayer")) +{} + +// ── onInitialize ────────────────────────────────────────────────────────────── + +void DepthCostmapLayer::onInitialize() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error("DepthCostmapLayer: node handle expired"); + } + + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("depth_topic", rclcpp::ParameterValue( + std::string("/camera/depth/image_rect_raw"))); + declareParameter("camera_info_topic", rclcpp::ParameterValue( + std::string("/camera/depth/camera_info"))); + declareParameter("min_height", rclcpp::ParameterValue(0.05)); + declareParameter("max_height", rclcpp::ParameterValue(0.80)); + declareParameter("obstacle_range", rclcpp::ParameterValue(3.5)); + declareParameter("clearing_range", rclcpp::ParameterValue(4.0)); + declareParameter("inflation_radius", rclcpp::ParameterValue(0.10)); + declareParameter("downsample_factor", rclcpp::ParameterValue(4)); + + enabled_ = node->get_parameter(name_ + ".enabled").as_bool(); + depth_topic_ = node->get_parameter(name_ + ".depth_topic").as_string(); + camera_info_topic_ = node->get_parameter(name_ + ".camera_info_topic").as_string(); + min_height_ = node->get_parameter(name_ + ".min_height").as_double(); + max_height_ = node->get_parameter(name_ + ".max_height").as_double(); + obstacle_range_ = node->get_parameter(name_ + ".obstacle_range").as_double(); + clearing_range_ = node->get_parameter(name_ + ".clearing_range").as_double(); + inflation_radius_ = node->get_parameter(name_ + ".inflation_radius").as_double(); + downsample_factor_ = node->get_parameter(name_ + ".downsample_factor").as_int(); + + if (downsample_factor_ < 1) downsample_factor_ = 1; + + RCLCPP_INFO( + logger_, + "DepthCostmapLayer '%s': depth=%s height=[%.2f, %.2f]m range=%.1fm inflate=%.2fm ds=%d", + name_.c_str(), depth_topic_.c_str(), + min_height_, max_height_, obstacle_range_, inflation_radius_, downsample_factor_); + + // TF2 + tf_buffer_ = std::make_shared(clock_); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Subscriptions + depth_sub_ = node->create_subscription( + depth_topic_, rclcpp::SensorDataQoS(), + std::bind(&DepthCostmapLayer::depthCallback, this, std::placeholders::_1)); + + info_sub_ = node->create_subscription( + camera_info_topic_, rclcpp::QoS(1).reliable(), + std::bind(&DepthCostmapLayer::cameraInfoCallback, this, std::placeholders::_1)); + + current_ = true; +} + +// ── cameraInfoCallback ──────────────────────────────────────────────────────── + +void DepthCostmapLayer::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + if (have_camera_info_) return; // Intrinsics are stable — only need once + + // Pinhole intrinsics from K matrix (row-major 3x3) + fx_ = msg->k[0]; // K[0,0] + fy_ = msg->k[4]; // K[1,1] + cx_ = msg->k[2]; // K[0,2] + cy_ = msg->k[5]; // K[1,2] + camera_frame_ = msg->header.frame_id; + + if (fx_ <= 0.0 || fy_ <= 0.0) { + RCLCPP_WARN(logger_, "DepthCostmapLayer: invalid camera intrinsics (fx=%.1f fy=%.1f)", fx_, fy_); + return; + } + + have_camera_info_ = true; + RCLCPP_INFO( + logger_, + "DepthCostmapLayer: camera_info received — frame=%s fx=%.1f fy=%.1f cx=%.1f cy=%.1f", + camera_frame_.c_str(), fx_, fy_, cx_, cy_); +} + +// ── depthCallback / processDepthImage ──────────────────────────────────────── + +void DepthCostmapLayer::depthCallback( + const sensor_msgs::msg::Image::SharedPtr msg) +{ + if (!enabled_ || !have_camera_info_) return; + processDepthImage(msg); +} + +void DepthCostmapLayer::processDepthImage( + const sensor_msgs::msg::Image::SharedPtr & img) +{ + // Resolve encoding + bool is_16u = (img->encoding == "16UC1" || img->encoding == "mono16"); + bool is_32f = (img->encoding == "32FC1"); + if (!is_16u && !is_32f) { + RCLCPP_WARN_ONCE(logger_, "DepthCostmapLayer: unsupported encoding '%s'", img->encoding.c_str()); + return; + } + + // Get TF from camera → costmap global frame (latest available) + const std::string global_frame = layered_costmap_->getGlobalFrameID(); + geometry_msgs::msg::TransformStamped tf_stamped; + try { + tf_stamped = tf_buffer_->lookupTransform( + global_frame, camera_frame_, + rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.1)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, + "DepthCostmapLayer: TF %s→%s failed: %s", + camera_frame_.c_str(), global_frame.c_str(), ex.what()); + return; + } + + // Build rotation matrix from quaternion (camera frame → world frame) + // p_world = R * p_cam + t + const auto & t = tf_stamped.transform.translation; + const auto & q = tf_stamped.transform.rotation; + double qw = q.w, qx = q.x, qy = q.y, qz = q.z; + + double R[3][3] = { + {1.0 - 2.0*(qy*qy + qz*qz), 2.0*(qx*qy - qz*qw), 2.0*(qx*qz + qy*qw)}, + { 2.0*(qx*qy + qz*qw), 1.0 - 2.0*(qx*qx + qz*qz), 2.0*(qy*qz - qx*qw)}, + { 2.0*(qx*qz - qy*qw), 2.0*(qy*qz + qx*qw), 1.0 - 2.0*(qx*qx + qy*qy)} + }; + + int rows = static_cast(img->height); + int cols = static_cast(img->width); + int step = static_cast(img->step); + + const double obstacle_range_sq = obstacle_range_ * obstacle_range_; + + std::vector new_points; + new_points.reserve(512); + + for (int v = 0; v < rows; v += downsample_factor_) { + const uint8_t * row_ptr = img->data.data() + v * step; + + for (int u = 0; u < cols; u += downsample_factor_) { + double depth_m = 0.0; + + if (is_16u) { + uint16_t raw; + std::memcpy(&raw, row_ptr + u * 2, sizeof(raw)); + if (raw == 0) continue; + depth_m = raw * 0.001; // mm → m + } else { + float raw; + std::memcpy(&raw, row_ptr + u * 4, sizeof(raw)); + if (!std::isfinite(raw) || raw <= 0.0f) continue; + depth_m = static_cast(raw); + } + + if (depth_m > clearing_range_) continue; + + // Back-project: depth optical frame (Z forward, X right, Y down) + double xc = (u - cx_) * depth_m / fx_; + double yc = (v - cy_) * depth_m / fy_; + double zc = depth_m; + + // Distance check in camera frame (cheaper than world-frame dist) + double dist2 = xc*xc + yc*yc + zc*zc; + if (dist2 > obstacle_range_sq) continue; + + // Transform to world frame + double xw = R[0][0]*xc + R[0][1]*yc + R[0][2]*zc + t.x; + double yw = R[1][0]*xc + R[1][1]*yc + R[1][2]*zc + t.y; + double zw = R[2][0]*xc + R[2][1]*yc + R[2][2]*zc + t.z; + + // Height filter (zw = height above ground in world frame) + if (zw < min_height_ || zw > max_height_) continue; + + new_points.push_back({xw, yw}); + } + } + + { + std::lock_guard lock(points_mutex_); + obstacle_points_ = std::move(new_points); + need_update_ = true; + } +} + +// ── updateBounds ────────────────────────────────────────────────────────────── + +void DepthCostmapLayer::updateBounds( + double robot_x, double robot_y, double /*robot_yaw*/, + double * min_x, double * min_y, + double * max_x, double * max_y) +{ + if (!enabled_) return; + + // Mark region around robot that depth camera can reach + double margin = obstacle_range_ + inflation_radius_; + last_min_x_ = robot_x - margin; + last_min_y_ = robot_y - margin; + last_max_x_ = robot_x + margin; + last_max_y_ = robot_y + margin; + + *min_x = std::min(*min_x, last_min_x_); + *min_y = std::min(*min_y, last_min_y_); + *max_x = std::max(*max_x, last_max_x_); + *max_y = std::max(*max_y, last_max_y_); +} + +// ── updateCosts ─────────────────────────────────────────────────────────────── + +void DepthCostmapLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) return; + + std::lock_guard lock(points_mutex_); + if (!need_update_ || obstacle_points_.empty()) return; + + double res = master.getResolution(); + // Number of cells to inflate in each direction + int inflate_cells = (inflation_radius_ > 0.0) + ? static_cast(std::ceil(inflation_radius_ / res)) + : 0; + + for (const auto & pt : obstacle_points_) { + unsigned int mx, my; + if (!master.worldToMap(pt.wx, pt.wy, mx, my)) continue; + + int ci = static_cast(mx); + int cj = static_cast(my); + + // Mark the obstacle cell as lethal + if (ci >= min_i && ci < max_i && cj >= min_j && cj < max_j) { + master.setCost(mx, my, LETHAL_OBSTACLE); + } + + // In-layer inflation: mark neighbouring cells within inflation_radius + // as INSCRIBED_INFLATED_OBSTACLE (if not already lethal) + for (int di = -inflate_cells; di <= inflate_cells; ++di) { + for (int dj = -inflate_cells; dj <= inflate_cells; ++dj) { + if (di == 0 && dj == 0) continue; + + double dist = std::hypot(static_cast(di), static_cast(dj)) * res; + if (dist > inflation_radius_) continue; + + int ni = ci + di; + int nj = cj + dj; + if (ni < min_i || ni >= max_i || nj < min_j || nj >= max_j) continue; + + auto uni = static_cast(ni); + auto unj = static_cast(nj); + unsigned char existing = master.getCost(uni, unj); + if (existing < INSCRIBED_INFLATED_OBSTACLE) { + master.setCost(uni, unj, INSCRIBED_INFLATED_OBSTACLE); + } + } + } + } + + need_update_ = false; +} + +// ── reset ───────────────────────────────────────────────────────────────────── + +void DepthCostmapLayer::reset() +{ + std::lock_guard lock(points_mutex_); + obstacle_points_.clear(); + need_update_ = false; + current_ = true; +} + +} // namespace saltybot_depth_costmap