DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
28a32ee67cd42da8718b46d837cb556759d92c9e
costmap_2d/plugins/voxel_layer.cpp
duongtd 28a32ee67c optimal voxel layer
2026-07-23 11:18:04 +07:00

829 lines
29 KiB
C++
Executable File
Raw Blame History

/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#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
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace robot_costmap_2d
{
void VoxelLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
ObstacleLayer::onInitialize();
std::string config_file_name = "voxel_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
VoxelLayer::~VoxelLayer()
{}
bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try
{
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
std::string folder;
if (env_config && std::filesystem::exists(env_config))
{
folder = std::string(env_config);
// robot::log_error("config_directory: %s", folder.c_str());
}
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["voxel_layer"];
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
enabled_ = loadParam(layer, "enabled", true);
footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
size_z_ = loadParam(layer, "z_voxels", 10);
origin_z_ = loadParam(layer, "origin_z", 0.0);
z_resolution_ = loadParam(layer, "z_resolution", 0.2);
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);
obstacle_decay_time_ = loadParam(layer, "obstacle_decay_time", 0.0);
frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled_);
if (nh.hasParam("footprint_clearing_enabled"))
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled_);
if (nh.hasParam("max_obstacle_height"))
nh.getParam("max_obstacle_height", max_obstacle_height_);
if (nh.hasParam("z_voxels"))
{
nh.getParam("z_voxels", size_z);
size_z_ = size_z;
}
if (nh.hasParam("origin_z"))
nh.getParam("origin_z", origin_z_);
if (nh.hasParam("unknown_threshold"))
{
nh.getParam("unknown_threshold", unknown_threshold);
unknown_threshold_ = unknown_threshold + (VOXEL_BITS - size_z_);
}
if (nh.hasParam("mark_threshold"))
{
nh.getParam("mark_threshold", mark_threshold);
mark_threshold_ = mark_threshold;
}
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method_);
if (nh.hasParam("obstacle_decay_time"))
nh.getParam("obstacle_decay_time", obstacle_decay_time_);
if (nh.hasParam("frustum_skip_distance"))
nh.getParam("frustum_skip_distance", frustum_skip_distance_);
if (obstacle_decay_time_ > 0.0)
{
robot::log_info(
"VoxelLayer obstacle decay enabled: LETHAL cells not re-observed for %.1f s are freed. "
"Intended for camera-only costmaps; decay clears obstacles outside the current FOV.\n",
obstacle_decay_time_);
}
this->matchSize();
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
void VoxelLayer::matchSize()
{
ObstacleLayer::matchSize();
robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_))
{
std::cerr << "[FATAL] Voxel grid size mismatch: "
<< "voxel(" << robot_voxel_grid_.sizeX() << ", " << robot_voxel_grid_.sizeY()
<< ") but costmap(" << size_x_ << ", " << size_y_ << ")\n";
std::abort(); // dừng chương trình
}
}
void VoxelLayer::reset()
{
deactivate();
resetMaps();
robot_voxel_grid_.reset();
activate();
}
void VoxelLayer::resetMaps()
{
Costmap2D::resetMaps();
robot_voxel_grid_.reset();
cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
}
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
std::vector<Observation> observations, clearing_observations;
std::vector<DepthCameraObservation> depth_observations;
// get the marking observations
current = getMarkingObservations(observations) && current;
// 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)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
const double now_sec =
obstacle_decay_time_ > 0.0 ? robot::Time::now().toSec() : 0.0;
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
// if the obstacle is too high or too far away from the robot we won't add it
if (*iter_z > max_obstacle_height_)
continue;
// compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
+ (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
+ (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
continue;
// now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if (*iter_z < origin_z_)
{
if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
continue;
}
else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
{
continue;
}
// mark the cell in the voxel grid and check if we should also mark it in the costmap
if (robot_voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
if (obstacle_decay_time_ > 0.0)
cell_last_marked_[index] = now_sec;
touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
}
}
}
if (obstacle_decay_time_ > 0.0)
decayStaleObstacles(now_sec, min_x, min_y, max_x, max_y);
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void VoxelLayer::decayStaleObstacles(double now_sec, double* min_x, double* min_y,
double* max_x, double* max_y)
{
const std::size_t map_size = static_cast<std::size_t>(size_x_) * size_y_;
if (cell_last_marked_.size() != map_size)
cell_last_marked_.assign(map_size, 0.0);
for (std::size_t index = 0; index < map_size; ++index)
{
if (costmap_[index] != LETHAL_OBSTACLE)
continue;
double& stamp = cell_last_marked_[index];
// A LETHAL cell without a stamp was marked before decay tracking covered
// it (e.g. right after a resize). Start its timer now instead of freeing
// an obstacle we have no age evidence for.
if (stamp <= 0.0)
{
stamp = now_sec;
continue;
}
if (now_sec - stamp <= obstacle_decay_time_)
continue;
costmap_[index] = FREE_SPACE;
robot_voxel_grid_.clearVoxelColumn(static_cast<unsigned int>(index));
stamp = 0.0;
unsigned int mx, my;
indexToCells(static_cast<unsigned int>(index), mx, my);
double wx, wy;
mapToWorld(mx, my, wx, wy);
touch(wx, wy, min_x, min_y, max_x, max_y);
}
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
// get the cell coordinates of the center point of the window
unsigned int mx, my;
if (!worldToMap(wx, wy, mx, my))
return;
// compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
// scale the window based on the bounds of the costmap
start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
// get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
// check for legality just in case
if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
return;
// we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for (unsigned int j = map_sy; j <= map_ey; ++j)
{
for (unsigned int i = map_sx; i <= map_ex; ++i)
{
// if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if (*current != LETHAL_OBSTACLE)
{
if (clear_no_info || *current != NO_INFORMATION)
{
*current = FREE_SPACE;
robot_voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
if (clearing_observation_cloud_size == 0)
return;
double sensor_x, sensor_y, sensor_z;
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
double oz = clearing_observation.origin_.z;
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
robot::log_error(
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy, oz);
return;
}
// 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();
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double wpx = *iter_x;
double wpy = *iter_y;
double wpz = *iter_z;
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = 1.0;
scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = 1.0;
// we can only raytrace to a maximum z height
if (wpz > max_obstacle_height_)
{
// we know we want the vector's z value to be max_z
t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
}
// and we can only raytrace down to the floor
else if (wpz < origin_z_)
{
// we know we want the vector's z value to be 0.0
t = std::min(t, (origin_z_ - oz) / c);
}
// the minimum value to raytrace from is the origin
if (wpx < origin_x_)
{
t = std::min(t, (origin_x_ - ox) / a);
}
if (wpy < origin_y_)
{
t = std::min(t, (origin_y_ - oy) / b);
}
// the maximum value to raytrace to is the end of the map
if (wpx > map_end_x)
{
t = std::min(t, (map_end_x - ox) / a);
}
if (wpy > map_end_y)
{
t = std::min(t, (map_end_y - oy) / b);
}
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
double point_x, point_y, point_z;
if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
{
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// robot_voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
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,
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
}
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;
}
void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
unsigned int pixel_step, double fx, double fy,
double cx, double cy)
{
if (cached_depth_width_ == width && cached_depth_height_ == height &&
cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
cached_cx_ == cx && cached_cy_ == cy)
{
return;
}
cached_depth_width_ = width;
cached_depth_height_ = height;
cached_depth_pixel_step_ = pixel_step;
cached_fx_ = fx;
cached_fy_ = fy;
cached_cx_ = cx;
cached_cy_ = cy;
const std::size_t rows = (height + pixel_step - 1) / pixel_step;
const std::size_t columns = (width + pixel_step - 1) / pixel_step;
depth_ray_cache_.clear();
depth_ray_cache_.reserve(rows * columns);
for (unsigned int v = 0; v < height; v += pixel_step)
{
for (unsigned int u = 0; u < width; u += pixel_step)
{
const double x = (static_cast<double>(u) - cx) / fx;
const double y = (static_cast<double>(v) - cy) / fy;
const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
depth_ray_cache_.push_back(
DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
}
}
}
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, unsigned int cell_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,
cell_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 =
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 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);
updateDepthRayCache(width, height, step, fx, fy, cx, cy);
double qx = tfm.transform.rotation.x;
double qy = tfm.transform.rotation.y;
double qz = tfm.transform.rotation.z;
double qw = tfm.transform.rotation.w;
const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
if (quaternion_norm <= 0.0)
return false;
qx /= quaternion_norm;
qy /= quaternion_norm;
qz /= quaternion_norm;
qw /= quaternion_norm;
const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
const double r01 = 2.0 * (qx * qy - qz * qw);
const double r02 = 2.0 * (qx * qz + qy * qw);
const double r10 = 2.0 * (qx * qy + qz * qw);
const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
const double r12 = 2.0 * (qy * qz - qx * qw);
const double r20 = 2.0 * (qx * qz - qy * qw);
const double r21 = 2.0 * (qy * qz + qx * qw);
const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
const unsigned int cell_raytrace_range = cellDistance(max_range);
bool cleared_any = false;
for (const DepthRay& local_ray : depth_ray_cache_)
{
double depth_m = 0.0;
bool valid = false;
if (!readDepthMeters(depth, local_ray.u, local_ray.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;
robot_geometry_msgs::Vector3 global_ray;
global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
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, cell_raytrace_range,
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
int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// Most update cycles do not cross a costmap cell boundary. Avoid copying and
// resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
if (cell_ox == 0 && cell_oy == 0)
return;
// compute the associated world coordinates for the origin cell
// beacuase we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min(std::max(cell_ox, 0), size_x);
lower_left_y = std::min(std::max(cell_oy, 0), size_y);
upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
rolling_costmap_scratch_.resize(overlap_size);
rolling_voxel_scratch_.resize(overlap_size);
rolling_stamp_scratch_.resize(overlap_size);
unsigned char* local_map = rolling_costmap_scratch_.data();
unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
double* local_stamp_map = rolling_stamp_scratch_.data();
unsigned int* voxel_map = robot_voxel_grid_.getData();
if (overlap_size > 0)
{
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(cell_last_marked_.data(), lower_left_x, lower_left_y, size_x_,
local_stamp_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
}
// we'll reset our maps to unknown space if appropriate
resetMaps();
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
if (overlap_size > 0)
{
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_stamp_map, 0, 0, cell_size_x, cell_last_marked_.data(),
start_x, start_y, size_x_, cell_size_x, cell_size_y);
}
}
// Export factory function
static boost::shared_ptr<Layer> create_voxel_plugin() {
return boost::make_shared<VoxelLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
} // namespace robot_costmap_2d
Reference in New Issue View Git Blame Copy Permalink