test costmap + cam depth

2026-07-21 11:10:55 +07:00
commit 5d734a68fd
9 changed files with 754 additions and 0 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(depth_local_costmap_noetic_test LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+find_package(catkin REQUIRED COMPONENTS
+  data_convert
+  geometry_msgs
+  nav_msgs
+  robot_depth_image_proc
+  robot_costmap_2d
+  robot_cpp
+  robot_geometry_msgs
+  robot_sensor_msgs
+  robot_time
+  roscpp
+  sensor_msgs
+  tf2_ros
+)
+
+find_library(TF3_LIBRARY
+  NAMES tf3
+  PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
+)
+
+if(NOT TF3_LIBRARY)
+  message(FATAL_ERROR "tf3 library not found")
+endif()
+
+catkin_package()
+
+add_executable(depth_local_costmap_noetic_test_node
+  src/depth_local_costmap_noetic_test_node.cpp
+)
+
+target_include_directories(depth_local_costmap_noetic_test_node
+  PRIVATE
+    ${catkin_INCLUDE_DIRS}
+)
+
+target_link_libraries(depth_local_costmap_noetic_test_node
+  PRIVATE
+    ${catkin_LIBRARIES}
+    ${TF3_LIBRARY}
+)
+
+add_dependencies(depth_local_costmap_noetic_test_node
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(TARGETS depth_local_costmap_noetic_test_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY config launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
--- a/README.md
+++ b/README.md
@@ -0,0 +1,73 @@
+# depth_local_costmap_noetic_test
+
+Package ROS Noetic C++ tối thiểu để chạy **local costmap only** bằng stack T800:
+
+- `/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/src/Libraries/costmap_2d`
+- package catkin: `robot_costmap_2d`
+
+Node test nhận dữ liệu ROS Noetic và convert sang message `robot_*` trước khi feed trực tiếp vào `VoxelLayer` bằng `layer->dataCallBack(...)`.
+
+Luồng dữ liệu chính:
+
+- `/camera/depth/points_proc` -> `robot_sensor_msgs::PointCloud2` -> source `pc_marking`
+- `/camera/depth/image_raw` + `/camera/depth/camera_info` -> `robot_sensor_msgs::DepthCameraData::ConstPtr` -> source `pc_clearing`
+
+`/camera/depth/data` không cần là ROS topic thật trong gói test này; nó là tên source logic để match với config `robot_costmap_2d`, giống contract trong `sensor_converter.cpp`.
+
+Topic phụ được node subscribe để log kiểm tra dữ liệu:
+
+- `/camera_right/depth/camera_info`
+- `/odom`
+- `/tf`
+- `/tf_static`
+
+## Build
+
+```bash
+cd /home/duongtd/T800_ws
+catkin_make --pkg depth_local_costmap_noetic_test
+source devel/setup.bash
+```
+
+## Chạy
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch
+```
+
+Nếu robot dùng `base_footprint`:
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch base_frame:=base_footprint
+```
+
+Nếu hệ thống chỉ có `/odom` nhưng chưa publish TF `odom -> base_link`, bật bridge tạm:
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch publish_odom_tf:=true
+```
+
+Không bật `publish_odom_tf` nếu đã có node khác publish TF này.
+
+## Kiểm tra runtime
+
+```bash
+rostopic echo -n1 /camera/depth/points_proc/header
+rostopic echo -n1 /camera/depth/image_raw/header
+rostopic echo -n1 /camera/depth/camera_info/header
+rosrun tf tf_echo odom base_link
+rostopic list | grep local_costmap
+```
+
+Frame của `/camera/depth/points_proc` và `/camera/depth/image_raw` phải transform được về `odom`; nếu không, obstacle/voxel layer sẽ drop dữ liệu.
+
+Log runtime cần thấy:
+
+- `Fed depth cloud to robot_costmap_2d`
+- `Fed DepthCameraData to robot_costmap_2d`
+
+Config local cho `robot_costmap_2d` nằm trong thư mục `config/` của package này. Launch file set:
+
+```xml
+<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
+```
--- a/config/costmap_params.yaml
+++ b/config/costmap_params.yaml
@@ -0,0 +1,31 @@
+robot_costmap_2d:
+  global_frame: odom
+  robot_base_frame: base_link
+  rolling_window: true
+  track_unknown_space: false
+
+  plugins:
+    - name: voxel_layer
+      type: VoxelLayer
+    - name: inflation_layer
+      type: InflationLayer
+
+  library_path: /home/duongtd/T800_ws/devel/lib/libplugins.so
+
+  footprint:
+    - [0.30, 0.30]
+    - [0.30, -0.30]
+    - [-0.30, -0.30]
+    - [-0.30, 0.30]
+
+  transform_tolerance: 0.5
+  performance_metrics_enabled: false
+  performance_metrics_period: 5.0
+  update_frequency: 5.0
+  width: 6.0
+  height: 6.0
+  resolution: 0.05
+  origin_x: 0.0
+  origin_y: 0.0
+  footprint_padding: 0.0
+  robot_radius: 0.0
--- a/config/inflation_layer_params.yaml
+++ b/config/inflation_layer_params.yaml
@@ -0,0 +1,5 @@
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 3.0
+  inflation_radius: 0.45
--- a/config/obstacle_layer_params.yaml
+++ b/config/obstacle_layer_params.yaml
@@ -0,0 +1,39 @@
+obstacle_layer:
+  track_unknown_space: false
+  transform_tolerance: 0.5
+  footprint_clearing_enabled: true
+  max_obstacle_height: 1.5
+  combination_method: 1
+
+  observation_sources: pc_marking pc_clearing
+
+  pc_marking:
+    topic: /camera/depth/points_proc
+    data_type: PointCloud2
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.0
+    min_obstacle_height: 0.10
+    max_obstacle_height: 1.0
+    frustum_clearing_enabled: false
+
+  pc_clearing:
+    topic: /camera/depth/data
+    data_type: DepthCameraData
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.5
+    min_obstacle_height: 0.10
+    max_obstacle_height: 1.0
+    frustum_clearing_enabled: true
+    frustum_clearing_pixel_step: 8
+    frustum_min_range: 0.20
+    frustum_max_range: 3.5
--- a/config/voxel_layer_params.yaml
+++ b/config/voxel_layer_params.yaml
@@ -0,0 +1,15 @@
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 1.5
+  origin_z: 0.0
+  z_resolution: 0.15
+  z_voxels: 8
+  unknown_threshold: 15.0
+  mark_threshold: 0
+  combination_method: 1
+  frustum_clearing_enabled: false
+  frustum_clearing_pixel_step: 8
+  frustum_min_range: 0.20
+  frustum_max_range: 3.5
+  frustum_depth_camera_topic: /camera/depth/data
--- a/launch/depth_local_costmap_test.launch
+++ b/launch/depth_local_costmap_test.launch
@@ -0,0 +1,47 @@
+<launch>
+  <arg name="base_frame" default="base_link"/>
+  <arg name="global_frame" default="odom"/>
+  <arg name="depth_cloud_topic" default="/camera/depth/points_proc"/>
+  <arg name="depth_image_topic" default="/camera/depth/image_raw"/>
+  <arg name="camera_info_topic" default="/camera/depth/camera_info"/>
+  <arg name="depth_camera_data_topic" default="/camera/depth/data"/>
+  <arg name="camera_right_info_topic" default="/camera_right/depth/camera_info"/>
+  <arg name="odom_topic" default="/odom"/>
+  <arg name="publish_odom_tf" default="false"/>
+
+  <node pkg="depth_local_costmap_noetic_test"
+        type="depth_local_costmap_noetic_test_node"
+        name="depth_local_costmap_noetic_test"
+        output="screen">
+    <env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
+
+    <param name="base_frame" value="$(arg base_frame)"/>
+    <param name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
+    <param name="odom_topic" value="$(arg odom_topic)"/>
+    <param name="depth_cloud_topic" value="$(arg depth_cloud_topic)"/>
+    <param name="depth_image_topic" value="$(arg depth_image_topic)"/>
+    <param name="camera_info_topic" value="$(arg camera_info_topic)"/>
+    <param name="depth_camera_data_topic" value="$(arg depth_camera_data_topic)"/>
+    <param name="camera_right_info_topic" value="$(arg camera_right_info_topic)"/>
+
+    <param name="local_costmap/robot_base_frame" value="$(arg base_frame)"/>
+    <param name="local_costmap/global_frame" value="$(arg global_frame)"/>
+    <param name="local_costmap/voxel_layer/observation_sources" value="pc_marking pc_clearing"/>
+
+    <param name="local_costmap/voxel_layer/pc_marking/topic" value="$(arg depth_cloud_topic)"/>
+    <param name="local_costmap/voxel_layer/pc_marking/data_type" value="PointCloud2"/>
+    <param name="local_costmap/voxel_layer/pc_marking/clearing" value="false"/>
+    <param name="local_costmap/voxel_layer/pc_marking/marking" value="true"/>
+    <param name="local_costmap/voxel_layer/pc_marking/frustum_clearing_enabled" value="false"/>
+
+    <param name="local_costmap/voxel_layer/pc_clearing/topic" value="$(arg depth_camera_data_topic)"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/data_type" value="DepthCameraData"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/clearing" value="false"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/marking" value="true"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_enabled" value="true"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_pixel_step" value="8"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/frustum_min_range" value="0.20"/>
+    <param name="local_costmap/voxel_layer/pc_clearing/frustum_max_range" value="3.5"/>
+    <param name="local_costmap/voxel_layer/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
+  </node>
+</launch>
--- a/package.xml
+++ b/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>depth_local_costmap_noetic_test</name>
+  <version>0.1.0</version>
+  <description>Minimal ROS Noetic C++ local test package for the T800 robot_costmap_2d stack.</description>
+
+  <maintainer email="duongtd@example.com">duongtd</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>data_convert</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>robot_depth_image_proc</depend>
+  <depend>robot_costmap_2d</depend>
+  <depend>robot_cpp</depend>
+  <depend>robot_geometry_msgs</depend>
+  <depend>robot_sensor_msgs</depend>
+  <depend>robot_time</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_ros</depend>
+</package>
--- a/src/depth_local_costmap_noetic_test_node.cpp
+++ b/src/depth_local_costmap_noetic_test_node.cpp
@@ -0,0 +1,461 @@
+#include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/layer.h>
+
+#include <geometry_msgs/TransformStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <robot_depth_image_proc/ros_message_conversions.h>
+#include <robot/robot.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_sensor_msgs/CameraInfo.h>
+#include <robot_sensor_msgs/DepthCameraData.h>
+#include <robot_sensor_msgs/Image.h>
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/PointField.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <tf2/exceptions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf3/buffer_core.h>
+
+#include <atomic>
+#include <exception>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace
+{
+robot_sensor_msgs::PointCloud2 toRobotPointCloud2(const sensor_msgs::PointCloud2& msg)
+{
+  robot_sensor_msgs::PointCloud2 robot_pc;
+
+  robot_pc.header.seq = msg.header.seq;
+  robot_pc.header.stamp = robot::Time(msg.header.stamp.toSec());
+  robot_pc.header.frame_id = msg.header.frame_id;
+  robot_pc.height = msg.height;
+  robot_pc.width = msg.width;
+
+  robot_pc.fields.clear();
+  robot_pc.fields.reserve(msg.fields.size());
+  for (const auto& field : msg.fields)
+  {
+    robot_sensor_msgs::PointField robot_field;
+    robot_field.name = field.name;
+    robot_field.offset = field.offset;
+    robot_field.datatype = field.datatype;
+    robot_field.count = field.count;
+    robot_pc.fields.push_back(robot_field);
+  }
+
+  robot_pc.is_bigendian = msg.is_bigendian;
+  robot_pc.point_step = msg.point_step;
+  robot_pc.row_step = msg.row_step;
+  robot_pc.is_dense = msg.is_dense;
+  robot_pc.data = msg.data;
+  return robot_pc;
+}
+
+bool isVoxelLayer(const robot_costmap_2d::Layer& layer)
+{
+  return layer.getType() == robot_costmap_2d::LayerType::VOXEL_LAYER ||
+         layer.getName() == "local_costmap/voxel_layer";
+}
+
+template <typename MessageT>
+void feedVoxelLayers(robot_costmap_2d::Costmap2DROBOT& local_costmap,
+                     const MessageT& message,
+                     const std::string& topic)
+{
+  if (local_costmap.getLayeredCostmap() == nullptr ||
+      local_costmap.getLayeredCostmap()->getPlugins() == nullptr)
+  {
+    return;
+  }
+
+  for (const auto& layer : *local_costmap.getLayeredCostmap()->getPlugins())
+  {
+    if (layer && isVoxelLayer(*layer))
+      layer->dataCallBack<MessageT>(message, topic);
+  }
+}
+
+class RosTfToTf3Bridge
+{
+public:
+  RosTfToTf3Bridge(std::shared_ptr<tf2_ros::Buffer> tf2_buffer, tf3::BufferCore& tf3_buffer)
+    : tf2_buffer_(std::move(tf2_buffer))
+    , tf3_buffer_(tf3_buffer)
+    , worker_(&RosTfToTf3Bridge::run, this)
+  {
+    tf3_buffer_.setUsingDedicatedThread(true);
+  }
+
+  ~RosTfToTf3Bridge()
+  {
+    stop_ = true;
+    if (worker_.joinable())
+      worker_.join();
+  }
+
+private:
+  struct TfEdge
+  {
+    std::string parent;
+    std::string child;
+  };
+
+  static std::vector<TfEdge> parseEdges(const std::string& tree)
+  {
+    std::vector<TfEdge> edges;
+    std::istringstream stream(tree);
+    std::string line;
+
+    while (std::getline(stream, line))
+    {
+      const std::size_t frame_pos = line.find("Frame ");
+      const std::size_t parent_pos = line.find(" exists with parent ");
+      if (frame_pos == std::string::npos || parent_pos == std::string::npos)
+        continue;
+
+      std::string child = line.substr(frame_pos + 6, parent_pos - (frame_pos + 6));
+      std::string parent = line.substr(parent_pos + 20);
+      if (!parent.empty() && parent.back() == '.')
+        parent.pop_back();
+      if (!parent.empty() && !child.empty())
+        edges.push_back({parent, child});
+    }
+
+    return edges;
+  }
+
+  void run()
+  {
+    ros::Rate rate(50.0);
+
+    while (ros::ok() && !stop_)
+    {
+      if (!tf2_buffer_)
+      {
+        rate.sleep();
+        continue;
+      }
+
+      const std::vector<TfEdge> edges = parseEdges(tf2_buffer_->allFramesAsString());
+      for (const auto& edge : edges)
+      {
+        try
+        {
+          if (!tf2_buffer_->canTransform(edge.parent, edge.child, ros::Time(0), ros::Duration(0.01)))
+            continue;
+
+          const geometry_msgs::TransformStamped ros_tf =
+              tf2_buffer_->lookupTransform(edge.parent, edge.child, ros::Time(0));
+
+          tf3::TransformStampedMsg tf3_msg;
+          tf3_msg.header.stamp = tf3::Time::now();
+          tf3_msg.header.frame_id = ros_tf.header.frame_id;
+          tf3_msg.child_frame_id = ros_tf.child_frame_id;
+          tf3_msg.transform.translation.x = ros_tf.transform.translation.x;
+          tf3_msg.transform.translation.y = ros_tf.transform.translation.y;
+          tf3_msg.transform.translation.z = ros_tf.transform.translation.z;
+          tf3_msg.transform.rotation.x = ros_tf.transform.rotation.x;
+          tf3_msg.transform.rotation.y = ros_tf.transform.rotation.y;
+          tf3_msg.transform.rotation.z = ros_tf.transform.rotation.z;
+          tf3_msg.transform.rotation.w = ros_tf.transform.rotation.w;
+          tf3_buffer_.setTransform(tf3_msg, "ros_tf_bridge");
+        }
+        catch (const tf2::TransformException& ex)
+        {
+          ROS_WARN_THROTTLE(5.0, "TF bridge failed %s -> %s: %s",
+                            edge.parent.c_str(),
+                            edge.child.c_str(),
+                            ex.what());
+        }
+      }
+
+      rate.sleep();
+    }
+  }
+
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  tf3::BufferCore& tf3_buffer_;
+  std::atomic<bool> stop_{false};
+  std::thread worker_;
+};
+
+class OdomTfBridge
+{
+public:
+  OdomTfBridge(ros::NodeHandle& private_nh, const std::string& base_frame)
+    : base_frame_(base_frame)
+  {
+    private_nh.param("odom_topic", odom_topic_, std::string("/odom"));
+    odom_sub_ = private_nh.subscribe(odom_topic_, 10, &OdomTfBridge::odomCallback, this);
+    ROS_WARN("Publishing ROS TF from %s. Keep disabled if another node already publishes odom -> %s.",
+             odom_topic_.c_str(),
+             base_frame_.c_str());
+  }
+
+private:
+  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+  {
+    if (!msg)
+      return;
+
+    geometry_msgs::TransformStamped transform;
+    transform.header = msg->header;
+    transform.child_frame_id = msg->child_frame_id.empty() ? base_frame_ : msg->child_frame_id;
+    transform.transform.translation.x = msg->pose.pose.position.x;
+    transform.transform.translation.y = msg->pose.pose.position.y;
+    transform.transform.translation.z = msg->pose.pose.position.z;
+    transform.transform.rotation = msg->pose.pose.orientation;
+    tf_broadcaster_.sendTransform(transform);
+  }
+
+  std::string base_frame_;
+  std::string odom_topic_;
+  ros::Subscriber odom_sub_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+};
+
+class DepthCloudFeeder
+{
+public:
+  DepthCloudFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap)
+    : local_costmap_(local_costmap)
+  {
+    private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc"));
+    depth_cloud_sub_ =
+        private_nh.subscribe(depth_cloud_topic_, 1, &DepthCloudFeeder::depthCloudCallback, this);
+  }
+
+private:
+  void depthCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
+  {
+    if (!msg)
+      return;
+
+    const robot_sensor_msgs::PointCloud2 robot_cloud = toRobotPointCloud2(*msg);
+    feedVoxelLayers(local_costmap_, robot_cloud, depth_cloud_topic_);
+
+    ROS_INFO_THROTTLE(5.0, "Fed depth cloud to robot_costmap_2d: %ux%u frame=%s",
+                      msg->width,
+                      msg->height,
+                      msg->header.frame_id.c_str());
+  }
+
+  robot_costmap_2d::Costmap2DROBOT& local_costmap_;
+  std::string depth_cloud_topic_;
+  ros::Subscriber depth_cloud_sub_;
+};
+
+class DepthCameraDataFeeder
+{
+public:
+  DepthCameraDataFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap)
+    : local_costmap_(local_costmap)
+  {
+    private_nh.param("depth_image_topic", depth_image_topic_, std::string("/camera/depth/image_raw"));
+    private_nh.param("camera_info_topic", camera_info_topic_, std::string("/camera/depth/camera_info"));
+    private_nh.param("depth_camera_data_topic",
+                     depth_camera_data_topic_,
+                     std::string("/camera/depth/data"));
+
+    camera_info_sub_ =
+        private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this);
+    depth_image_sub_ =
+        private_nh.subscribe(depth_image_topic_, 1, &DepthCameraDataFeeder::depthImageCallback, this);
+  }
+
+private:
+  void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
+  {
+    if (!msg)
+      return;
+
+    std::lock_guard<std::mutex> lock(camera_info_mutex_);
+    camera_info_ = depth_image_proc::toRobotCameraInfo(*msg);
+    has_camera_info_ = true;
+
+    ROS_INFO_THROTTLE(5.0, "Cached camera_info for depth clearing: %ux%u frame=%s",
+                      msg->width,
+                      msg->height,
+                      msg->header.frame_id.c_str());
+  }
+
+  void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg)
+  {
+    if (!msg)
+      return;
+
+    robot_sensor_msgs::CameraInfo camera_info;
+    {
+      std::lock_guard<std::mutex> lock(camera_info_mutex_);
+      if (!has_camera_info_)
+      {
+        ROS_WARN_THROTTLE(5.0,
+                          "Waiting for %s before feeding DepthCameraData to robot_costmap_2d",
+                          camera_info_topic_.c_str());
+        return;
+      }
+      camera_info = camera_info_;
+    }
+
+    if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0)
+    {
+      ROS_ERROR_THROTTLE(5.0, "Invalid depth camera intrinsics on %s", camera_info_topic_.c_str());
+      return;
+    }
+
+    if (msg->encoding != "16UC1" && msg->encoding != "mono16" && msg->encoding != "32FC1")
+    {
+      ROS_ERROR_THROTTLE(
+          5.0,
+          "Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1",
+          msg->encoding.c_str());
+      return;
+    }
+
+    robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData());
+    depth_camera_data->depth = depth_image_proc::toRobotImage(*msg);
+    depth_camera_data->camera_info = camera_info;
+    depth_camera_data->header = depth_camera_data->depth.header;
+    if (depth_camera_data->header.frame_id.empty())
+      depth_camera_data->header.frame_id = depth_camera_data->camera_info.header.frame_id;
+
+    robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data;
+    feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_);
+
+    ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s",
+                      msg->width,
+                      msg->height,
+                      msg->encoding.c_str(),
+                      depth_camera_data->header.frame_id.c_str(),
+                      depth_camera_data_topic_.c_str());
+  }
+
+  robot_costmap_2d::Costmap2DROBOT& local_costmap_;
+  std::string depth_image_topic_;
+  std::string camera_info_topic_;
+  std::string depth_camera_data_topic_;
+  robot_sensor_msgs::CameraInfo camera_info_;
+  bool has_camera_info_{false};
+  std::mutex camera_info_mutex_;
+  ros::Subscriber depth_image_sub_;
+  ros::Subscriber camera_info_sub_;
+};
+
+class RightCameraInfoWatchdog
+{
+public:
+  explicit RightCameraInfoWatchdog(ros::NodeHandle& private_nh)
+  {
+    std::string camera_right_info_topic;
+    private_nh.param("camera_right_info_topic",
+                     camera_right_info_topic,
+                     std::string("/camera_right/depth/camera_info"));
+
+    camera_right_info_sub_ =
+        private_nh.subscribe(camera_right_info_topic, 1, &RightCameraInfoWatchdog::cameraRightInfoCallback, this);
+  }
+
+private:
+  void cameraRightInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
+  {
+    if (msg)
+      ROS_INFO_THROTTLE(5.0, "Right camera info OK: %ux%u frame=%s",
+                        msg->width,
+                        msg->height,
+                        msg->header.frame_id.c_str());
+  }
+
+  ros::Subscriber camera_right_info_sub_;
+};
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "depth_local_costmap_noetic_test");
+
+  ros::NodeHandle private_nh("~");
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
+
+  std::string base_frame;
+  bool publish_odom_tf = false;
+  private_nh.param("base_frame", base_frame, std::string("base_link"));
+  private_nh.param("publish_odom_tf", publish_odom_tf, false);
+
+  auto tf2_buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(10.0));
+  tf2_ros::TransformListener tf2_listener(*tf2_buffer);
+  tf3::BufferCore tf3_buffer;
+  RosTfToTf3Bridge tf_bridge(tf2_buffer, tf3_buffer);
+
+  std::unique_ptr<OdomTfBridge> odom_tf_bridge;
+  if (publish_odom_tf)
+    odom_tf_bridge = std::make_unique<OdomTfBridge>(private_nh, base_frame);
+
+  RightCameraInfoWatchdog right_camera_info_watchdog(private_nh);
+
+  try
+  {
+    robot_costmap_2d::Costmap2DROBOT local_costmap("local_costmap", tf3_buffer);
+    DepthCloudFeeder depth_cloud_feeder(private_nh, local_costmap);
+    DepthCameraDataFeeder depth_camera_data_feeder(private_nh, local_costmap);
+    local_costmap.start();
+
+    ROS_INFO("T800 robot_costmap_2d local costmap test started.");
+
+    ros::Rate rate(2.0);
+    while (ros::ok())
+    {
+      robot_geometry_msgs::PoseStamped robot_pose;
+      const bool pose_ok = local_costmap.getRobotPose(robot_pose);
+      const robot_costmap_2d::Costmap2D* costmap = local_costmap.getCostmap();
+
+      if (costmap == nullptr)
+      {
+        ROS_WARN_THROTTLE(2.0, "robot_costmap_2d local costmap pointer is null");
+      }
+      else if (!pose_ok)
+      {
+        ROS_WARN_THROTTLE(
+            2.0,
+            "robot_costmap_2d has no robot pose. Check TF: odom -> %s and cloud frame -> odom.",
+            base_frame.c_str());
+      }
+      else
+      {
+        ROS_INFO_THROTTLE(
+            2.0,
+            "robot_costmap_2d OK: size=%ux%u res=%.3f origin=(%.2f, %.2f) robot=(%.2f, %.2f)",
+            costmap->getSizeInCellsX(),
+            costmap->getSizeInCellsY(),
+            costmap->getResolution(),
+            costmap->getOriginX(),
+            costmap->getOriginY(),
+            robot_pose.pose.position.x,
+            robot_pose.pose.position.y);
+      }
+
+      rate.sleep();
+    }
+
+    local_costmap.stop();
+  }
+  catch (const std::exception& e)
+  {
+    ROS_FATAL("depth_local_costmap_noetic_test failed: %s", e.what());
+    return 1;
+  }
+
+  return 0;
+}