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;

43
plugins/obstacle_layer.cpp
View File

@@ -56,20 +56,23 @@ namespace costmap_2d
void ObstacleLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
rolling_window_ = layered_costmap_->isRolling();
ObstacleLayer::matchSize();
current_ = true;
stop_receiving_data_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
std::string config_file_name = "obstacle_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
ObstacleLayer::~ObstacleLayer()
{}
bool ObstacleLayer::getParams(const std::string& config_file_name)
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
@@ -104,6 +107,30 @@ bool ObstacleLayer::getParams(const std::string& config_file_name)
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance);
if (nh.hasParam("topic"))
nh.getParam("topic", topic);
if (nh.hasParam("sensor_frame"))
nh.getParam("sensor_frame", sensor_frame);
if (nh.hasParam("observation_persistence"))
nh.getParam("observation_persistence", observation_keep_time);
if (nh.hasParam("expected_update_rate"))
nh.getParam("expected_update_rate", expected_update_rate);
if (nh.hasParam("data_type"))
nh.getParam("data_type", data_type);
if (nh.hasParam("min_obstacle_height"))
nh.getParam("min_obstacle_height", min_obstacle_height);
if (nh.hasParam("max_obstacle_height"))
nh.getParam("max_obstacle_height", max_obstacle_height);
if (nh.hasParam("inf_is_valid"))
nh.getParam("inf_is_valid", inf_is_valid);
if (nh.hasParam("clearing"))
nh.getParam("clearing", clearing);
if (nh.hasParam("marking"))
nh.getParam("marking", marking);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
printf("Only topics that use point clouds or laser scans are currently supported\n");
@@ -117,10 +144,18 @@ bool ObstacleLayer::getParams(const std::string& config_file_name)
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
int combination_method = loadParam(layer, "combination_method", 1);
if (nh.hasParam("obstacle_range"))
nh.getParam("obstacle_range", obstacle_range);
if (nh.hasParam("raytrace_range"))
nh.getParam("raytrace_range", raytrace_range);
if (nh.hasParam("footprint_clearing_enabled"))
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled);
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method);
// enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;

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;

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()