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

update file voxel_layer

Browse Source
This commit is contained in:
duongtd 2025-11-17 10:50:23 +07:00
parent 49a72383c8
commit 42840e3bbc
5 changed files with 393 additions and 348 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
CMakeLists.txt
View File

@ -135,24 +135,24 @@ find_package(PCL REQUIRED COMPONENTS common io)
set(TF2_LIBRARY /usr/lib/libtf2.so) set(TF2_LIBRARY /usr/lib/libtf2.so)
# --- Include other message packages if needed --- # --- Include other message packages if needed ---
if (NOT TARGET sensor_msgs) # if (NOT TARGET sensor_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
endif() # endif()
if (NOT TARGET geometry_msgs) # if (NOT TARGET geometry_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
endif() # endif()
if (NOT TARGET nav_msgs) # if (NOT TARGET nav_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build)
endif() # endif()
if (NOT TARGET map_msgs) # if (NOT TARGET map_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
endif() # endif()
if (NOT TARGET laser_geometry) # if (NOT TARGET laser_geometry)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build)
endif() # endif()
if (NOT TARGET voxel_grid) # if (NOT TARGET voxel_grid)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
endif() # endif()
include_directories( include_directories(
include include

32
include/costmap_2d/voxel_grid.h Normal file
View File

@ -0,0 +1,32 @@
// Header header
// uint32[] data
// geometry_msgs/Point32 origin
// geometry_msgs/Vector3 resolutions
// uint32 size_x
// uint32 size_y
// uint32 size_z
#ifndef VOXEL_GRID_COSTMAP_2D_H
#define VOXEL_GRID_COSTMAP_2D_H
#include <vector>
#include <std_msgs/Header.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Vector3.h>
namespace costmap_2d
{
struct VoxelGrid
{
std_msgs::Header header;
std::vector<uint32_t> data;
geometry_msgs::Point32 origin;
geometry_msgs::Vector3 resolutions;
uint32_t size_x;
uint32_t size_y;
uint32_t size_z;
};
}
#endif // VOXEL_GRID_COSTMAP_2D_H

21
include/costmap_2d/voxel_layer.h
View File

@ -38,11 +38,13 @@
#ifndef COSTMAP_2D_VOXEL_LAYER_H_ #ifndef COSTMAP_2D_VOXEL_LAYER_H_
#define COSTMAP_2D_VOXEL_LAYER_H_ #define COSTMAP_2D_VOXEL_LAYER_H_
// #include <ros/ros.h>
#include <costmap_2d/layer.h> #include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h> #include <costmap_2d/observation_buffer.h>
// #include <costmap_2d/voxel_grid.h> #include <costmap_2d/voxel_grid.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/utils.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
// #include <laser_geometry/laser_geometry.h> // #include <laser_geometry/laser_geometry.h>
@ -52,16 +54,16 @@
// #include <message_filters/subscriber.h> // #include <message_filters/subscriber.h>
// #include <dynamic_reconfigure/server.h> // #include <dynamic_reconfigure/server.h>
// #include <costmap_2d/VoxelPluginConfig.h> // #include <costmap_2d/VoxelPluginConfig.h>
#include <costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h> #include <voxel_grid/voxel_grid.h>
namespace costmap_2d namespace costmap_2d
{ {
class VoxelLayer : public ObstacleLayer class VoxelLayer : public ObstacleLayer
{ {
public: public:
VoxelLayer() //: voxel_grid_(0, 0, 0) VoxelLayer() : voxel_grid_(0, 0, 0)
{ {
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D. costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
} }
@ -77,29 +79,23 @@ public:
{ {
return true; return true;
} }
virtual void matchSize(); virtual void matchSize();
virtual void reset(); virtual void reset();
protected: protected:
// virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
virtual void resetMaps(); virtual void resetMaps();
private: private:
// void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
// dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
bool publish_voxel_; bool publish_voxel_;
// ros::Publisher voxel_pub_; voxel_grid::VoxelGrid voxel_grid_;
// voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_; double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_;
// ros::Publisher clearing_endpoints_pub_;
sensor_msgs::PointCloud clearing_endpoints_; sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz) inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
@ -142,6 +138,7 @@ private:
{ {
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0)); return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
} }
bool getParams();
}; };
} // namespace costmap_2d } // namespace costmap_2d

2
plugins/static_layer.cpp
View File

@ -35,7 +35,7 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams() bool StaticLayer::getParams()
{ {
try { try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
YAML::Node layer = config["static_layer"]; YAML::Node layer = config["static_layer"];

650
plugins/voxel_layer.cpp
View File

@ -56,393 +56,409 @@ namespace costmap_2d
void VoxelLayer::onInitialize() void VoxelLayer::onInitialize()
{ {
ObstacleLayer::onInitialize(); ObstacleLayer::onInitialize();
// ros::NodeHandle private_nh("~/" + name_); getParams();
// private_nh.param("publish_voxel_map", publish_voxel_, false);
// if (publish_voxel_)
// voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
// clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
} }
// void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh) bool VoxelLayer::getParams()
// { {
// voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh); try {
// dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
// [this](auto& config, auto level){ reconfigureCB(config, level); }; YAML::Node layer = config["voxel_layer"];
// voxel_dsrv_->setCallback(cb);
// } 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", true);
size_z_ = loadParam(layer, "z_voxels", true);
origin_z_ = loadParam(layer, "origin_z", true);
z_resolution_ = loadParam(layer, "z_resolution", true);
unknown_threshold_ = loadParam(layer, "max_obstacle_height", true);
mark_threshold_ = loadParam(layer, "mark_threshold", true);
combination_method_ = loadParam(layer, "combination_method", true);
this->matchSize();
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
VoxelLayer::~VoxelLayer() VoxelLayer::~VoxelLayer()
{} {}
// void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
// {
// enabled_ = config.enabled;
// footprint_clearing_enabled_ = config.footprint_clearing_enabled;
// max_obstacle_height_ = config.max_obstacle_height;
// size_z_ = config.z_voxels;
// origin_z_ = config.origin_z;
// z_resolution_ = config.z_resolution;
// unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
// mark_threshold_ = config.mark_threshold;
// combination_method_ = config.combination_method;
// matchSize();
// }
void VoxelLayer::matchSize() void VoxelLayer::matchSize()
{ {
ObstacleLayer::matchSize(); ObstacleLayer::matchSize();
// voxel_grid_.resize(size_x_, size_y_, size_z_); voxel_grid_.resize(size_x_, size_y_, size_z_);
// ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); if (!(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_))
{
std::cerr << "[FATAL] Voxel grid size mismatch: "
<< "voxel(" << voxel_grid_.sizeX() << ", " << voxel_grid_.sizeY()
<< ") but costmap(" << size_x_ << ", " << size_y_ << ")\n";
std::abort(); // dừng chương trình
}
} }
// void VoxelLayer::reset() void VoxelLayer::reset()
// { {
// deactivate(); deactivate();
// resetMaps(); resetMaps();
// voxel_grid_.reset(); voxel_grid_.reset();
// activate(); activate();
// } }
// void VoxelLayer::resetMaps() void VoxelLayer::resetMaps()
// { {
// Costmap2D::resetMaps(); Costmap2D::resetMaps();
// voxel_grid_.reset(); voxel_grid_.reset();
// } }
// void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
// double* min_y, double* max_x, double* max_y) double* min_y, double* max_x, double* max_y)
// { {
// if (rolling_window_) if (rolling_window_)
// updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
// useExtraBounds(min_x, min_y, max_x, max_y); useExtraBounds(min_x, min_y, max_x, max_y);
// bool current = true; bool current = true;
// std::vector<Observation> observations, clearing_observations; std::vector<Observation> observations, clearing_observations;
// // get the marking observations // get the marking observations
// current = getMarkingObservations(observations) && current; current = getMarkingObservations(observations) && current;
// // get the clearing observations // get the clearing observations
// current = getClearingObservations(clearing_observations) && current; current = getClearingObservations(clearing_observations) && current;
// // update the global current status // update the global current status
// current_ = current; current_ = current;
// // raytrace freespace // raytrace freespace
// for (unsigned int i = 0; i < clearing_observations.size(); ++i) for (unsigned int i = 0; i < clearing_observations.size(); ++i)
// { {
// raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); 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 // place the new obstacles into a priority queue... each with a priority of zero to begin with
// for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it) for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
// { {
// const Observation& obs = *it; const Observation& obs = *it;
// const sensor_msgs::PointCloud2& cloud = *(obs.cloud_); const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
// double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y"); sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z"); sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
// for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_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 the obstacle is too high or too far away from the robot we won't add it
// if (*iter_z > max_obstacle_height_) if (*iter_z > max_obstacle_height_)
// continue; continue;
// // compute the squared distance from the hitpoint to the pointcloud's origin // 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) double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
// + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
// + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z); + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
// // if the point is far enough away... we won't consider it // if the point is far enough away... we won't consider it
// if (sq_dist >= sq_obstacle_range) if (sq_dist >= sq_obstacle_range)
// continue; continue;
// // now we need to compute the map coordinates for the observation // now we need to compute the map coordinates for the observation
// unsigned int mx, my, mz; unsigned int mx, my, mz;
// if (*iter_z < origin_z_) if (*iter_z < origin_z_)
// { {
// if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz)) if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
// continue; continue;
// } }
// else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
// { {
// continue; continue;
// } }
// // mark the cell in the voxel grid and check if we should also mark it in the costmap // mark the cell in the voxel grid and check if we should also mark it in the costmap
// if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
// { {
// unsigned int index = getIndex(mx, my); unsigned int index = getIndex(mx, my);
// costmap_[index] = LETHAL_OBSTACLE; costmap_[index] = LETHAL_OBSTACLE;
// touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y); touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
// } }
// } }
// } }
// if (publish_voxel_) // if (publish_voxel_)
// { // {
// costmap_2d::VoxelGrid grid_msg; // costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); // unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX(); // grid_msg.size_x = voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY(); // grid_msg.size_y = voxel_grid_.sizeY();
// grid_msg.size_z = voxel_grid_.sizeZ(); // grid_msg.size_z = voxel_grid_.sizeZ();
// grid_msg.data.resize(size); // grid_msg.data.resize(size);
// memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); // memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
// grid_msg.origin.x = origin_x_; // grid_msg.origin.x = origin_x_;
// grid_msg.origin.y = origin_y_; // grid_msg.origin.y = origin_y_;
// grid_msg.origin.z = origin_z_; // grid_msg.origin.z = origin_z_;
// grid_msg.resolutions.x = resolution_; // grid_msg.resolutions.x = resolution_;
// grid_msg.resolutions.y = resolution_; // grid_msg.resolutions.y = resolution_;
// grid_msg.resolutions.z = z_resolution_; // grid_msg.resolutions.z = z_resolution_;
// grid_msg.header.frame_id = global_frame_; // grid_msg.header.frame_id = global_frame_;
// grid_msg.header.stamp = ros::Time::now(); // 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); // ///////////////////////////////////////////
// } // ////////////THAY THẾ PUBLISH NÀY///////////
// ///////////////////////////////////////////
// // voxel_pub_.publish(grid_msg);
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// ///////////////////////////////////////////
// }
// void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info) updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
// { }
// // 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 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
// double start_x = wx - w_size_x / 2; {
// double start_y = wy - w_size_y / 2; // get the cell coordinates of the center point of the window
// double end_x = start_x + w_size_x; unsigned int mx, my;
// double end_y = start_y + w_size_y; if (!worldToMap(wx, wy, mx, my))
return;
// // scale the window based on the bounds of the costmap // compute the bounds of the window
// start_x = std::max(origin_x_, start_x); double start_x = wx - w_size_x / 2;
// start_y = std::max(origin_y_, start_y); double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
// end_x = std::min(origin_x_ + getSizeInMetersX(), end_x); // scale the window based on the bounds of the costmap
// end_y = std::min(origin_y_ + getSizeInMetersY(), end_y); start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
// // get the map coordinates of the bounds of the window end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
// unsigned int map_sx, map_sy, map_ex, map_ey; end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
// // check for legality just in case // get the map coordinates of the bounds of the window
// if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)) unsigned int map_sx, map_sy, 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 // check for legality just in case
// unsigned int index = getIndex(map_sx, map_sy); if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
// unsigned char* current = &costmap_[index]; return;
// 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;
// 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, // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
// double* max_x, double* max_y) unsigned int index = getIndex(map_sx, map_sy);
// { unsigned char* current = &costmap_[index];
// size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width; for (unsigned int j = map_sy; j <= map_ey; ++j)
// if (clearing_observation_cloud_size == 0) {
// return; 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;
voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
// double sensor_x, sensor_y, sensor_z; void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
// double ox = clearing_observation.origin_.x; double* max_x, double* max_y)
// double oy = clearing_observation.origin_.y; {
// double oz = clearing_observation.origin_.z; size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
if (clearing_observation_cloud_size == 0)
return;
// if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) double sensor_x, sensor_y, sensor_z;
// { double ox = clearing_observation.origin_.x;
// ROS_WARN_THROTTLE( double oy = clearing_observation.origin_.y;
// 1.0, double oz = clearing_observation.origin_.z;
// "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
// ox, oy, oz);
// return;
// }
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0); if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
// if (publish_clearing_points) {
// { printf("The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
// clearing_endpoints_.points.clear(); ox, oy, oz);
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size); 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(); ////////////THAY THẾ PUBLISH NÀY///////////
// double map_end_y = origin_y_ + getSizeInMetersY(); ///////////////////////////////////////////
// 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);
// }
///////////////////////////////////////////
///////////////////////////////////////////
///////////////////////////////////////////
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x"); // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y"); double map_end_x = origin_x_ + getSizeInMetersX();
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z"); double map_end_y = origin_y_ + getSizeInMetersY();
// for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
// { sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
// double wpx = *iter_x; sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
// double wpy = *iter_y;
// double wpz = *iter_z;
// double distance = dist(ox, oy, oz, wpx, wpy, wpz); for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
// double scaling_fact = 1.0; {
// scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0); double wpx = *iter_x;
// wpx = scaling_fact * (wpx - ox) + ox; double wpy = *iter_y;
// wpy = scaling_fact * (wpy - oy) + oy; double wpz = *iter_z;
// wpz = scaling_fact * (wpz - oz) + oz;
// double a = wpx - ox; double distance = dist(ox, oy, oz, wpx, wpy, wpz);
// double b = wpy - oy; double scaling_fact = 1.0;
// double c = wpz - oz; scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
// double t = 1.0; wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
// // we can only raytrace to a maximum z height double a = wpx - ox;
// if (wpz > max_obstacle_height_) double b = wpy - oy;
// { double c = wpz - oz;
// // we know we want the vector's z value to be max_z double t = 1.0;
// 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 // we can only raytrace to a maximum z height
// if (wpx < origin_x_) if (wpz > max_obstacle_height_)
// { {
// t = std::min(t, (origin_x_ - ox) / a); // 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));
// if (wpy < origin_y_) }
// { // and we can only raytrace down to the floor
// t = std::min(t, (origin_y_ - oy) / b); 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 maximum value to raytrace to is the end of the map // the minimum value to raytrace from is the origin
// if (wpx > map_end_x) if (wpx < origin_x_)
// { {
// t = std::min(t, (map_end_x - ox) / a); t = std::min(t, (origin_x_ - ox) / a);
// } }
// if (wpy > map_end_y) if (wpy < origin_y_)
// { {
// t = std::min(t, (map_end_y - oy) / b); t = std::min(t, (origin_y_ - oy) / b);
// } }
// wpx = ox + a * t; // the maximum value to raytrace to is the end of the map
// wpy = oy + b * t; if (wpx > map_end_x)
// wpz = oz + c * t; {
t = std::min(t, (map_end_x - ox) / a);
}
if (wpy > map_end_y)
{
t = std::min(t, (map_end_y - oy) / b);
}
// double point_x, point_y, point_z; wpx = ox + a * t;
// if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) wpy = oy + b * t;
// { wpz = oz + c * t;
// unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); double point_x, point_y, point_z;
// voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
// unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, {
// cell_raytrace_range); unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
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);
// if (publish_clearing_points) updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
// {
// geometry_msgs::Point32 point;
// point.x = wpx;
// point.y = wpy;
// point.z = wpz;
// clearing_endpoints_.points.push_back(point);
// }
// }
// }
// if (publish_clearing_points) // if (publish_clearing_points)
// { // {
// clearing_endpoints_.header.frame_id = global_frame_; geometry_msgs::Point32 point;
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; point.x = wpx;
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back(point);
// }
}
}
// clearing_endpoints_pub_.publish(clearing_endpoints_); // 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;
// void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) // clearing_endpoints_pub_.publish(clearing_endpoints_);
// { // }
// // 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_);
// // compute the associated world coordinates for the origin cell void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
// // beacuase we want to keep things grid-aligned {
// double new_grid_ox, new_grid_oy; // project the new origin into the grid
// new_grid_ox = origin_x_ + cell_ox * resolution_; int cell_ox, cell_oy;
// new_grid_oy = origin_y_ + cell_oy * resolution_; cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// // To save casting from unsigned int to int a bunch of times // compute the associated world coordinates for the origin cell
// int size_x = size_x_; // beacuase we want to keep things grid-aligned
// int size_y = size_y_; double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// // we need to compute the overlap of the new and existing windows // To save casting from unsigned int to int a bunch of times
// int lower_left_x, lower_left_y, upper_right_x, upper_right_y; int size_x = size_x_;
// lower_left_x = std::min(std::max(cell_ox, 0), size_x); int size_y = size_y_;
// 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; // we need to compute the overlap of the new and existing windows
// unsigned int cell_size_y = upper_right_y - lower_left_y; 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);
// // we need a map to store the obstacles in the window temporarily unsigned int cell_size_x = upper_right_x - lower_left_x;
// unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; unsigned int cell_size_y = upper_right_y - lower_left_y;
// unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
// unsigned int* voxel_map = voxel_grid_.getData();
// // copy the local window in the costmap to the local map // we need a map to store the obstacles in the window temporarily
// copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); unsigned char* local_map = new unsigned char[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, unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
// cell_size_y); unsigned int* voxel_map = voxel_grid_.getData();
// // we'll reset our maps to unknown space if appropriate // copy the local window in the costmap to the local map
// resetMaps(); 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);
// // update the origin with the appropriate world coordinates // we'll reset our maps to unknown space if appropriate
// origin_x_ = new_grid_ox; resetMaps();
// origin_y_ = new_grid_oy;
// // compute the starting cell location for copying data back in // update the origin with the appropriate world coordinates
// int start_x = lower_left_x - cell_ox; origin_x_ = new_grid_ox;
// int start_y = lower_left_y - cell_oy; origin_y_ = new_grid_oy;
// // now we want to copy the overlapping information back into the map, but in its new location // compute the starting cell location for copying data back in
// copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); int start_x = lower_left_x - cell_ox;
// copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y); int start_y = lower_left_y - cell_oy;
// // make sure to clean up // now we want to copy the overlapping information back into the map, but in its new location
// delete[] local_map; copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// delete[] local_voxel_map; copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// }
// make sure to clean up
delete[] local_map;
delete[] local_voxel_map;
}
// Export factory function // Export factory function
static PluginLayerPtr create_voxel_plugin() { static PluginLayerPtr create_voxel_plugin() {