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

DuongTD gui zalo

Browse Source
This commit is contained in:
HiepLM 2026-01-10 10:16:51 +07:00
parent 384897b750
commit b66bd7c751
4 changed files with 29 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
config/static_layer_params.yaml
View File

@ -1,5 +1,6 @@
static_layer:
enabled: true
map_topic: "map"
first_map_only: false
subscribe_to_updates: false
track_unknown_space: true

2
config/voxel_layer_params.yaml
View File

@ -7,5 +7,5 @@ voxel_layer:
z_voxels: 16
unknown_threshold: 15.0
mark_threshold: 0
combination_method: 3
combination_method: 1

51
src/costmap_2d_robot.cpp
View File

@ -102,6 +102,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
global_frame_ = global_frame;
robot_base_frame_ = robot_base_frame;
robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());
robot::Time last_error = robot::Time::now();
std::string tf_error;
@ -137,8 +138,33 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
if (nh.hasParam("library_path"))
path_plugins = loader.findLibraryPath(name_);
robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// find size parameters
double map_width_meters = loadParam(layer, "width", 0.0);
double map_height_meters = loadParam(layer, "height", 0.0);
double resolution = loadParam(layer, "resolution", 0.0);
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (nh.hasParam("width"))
nh.getParam("width", map_width_meters);
if (nh.hasParam("height"))
nh.getParam("height", map_height_meters);
if (nh.hasParam("resolution"))
nh.getParam("resolution", resolution);
if (nh.hasParam("origin_x"))
nh.getParam("origin_x", origin_x);
if (nh.hasParam("origin_y"))
nh.getParam("origin_y", origin_y);
if (!layered_costmap_->isSizeLocked())
{
// robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
struct PluginConfig
{
@ -253,31 +279,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
map_update_thread_shutdown_ = false;
double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
// find size parameters
double map_width_meters = loadParam(layer, "width", 0.0);
double map_height_meters = loadParam(layer, "height", 0.0);
double resolution = loadParam(layer, "resolution", 0.0);
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (nh.hasParam("update_frequency"))
nh.getParam("update_frequency", map_update_frequency);
if (nh.hasParam("width"))
nh.getParam("width", map_width_meters);
if (nh.hasParam("height"))
nh.getParam("height", map_height_meters);
if (nh.hasParam("resolution"))
nh.getParam("resolution", resolution);
if (nh.hasParam("origin_x"))
nh.getParam("origin_x", origin_x);
if (nh.hasParam("origin_y"))
nh.getParam("origin_y", origin_y);
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.

3
src/layered_costmap.cpp
View File

@ -111,7 +111,6 @@ namespace robot_costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
printf("START\n");
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
@ -121,7 +120,6 @@ namespace robot_costmap_2d
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
std::cout << "robot x: " << robot_x << "\nrobot y: " << robot_y << "\nrobot yaw: " << robot_yaw << std::endl;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{
@ -132,7 +130,6 @@ namespace robot_costmap_2d
(*plugin)->getName().c_str());
}
}
printf("END\n");
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);