Merge remote-tracking branch 'origin/sl-perception/issue-532-depth-costmap'
This commit is contained in:
commit
2d97033539
@ -10,12 +10,14 @@
|
|||||||
# → No AMCL, no map_server needed.
|
# → No AMCL, no map_server needed.
|
||||||
#
|
#
|
||||||
# Sensors (Issue #478 — Costmap configuration):
|
# Sensors (Issue #478 — Costmap configuration):
|
||||||
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle)
|
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle)
|
||||||
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle)
|
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle)
|
||||||
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
|
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
|
||||||
|
# /camera/depth/image_rect_raw — RealSense D435i depth image (Issue #532 DepthCostmapLayer)
|
||||||
#
|
#
|
||||||
# Inflation:
|
# Inflation:
|
||||||
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
|
# 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.
|
# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
|
||||||
|
|
||||||
@ -256,7 +258,21 @@ local_costmap:
|
|||||||
resolution: 0.05
|
resolution: 0.05
|
||||||
robot_radius: 0.15
|
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
|
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:
|
obstacle_layer:
|
||||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
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
|
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
|
resolution: 0.05
|
||||||
track_unknown_space: true
|
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:
|
static_layer:
|
||||||
plugin: "nav2_costmap_2d::StaticLayer"
|
plugin: "nav2_costmap_2d::StaticLayer"
|
||||||
|
|||||||
78
jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt
Normal file
78
jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt
Normal file
@ -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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
|
||||||
|
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()
|
||||||
@ -0,0 +1,113 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#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<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr info_sub_;
|
||||||
|
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||||
|
std::shared_ptr<tf2_ros::TransformListener> 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<ObstaclePoint> 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
|
||||||
30
jetson/ros2_ws/src/saltybot_depth_costmap/package.xml
Normal file
30
jetson/ros2_ws/src/saltybot_depth_costmap/package.xml
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>saltybot_depth_costmap</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>
|
||||||
|
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.
|
||||||
|
</description>
|
||||||
|
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>nav2_costmap_2d</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>tf2_ros</depend>
|
||||||
|
<depend>tf2</depend>
|
||||||
|
<depend>pluginlib</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
15
jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml
Normal file
15
jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
<library path="saltybot_depth_costmap">
|
||||||
|
<class
|
||||||
|
name="saltybot_depth_costmap::DepthCostmapLayer"
|
||||||
|
type="saltybot_depth_costmap::DepthCostmapLayer"
|
||||||
|
base_class_type="nav2_costmap_2d::Layer">
|
||||||
|
<description>
|
||||||
|
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).
|
||||||
|
</description>
|
||||||
|
</class>
|
||||||
|
</library>
|
||||||
@ -0,0 +1,306 @@
|
|||||||
|
#include "saltybot_depth_costmap/depth_costmap_layer.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cmath>
|
||||||
|
#include <cstring>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#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<tf2_ros::Buffer>(clock_);
|
||||||
|
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||||
|
|
||||||
|
// Subscriptions
|
||||||
|
depth_sub_ = node->create_subscription<sensor_msgs::msg::Image>(
|
||||||
|
depth_topic_, rclcpp::SensorDataQoS(),
|
||||||
|
std::bind(&DepthCostmapLayer::depthCallback, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
info_sub_ = node->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||||
|
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<int>(img->height);
|
||||||
|
int cols = static_cast<int>(img->width);
|
||||||
|
int step = static_cast<int>(img->step);
|
||||||
|
|
||||||
|
const double obstacle_range_sq = obstacle_range_ * obstacle_range_;
|
||||||
|
|
||||||
|
std::vector<ObstaclePoint> 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<double>(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<std::mutex> 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<std::mutex> 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<int>(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<int>(mx);
|
||||||
|
int cj = static_cast<int>(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<double>(di), static_cast<double>(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<unsigned int>(ni);
|
||||||
|
auto unj = static_cast<unsigned int>(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<std::mutex> lock(points_mutex_);
|
||||||
|
obstacle_points_.clear();
|
||||||
|
need_update_ = false;
|
||||||
|
current_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace saltybot_depth_costmap
|
||||||
Loading…
x
Reference in New Issue
Block a user