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

update file voxel_layer

Browse Source
This commit is contained in:
duongtd
2025-11-15 17:36:52 +07:00
parent bd98bf4e08
commit 49a72383c8
23 changed files with 977 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/layered_costmap.cpp
View File

@@ -4,6 +4,7 @@
#include <string>
#include <algorithm>
#include <vector>
#include <memory>
using std::vector;
@@ -48,7 +49,7 @@ namespace costmap_2d
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
size_locked_ = size_locked;
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->matchSize();
@@ -75,7 +76,7 @@ namespace costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if (!(*plugin)->isEnabled())
@@ -111,7 +112,7 @@ namespace costmap_2d
costmap_.resetMap(x0, y0, xn, yn);
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -129,7 +130,7 @@ namespace costmap_2d
bool LayeredCostmap::isCurrent()
{
current_ = true;
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -143,7 +144,7 @@ namespace costmap_2d
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->onFootprintChanged();