void AstarSearch::setMap(const nav_msgs::OccupancyGrid &map)
{
map_info_ = map.info;
// TODO: what frame do we use?
std::string map_frame = "map";
std::string ogm_frame = map.header.frame_id;
// Set transform
tf::StampedTransform map2ogm_frame;
try
{
tf_listener_.lookupTransform(map_frame, ogm_frame, ros::Time(0), map2ogm_frame);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
tf::Transform map2ogm;
geometry_msgs::Pose ogm_in_map = astar::transformPose(map_info_.origin, map2ogm_frame);
tf::poseMsgToTF(ogm_in_map, map2ogm_);
// Initialize node according to map size
if (!node_initialized_) {
resizeNode(map.info.width, map.info.height, angle_size_);
node_initialized_ = true;
}
for (size_t i = 0; i < map.info.height; i++) {
for (size_t j = 0; j < map.info.width; j++) {
// Index of subscribing OccupancyGrid message
size_t og_index = i * map.info.width + j;
int cost = map.data[og_index];
// more than threshold or unknown area
if (cost > obstacle_threshold_/* || cost < 0 */) {
nodes_[i][j][0].status = STATUS::OBS;
}
else
{
for (int k = 0; k < angle_size_; k++) {
//nodes_[i][j][k].gc = 0;
nodes_[i][j][k].hc = 0;
nodes_[i][j][k].status = STATUS::NONE;
nodes_[i][j][k].parent = NULL;
}
}
}
}
}
/**
* @brief Write data on an opened file
*
* Help from FUSE
*
* Write should return exactly the number of bytes requested except on error.
*
* @param path file path
* @param buf buffer where we have data to write
* @param size quantity of bytes to write
* @param offset offset over the writing
* @param fi FUSE structure linked to the opened file
* @return 0 on success and <0 on error
**/
static int my_write(const char *path, const char *buf, size_t size, off_t offset, struct fuse_file_info *fi) {
char buffer[BLOCK_SIZE_BYTES]; // 1 bloque de caracteres.
int bytes2Write = size, totalWrite = 0;
NodeStruct *node = myFileSystem.nodes[fi->fh]; // fh==File handle. May be filled in by filesystem in open().
fprintf(stderr, "--->>>my_write: path %s, size %zu, offset %jd, fh %"PRIu64"\n", path, size, (intmax_t)offset, fi->fh);
// Increase the file size if it is needed
if(resizeNode(fi->fh, size + offset) < 0)
return -EIO;
// Write data
while(bytes2Write) {
int i;
int currentBlock, offBloque;
currentBlock = node->blocks[offset / BLOCK_SIZE_BYTES];
offBloque = offset % BLOCK_SIZE_BYTES;
// posicionas el cursor del archivo en el bloque + offset.
// lees un bloque entero empezando en esa posición. SI alguno de los dos falla exit.
if((lseek(myFileSystem.fdVirtualDisk, currentBlock * BLOCK_SIZE_BYTES, SEEK_SET) == (off_t) - 1) ||
(read(myFileSystem.fdVirtualDisk, &buffer, BLOCK_SIZE_BYTES) == -1)) {
perror("Failed lseek/read in my_write");
return -EIO;
}
// Desde el punto inicial del offset, hasta el final del bloque, escribes la información
// del buf (texto que nos pasan en la función) en el buffer.
for(i = offBloque; (i < BLOCK_SIZE_BYTES) && (totalWrite < size); i++) {
buffer[i] = buf[totalWrite++];
}
// GUardas el buffer con la información modificada en el archivo indicado en el descriptor físico.
if((lseek(myFileSystem.fdVirtualDisk, currentBlock * BLOCK_SIZE_BYTES, SEEK_SET) == (off_t) - 1) ||
(write(myFileSystem.fdVirtualDisk, &buffer, BLOCK_SIZE_BYTES) == -1)) {
perror("Failed lseek/write in my_write");
return -EIO;
}
// Discont the written stuff
bytes2Write -= (i - offBloque);
offset += i;
}
sync();
node->modificationTime = time(NULL);
updateSuperBlock(&myFileSystem);
updateBitmap(&myFileSystem);
updateNode(&myFileSystem, fi->fh, node);
return size;
}
/**
* @brief Change the size of a file
*
* @param path file path
* @param size new size
* @return 0 on success and <0 on error
**/
static int my_truncate(const char *path, off_t size) {
int idxDir;
fprintf(stderr, "--->>>my_truncate: path %s, size %jd\n", path, size);
if((idxDir = findFileByName(&myFileSystem, (char *)path + 1)) == -1) {
return -ENOENT;
}
// Modify the size
if(resizeNode(myFileSystem.directory.files[idxDir].nodeIdx, size) < 0)
return -EIO;
return 0;
}
