#### Package Summary

 Tags No category tags. Version 0.3.0 License BSD Build type CATKIN Use RECOMMENDED

### Repository Summary

 Checkout URI https://github.com/locusrobotics/robot_navigation.git VCS Type git VCS Version noetic Last Updated 2022-06-27 Dev Status DEVELOPED CI status Released RELEASED Tags No category tags. Contributing Help Wanted (0) Good First Issues (0) Pull Requests to Review (0)

### Package Description

Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.

#### Maintainers

• David V. Lu!!

# costmap_queue

This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.

## Inflation Example

Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.

One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.

The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.

## Map Based Queue

While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the optimizations to the Inflation algorithm.

Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.

## Costmap Queue

The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.

The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.

By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

CHANGELOG
No CHANGELOG found.

### Wiki Tutorials

See ROS Wiki Tutorials for more details.

### Source Tutorials

Not currently indexed.

### System Dependencies

No direct system dependencies.

### Launch files

No launch files found

### Messages

No message files found.

### Services

No service files found

### Plugins

No plugins found.

### Recent questions tagged costmap_queue at Robotics Stack Exchange

#### Package Summary

 Tags No category tags. Version 0.2.5 License BSD Build type CATKIN Use RECOMMENDED

### Repository Summary

 Checkout URI https://github.com/locusrobotics/robot_navigation.git VCS Type git VCS Version master Last Updated 2020-07-03 Dev Status DEVELOPED CI status Continuous Integration Released RELEASED Tags No category tags. Contributing Help Wanted (0) Good First Issues (0) Pull Requests to Review (0)

### Package Description

Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.

#### Maintainers

• David V. Lu!!

# costmap_queue

This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.

## Inflation Example

Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.

One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.

The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.

## Map Based Queue

While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the optimizations to the Inflation algorithm.

Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.

## Costmap Queue

The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.

The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.

By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

CHANGELOG
No CHANGELOG found.

### Wiki Tutorials

See ROS Wiki Tutorials for more details.

### Source Tutorials

Not currently indexed.

### System Dependencies

No direct system dependencies.

### Launch files

No launch files found

### Messages

No message files found.

### Services

No service files found

### Plugins

No plugins found.

### Recent questions tagged costmap_queue at Robotics Stack Exchange

#### Package Summary

 Tags No category tags. Version 0.2.5 License BSD Build type CATKIN Use RECOMMENDED

### Repository Summary

 Checkout URI https://github.com/locusrobotics/robot_navigation.git VCS Type git VCS Version master Last Updated 2020-07-03 Dev Status DEVELOPED CI status Continuous Integration Released RELEASED Tags No category tags. Contributing Help Wanted (0) Good First Issues (0) Pull Requests to Review (0)

### Package Description

Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.

#### Maintainers

• David V. Lu!!

# costmap_queue

This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.

## Inflation Example

Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.

One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.

The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.

## Map Based Queue

While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the optimizations to the Inflation algorithm.

Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.

## Costmap Queue

The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.

The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.

By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

CHANGELOG
No CHANGELOG found.

### Wiki Tutorials

See ROS Wiki Tutorials for more details.

### Source Tutorials

Not currently indexed.

### System Dependencies

No direct system dependencies.

### Launch files

No launch files found

### Messages

No message files found.

### Services

No service files found

### Plugins

No plugins found.

### Recent questions tagged costmap_queue at Robotics Stack Exchange

#### Package Summary

 Tags No category tags. Version 0.3.0 License BSD Build type CATKIN Use RECOMMENDED

### Repository Summary

 Checkout URI https://github.com/locusrobotics/robot_navigation.git VCS Type git VCS Version kinetic Last Updated 2021-01-08 Dev Status DEVELOPED CI status Continuous Integration Released RELEASED Tags No category tags. Contributing Help Wanted (0) Good First Issues (0) Pull Requests to Review (0)

### Package Description

Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.

#### Maintainers

• David V. Lu!!

# costmap_queue

This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.

## Inflation Example

Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.

One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.

The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.

## Map Based Queue

While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the optimizations to the Inflation algorithm.

Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.

## Costmap Queue

The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.

The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.

By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

CHANGELOG
No CHANGELOG found.

### Wiki Tutorials

See ROS Wiki Tutorials for more details.

### Source Tutorials

Not currently indexed.

### System Dependencies

No direct system dependencies.

### Launch files

No launch files found

### Messages

No message files found.

### Services

No service files found

### Plugins

No plugins found.

### Recent questions tagged costmap_queue at Robotics Stack Exchange

#### Package Summary

 Tags No category tags. Version 0.3.0 License BSD Build type CATKIN Use RECOMMENDED

### Repository Summary

 Checkout URI https://github.com/locusrobotics/robot_navigation.git VCS Type git VCS Version melodic Last Updated 2021-07-30 Dev Status DEVELOPED CI status Released RELEASED Tags No category tags. Contributing Help Wanted (0) Good First Issues (0) Pull Requests to Review (0)

### Package Description

Tool for iterating through the cells of a costmap to find the closest distance to a subset of cells.

#### Maintainers

• David V. Lu!!

# costmap_queue

This package contains helper classes that implement one particular common costmap operation: figuring out the distance of every cell to a subset of those cells.

## Inflation Example

Let's assume you have a grid `G` and you know that there are lethal obstacles in a subset of those cells `O`. You want to mark all cells within distance `r` with one value, and all cells within a greater distance `r2` with another.

One costly way to do this would be to to iterate through all the cells `g` in `G`, calculate the distance from `g` to each cell in `O`, find the minimum distance, and compare to `r` and `r2`. This is roughly quadratic with the number of cells.

The more efficient way to do it is to start with all the cells in `O` in a queue. For each cell you dequeue, you calculate the distance to its origin cell from `O` and then enqueue all of its neighbors. This approach is roughly linear with the number of cells, although you need to be careful to not incur too many costs from resorting the queue.

## Map Based Queue

While this operation could be done with the standard priority queue implementation, it can be optimized by using a custom priority queue based on `std::map`. This relies on the fact that many of the items inserted into the queue will have the same priority. In the `costmap_queue`, the priorities used are distances on the grid, of which there are a finite number. Thus by grouping all of the elements of the same weight into a vector together, and storing them in the `std::map` with their priority, we can eliminate the need to resort after each individual item is popped. This is based on the optimizations to the Inflation algorithm.

Also, since the same (or similar) distances will be used in the `costmap_queue` from iteration to iteration, additional time can be saved by not destroying the vectors in the map, and reusing them iteration to iteration.

## Costmap Queue

The `CostmapQueue` class operates on a `nav_core2::Costmap`. First, you must enqueue all of the "subset" of cells (i.e. `O` in the above example) using `enqueueCell`. Then, while the queue is not empty (i.e. `isEmpty` is false), you can call `costmap_queue::CellData cell = q.getNextCell();` to get the next cell in the queue.

The `CellData` class contains 5 values: `x_` and `y_` are the coordinates for the current cell, `src_x_` and `src_y_` are the coordinates for the original cell, and `distance_` is the distance between them.

By default, `CostmapQueue` will iterate through all the cells in the `Costmap`. If you want to limit it to only cells within a certain distance, you can use `LimitedCostmapQueue` instead.

CHANGELOG
No CHANGELOG found.

### Wiki Tutorials

See ROS Wiki Tutorials for more details.

### Source Tutorials

Not currently indexed.

### System Dependencies

No direct system dependencies.

### Launch files

No launch files found

### Messages

No message files found.

### Services

No service files found

### Plugins

No plugins found.