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

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;