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

update file costmap_2d_robot and file config params

Browse Source
This commit is contained in:
duongtd
2025-12-03 11:36:06 +07:00
parent 64db092d46
commit 4fb2554291
16 changed files with 169 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
plugins/directional_layer.cpp
View File

@@ -15,10 +15,8 @@ using costmap_2d::NO_INFORMATION;
namespace costmap_2d
{
DirectionalLayer::DirectionalLayer()
{
// ros::NodeHandle nh("~/" + name_);
// lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
}
{}
DirectionalLayer::~DirectionalLayer() {}
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)

15
plugins/inflation_layer.cpp
View File

@@ -79,26 +79,19 @@ void InflationLayer::onInitialize()
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);
}
matchSize();
}
bool InflationLayer::getParams()
bool InflationLayer::getParams(const std::string& config_file_name)
{
try {
std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["inflation_layer"];

20
plugins/obstacle_layer.cpp
View File

@@ -62,26 +62,18 @@ void ObstacleLayer::onInitialize()
current_ = true;
stop_receiving_data_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
getParams();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
}
ObstacleLayer::~ObstacleLayer()
{}
bool ObstacleLayer::getParams()
bool ObstacleLayer::getParams(const std::string& config_file_name)
{
try {
std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
@@ -94,9 +86,9 @@ bool ObstacleLayer::getParams()
default_value_ = FREE_SPACE;
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
// get the topics that we'll subscribe to from the parameter server
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
printf(" Subscribed to Topics: %s\n", topics_string.c_str());
// // get the topics that we'll subscribe to from the parameter server
// std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
// printf("Subscribed to Topics: %s\n", topics_string.c_str());
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;

14
plugins/static_layer.cpp
View File

@@ -65,24 +65,16 @@ void StaticLayer::onInitialize()
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
getParams();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
}
bool StaticLayer::getParams()
bool StaticLayer::getParams(const std::string& config_file_name)
{
try {
std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);

22
plugins/voxel_layer.cpp
View File

@@ -54,26 +54,18 @@ namespace costmap_2d
void VoxelLayer::onInitialize()
{
ObstacleLayer::onInitialize();
getParams();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
}
VoxelLayer::~VoxelLayer()
{}
bool VoxelLayer::getParams()
bool VoxelLayer::getParams(const std::string& config_file_name)
{
try {
std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout << "/cfg folder not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["voxel_layer"];
@@ -82,12 +74,12 @@ bool VoxelLayer::getParams()
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);
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, "max_obstacle_height", 15);
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", true);
combination_method_ = loadParam(layer, "combination_method", 0.0);
this->matchSize();
}
catch (const YAML::BadFile& e) {