HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix

Browse Source
This commit is contained in:
HiepLM
2026-03-11 15:11:59 +07:00
parent 9e7d98934d
commit ae2f647fc9
3 changed files with 23 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/Navigations/Packages/move_base/src/convert_data.cpp
View File

@@ -1,7 +1,6 @@
#include <move_base/convert_data.h>
#include <robot/robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <fstream>
char* move_base::ConvertData::cost_translation_table_ = NULL;
@@ -42,19 +41,7 @@ move_base::ConvertData::~ConvertData()
// prepare grid_ message for publication.
void move_base::ConvertData::prepareGrid()
{
printf("Preparing costmap data for publication\n");
std::ofstream file("/home/robotics/sonvh/pnkx_nav_core/obstacle/hybrid_local_planner/cfg/costmap_data.txt");
if (!file.is_open())
{
std::cout << "Cannot open file!" << std::endl;
return;
}
// boost::unique_lock<costmap_2d::Costmap2D::mutex_t>
// lock(*(costmap_->getCostmap()->getMutex()));
// boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
double resolution = costmap_->getCostmap()->getResolution();
grid_.header.frame_id = global_frame_;
@@ -66,44 +53,22 @@ void move_base::ConvertData::prepareGrid()
double wx, wy;
costmap_->getCostmap()->mapToWorld(0, 0, wx, wy);
grid_.info.origin.position.x = wx - resolution / 2;
grid_.info.origin.position.y = wy - resolution / 2;
grid_.info.origin.position.z = 0.0;
grid_.info.origin.orientation.w = 1.0;
saved_origin_x_ = costmap_->getCostmap()->getOriginX();
saved_origin_y_ = costmap_->getCostmap()->getOriginY();
grid_.data.resize(grid_.info.width * grid_.info.height);
file << "frame: " << global_frame_ << std::endl;
file << "time: " << robot::Time::now().toSec() << std::endl;
file << "resolution: " << resolution << std::endl;
file << "width: " << grid_.info.width << std::endl;
file << "height: " << grid_.info.height << std::endl;
file << "origin_x: " << grid_.info.origin.position.x << std::endl;
file << "origin_y: " << grid_.info.origin.position.y << std::endl;
unsigned char* data = costmap_->getCostmap()->getCharMap();
for (unsigned int y = 0; y < grid_.info.height; y++)
for (unsigned int i = 0; i < grid_.data.size(); i++)
{
for (unsigned int x = 0; x < grid_.info.width; x++)
{
unsigned int i = y * grid_.info.width + x;
grid_.data[i] = cost_translation_table_[data[i]];
file << int(grid_.data[i]) << " ";
}
file << "\n";
grid_.data[i] = cost_translation_table_[ data[ i ]];
}
file.close();
}
void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated)
{
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getCostmap()->getMutex()));