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

38
plugins/inflation_layer.cpp
View File

@@ -71,23 +71,24 @@ InflationLayer::InflationLayer()
void InflationLayer::onInitialize()
{
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
current_ = true;
if (seen_)
delete[] seen_;
seen_ = NULL;
seen_size_ = 0;
need_reinflation_ = false;
std::string config_file_name = name_ + ".yaml";
std::cout << "InflationLayer: " << config_file_name << std::endl;
getParams(config_file_name);
}
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
current_ = true;
if (seen_)
delete[] seen_;
seen_ = NULL;
seen_size_ = 0;
need_reinflation_ = false;
std::string config_file_name = "inflation_layer_params.yaml";
// std::cout << "InflationLayer: " << config_file_name << std::endl;
getParams(config_file_name, priv_nh);
matchSize();
}
bool InflationLayer::getParams(const std::string& config_file_name)
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
@@ -97,11 +98,22 @@ bool InflationLayer::getParams(const std::string& config_file_name)
YAML::Node layer = config["inflation_layer"];
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
if (nh.hasParam("cost_scaling_factor"))
nh.getParam("cost_scaling_factor", cost_scaling_factor);
if (nh.hasParam("inflation_radius"))
nh.getParam("inflation_radius", inflation_radius);
setInflationParameters(inflation_radius, cost_scaling_factor);
bool enabled = loadParam(layer, "enabled", true);
bool inflate_unknown = loadParam(layer, "inflate_unknown", false);
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled);
if (nh.hasParam("inflate_unknown"))
nh.getParam("inflate_unknown", inflate_unknown);
if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) {
enabled_ = enabled;
inflate_unknown_ = inflate_unknown;