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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
src/layered_costmap.cpp Normal file → Executable file
View File

@@ -1,10 +1,46 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h>
#include <cstdio>
#include <string>
#include <algorithm>
#include <vector>
#include <memory>
using std::vector;
@@ -49,7 +85,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<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->matchSize();
@@ -76,7 +112,7 @@ namespace costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if (!(*plugin)->isEnabled())
@@ -88,8 +124,8 @@ namespace costmap_2d
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{
std::printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
(*plugin)->getName().c_str());
@@ -105,14 +141,14 @@ namespace costmap_2d
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
std::printf("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
if (xn < x0 || yn < y0)
return;
costmap_.resetMap(x0, y0, xn, yn);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -130,7 +166,7 @@ namespace costmap_2d
bool LayeredCostmap::isCurrent()
{
current_ = true;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -144,7 +180,7 @@ namespace costmap_2d
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->onFootprintChanged();