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

fix file utils

Browse Source
This commit is contained in:
duongtd 2025-12-08 11:03:49 +07:00
parent af43332b28
commit d37daab594
4 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -62,6 +62,7 @@ target_link_libraries(costmap_2d
nav_msgs nav_msgs
map_msgs map_msgs
laser_geometry laser_geometry
visualization_msgs
voxel_grid voxel_grid
tf3 tf3
tf3_geometry_msgs tf3_geometry_msgs

2
config/costmap_params.yaml
View File

@ -27,7 +27,7 @@ costmap_2d:
layer_config_file_name: layer_params layer_config_file_name: layer_params
transform_tolerance: 0.0 transform_tolerance: 0.0
update_frequency: 0.0 update_frequency: 1.0
width: 0.0 width: 0.0
height: 0.0 height: 0.0
resolution: 0.0 resolution: 0.0

6
include/costmap_2d/utils.h
View File

@ -25,8 +25,10 @@ namespace costmap_2d
for (const auto& p : node) { for (const auto& p : node) {
if (p.size() != 2) if (p.size() != 2)
throw std::runtime_error("Footprint point must be [x, y]"); throw std::runtime_error("Footprint point must be [x, y]");
geometry_msgs::Point point;
fp.push_back(geometry_msgs::Point{p[0].as<double>(), p[1].as<double>()}); point.x = p[0].as<double>();
point.y = p[1].as<double>();
fp.push_back(point);
} }
std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl; std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;

6
src/costmap_2d_robot.cpp
View File

@ -82,7 +82,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
std::string folder = COSTMAP_2D_DIR; std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source); YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["costmap_2d"]; YAML::Node layer = config["costmap_2d"];
@ -97,7 +96,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
{ {
if (last_error + robot::Duration(5.0) < robot::Time::now()) if (last_error + robot::Duration(5.0) < robot::Time::now())
{ {
printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = robot::Time::now(); last_error = robot::Time::now();
} }
@ -274,7 +274,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
while (!map_update_thread_shutdown_) while (!map_update_thread_shutdown_)
{ {
updateMap(); updateMap();
std::cout << "Costmap2DROBOT::mapUpdateLoop updateMap\n"; std::cout << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n";
r.sleep(); r.sleep();
// make sure to sleep for the remainder of our cycle time // make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency)) if (r.cycleTime() > robot::Duration(1 / frequency))