Hiep update

This commit is contained in:
2025-12-30 10:24:18 +07:00
parent 4246453ae6
commit 72b2f3c639
49 changed files with 476 additions and 476 deletions

View File

@@ -35,20 +35,20 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/voxel_layer.h>
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace costmap_2d
namespace robot_costmap_2d
{
void VoxelLayer::onInitialize()
@@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
{
try
{
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -228,7 +228,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// if (publish_voxel_)
// {
// costmap_2d::VoxelGrid grid_msg;
// 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();
@@ -490,4 +490,4 @@ static boost::shared_ptr<Layer> create_voxel_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
} // namespace costmap_2d
} // namespace robot_costmap_2d