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

Compare commits

base: DuongTD:22cfd075193bb1e35c0cc9217d4926d74fa11b96
Branches Tags
DuongTD:main
..
compare: DuongTD:ddff75465ca49b04bf22d2fbe26f61aa4bff7acb
Branches Tags
DuongTD:main

No commits in common. "22cfd075193bb1e35c0cc9217d4926d74fa11b96" and "ddff75465ca49b04bf22d2fbe26f61aa4bff7acb" have entirely different histories.
22cfd07519 ... ddff75465c

4 changed files with 5 additions and 5 deletions
Show Stats Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -62,7 +62,6 @@ 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: 1.0 update_frequency: 0.0
width: 0.0 width: 0.0
height: 0.0 height: 0.0
resolution: 0.0 resolution: 0.0

1
include/costmap_2d/utils.h
View File

@ -25,6 +25,7 @@ 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; geometry_msgs::Point point;
point.x = p[0].as<double>(); point.x = p[0].as<double>();
point.y = p[1].as<double>(); point.y = p[1].as<double>();

6
src/costmap_2d_robot.cpp
View File

@ -82,6 +82,7 @@ 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"];
@ -96,8 +97,7 @@ 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())
{ {
// std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl; printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
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 << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n"; std::cout << "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))