diff --git a/CMakeLists.txt b/CMakeLists.txt index b29e3d7..cda8307 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,24 +135,24 @@ find_package(PCL REQUIRED COMPONENTS common io) set(TF2_LIBRARY /usr/lib/libtf2.so) # --- Include other message packages if needed --- -if (NOT TARGET sensor_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) -endif() -if (NOT TARGET geometry_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) -endif() -if (NOT TARGET nav_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build) -endif() -if (NOT TARGET map_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build) -endif() -if (NOT TARGET laser_geometry) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build) -endif() -if (NOT TARGET voxel_grid) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build) -endif() +# if (NOT TARGET sensor_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) +# endif() +# if (NOT TARGET geometry_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) +# endif() +# if (NOT TARGET nav_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build) +# endif() +# if (NOT TARGET map_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build) +# endif() +# if (NOT TARGET laser_geometry) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build) +# endif() +# if (NOT TARGET voxel_grid) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build) +# endif() include_directories( include diff --git a/include/costmap_2d/voxel_grid.h b/include/costmap_2d/voxel_grid.h new file mode 100644 index 0000000..6363e6c --- /dev/null +++ b/include/costmap_2d/voxel_grid.h @@ -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 +#include +#include +#include + +namespace costmap_2d +{ + struct VoxelGrid + { + std_msgs::Header header; + std::vector 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 diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h index 7b4fc66..21b6a83 100644 --- a/include/costmap_2d/voxel_layer.h +++ b/include/costmap_2d/voxel_layer.h @@ -38,11 +38,13 @@ #ifndef COSTMAP_2D_VOXEL_LAYER_H_ #define COSTMAP_2D_VOXEL_LAYER_H_ -// #include #include #include #include -// #include +#include +#include +#include + #include #include // #include @@ -52,16 +54,16 @@ // #include // #include // #include -#include #include + namespace costmap_2d { class VoxelLayer : public ObstacleLayer { 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. } @@ -77,29 +79,23 @@ public: { return true; } + virtual void matchSize(); virtual void reset(); protected: - // virtual void setupDynamicReconfigure(ros::NodeHandle& nh); - virtual void resetMaps(); 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); virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y); - // dynamic_reconfigure::Server *voxel_dsrv_; - bool publish_voxel_; - // ros::Publisher voxel_pub_; - // voxel_grid::VoxelGrid voxel_grid_; + voxel_grid::VoxelGrid voxel_grid_; double z_resolution_, origin_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_; - // ros::Publisher clearing_endpoints_pub_; sensor_msgs::PointCloud clearing_endpoints_; 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)); } + bool getParams(); }; } // namespace costmap_2d diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 7c19373..fbc1ffa 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -35,7 +35,7 @@ void StaticLayer::onInitialize() bool StaticLayer::getParams() { 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"]; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index ae2c9b2..3d82878 100644 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -56,393 +56,409 @@ namespace costmap_2d void VoxelLayer::onInitialize() { ObstacleLayer::onInitialize(); - // ros::NodeHandle private_nh("~/" + name_); - -// 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("clearing_endpoints", 1); + getParams(); } -// void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh) -// { -// voxel_dsrv_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = -// [this](auto& config, auto level){ reconfigureCB(config, level); }; -// voxel_dsrv_->setCallback(cb); -// } +bool VoxelLayer::getParams() +{ + try { + YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); + 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", 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() {} -// 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() { ObstacleLayer::matchSize(); - // voxel_grid_.resize(size_x_, size_y_, size_z_); - // ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); + voxel_grid_.resize(size_x_, size_y_, size_z_); + 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() -// { -// deactivate(); -// resetMaps(); -// voxel_grid_.reset(); -// activate(); -// } +void VoxelLayer::reset() +{ + deactivate(); + resetMaps(); + voxel_grid_.reset(); + activate(); +} -// void VoxelLayer::resetMaps() -// { -// Costmap2D::resetMaps(); -// voxel_grid_.reset(); -// } +void VoxelLayer::resetMaps() +{ + Costmap2D::resetMaps(); + voxel_grid_.reset(); +} -// 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); +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 observations, clearing_observations; + bool current = true; + std::vector observations, clearing_observations; -// // get the marking observations -// current = getMarkingObservations(observations) && current; + // get the marking observations + current = getMarkingObservations(observations) && current; -// // get the clearing observations -// current = getClearingObservations(clearing_observations) && current; + // get the clearing observations + current = getClearingObservations(clearing_observations) && current; -// // update the global current status -// current_ = current; + // update the global current status + current_ = current; -// // raytrace freespace -// for (unsigned int i = 0; i < clearing_observations.size(); ++i) -// { -// raytraceFreespace(clearing_observations[i], 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 -// for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++it) -// { -// const Observation& obs = *it; + // place the new obstacles into a priority queue... each with a priority of zero to begin with + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++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 iter_x(cloud, "x"); -// sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); -// sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2ConstIterator 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; + 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); + // 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; + // 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; -// } + // 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 (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) -// { -// unsigned int index = getIndex(mx, my); + // 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_)) + { + unsigned int index = getIndex(mx, my); -// costmap_[index] = LETHAL_OBSTACLE; -// touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y); -// } -// } -// } + costmap_[index] = LETHAL_OBSTACLE; + touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y); + } + } + } -// if (publish_voxel_) -// { -// costmap_2d::VoxelGrid grid_msg; -// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); -// grid_msg.size_x = voxel_grid_.sizeX(); -// grid_msg.size_y = voxel_grid_.sizeY(); -// grid_msg.size_z = voxel_grid_.sizeZ(); -// grid_msg.data.resize(size); -// memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + // if (publish_voxel_) + // { + // costmap_2d::VoxelGrid grid_msg; + // unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); + // grid_msg.size_x = voxel_grid_.sizeX(); + // grid_msg.size_y = voxel_grid_.sizeY(); + // grid_msg.size_z = voxel_grid_.sizeZ(); + // grid_msg.data.resize(size); + // memcpy(&grid_msg.data[0], 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.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 = ros::Time::now(); -// voxel_pub_.publish(grid_msg); -// } + // 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(); -// 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) -// { -// // get the cell coordinates of the center point of the window -// unsigned int mx, my; -// if (!worldToMap(wx, wy, mx, my)) -// return; + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} -// // 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; +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; -// // 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); + // 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; -// end_x = std::min(origin_x_ + getSizeInMetersX(), end_x); -// end_y = std::min(origin_y_ + getSizeInMetersY(), end_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); -// // get the map coordinates of the bounds of the window -// unsigned int map_sx, map_sy, map_ex, map_ey; + end_x = std::min(origin_x_ + getSizeInMetersX(), end_x); + end_y = std::min(origin_y_ + getSizeInMetersY(), end_y); -// // 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; + // get the map coordinates of the bounds of the window + unsigned int map_sx, map_sy, map_ex, map_ey; -// // 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; -// voxel_grid_.clearVoxelColumn(index); -// } -// } -// current++; -// index++; -// } -// current += size_x_ - (map_ex - map_sx) - 1; -// index += size_x_ - (map_ex - map_sx) - 1; -// } -// } + // 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; -// 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; + // 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; + 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; -// double ox = clearing_observation.origin_.x; -// double oy = clearing_observation.origin_.y; -// double oz = clearing_observation.origin_.z; +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; -// if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) -// { -// ROS_WARN_THROTTLE( -// 1.0, -// "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; -// } + 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; -// 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); -// } + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) + { + printf("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(); + /////////////////////////////////////////// + ////////////THAY THẾ PUBLISH NÀY/////////// + /////////////////////////////////////////// + // 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 iter_x(*(clearing_observation.cloud_), "x"); -// sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); -// sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + // 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(); -// for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) -// { -// double wpx = *iter_x; -// double wpy = *iter_y; -// double wpz = *iter_z; + sensor_msgs::PointCloud2ConstIterator iter_x(*(clearing_observation.cloud_), "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "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; + 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 a = wpx - ox; -// double b = wpy - oy; -// double c = wpz - oz; -// double t = 1.0; + 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; -// // 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); -// } + double a = wpx - ox; + double b = wpy - oy; + double c = wpz - oz; + double t = 1.0; -// // 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); -// } + // 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 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); -// } + // 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); + } -// wpx = ox + a * t; -// wpy = oy + b * t; -// wpz = oz + c * t; + // 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); + } -// 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_); + wpx = ox + a * t; + wpy = oy + b * t; + wpz = oz + c * t; -// // 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); + 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_); -// 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) -// { -// geometry_msgs::Point32 point; -// point.x = wpx; -// point.y = wpy; -// point.z = wpz; -// clearing_endpoints_.points.push_back(point); -// } -// } -// } + updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); -// 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; + // if (publish_clearing_points) + // { + geometry_msgs::Point32 point; + point.x = wpx; + 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) -// { -// // 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_); + // clearing_endpoints_pub_.publish(clearing_endpoints_); + // } +} -// // 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_; +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_); -// // To save casting from unsigned int to int a bunch of times -// int size_x = size_x_; -// int size_y = size_y_; + // 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_; -// // 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); + // To save casting from unsigned int to int a bunch of times + int size_x = size_x_; + int size_y = size_y_; -// unsigned int cell_size_x = upper_right_x - lower_left_x; -// unsigned int cell_size_y = upper_right_y - lower_left_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); -// // we need a map to store the obstacles in the window temporarily -// unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; -// unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; -// unsigned int* voxel_map = voxel_grid_.getData(); + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; -// // copy the local window in the costmap to the local map -// 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); + // we need a map to store the obstacles in the window temporarily + unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; + unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; + unsigned int* voxel_map = voxel_grid_.getData(); -// // we'll reset our maps to unknown space if appropriate -// resetMaps(); + // copy the local window in the costmap to the local map + 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 -// origin_x_ = new_grid_ox; -// origin_y_ = new_grid_oy; + // we'll reset our maps to unknown space if appropriate + resetMaps(); -// // 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; + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; -// // now we want to copy the overlapping information back into the map, but in its new location -// 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); + // 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; -// // make sure to clean up -// delete[] local_map; -// delete[] local_voxel_map; -// } + // now we want to copy the overlapping information back into the map, but in its new location + 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); + + // make sure to clean up + delete[] local_map; + delete[] local_voxel_map; +} // Export factory function static PluginLayerPtr create_voxel_plugin() {