diff --git a/CMakeLists.txt b/CMakeLists.txt index a638b93..579968c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,9 +61,9 @@ target_link_libraries(robot_costmap_2d geometry_msgs robot_nav_msgs robot_map_msgs - laser_geometry + robot_laser_geometry robot_visualization_msgs - voxel_grid + robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs diff --git a/include/robot_costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h index c323bf9..1db6bab 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include #include @@ -149,7 +149,7 @@ protected: std::string global_frame_; ///< @brief The global frame for the costmap double max_obstacle_height_; ///< @brief Max Obstacle Height - laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds + robot_laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index a38dff7..3bdcf8c 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -44,12 +44,12 @@ #include #include #include -#include +#include #include #include #include #include -#include +#include namespace robot_costmap_2d { @@ -58,7 +58,7 @@ class VoxelLayer : public ObstacleLayer { public: VoxelLayer() : - voxel_grid_(0, 0, 0) + robot_voxel_grid_(0, 0, 0) { costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D. } @@ -90,7 +90,7 @@ private: bool publish_voxel_; - voxel_grid::VoxelGrid voxel_grid_; + robot_voxel_grid::VoxelGrid robot_voxel_grid_; double z_resolution_, origin_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_; robot_sensor_msgs::PointCloud clearing_endpoints_; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index e8a746d..e021846 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -126,11 +126,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl void VoxelLayer::matchSize() { ObstacleLayer::matchSize(); - voxel_grid_.resize(size_x_, size_y_, size_z_); - if (!(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_)) + robot_voxel_grid_.resize(size_x_, size_y_, size_z_); + if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_)) { std::cerr << "[FATAL] Voxel grid size mismatch: " - << "voxel(" << voxel_grid_.sizeX() << ", " << voxel_grid_.sizeY() + << "voxel(" << robot_voxel_grid_.sizeX() << ", " << robot_voxel_grid_.sizeY() << ") but costmap(" << size_x_ << ", " << size_y_ << ")\n"; std::abort(); // dừng chương trình } @@ -140,14 +140,14 @@ void VoxelLayer::reset() { deactivate(); resetMaps(); - voxel_grid_.reset(); + robot_voxel_grid_.reset(); activate(); } void VoxelLayer::resetMaps() { Costmap2D::resetMaps(); - voxel_grid_.reset(); + robot_voxel_grid_.reset(); } void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, @@ -216,7 +216,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, } // 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 (robot_voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) { unsigned int index = getIndex(mx, my); @@ -229,12 +229,12 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, // if (publish_voxel_) // { // robot_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(); + // unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY(); + // grid_msg.size_x = robot_voxel_grid_.sizeX(); + // grid_msg.size_y = robot_voxel_grid_.sizeY(); + // grid_msg.size_z = robot_voxel_grid_.sizeZ(); // grid_msg.data.resize(size); - // memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + // memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int)); // grid_msg.origin.x = origin_x_; // grid_msg.origin.y = origin_y_; @@ -291,7 +291,7 @@ void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_ if (clear_no_info || *current != NO_INFORMATION) { *current = FREE_SPACE; - voxel_grid_.clearVoxelColumn(index); + robot_voxel_grid_.clearVoxelColumn(index); } } current++; @@ -397,8 +397,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub { 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); - voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, + // 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); @@ -455,7 +455,7 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_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* voxel_map = robot_voxel_grid_.getData(); // 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);