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

update obstacle_layer

Browse Source
This commit is contained in:
duongtd
2026-01-06 17:33:13 +07:00
parent ae469e3271
commit 800e5c1735
6 changed files with 637 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/costmap_2d_robot.cpp
View File

@@ -67,6 +67,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL),
footprint_padding_(0.0)
{
std::signal(SIGINT, signalHandler);
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name);
name_ = name;
@@ -107,7 +108,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time(), &tf_error))
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
@@ -372,11 +373,11 @@ void Costmap2DROBOT::checkMovement()
void Costmap2DROBOT::mapUpdateLoop(double frequency)
{
std::cout << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n";
robot::Rate r(frequency);
while (!map_update_thread_shutdown_)
{
updateMap();
std::cout << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n";
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency))
@@ -430,11 +431,11 @@ void Costmap2DROBOT::start()
robot::Rate r(100.0);
while (!initialized_ && map_update_thread_)
{
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
break;
}
// if (robot::Time::now() - start_time > robot::Duration(5.0))
// {
// printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
// break;
// }
r.sleep();
}
}
@@ -546,9 +547,8 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
// check global_pose timeout
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
{
printf(
"Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
printf("Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
return false;
}