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

49
plugins/static_layer.cpp
View File

@@ -62,34 +62,55 @@ StaticLayer::~StaticLayer()
void StaticLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
std::string config_file_name = "static_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
bool StaticLayer::getParams(const std::string& config_file_name)
bool StaticLayer::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["static_layer"];
// ===== 1. Load default từ layer (yaml / plugin config) =====
bool enabled = loadParam(layer, "enabled", true);
bool first_map_only = loadParam(layer, "first_map_only", false);
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
bool use_maximum = loadParam(layer, "use_maximum", false);
int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));
bool enabled = loadParam(layer, "enabled", true);
bool first_map_only = loadParam(layer, "first_map_only", false);
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
bool use_maximum = loadParam(layer, "use_maximum", false);
int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));
// ===== 2. Override từ ROS param server (chỉ khi param tồn tại) =====
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled, enabled);
if (nh.hasParam("first_map_only"))
nh.getParam("first_map_only", first_map_only);
if (nh.hasParam("subscribe_to_updates"))
nh.getParam("subscribe_to_updates", subscribe_to_updates);
if (nh.hasParam("track_unknown_space"))
nh.getParam("track_unknown_space", track_unknown_space);
if (nh.hasParam("use_maximum"))
nh.getParam("use_maximum", use_maximum);
if (nh.hasParam("trinary_costmap"))
nh.getParam("trinary_costmap", trinary_costmap);
if (nh.hasParam("base_frame_id"))
nh.getParam("base_frame_id", base_frame_id);
if (nh.hasParam("lethal_cost_threshold"))
nh.getParam("lethal_cost_threshold", lethal_cost_threshold);
if (nh.hasParam("unknown_cost_value"))
nh.getParam("unknown_cost_value", unknown_cost_value);
first_map_only_ = first_map_only;
subscribe_to_updates_ = subscribe_to_updates;