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

update load param using node handle

Browse Source
This commit is contained in:
duongtd
2025-12-11 17:16:46 +07:00
parent 22cfd07519
commit c53db2280e
20 changed files with 397 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
plugins/voxel_layer.cpp
View File

@@ -53,41 +53,77 @@ namespace costmap_2d
void VoxelLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
ObstacleLayer::onInitialize();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
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)
bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["voxel_layer"];
try
{
std::string folder = COSTMAP_2D_DIR;
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.0);
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);
this->matchSize();
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
// 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);
int size_z, unknown_threshold, mark_threshold;
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;
}
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_);
return true;
this->matchSize();
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
void VoxelLayer::matchSize()