DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add multi camera depth

Browse Source
This commit is contained in:
duongtd
2026-07-14 09:42:35 +07:00
parent a2a021c114
commit 6a9834d3a8
9 changed files with 711 additions and 1374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

303
plugins/voxel_layer.cpp
View File

@@ -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
@@ -92,7 +99,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", 0.0);
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"))
@@ -165,6 +172,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
bool current = true;
std::vector<Observation> observations, clearing_observations;
std::vector<DepthCameraObservation> depth_observations;
// get the marking observations
current = getMarkingObservations(observations) && current;
@@ -172,9 +180,16 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// get the clearing observations
current = getClearingObservations(clearing_observations) && current;
current = getFrustumClearingObservations(depth_observations) && current;
// update the global current status
current_ = current;
for (const DepthCameraObservation& depth_observation : depth_observations)
{
raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
}
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
@@ -231,29 +246,6 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
}
}
}
// if (publish_voxel_)
// {
// robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
// grid_msg.size_x = robot_voxel_grid_.sizeX();
// grid_msg.size_y = robot_voxel_grid_.sizeY();
// grid_msg.size_z = robot_voxel_grid_.sizeZ();
// grid_msg.data.resize(size);
// memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
// grid_msg.origin.x = origin_x_;
// grid_msg.origin.y = origin_y_;
// grid_msg.origin.z = origin_z_;
// grid_msg.resolutions.x = resolution_;
// grid_msg.resolutions.y = resolution_;
// grid_msg.resolutions.z = z_resolution_;
// grid_msg.header.frame_id = global_frame_;
// grid_msg.header.stamp = robot::Time::now();
// voxel_pub_.publish(grid_msg);
// }
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
@@ -327,14 +319,6 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
ox, oy, oz);
return;
}
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
// if (publish_clearing_points)
// {
// clearing_endpoints_.points.clear();
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// }
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
@@ -409,26 +393,249 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
}
// if (publish_clearing_points)
// {
// robot_geometry_msgs::Point32 point;
// point.x = wpx;
// point.y = wpy;
// point.z = wpz;
// clearing_endpoints_.points.push_back(point);
// }
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(const DepthCameraObservation& observation,
double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!observation.data_)
return false;
const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.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;
std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id;
if (depth_frame.empty())
depth_frame = camera_info.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;
if (local_origin.header.stamp.isZero())
local_origin.header.stamp = depth_camera_data.header.stamp;
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_throttle(
5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
observation.topic_.c_str(), 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_throttle(
5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n",
observation.topic_.c_str(), ox, oy, oz);
return false;
}
const unsigned int step = std::max(1u, observation.pixel_step_);
const double min_range = observation.min_range_;
const double max_range = observation.max_range_;
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 = max_range;
if (valid && depth_m < max_range)
ray_len = std::max(0.0, depth_m - skip_dist);
if (ray_len <= 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 * min_range;
const double sy = oy + global_ray.y * min_range;
const double sz = oz + global_ray.z * 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;
}
}
// if (publish_clearing_points)
// {
// clearing_endpoints_.header.frame_id = global_frame_;
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// }
return cleared_any;
}
void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)