change topic and msg depth camera

2026-07-11 09:50:40 +07:00
parent a2a021c114
commit faa57d9c93
5 changed files with 311 additions and 7 deletions
--- a/plugins/voxel_layer.cpp
+++ b/plugins/voxel_layer.cpp
@@ -37,7 +37,14 @@
 *********************************************************************/
 #include <robot_costmap_2d/voxel_layer.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_geometry_msgs/Vector3.h>
+#include <tf3/exceptions.h>
 #include <boost/dll/alias.hpp>
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <cstring>

 #define VOXEL_BITS 16

@@ -91,8 +98,13 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
    unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
    mark_threshold_ = loadParam(layer, "mark_threshold", 0);
    combination_method_ = loadParam(layer, "combination_method", 0.0);
+    frustum_clearing_enabled_ = loadParam(layer, "frustum_clearing_enabled", false);
+    frustum_clearing_pixel_step_ = loadParam(layer, "frustum_clearing_pixel_step", 8);
+    frustum_min_range_ = loadParam(layer, "frustum_min_range", 0.2);
+    frustum_max_range_ = loadParam(layer, "frustum_max_range", 3.0);
+    frustum_depth_camera_topic_ = loadParam(layer, "frustum_depth_camera_topic", std::string("/camera/depth/image_raw"));

-    int size_z, unknown_threshold, mark_threshold;
+    int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
    if (nh.hasParam("enabled"))
      nh.getParam("enabled", enabled_);
    if (nh.hasParam("footprint_clearing_enabled"))
@@ -118,6 +130,25 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
    }
    if (nh.hasParam("combination_method"))
      nh.getParam("combination_method", combination_method_);
+    if (nh.hasParam("frustum_clearing_enabled"))
+      nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled_);
+    if (nh.hasParam("frustum_clearing_pixel_step"))
+    {
+      nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step);
+      frustum_clearing_pixel_step_ = std::max(1, frustum_pixel_step);
+    }
+    if (nh.hasParam("frustum_min_range"))
+      nh.getParam("frustum_min_range", frustum_min_range_);
+    if (nh.hasParam("frustum_max_range"))
+      nh.getParam("frustum_max_range", frustum_max_range_);
+    if (nh.hasParam("frustum_depth_camera_topic"))
+      nh.getParam("frustum_depth_camera_topic", frustum_depth_camera_topic_);
+    else if (nh.hasParam("frustum_depth_topic"))
+      nh.getParam("frustum_depth_topic", frustum_depth_camera_topic_);
+
+    frustum_clearing_pixel_step_ = std::max(1u, frustum_clearing_pixel_step_);
+    frustum_max_range_ = std::max(frustum_max_range_, frustum_min_range_ + resolution_);
+    depth_camera_topic_ = frustum_depth_camera_topic_;

    this->matchSize();
  } 
@@ -175,6 +206,9 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
  // update the global current status
  current_ = current;

+  if (frustum_clearing_enabled_)
+    raytraceDepthFrustum(min_x, min_y, max_x, max_y);
+
  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
@@ -431,6 +465,243 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
  // }
 }

+bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
+                                 double& depth_m, bool& is_valid) const
+{
+  depth_m = 0.0;
+  is_valid = false;
+
+  if (u >= depth.width || v >= depth.height)
+    return false;
+
+  if (depth.encoding == "16UC1" || depth.encoding == "mono16")
+  {
+    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 2;
+    if (offset + sizeof(std::uint16_t) > depth.data.size())
+      return false;
+
+    std::uint16_t raw = 0;
+    if (depth.is_bigendian)
+      raw = static_cast<std::uint16_t>((depth.data[offset] << 8) | depth.data[offset + 1]);
+    else
+      raw = static_cast<std::uint16_t>(depth.data[offset] | (depth.data[offset + 1] << 8));
+
+    if (raw == 0)
+      return true;
+
+    depth_m = static_cast<double>(raw) * 0.001;
+    is_valid = true;
+    return true;
+  }
+
+  if (depth.encoding == "32FC1")
+  {
+    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 4;
+    if (offset + sizeof(float) > depth.data.size())
+      return false;
+
+    float raw = 0.0f;
+    if (depth.is_bigendian)
+    {
+      unsigned char bytes[sizeof(float)] = {
+          depth.data[offset + 3], depth.data[offset + 2], depth.data[offset + 1], depth.data[offset]};
+      std::memcpy(&raw, bytes, sizeof(float));
+    }
+    else
+    {
+      std::memcpy(&raw, &depth.data[offset], sizeof(float));
+    }
+
+    if (!std::isfinite(raw) || raw <= 0.0f)
+      return true;
+
+    depth_m = static_cast<double>(raw);
+    is_valid = true;
+    return true;
+  }
+
+  robot::log_error("VoxelLayer unsupported depth encoding for frustum clearing: %s\n", depth.encoding.c_str());
+  return false;
+}
+
+bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
+{
+  double a = wx - ox;
+  double b = wy - oy;
+  double c = wz - oz;
+  double t = 1.0;
+  constexpr double kEpsilon = 1e-9;
+
+  if (std::fabs(a) < kEpsilon && std::fabs(b) < kEpsilon && std::fabs(c) < kEpsilon)
+    return false;
+
+  if (wz > max_obstacle_height_ && std::fabs(c) > kEpsilon)
+    t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+  else if (wz < origin_z_ && std::fabs(c) > kEpsilon)
+    t = std::min(t, (origin_z_ - oz) / c);
+
+  const double map_end_x = origin_x_ + getSizeInMetersX();
+  const double map_end_y = origin_y_ + getSizeInMetersY();
+
+  if (wx < origin_x_ && std::fabs(a) > kEpsilon)
+    t = std::min(t, (origin_x_ - ox) / a);
+  if (wy < origin_y_ && std::fabs(b) > kEpsilon)
+    t = std::min(t, (origin_y_ - oy) / b);
+  if (wx > map_end_x && std::fabs(a) > kEpsilon)
+    t = std::min(t, (map_end_x - ox) / a);
+  if (wy > map_end_y && std::fabs(b) > kEpsilon)
+    t = std::min(t, (map_end_y - oy) / b);
+
+  if (!std::isfinite(t) || t <= 0.0)
+    return false;
+
+  wx = ox + a * t;
+  wy = oy + b * t;
+  wz = oz + c * t;
+  return true;
+}
+
+bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
+                               double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y)
+{
+  double sensor_x, sensor_y, sensor_z;
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+    return false;
+
+  if (!clipRaytraceEndpoint(ox, oy, oz, wx, wy, wz))
+    return false;
+
+  double point_x, point_y, point_z;
+  if (!worldToMap3DFloat(wx, wy, wz, point_x, point_y, point_z))
+    return false;
+
+  robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
+                                        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+                                        cellDistance(raytrace_range));
+  updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
+  return true;
+}
+
+bool VoxelLayer::raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y)
+{
+  robot_sensor_msgs::DepthCameraData depth_camera_data;
+  {
+    std::lock_guard<std::mutex> lock(depth_camera_mutex_);
+    if (!have_depth_camera_data_)
+      return false;
+    depth_camera_data = latest_depth_camera_data_;
+  }
+
+  const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
+  const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
+
+  if (depth.width == 0 || depth.height == 0 || depth.data.empty())
+    return false;
+
+  const double fx = camera_info.K[0];
+  const double fy = camera_info.K[4];
+  const double cx = camera_info.K[2];
+  const double cy = camera_info.K[5];
+  if (fx <= 0.0 || fy <= 0.0)
+    return false;
+
+  const std::string depth_frame = depth.header.frame_id.empty() ? camera_info.header.frame_id : depth.header.frame_id;
+  if (depth_frame.empty() || tf_ == nullptr)
+    return false;
+
+  robot_geometry_msgs::PointStamped local_origin;
+  local_origin.header = depth.header;
+  local_origin.header.frame_id = depth_frame;
+  local_origin.point.x = 0.0;
+  local_origin.point.y = 0.0;
+  local_origin.point.z = 0.0;
+
+  robot_geometry_msgs::PointStamped global_origin;
+  tf3::TransformStampedMsg tfm;
+  try
+  {
+    tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time());
+    tf3::doTransform(local_origin, global_origin, tfm);
+  }
+  catch (tf3::TransformException& ex)
+  {
+    robot::log_error("VoxelLayer frustum clearing TF exception from %s to %s: %s\n",
+                     depth_frame.c_str(), global_frame_.c_str(), ex.what());
+    return false;
+  }
+
+  const double ox = global_origin.point.x;
+  const double oy = global_origin.point.y;
+  const double oz = global_origin.point.z;
+
+  double sensor_x, sensor_y, sensor_z;
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+  {
+    robot::log_error(
+        "The origin for the depth sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot frustum-clear for it.\n",
+        ox, oy, oz);
+    return false;
+  }
+
+  const unsigned int step = std::max(1u, frustum_clearing_pixel_step_);
+  const double skip_dist = 2.0 * resolution_;
+  const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
+  const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
+  bool cleared_any = false;
+
+  for (unsigned int v = 0; v < height; v += step)
+  {
+    for (unsigned int u = 0; u < width; u += step)
+    {
+      double depth_m = 0.0;
+      bool valid = false;
+      if (!readDepthMeters(depth, u, v, depth_m, valid))
+        continue;
+
+      double ray_len = frustum_max_range_;
+      if (valid && depth_m < frustum_max_range_)
+        ray_len = std::max(0.0, depth_m - skip_dist);
+
+      if (ray_len <= frustum_min_range_)
+        continue;
+
+      double dx = (static_cast<double>(u) - cx) / fx;
+      double dy = (static_cast<double>(v) - cy) / fy;
+      double dz = 1.0;
+      const double norm = std::sqrt(dx * dx + dy * dy + dz * dz);
+      if (norm <= 0.0)
+        continue;
+
+      robot_geometry_msgs::Vector3 local_ray;
+      local_ray.x = dx / norm;
+      local_ray.y = dy / norm;
+      local_ray.z = dz / norm;
+
+      robot_geometry_msgs::Vector3 global_ray;
+      tf3::doTransform(local_ray, global_ray, tfm);
+      const double global_norm =
+          std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z);
+      if (global_norm <= 0.0)
+        continue;
+
+      global_ray.x /= global_norm;
+      global_ray.y /= global_norm;
+      global_ray.z /= global_norm;
+
+      const double sx = ox + global_ray.x * frustum_min_range_;
+      const double sy = oy + global_ray.y * frustum_min_range_;
+      const double sz = oz + global_ray.z * frustum_min_range_;
+      const double wx = ox + global_ray.x * ray_len;
+      const double wy = oy + global_ray.y * ray_len;
+      const double wz = oz + global_ray.z * ray_len;
+
+      cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any;
+    }
+  }
+
+  return cleared_any;
+}
+
 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
 {
  // project the new origin into the grid