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

update from DuongTD

Browse Source
This commit is contained in:
HiepLM
2026-01-08 10:35:27 +07:00
parent f052dac142
commit 384897b750
15 changed files with 110 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
plugins/inflation_layer.cpp
View File

@@ -198,7 +198,8 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
printf("The inflation list must be empty at the beginning of inflation\n");
if(!inflation_cells_.empty())
printf("The inflation list must be empty at the beginning of inflation\n");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

16
plugins/obstacle_layer.cpp
View File

@@ -165,6 +165,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
}
CallBackInfo info_tmp;
info_tmp.observation_source = source;
info_tmp.data_type = data_type;
info_tmp.topic = topic;
info_tmp.inf_is_valid = inf_is_valid;
@@ -237,6 +238,8 @@ void ObstacleLayer::handleImpl(const void* data,
topic == callback_infos_[i].topic &&
!callback_infos_[i].inf_is_valid)
{
// if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i);
// if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i);
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
}
else if (type == typeid(robot_sensor_msgs::LaserScan) &&
@@ -244,6 +247,7 @@ void ObstacleLayer::handleImpl(const void* data,
topic == callback_infos_[i].topic &&
callback_infos_[i].inf_is_valid)
{
laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
}
else if (type == typeid(robot_sensor_msgs::PointCloud) &&
@@ -266,10 +270,14 @@ void ObstacleLayer::handleImpl(const void* data,
}
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
}
else
{
std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
}
// else
// {
// std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl
// << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl
// << "data type: " << callback_infos_[i].data_type << std::endl
// << "topic: " << topic << std::endl
// << "topic check: " << callback_infos_[i].topic << std::endl << std::endl;
// }
}
}
else