change topic and msg depth camera

config/voxel_layer_params.yaml

@@ -10,3 +10,6 @@ voxel_layer:
   combination_method: 1
   frustum_clearing_enabled: true
   frustum_clearing_pixel_step: 8
+  frustum_min_range: 0.20
+  frustum_max_range: 3.0
+  frustum_depth_camera_topic: /camera/depth/image_raw

include/robot_costmap_2d/obstacle_layer.h

@@ -48,9 +48,11 @@
 
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_laser_geometry/laser_geometry.hpp>
+#include <robot_sensor_msgs/DepthCameraData.h>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
+#include <mutex>
 
 
 
@@ -67,7 +69,8 @@ struct CallBackInfo
 class ObstacleLayer : public CostmapLayer
 {
 public:
-  ObstacleLayer()
+  ObstacleLayer() :
+      have_depth_camera_data_(false)
   {
     costmap_ = NULL;  // this is the unsigned char* member of parent class Costmap2D.
   }
@@ -178,6 +181,10 @@ protected:
 
   int combination_method_;
   std::vector<CallBackInfo> callback_infos_;
+  std::string depth_camera_topic_;
+  mutable std::mutex depth_camera_mutex_;
+  robot_sensor_msgs::DepthCameraData latest_depth_camera_data_;
+  bool have_depth_camera_data_;
 
 private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);

include/robot_costmap_2d/voxel_layer.h

@@ -45,6 +45,7 @@
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_laser_geometry/laser_geometry.hpp>
+#include <robot_sensor_msgs/Image.h>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
@@ -58,7 +59,11 @@ class VoxelLayer : public ObstacleLayer
 {
 public:
   VoxelLayer() :
-      robot_voxel_grid_(0, 0, 0)
+      robot_voxel_grid_(0, 0, 0),
+      frustum_clearing_enabled_(false),
+      frustum_clearing_pixel_step_(8),
+      frustum_min_range_(0.2),
+      frustum_max_range_(3.0)
   {
     costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D.
   }
@@ -91,10 +96,9 @@ private:
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
   virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
-  bool raytraceDepthFrustum(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
-                            double* max_x, double* max_y);
-  bool getCloudPoint(const robot_sensor_msgs::PointCloud2& cloud, unsigned int u, unsigned int v,
-                     double& wx, double& wy, double& wz) const;
+  bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
+  bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
+                       double& depth_m, bool& is_valid) const;
   bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
   bool 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);
@@ -106,6 +110,9 @@ private:
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
   bool frustum_clearing_enabled_;
   unsigned int frustum_clearing_pixel_step_;
+  double frustum_min_range_;
+  double frustum_max_range_;
+  std::string frustum_depth_camera_topic_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)

plugins/obstacle_layer.cpp

@@ -232,6 +232,22 @@ void ObstacleLayer::handleImpl(const void* data,
 {
   if(!stop_receiving_data_)
   {
+    if (type == typeid(robot_sensor_msgs::DepthCameraData) &&
+        (depth_camera_topic_.empty() || topic == depth_camera_topic_))
+    {
+      const robot_sensor_msgs::DepthCameraData& depth_camera_data =
+          *static_cast<const robot_sensor_msgs::DepthCameraData*>(data);
+      if (depth_camera_data.camera_info.K[0] <= 0.0 || depth_camera_data.camera_info.K[4] <= 0.0)
+      {
+        robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n");
+        return;
+      }
+
+      std::lock_guard<std::mutex> lock(depth_camera_mutex_);
+      latest_depth_camera_data_ = depth_camera_data;
+      have_depth_camera_data_ = true;
+      return;
+    }
     
     if(observation_buffers_.empty() || callback_infos_.empty()) return;
     

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