Add costmap_2d package sources
Convert navigations/costmap_2d from gitlink to normal tracked files.
This commit is contained in:
115
navigations/costmap_2d/src/array_parser.cpp
Executable file
115
navigations/costmap_2d/src/array_parser.cpp
Executable file
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Copyright (c) 2012, 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 the 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: Dave Hershberger
|
||||
*/
|
||||
|
||||
#include <cstdio> // for EOF
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
/** @brief Parse a vector of vector of floats from a string.
|
||||
* @param input
|
||||
* @param error_return
|
||||
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
|
||||
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return)
|
||||
{
|
||||
std::vector<std::vector<float> > result;
|
||||
|
||||
std::stringstream input_ss(input);
|
||||
int depth = 0;
|
||||
std::vector<float> current_vector;
|
||||
while (!!input_ss && !input_ss.eof())
|
||||
{
|
||||
switch (input_ss.peek())
|
||||
{
|
||||
case EOF:
|
||||
break;
|
||||
case '[':
|
||||
depth++;
|
||||
if (depth > 2)
|
||||
{
|
||||
error_return = "Array depth greater than 2";
|
||||
return result;
|
||||
}
|
||||
input_ss.get();
|
||||
current_vector.clear();
|
||||
break;
|
||||
case ']':
|
||||
depth--;
|
||||
if (depth < 0)
|
||||
{
|
||||
error_return = "More close ] than open [";
|
||||
return result;
|
||||
}
|
||||
input_ss.get();
|
||||
if (depth == 1)
|
||||
{
|
||||
result.push_back(current_vector);
|
||||
}
|
||||
break;
|
||||
case ',':
|
||||
case ' ':
|
||||
case '\t':
|
||||
input_ss.get();
|
||||
break;
|
||||
default: // All other characters should be part of the numbers.
|
||||
if (depth != 2)
|
||||
{
|
||||
std::stringstream err_ss;
|
||||
err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
|
||||
error_return = err_ss.str();
|
||||
return result;
|
||||
}
|
||||
float value;
|
||||
input_ss >> value;
|
||||
if (!!input_ss)
|
||||
{
|
||||
current_vector.push_back(value);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (depth != 0)
|
||||
{
|
||||
error_return = "Unterminated vector string.";
|
||||
}
|
||||
else
|
||||
{
|
||||
error_return = "";
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
} // end namespace costmap_2d
|
||||
488
navigations/costmap_2d/src/costmap_2d.cpp
Executable file
488
navigations/costmap_2d/src/costmap_2d.cpp
Executable file
@@ -0,0 +1,488 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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/costmap_2d.h>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
|
||||
double origin_x, double origin_y, unsigned char default_value) :
|
||||
size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
|
||||
origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
|
||||
{
|
||||
access_ = new mutex_t();
|
||||
|
||||
// create the costmap
|
||||
initMaps(size_x_, size_y_);
|
||||
resetMaps();
|
||||
}
|
||||
|
||||
void Costmap2D::deleteMaps()
|
||||
{
|
||||
// clean up data
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
delete[] costmap_;
|
||||
costmap_ = NULL;
|
||||
}
|
||||
|
||||
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
delete[] costmap_;
|
||||
costmap_ = new unsigned char[size_x * size_y];
|
||||
}
|
||||
|
||||
void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
|
||||
double origin_x, double origin_y)
|
||||
{
|
||||
size_x_ = size_x;
|
||||
size_y_ = size_y;
|
||||
resolution_ = resolution;
|
||||
origin_x_ = origin_x;
|
||||
origin_y_ = origin_y;
|
||||
|
||||
initMaps(size_x, size_y);
|
||||
|
||||
// reset our maps to have no information
|
||||
resetMaps();
|
||||
}
|
||||
|
||||
void Costmap2D::resetMaps()
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*access_);
|
||||
memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
|
||||
}
|
||||
|
||||
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*(access_));
|
||||
unsigned int len = xn - x0;
|
||||
for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
|
||||
memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
|
||||
}
|
||||
|
||||
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
|
||||
double win_size_y)
|
||||
{
|
||||
// check for self windowing
|
||||
if (this == &map)
|
||||
{
|
||||
// ROS_ERROR("Cannot convert this costmap into a window of itself");
|
||||
return false;
|
||||
}
|
||||
|
||||
// clean up old data
|
||||
deleteMaps();
|
||||
|
||||
// compute the bounds of our new map
|
||||
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
|
||||
if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|
||||
|| !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
|
||||
{
|
||||
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
|
||||
return false;
|
||||
}
|
||||
|
||||
size_x_ = upper_right_x - lower_left_x;
|
||||
size_y_ = upper_right_y - lower_left_y;
|
||||
resolution_ = map.resolution_;
|
||||
origin_x_ = win_origin_x;
|
||||
origin_y_ = win_origin_y;
|
||||
|
||||
// initialize our various maps and reset markers for inflation
|
||||
initMaps(size_x_, size_y_);
|
||||
|
||||
// copy the window of the static map and the costmap that we're taking
|
||||
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
|
||||
return true;
|
||||
}
|
||||
|
||||
Costmap2D& Costmap2D::operator=(const Costmap2D& map)
|
||||
{
|
||||
// check for self assignement
|
||||
if (this == &map)
|
||||
return *this;
|
||||
|
||||
// clean up old data
|
||||
deleteMaps();
|
||||
|
||||
size_x_ = map.size_x_;
|
||||
size_y_ = map.size_y_;
|
||||
resolution_ = map.resolution_;
|
||||
origin_x_ = map.origin_x_;
|
||||
origin_y_ = map.origin_y_;
|
||||
|
||||
// initialize our various maps
|
||||
initMaps(size_x_, size_y_);
|
||||
|
||||
// copy the cost map
|
||||
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Costmap2D::Costmap2D(const Costmap2D& map) :
|
||||
costmap_(NULL)
|
||||
{
|
||||
access_ = new mutex_t();
|
||||
*this = map;
|
||||
}
|
||||
|
||||
// just initialize everything to NULL by default
|
||||
Costmap2D::Costmap2D() :
|
||||
size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
|
||||
{
|
||||
access_ = new mutex_t();
|
||||
}
|
||||
|
||||
Costmap2D::~Costmap2D()
|
||||
{
|
||||
deleteMaps();
|
||||
delete access_;
|
||||
}
|
||||
|
||||
unsigned int Costmap2D::cellDistance(double world_dist)
|
||||
{
|
||||
double cells_dist = max(0.0, ceil(world_dist / resolution_));
|
||||
return (unsigned int)cells_dist;
|
||||
}
|
||||
|
||||
unsigned char* Costmap2D::getCharMap() const
|
||||
{
|
||||
return costmap_;
|
||||
}
|
||||
|
||||
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
|
||||
{
|
||||
return costmap_[getIndex(mx, my)];
|
||||
}
|
||||
|
||||
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
|
||||
{
|
||||
costmap_[getIndex(mx, my)] = cost;
|
||||
}
|
||||
|
||||
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
|
||||
{
|
||||
wx = origin_x_ + (mx + 0.5) * resolution_;
|
||||
wy = origin_y_ + (my + 0.5) * resolution_;
|
||||
}
|
||||
|
||||
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
|
||||
{
|
||||
if (wx < origin_x_ || wy < origin_y_)
|
||||
return false;
|
||||
|
||||
mx = (int)((wx - origin_x_) / resolution_);
|
||||
my = (int)((wy - origin_y_) / resolution_);
|
||||
|
||||
if (mx < size_x_ && my < size_y_)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
|
||||
{
|
||||
mx = (int)((wx - origin_x_) / resolution_);
|
||||
my = (int)((wy - origin_y_) / resolution_);
|
||||
}
|
||||
|
||||
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
|
||||
{
|
||||
// Here we avoid doing any math to wx,wy before comparing them to
|
||||
// the bounds, so their values can go out to the max and min values
|
||||
// of double floating point.
|
||||
if (wx < origin_x_)
|
||||
{
|
||||
mx = 0;
|
||||
}
|
||||
else if (wx >= resolution_ * size_x_ + origin_x_)
|
||||
{
|
||||
mx = size_x_ - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
mx = (int)((wx - origin_x_) / resolution_);
|
||||
}
|
||||
|
||||
if (wy < origin_y_)
|
||||
{
|
||||
my = 0;
|
||||
}
|
||||
else if (wy >= resolution_ * size_y_ + origin_y_)
|
||||
{
|
||||
my = size_y_ - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
my = (int)((wy - origin_y_) / resolution_);
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
|
||||
{
|
||||
// project the new origin into the grid
|
||||
int cell_ox, cell_oy;
|
||||
cell_ox = int((new_origin_x - origin_x_) / resolution_);
|
||||
cell_oy = int((new_origin_y - origin_y_) / resolution_);
|
||||
|
||||
// Nothing to update
|
||||
if (cell_ox == 0 && cell_oy == 0)
|
||||
return;
|
||||
|
||||
// compute the associated world coordinates for the origin cell
|
||||
// because we want to keep things grid-aligned
|
||||
double new_grid_ox, new_grid_oy;
|
||||
new_grid_ox = origin_x_ + cell_ox * resolution_;
|
||||
new_grid_oy = origin_y_ + cell_oy * resolution_;
|
||||
|
||||
// To save casting from unsigned int to int a bunch of times
|
||||
int size_x = size_x_;
|
||||
int size_y = size_y_;
|
||||
|
||||
// we need to compute the overlap of the new and existing windows
|
||||
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
|
||||
lower_left_x = min(max(cell_ox, 0), size_x);
|
||||
lower_left_y = min(max(cell_oy, 0), size_y);
|
||||
upper_right_x = min(max(cell_ox + size_x, 0), size_x);
|
||||
upper_right_y = min(max(cell_oy + size_y, 0), size_y);
|
||||
|
||||
unsigned int cell_size_x = upper_right_x - lower_left_x;
|
||||
unsigned int cell_size_y = upper_right_y - lower_left_y;
|
||||
|
||||
// we need a map to store the obstacles in the window temporarily
|
||||
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
|
||||
|
||||
// copy the local window in the costmap to the local map
|
||||
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
|
||||
|
||||
// now we'll set the costmap to be completely unknown if we track unknown space
|
||||
resetMaps();
|
||||
|
||||
// update the origin with the appropriate world coordinates
|
||||
origin_x_ = new_grid_ox;
|
||||
origin_y_ = new_grid_oy;
|
||||
|
||||
// compute the starting cell location for copying data back in
|
||||
int start_x = lower_left_x - cell_ox;
|
||||
int start_y = lower_left_y - cell_oy;
|
||||
|
||||
// now we want to copy the overlapping information back into the map, but in its new location
|
||||
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
|
||||
|
||||
// make sure to clean up
|
||||
delete[] local_map;
|
||||
}
|
||||
|
||||
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
|
||||
{
|
||||
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
|
||||
std::vector<MapLocation> map_polygon;
|
||||
for (unsigned int i = 0; i < polygon.size(); ++i)
|
||||
{
|
||||
MapLocation loc;
|
||||
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
|
||||
{
|
||||
// ("Polygon lies outside map bounds, so we can't fill it");
|
||||
return false;
|
||||
}
|
||||
map_polygon.push_back(loc);
|
||||
}
|
||||
|
||||
std::vector<MapLocation> polygon_cells;
|
||||
|
||||
// get the cells that fill the polygon
|
||||
convexFillCells(map_polygon, polygon_cells);
|
||||
|
||||
// set the cost of those cells
|
||||
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
|
||||
{
|
||||
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
|
||||
costmap_[index] = cost_value;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
|
||||
{
|
||||
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
|
||||
for (unsigned int i = 0; i < polygon.size() - 1; ++i)
|
||||
{
|
||||
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
|
||||
}
|
||||
if (!polygon.empty())
|
||||
{
|
||||
unsigned int last_index = polygon.size() - 1;
|
||||
// we also need to close the polygon by going from the last point to the first
|
||||
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
|
||||
{
|
||||
// we need a minimum polygon of a triangle
|
||||
if (polygon.size() < 3)
|
||||
return;
|
||||
|
||||
// first get the cells that make up the outline of the polygon
|
||||
polygonOutlineCells(polygon, polygon_cells);
|
||||
|
||||
// quick bubble sort to sort points by x
|
||||
MapLocation swap;
|
||||
unsigned int i = 0;
|
||||
while (i < polygon_cells.size() - 1)
|
||||
{
|
||||
if (polygon_cells[i].x > polygon_cells[i + 1].x)
|
||||
{
|
||||
swap = polygon_cells[i];
|
||||
polygon_cells[i] = polygon_cells[i + 1];
|
||||
polygon_cells[i + 1] = swap;
|
||||
|
||||
if (i > 0)
|
||||
--i;
|
||||
}
|
||||
else
|
||||
++i;
|
||||
}
|
||||
|
||||
i = 0;
|
||||
MapLocation min_pt;
|
||||
MapLocation max_pt;
|
||||
unsigned int min_x = polygon_cells[0].x;
|
||||
unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
|
||||
|
||||
// walk through each column and mark cells inside the polygon
|
||||
for (unsigned int x = min_x; x <= max_x; ++x)
|
||||
{
|
||||
if (i >= polygon_cells.size() - 1)
|
||||
break;
|
||||
|
||||
if (polygon_cells[i].y < polygon_cells[i + 1].y)
|
||||
{
|
||||
min_pt = polygon_cells[i];
|
||||
max_pt = polygon_cells[i + 1];
|
||||
}
|
||||
else
|
||||
{
|
||||
min_pt = polygon_cells[i + 1];
|
||||
max_pt = polygon_cells[i];
|
||||
}
|
||||
|
||||
i += 2;
|
||||
while (i < polygon_cells.size() && polygon_cells[i].x == x)
|
||||
{
|
||||
if (polygon_cells[i].y < min_pt.y)
|
||||
min_pt = polygon_cells[i];
|
||||
else if (polygon_cells[i].y > max_pt.y)
|
||||
max_pt = polygon_cells[i];
|
||||
++i;
|
||||
}
|
||||
|
||||
MapLocation pt;
|
||||
// loop though cells in the column
|
||||
for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
|
||||
{
|
||||
pt.x = x;
|
||||
pt.y = y;
|
||||
polygon_cells.push_back(pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int Costmap2D::getSizeInCellsX() const
|
||||
{
|
||||
return size_x_;
|
||||
}
|
||||
|
||||
unsigned int Costmap2D::getSizeInCellsY() const
|
||||
{
|
||||
return size_y_;
|
||||
}
|
||||
|
||||
double Costmap2D::getSizeInMetersX() const
|
||||
{
|
||||
return (size_x_ - 1 + 0.5) * resolution_;
|
||||
}
|
||||
|
||||
double Costmap2D::getSizeInMetersY() const
|
||||
{
|
||||
return (size_y_ - 1 + 0.5) * resolution_;
|
||||
}
|
||||
|
||||
double Costmap2D::getOriginX() const
|
||||
{
|
||||
return origin_x_;
|
||||
}
|
||||
|
||||
double Costmap2D::getOriginY() const
|
||||
{
|
||||
return origin_y_;
|
||||
}
|
||||
|
||||
double Costmap2D::getResolution() const
|
||||
{
|
||||
return resolution_;
|
||||
}
|
||||
|
||||
bool Costmap2D::saveMap(std::string file_name)
|
||||
{
|
||||
FILE *fp = fopen(file_name.c_str(), "w");
|
||||
|
||||
if (!fp)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
|
||||
for (unsigned int iy = 0; iy < size_y_; iy++)
|
||||
{
|
||||
for (unsigned int ix = 0; ix < size_x_; ix++)
|
||||
{
|
||||
unsigned char cost = getCost(ix, iy);
|
||||
fprintf(fp, "%d ", cost);
|
||||
}
|
||||
fprintf(fp, "\n");
|
||||
}
|
||||
fclose(fp);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace costmap_2d
|
||||
207
navigations/costmap_2d/src/costmap_2d_cloud.cpp
Executable file
207
navigations/costmap_2d/src/costmap_2d_cloud.cpp
Executable file
@@ -0,0 +1,207 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <costmap_2d/VoxelGrid.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
static inline void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz,
|
||||
const double origin_x, const double origin_y, const double origin_z,
|
||||
const double x_resolution, const double y_resolution, const double z_resolution,
|
||||
double& wx, double& wy, double& wz)
|
||||
{
|
||||
// returns the center point of the cell
|
||||
wx = origin_x + (mx + 0.5) * x_resolution;
|
||||
wy = origin_y + (my + 0.5) * y_resolution;
|
||||
wz = origin_z + (mz + 0.5) * z_resolution;
|
||||
}
|
||||
|
||||
struct Cell
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
voxel_grid::VoxelStatus status;
|
||||
};
|
||||
typedef std::vector<Cell> V_Cell;
|
||||
|
||||
float g_colors_r[] = {0.0f, 0.0f, 1.0f};
|
||||
float g_colors_g[] = {0.0f, 0.0f, 0.0f};
|
||||
float g_colors_b[] = {0.0f, 1.0f, 0.0f};
|
||||
float g_colors_a[] = {0.0f, 0.5f, 1.0f};
|
||||
|
||||
V_Cell g_marked;
|
||||
V_Cell g_unknown;
|
||||
void voxelCallback(const ros::Publisher& pub_marked, const ros::Publisher& pub_unknown,
|
||||
const costmap_2d::VoxelGridConstPtr& grid)
|
||||
{
|
||||
if (grid->data.empty())
|
||||
{
|
||||
ROS_ERROR("Received empty voxel grid");
|
||||
return;
|
||||
}
|
||||
|
||||
ros::WallTime start = ros::WallTime::now();
|
||||
|
||||
ROS_DEBUG("Received voxel grid");
|
||||
const std::string frame_id = grid->header.frame_id;
|
||||
const ros::Time stamp = grid->header.stamp;
|
||||
const uint32_t* data = &grid->data.front();
|
||||
const double x_origin = grid->origin.x;
|
||||
const double y_origin = grid->origin.y;
|
||||
const double z_origin = grid->origin.z;
|
||||
const double x_res = grid->resolutions.x;
|
||||
const double y_res = grid->resolutions.y;
|
||||
const double z_res = grid->resolutions.z;
|
||||
const uint32_t x_size = grid->size_x;
|
||||
const uint32_t y_size = grid->size_y;
|
||||
const uint32_t z_size = grid->size_z;
|
||||
|
||||
g_marked.clear();
|
||||
g_unknown.clear();
|
||||
uint32_t num_marked = 0;
|
||||
uint32_t num_unknown = 0;
|
||||
for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
|
||||
{
|
||||
for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
|
||||
{
|
||||
for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
|
||||
{
|
||||
voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
|
||||
data);
|
||||
|
||||
if (status == voxel_grid::UNKNOWN)
|
||||
{
|
||||
Cell c;
|
||||
c.status = status;
|
||||
mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res,
|
||||
z_res, c.x, c.y, c.z);
|
||||
|
||||
g_unknown.push_back(c);
|
||||
|
||||
++num_unknown;
|
||||
}
|
||||
else if (status == voxel_grid::MARKED)
|
||||
{
|
||||
Cell c;
|
||||
c.status = status;
|
||||
mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res,
|
||||
z_res, c.x, c.y, c.z);
|
||||
|
||||
g_marked.push_back(c);
|
||||
|
||||
++num_marked;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
sensor_msgs::PointCloud cloud;
|
||||
cloud.points.resize(num_marked);
|
||||
cloud.channels.resize(1);
|
||||
cloud.channels[0].values.resize(num_marked);
|
||||
cloud.channels[0].name = "rgb";
|
||||
cloud.header.frame_id = frame_id;
|
||||
cloud.header.stamp = stamp;
|
||||
|
||||
sensor_msgs::ChannelFloat32& chan = cloud.channels[0];
|
||||
for (uint32_t i = 0; i < num_marked; ++i)
|
||||
{
|
||||
geometry_msgs::Point32& p = cloud.points[i];
|
||||
float& cval = chan.values[i];
|
||||
Cell& c = g_marked[i];
|
||||
|
||||
p.x = c.x;
|
||||
p.y = c.y;
|
||||
p.z = c.z;
|
||||
|
||||
uint32_t r = g_colors_r[c.status] * 255.0;
|
||||
uint32_t g = g_colors_g[c.status] * 255.0;
|
||||
uint32_t b = g_colors_b[c.status] * 255.0;
|
||||
// uint32_t a = g_colors_a[c.status] * 255.0;
|
||||
|
||||
uint32_t col = (r << 16) | (g << 8) | b;
|
||||
cval = *reinterpret_cast<float*>(&col);
|
||||
}
|
||||
|
||||
pub_marked.publish(cloud);
|
||||
}
|
||||
|
||||
{
|
||||
sensor_msgs::PointCloud cloud;
|
||||
cloud.points.resize(num_unknown);
|
||||
cloud.channels.resize(1);
|
||||
cloud.channels[0].values.resize(num_unknown);
|
||||
cloud.channels[0].name = "rgb";
|
||||
cloud.header.frame_id = frame_id;
|
||||
cloud.header.stamp = stamp;
|
||||
|
||||
sensor_msgs::ChannelFloat32& chan = cloud.channels[0];
|
||||
for (uint32_t i = 0; i < num_unknown; ++i)
|
||||
{
|
||||
geometry_msgs::Point32& p = cloud.points[i];
|
||||
float& cval = chan.values[i];
|
||||
Cell& c = g_unknown[i];
|
||||
|
||||
p.x = c.x;
|
||||
p.y = c.y;
|
||||
p.z = c.z;
|
||||
|
||||
uint32_t r = g_colors_r[c.status] * 255.0;
|
||||
uint32_t g = g_colors_g[c.status] * 255.0;
|
||||
uint32_t b = g_colors_b[c.status] * 255.0;
|
||||
// uint32_t a = g_colors_a[c.status] * 255.0;
|
||||
|
||||
uint32_t col = (r << 16) | (g << 8) | b;
|
||||
cval = *reinterpret_cast<float*>(&col);
|
||||
}
|
||||
|
||||
pub_unknown.publish(cloud);
|
||||
}
|
||||
|
||||
ros::WallTime end = ros::WallTime::now();
|
||||
ROS_DEBUG("Published %d points in %f seconds", num_marked + num_unknown, (end - start).toSec());
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "costmap_2d_cloud");
|
||||
ros::NodeHandle n;
|
||||
|
||||
ROS_DEBUG("Startup");
|
||||
|
||||
ros::Publisher pub_marked = n.advertise < sensor_msgs::PointCloud > ("voxel_marked_cloud", 2);
|
||||
ros::Publisher pub_unknown = n.advertise < sensor_msgs::PointCloud > ("voxel_unknown_cloud", 2);
|
||||
ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid
|
||||
> ("voxel_grid", 1, [&pub_marked,&pub_unknown](auto& msg){ voxelCallback(pub_marked, pub_unknown, msg); });
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
155
navigations/costmap_2d/src/costmap_2d_markers.cpp
Executable file
155
navigations/costmap_2d/src/costmap_2d_markers.cpp
Executable file
@@ -0,0 +1,155 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <costmap_2d/VoxelGrid.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
struct Cell
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
voxel_grid::VoxelStatus status;
|
||||
};
|
||||
typedef std::vector<Cell> V_Cell;
|
||||
|
||||
float g_colors_r[] = {0.0f, 0.0f, 1.0f};
|
||||
float g_colors_g[] = {0.0f, 0.0f, 0.0f};
|
||||
float g_colors_b[] = {0.0f, 1.0f, 0.0f};
|
||||
float g_colors_a[] = {0.0f, 0.5f, 1.0f};
|
||||
|
||||
std::string g_marker_ns;
|
||||
V_Cell g_cells;
|
||||
void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid)
|
||||
{
|
||||
if (grid->data.empty())
|
||||
{
|
||||
ROS_ERROR("Received empty voxel grid");
|
||||
return;
|
||||
}
|
||||
|
||||
ros::WallTime start = ros::WallTime::now();
|
||||
|
||||
ROS_DEBUG("Received voxel grid");
|
||||
const std::string frame_id = grid->header.frame_id;
|
||||
const ros::Time stamp = grid->header.stamp;
|
||||
const uint32_t* data = &grid->data.front();
|
||||
const double x_origin = grid->origin.x;
|
||||
const double y_origin = grid->origin.y;
|
||||
const double z_origin = grid->origin.z;
|
||||
const double x_res = grid->resolutions.x;
|
||||
const double y_res = grid->resolutions.y;
|
||||
const double z_res = grid->resolutions.z;
|
||||
const uint32_t x_size = grid->size_x;
|
||||
const uint32_t y_size = grid->size_y;
|
||||
const uint32_t z_size = grid->size_z;
|
||||
|
||||
g_cells.clear();
|
||||
uint32_t num_markers = 0;
|
||||
for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
|
||||
{
|
||||
for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
|
||||
{
|
||||
for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
|
||||
{
|
||||
voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
|
||||
data);
|
||||
|
||||
if (status == voxel_grid::MARKED)
|
||||
{
|
||||
Cell c;
|
||||
c.status = status;
|
||||
c.x = x_origin + (x_grid + 0.5) * x_res;
|
||||
c.y = y_origin + (y_grid + 0.5) * y_res;
|
||||
c.z = z_origin + (z_grid + 0.5) * z_res;
|
||||
g_cells.push_back(c);
|
||||
|
||||
++num_markers;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
visualization_msgs::Marker m;
|
||||
m.header.frame_id = frame_id;
|
||||
m.header.stamp = stamp;
|
||||
m.ns = g_marker_ns;
|
||||
m.id = 0;
|
||||
m.type = visualization_msgs::Marker::CUBE_LIST;
|
||||
m.action = visualization_msgs::Marker::ADD;
|
||||
m.pose.orientation.w = 1.0;
|
||||
m.scale.x = x_res;
|
||||
m.scale.y = y_res;
|
||||
m.scale.z = z_res;
|
||||
m.color.r = g_colors_r[voxel_grid::MARKED];
|
||||
m.color.g = g_colors_g[voxel_grid::MARKED];
|
||||
m.color.b = g_colors_b[voxel_grid::MARKED];
|
||||
m.color.a = g_colors_a[voxel_grid::MARKED];
|
||||
m.points.resize(num_markers);
|
||||
for (uint32_t i = 0; i < num_markers; ++i)
|
||||
{
|
||||
Cell& c = g_cells[i];
|
||||
geometry_msgs::Point& p = m.points[i];
|
||||
p.x = c.x;
|
||||
p.y = c.y;
|
||||
p.z = c.z;
|
||||
}
|
||||
|
||||
pub.publish(m);
|
||||
|
||||
ros::WallTime end = ros::WallTime::now();
|
||||
ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec());
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "costmap_2d_markers");
|
||||
ros::NodeHandle n;
|
||||
|
||||
ROS_DEBUG("Startup");
|
||||
|
||||
ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1);
|
||||
ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, [&pub](auto& msg){ voxelCallback(pub, msg); });
|
||||
g_marker_ns = n.resolveName("voxel_grid");
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
52
navigations/costmap_2d/src/costmap_2d_node.cpp
Executable file
52
navigations/costmap_2d/src/costmap_2d_node.cpp
Executable file
@@ -0,0 +1,52 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "costmap_node");
|
||||
tf2_ros::Buffer buffer(ros::Duration(10));
|
||||
tf2_ros::TransformListener tf(buffer);
|
||||
costmap_2d::Costmap2DROS lcr("costmap", buffer);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return (0);
|
||||
}
|
||||
182
navigations/costmap_2d/src/costmap_2d_publisher.cpp
Executable file
182
navigations/costmap_2d/src/costmap_2d_publisher.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <boost/bind.hpp>
|
||||
#include <costmap_2d/costmap_2d_publisher.h>
|
||||
#include <costmap_2d/cost_values.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
char* Costmap2DPublisher::cost_translation_table_ = NULL;
|
||||
|
||||
Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle * ros_node, Costmap2D* costmap, std::string global_frame,
|
||||
std::string topic_name, bool always_send_full_costmap) :
|
||||
node(ros_node), costmap_(costmap), global_frame_(global_frame), active_(false),
|
||||
always_send_full_costmap_(always_send_full_costmap)
|
||||
{
|
||||
costmap_pub_ = ros_node->advertise<nav_msgs::OccupancyGrid>(topic_name, 1,
|
||||
boost::bind(&Costmap2DPublisher::onNewSubscription, this, _1));
|
||||
costmap_update_pub_ = ros_node->advertise<map_msgs::OccupancyGridUpdate>(topic_name + "_updates", 1);
|
||||
|
||||
if (cost_translation_table_ == NULL)
|
||||
{
|
||||
cost_translation_table_ = new char[256];
|
||||
|
||||
// special values:
|
||||
cost_translation_table_[(const int)PREFERRED_SPACE] = 120; // Preferred place
|
||||
for(int i = (int)PREFERRED_SPACE+1; i <= (const int)FREE_SPACE; i++)
|
||||
cost_translation_table_[i] = 0; // NO obstacle
|
||||
|
||||
cost_translation_table_[(const int)CRITICAL_SPACE] = 150; // CRITICAL_SPACE
|
||||
cost_translation_table_[(const int)INSCRIBED_INFLATED_OBSTACLE] = 99; // INSCRIBED obstacle
|
||||
cost_translation_table_[(const int)LETHAL_OBSTACLE] = 100; // LETHAL obstacle
|
||||
cost_translation_table_[(const int)NO_INFORMATION] = -1; // UNKNOWN
|
||||
|
||||
// regular cost values scale the range 10 to 252 (inclusive) to fit
|
||||
// into 1 to 98 (inclusive).
|
||||
for (int i = (int)FREE_SPACE + 1; i < (const int)INSCRIBED_INFLATED_OBSTACLE; i++)
|
||||
{
|
||||
cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
|
||||
}
|
||||
}
|
||||
|
||||
xn_ = yn_ = 0;
|
||||
x0_ = costmap_->getSizeInCellsX();
|
||||
y0_ = costmap_->getSizeInCellsY();
|
||||
}
|
||||
|
||||
Costmap2DPublisher::~Costmap2DPublisher()
|
||||
{
|
||||
}
|
||||
|
||||
void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub)
|
||||
{
|
||||
prepareGrid();
|
||||
pub.publish(grid_);
|
||||
}
|
||||
|
||||
// prepare grid_ message for publication.
|
||||
void Costmap2DPublisher::prepareGrid()
|
||||
{
|
||||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
|
||||
double resolution = costmap_->getResolution();
|
||||
|
||||
grid_.header.frame_id = global_frame_;
|
||||
grid_.header.stamp = ros::Time::now();
|
||||
grid_.info.resolution = resolution;
|
||||
|
||||
grid_.info.width = costmap_->getSizeInCellsX();
|
||||
grid_.info.height = costmap_->getSizeInCellsY();
|
||||
|
||||
double wx, wy;
|
||||
costmap_->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_->getOriginX();
|
||||
saved_origin_y_ = costmap_->getOriginY();
|
||||
|
||||
grid_.data.resize(grid_.info.width * grid_.info.height);
|
||||
|
||||
unsigned char* data = costmap_->getCharMap();
|
||||
for (unsigned int i = 0; i < grid_.data.size(); i++)
|
||||
{
|
||||
grid_.data[i] = cost_translation_table_[ data[ i ]];
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DPublisher::publishCostmap()
|
||||
{
|
||||
if (costmap_pub_.getNumSubscribers() == 0)
|
||||
{
|
||||
// No subscribers, so why do any work?
|
||||
return;
|
||||
}
|
||||
|
||||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
|
||||
float resolution = costmap_->getResolution();
|
||||
// ROS_WARN("%s %d %d %d %d %d %d",
|
||||
// global_frame_.c_str(),
|
||||
// always_send_full_costmap_,
|
||||
// grid_.info.resolution != resolution,
|
||||
// grid_.info.width != costmap_->getSizeInCellsX(),
|
||||
// grid_.info.height != costmap_->getSizeInCellsY(),
|
||||
// saved_origin_x_ != costmap_->getOriginX(),
|
||||
// saved_origin_y_ != costmap_->getOriginY()
|
||||
// );
|
||||
|
||||
if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
|
||||
grid_.info.width != costmap_->getSizeInCellsX() ||
|
||||
grid_.info.height != costmap_->getSizeInCellsY() ||
|
||||
saved_origin_x_ != costmap_->getOriginX() ||
|
||||
saved_origin_y_ != costmap_->getOriginY())
|
||||
{
|
||||
prepareGrid();
|
||||
costmap_pub_.publish(grid_);
|
||||
}
|
||||
else if (x0_ < xn_)
|
||||
{
|
||||
// Publish Just an Update
|
||||
map_msgs::OccupancyGridUpdate update;
|
||||
update.header.stamp = ros::Time::now();
|
||||
update.header.frame_id = global_frame_;
|
||||
update.x = x0_;
|
||||
update.y = y0_;
|
||||
update.width = xn_ - x0_;
|
||||
update.height = yn_ - y0_;
|
||||
update.data.resize(update.width * update.height);
|
||||
|
||||
unsigned int i = 0;
|
||||
for (unsigned int y = y0_; y < yn_; y++)
|
||||
{
|
||||
for (unsigned int x = x0_; x < xn_; x++)
|
||||
{
|
||||
unsigned char cost = costmap_->getCost(x, y);
|
||||
update.data[i++] = cost_translation_table_[ cost ];
|
||||
}
|
||||
}
|
||||
costmap_update_pub_.publish(update);
|
||||
}
|
||||
|
||||
xn_ = yn_ = 0;
|
||||
x0_ = costmap_->getSizeInCellsX();
|
||||
y0_ = costmap_->getSizeInCellsY();
|
||||
}
|
||||
|
||||
} // end namespace costmap_2d
|
||||
619
navigations/costmap_2d/src/costmap_2d_ros.cpp
Executable file
619
navigations/costmap_2d/src/costmap_2d_ros.cpp
Executable file
@@ -0,0 +1,619 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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/costmap_2d_ros.h>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
|
||||
{
|
||||
if (!old_h.hasParam(name))
|
||||
return;
|
||||
|
||||
XmlRpc::XmlRpcValue value;
|
||||
old_h.getParam(name, value);
|
||||
new_h.setParam(name, value);
|
||||
if (should_delete) old_h.deleteParam(name);
|
||||
}
|
||||
|
||||
Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) :
|
||||
layered_costmap_(NULL),
|
||||
name_(name),
|
||||
tf_(tf),
|
||||
transform_tolerance_(0.3),
|
||||
map_update_thread_shutdown_(false),
|
||||
stop_updates_(false),
|
||||
initialized_(true),
|
||||
stopped_(false),
|
||||
map_update_thread_(NULL),
|
||||
last_publish_(0),
|
||||
plugin_loader_("costmap_2d", "costmap_2d::Layer"),
|
||||
publisher_(NULL),
|
||||
dsrv_(NULL),
|
||||
footprint_padding_(0.0)
|
||||
{
|
||||
|
||||
ros::NodeHandle private_nh("~/" + name);
|
||||
ros::NodeHandle g_nh;
|
||||
|
||||
// get global and robot base frame names
|
||||
private_nh.param("global_frame", global_frame_, std::string("map"));
|
||||
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
|
||||
// if(name_.find("global_costmap")) ROS_ERROR("Hiep- name_: %s global_frame: %s robot_base_frame: %s", name_.c_str(), global_frame_.c_str(), robot_base_frame_.c_str());
|
||||
ros::Time last_error = ros::Time::now();
|
||||
std::string tf_error;
|
||||
// we need to make sure that the transform between the robot base frame and the global frame is available
|
||||
while (ros::ok()
|
||||
&& !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
|
||||
{
|
||||
ros::spinOnce();
|
||||
if (last_error + ros::Duration(5.0) < ros::Time::now())
|
||||
{
|
||||
ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
|
||||
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
|
||||
last_error = ros::Time::now();
|
||||
}
|
||||
// The error string will accumulate and errors will typically be the same, so the last
|
||||
// will do for the warning above. Reset the string here to avoid accumulation.
|
||||
tf_error.clear();
|
||||
}
|
||||
|
||||
// check if we want a rolling window version of the costmap
|
||||
bool rolling_window, track_unknown_space, always_send_full_costmap;
|
||||
private_nh.param("rolling_window", rolling_window, false);
|
||||
private_nh.param("track_unknown_space", track_unknown_space, false);
|
||||
private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
|
||||
|
||||
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
|
||||
|
||||
if (!private_nh.hasParam("plugins"))
|
||||
{
|
||||
loadOldParameters(private_nh);
|
||||
} else {
|
||||
warnForOldParameters(private_nh);
|
||||
}
|
||||
|
||||
if (private_nh.hasParam("plugins"))
|
||||
{
|
||||
XmlRpc::XmlRpcValue my_list;
|
||||
private_nh.getParam("plugins", my_list);
|
||||
for (int32_t i = 0; i < my_list.size(); ++i)
|
||||
{
|
||||
std::string pname = static_cast<std::string>(my_list[i]["name"]);
|
||||
std::string type = static_cast<std::string>(my_list[i]["type"]);
|
||||
ROS_INFO("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());
|
||||
copyParentParameters(pname, type, private_nh);
|
||||
|
||||
boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
|
||||
layered_costmap_->addPlugin(plugin);
|
||||
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
|
||||
}
|
||||
}
|
||||
|
||||
// subscribe to the footprint topic
|
||||
std::string topic_param, topic;
|
||||
if (!private_nh.searchParam("footprint_topic", topic_param))
|
||||
{
|
||||
topic_param = "footprint_topic";
|
||||
}
|
||||
|
||||
private_nh.param(topic_param, topic, std::string("footprint"));
|
||||
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);
|
||||
|
||||
if (!private_nh.searchParam("published_footprint_topic", topic_param))
|
||||
{
|
||||
topic_param = "published_footprint";
|
||||
}
|
||||
|
||||
private_nh.param(topic_param, topic, std::string("footprint")); // TODO: revert to oriented_footprint in N-turtle
|
||||
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
|
||||
|
||||
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
|
||||
|
||||
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
|
||||
always_send_full_costmap);
|
||||
|
||||
// create a thread to handle updating the map
|
||||
stop_updates_ = false;
|
||||
initialized_ = true;
|
||||
stopped_ = false;
|
||||
|
||||
dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
|
||||
dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
dsrv_->setCallback(cb);
|
||||
}
|
||||
|
||||
void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
|
||||
{
|
||||
setUnpaddedRobotFootprint(toPointVector(footprint));
|
||||
}
|
||||
|
||||
Costmap2DROS::~Costmap2DROS()
|
||||
{
|
||||
map_update_thread_shutdown_ = true;
|
||||
if (map_update_thread_ != NULL)
|
||||
{
|
||||
map_update_thread_->join();
|
||||
delete map_update_thread_;
|
||||
}
|
||||
if (publisher_ != NULL)
|
||||
delete publisher_;
|
||||
|
||||
delete layered_costmap_;
|
||||
delete dsrv_;
|
||||
}
|
||||
|
||||
void Costmap2DROS::loadOldParameters(ros::NodeHandle& nh)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::loadOldParameters");
|
||||
ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
|
||||
bool flag;
|
||||
std::string s;
|
||||
std::vector < XmlRpc::XmlRpcValue > plugins;
|
||||
|
||||
XmlRpc::XmlRpcValue::ValueStruct map;
|
||||
SuperValue super_map;
|
||||
SuperValue super_array;
|
||||
|
||||
if (nh.getParam("static_map", flag) && flag)
|
||||
{
|
||||
map["name"] = XmlRpc::XmlRpcValue("static_layer");
|
||||
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
|
||||
super_map.setStruct(&map);
|
||||
plugins.push_back(super_map);
|
||||
|
||||
ros::NodeHandle map_layer(nh, "static_layer");
|
||||
move_parameter(nh, map_layer, "map_topic");
|
||||
move_parameter(nh, map_layer, "unknown_cost_value");
|
||||
move_parameter(nh, map_layer, "lethal_cost_threshold");
|
||||
move_parameter(nh, map_layer, "track_unknown_space", false);
|
||||
}
|
||||
|
||||
ros::NodeHandle obstacles(nh, "obstacle_layer");
|
||||
if (nh.getParam("map_type", s) && s == "voxel")
|
||||
{
|
||||
map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
|
||||
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
|
||||
super_map.setStruct(&map);
|
||||
plugins.push_back(super_map);
|
||||
|
||||
move_parameter(nh, obstacles, "origin_z");
|
||||
move_parameter(nh, obstacles, "z_resolution");
|
||||
move_parameter(nh, obstacles, "z_voxels");
|
||||
move_parameter(nh, obstacles, "mark_threshold");
|
||||
move_parameter(nh, obstacles, "unknown_threshold");
|
||||
move_parameter(nh, obstacles, "publish_voxel_map");
|
||||
}
|
||||
else
|
||||
{
|
||||
map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
|
||||
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
|
||||
super_map.setStruct(&map);
|
||||
plugins.push_back(super_map);
|
||||
}
|
||||
|
||||
move_parameter(nh, obstacles, "max_obstacle_height");
|
||||
move_parameter(nh, obstacles, "raytrace_range");
|
||||
move_parameter(nh, obstacles, "obstacle_range");
|
||||
move_parameter(nh, obstacles, "track_unknown_space", true);
|
||||
nh.param("observation_sources", s, std::string(""));
|
||||
std::stringstream ss(s);
|
||||
std::string source;
|
||||
while (ss >> source)
|
||||
{
|
||||
move_parameter(nh, obstacles, source);
|
||||
}
|
||||
move_parameter(nh, obstacles, "observation_sources");
|
||||
|
||||
ros::NodeHandle inflation(nh, "inflation_layer");
|
||||
move_parameter(nh, inflation, "cost_scaling_factor");
|
||||
move_parameter(nh, inflation, "inflation_radius");
|
||||
map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
|
||||
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
|
||||
super_map.setStruct(&map);
|
||||
plugins.push_back(super_map);
|
||||
|
||||
super_array.setArray(&plugins);
|
||||
nh.setParam("plugins", super_array);
|
||||
}
|
||||
|
||||
void Costmap2DROS::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
|
||||
{
|
||||
ros::NodeHandle target_layer(nh, plugin_name);
|
||||
|
||||
if(plugin_type == "costmap_2d::StaticLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "map_topic", false);
|
||||
move_parameter(nh, target_layer, "unknown_cost_value", false);
|
||||
move_parameter(nh, target_layer, "lethal_cost_threshold", false);
|
||||
move_parameter(nh, target_layer, "track_unknown_space", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::VoxelLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "origin_z", false);
|
||||
move_parameter(nh, target_layer, "z_resolution", false);
|
||||
move_parameter(nh, target_layer, "z_voxels", false);
|
||||
move_parameter(nh, target_layer, "mark_threshold", false);
|
||||
move_parameter(nh, target_layer, "unknown_threshold", false);
|
||||
move_parameter(nh, target_layer, "publish_voxel_map", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::ObstacleLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "max_obstacle_height", false);
|
||||
move_parameter(nh, target_layer, "raytrace_range", false);
|
||||
move_parameter(nh, target_layer, "obstacle_range", false);
|
||||
move_parameter(nh, target_layer, "track_unknown_space", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::InflationLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "cost_scaling_factor", false);
|
||||
move_parameter(nh, target_layer, "inflation_radius", false);
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::warnForOldParameters(ros::NodeHandle& nh)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::warnForOldParameters");
|
||||
checkOldParam(nh, "static_map");
|
||||
checkOldParam(nh, "map_type");
|
||||
}
|
||||
|
||||
void Costmap2DROS::checkOldParam(ros::NodeHandle& nh, const std::string ¶m_name){
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::checkOldParam");
|
||||
if(nh.hasParam(param_name)){
|
||||
ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::reconfigureCB");
|
||||
transform_tolerance_ = config.transform_tolerance;
|
||||
if (map_update_thread_ != NULL)
|
||||
{
|
||||
map_update_thread_shutdown_ = true;
|
||||
map_update_thread_->join();
|
||||
delete map_update_thread_;
|
||||
map_update_thread_ = NULL;
|
||||
}
|
||||
map_update_thread_shutdown_ = false;
|
||||
double map_update_frequency = config.update_frequency;
|
||||
|
||||
double map_publish_frequency = config.publish_frequency;
|
||||
if (map_publish_frequency > 0)
|
||||
publish_cycle = ros::Duration(1 / map_publish_frequency);
|
||||
else
|
||||
publish_cycle = ros::Duration(-1);
|
||||
|
||||
// find size parameters
|
||||
double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
|
||||
config.origin_x,
|
||||
origin_y = config.origin_y;
|
||||
|
||||
if (!layered_costmap_->isSizeLocked())
|
||||
{
|
||||
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
|
||||
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
|
||||
}
|
||||
|
||||
// If the padding has changed, call setUnpaddedRobotFootprint() to
|
||||
// re-apply the padding.
|
||||
if (footprint_padding_ != config.footprint_padding)
|
||||
{
|
||||
footprint_padding_ = config.footprint_padding;
|
||||
setUnpaddedRobotFootprint(unpadded_footprint_);
|
||||
}
|
||||
|
||||
readFootprintFromConfig(config, old_config_);
|
||||
|
||||
old_config_ = config;
|
||||
|
||||
// only construct the thread if the frequency is positive
|
||||
if(map_update_frequency > 0.0)
|
||||
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
|
||||
}
|
||||
|
||||
void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
|
||||
const costmap_2d::Costmap2DConfig &old_config)
|
||||
{
|
||||
// Only change the footprint if footprint or robot_radius has
|
||||
// changed. Otherwise we might overwrite a footprint sent on a
|
||||
// topic by a dynamic_reconfigure call which was setting some other
|
||||
// variable.
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::readFootprintFromConfig");
|
||||
if (new_config.footprint == old_config.footprint &&
|
||||
new_config.robot_radius == old_config.robot_radius)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (new_config.footprint != "" && new_config.footprint != "[]")
|
||||
{
|
||||
std::vector<geometry_msgs::Point> new_footprint;
|
||||
if (makeFootprintFromString(new_config.footprint, new_footprint))
|
||||
{
|
||||
setUnpaddedRobotFootprint(new_footprint);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Invalid footprint string from dynamic reconfigure");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// robot_radius may be 0, but that must be intended at this point.
|
||||
setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
|
||||
{
|
||||
unpadded_footprint_ = points;
|
||||
padded_footprint_ = points;
|
||||
padFootprint(padded_footprint_, footprint_padding_);
|
||||
|
||||
layered_costmap_->setFootprint(padded_footprint_);
|
||||
}
|
||||
|
||||
void Costmap2DROS::movementCB(const ros::TimerEvent &event)
|
||||
{
|
||||
// don't allow configuration to happen while this check occurs
|
||||
// boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::movementCB");
|
||||
geometry_msgs::PoseStamped new_pose;
|
||||
|
||||
if (!getRobotPose(new_pose))
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::mapUpdateLoop(double frequency)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::Rate r(frequency);
|
||||
while (nh.ok() && !map_update_thread_shutdown_)
|
||||
{
|
||||
#ifdef HAVE_SYS_TIME_H
|
||||
struct timeval start, end;
|
||||
double start_t, end_t, t_diff;
|
||||
gettimeofday(&start, NULL);
|
||||
#endif
|
||||
|
||||
updateMap();
|
||||
|
||||
#ifdef HAVE_SYS_TIME_H
|
||||
gettimeofday(&end, NULL);
|
||||
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
|
||||
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
|
||||
t_diff = end_t - start_t;
|
||||
ROS_DEBUG("Map update time: %.9f", t_diff);
|
||||
#endif
|
||||
|
||||
if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
|
||||
{
|
||||
unsigned int x0, y0, xn, yn;
|
||||
layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
|
||||
publisher_->updateBounds(x0, xn, y0, yn);
|
||||
|
||||
ros::Time now = ros::Time::now();
|
||||
if (last_publish_ + publish_cycle < now)
|
||||
{
|
||||
publisher_->publishCostmap();
|
||||
last_publish_ = now;
|
||||
}
|
||||
}
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
// make sure to sleep for the remainder of our cycle time
|
||||
if (r.cycleTime() > ros::Duration(1 / frequency))
|
||||
ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
|
||||
r.cycleTime().toSec());
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::updateMap()
|
||||
{
|
||||
if (!stop_updates_)
|
||||
{
|
||||
// get global pose
|
||||
geometry_msgs::PoseStamped pose;
|
||||
if (getRobotPose (pose))
|
||||
{
|
||||
double x = pose.pose.position.x,
|
||||
y = pose.pose.position.y,
|
||||
yaw = tf2::getYaw(pose.pose.orientation);
|
||||
layered_costmap_->updateMap(x, y, yaw);
|
||||
geometry_msgs::PolygonStamped footprint;
|
||||
footprint.header.frame_id = global_frame_;
|
||||
footprint.header.stamp = ros::Time::now();
|
||||
transformFootprint(x, y, yaw, padded_footprint_, footprint);
|
||||
|
||||
footprint_pub_.publish(footprint);
|
||||
|
||||
initialized_ = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::start()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::start");
|
||||
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
|
||||
// check if we're stopped or just paused
|
||||
if (stopped_)
|
||||
{
|
||||
// if we're stopped we need to re-subscribe to topics
|
||||
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->activate();
|
||||
}
|
||||
stopped_ = false;
|
||||
}
|
||||
stop_updates_ = false;
|
||||
|
||||
// block until the costmap is re-initialized.. meaning one update cycle has run
|
||||
// note: this does not hold, if the user has disabled map-updates allgother
|
||||
ros::Rate r(100.0);
|
||||
while (ros::ok() && !initialized_ && map_update_thread_)
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
void Costmap2DROS::stop()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::stop");
|
||||
stop_updates_ = true;
|
||||
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
|
||||
// unsubscribe from topics
|
||||
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->deactivate();
|
||||
}
|
||||
initialized_ = false;
|
||||
stopped_ = true;
|
||||
}
|
||||
|
||||
void Costmap2DROS::pause()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::pause");
|
||||
stop_updates_ = true;
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
void Costmap2DROS::resume()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::resume");
|
||||
stop_updates_ = false;
|
||||
|
||||
// block until the costmap is re-initialized.. meaning one update cycle has run
|
||||
ros::Rate r(100.0);
|
||||
while (!initialized_)
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
|
||||
void Costmap2DROS::resetLayers()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::resetLayers");
|
||||
Costmap2D* top = layered_costmap_->getCostmap();
|
||||
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
|
||||
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
|
||||
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->reset();
|
||||
}
|
||||
}
|
||||
|
||||
bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
|
||||
geometry_msgs::PoseStamped robot_pose;
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
|
||||
robot_pose.header.frame_id = robot_base_frame_;
|
||||
robot_pose.header.stamp = ros::Time();
|
||||
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
|
||||
|
||||
// get the global pose of the robot
|
||||
try
|
||||
{
|
||||
// use current time if possible (makes sure it's not in the future)
|
||||
if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
|
||||
{
|
||||
geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
|
||||
tf2::doTransform(robot_pose, global_pose, transform);
|
||||
}
|
||||
// use the latest otherwise
|
||||
else
|
||||
{
|
||||
tf_.transform(robot_pose, global_pose, global_frame_);
|
||||
}
|
||||
}
|
||||
catch (tf2::LookupException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ConnectivityException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ExtrapolationException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
// check global_pose timeout
|
||||
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0,
|
||||
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
|
||||
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getOrientedFootprint");
|
||||
geometry_msgs::PoseStamped global_pose;
|
||||
if (!getRobotPose(global_pose))
|
||||
return;
|
||||
|
||||
double yaw = tf2::getYaw(global_pose.pose.orientation);
|
||||
transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
|
||||
padded_footprint_, oriented_footprint);
|
||||
}
|
||||
|
||||
} // namespace costmap_2d
|
||||
173
navigations/costmap_2d/src/costmap_layer.cpp
Executable file
173
navigations/costmap_2d/src/costmap_layer.cpp
Executable file
@@ -0,0 +1,173 @@
|
||||
#include<costmap_2d/costmap_layer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
*min_x = std::min(x, *min_x);
|
||||
*min_y = std::min(y, *min_y);
|
||||
*max_x = std::max(x, *max_x);
|
||||
*max_y = std::max(y, *max_y);
|
||||
}
|
||||
|
||||
void CostmapLayer::matchSize()
|
||||
{
|
||||
Costmap2D* master = layered_costmap_->getCostmap();
|
||||
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
|
||||
master->getOriginX(), master->getOriginY());
|
||||
}
|
||||
|
||||
void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
|
||||
{
|
||||
unsigned char* grid = getCharMap();
|
||||
for(int x=0; x<(int)getSizeInCellsX(); x++){
|
||||
bool xrange = x>start_x && x<end_x;
|
||||
|
||||
for(int y=0; y<(int)getSizeInCellsY(); y++){
|
||||
if((xrange && y>start_y && y<end_y)!=invert_area)
|
||||
continue;
|
||||
int index = getIndex(x,y);
|
||||
if(grid[index]!=NO_INFORMATION){
|
||||
grid[index] = NO_INFORMATION;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
|
||||
{
|
||||
extra_min_x_ = std::min(mx0, extra_min_x_);
|
||||
extra_max_x_ = std::max(mx1, extra_max_x_);
|
||||
extra_min_y_ = std::min(my0, extra_min_y_);
|
||||
extra_max_y_ = std::max(my1, extra_max_y_);
|
||||
has_extra_bounds_ = true;
|
||||
}
|
||||
|
||||
void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
if (!has_extra_bounds_)
|
||||
return;
|
||||
|
||||
*min_x = std::min(extra_min_x_, *min_x);
|
||||
*min_y = std::min(extra_min_y_, *min_y);
|
||||
*max_x = std::max(extra_max_x_, *max_x);
|
||||
*max_y = std::max(extra_max_y_, *max_y);
|
||||
extra_min_x_ = 1e6;
|
||||
extra_min_y_ = 1e6;
|
||||
extra_max_x_ = -1e6;
|
||||
extra_max_y_ = -1e6;
|
||||
has_extra_bounds_ = false;
|
||||
}
|
||||
|
||||
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!enabled_)
|
||||
return;
|
||||
|
||||
unsigned char* master_array = master_grid.getCharMap();
|
||||
unsigned int span = master_grid.getSizeInCellsX();
|
||||
|
||||
for (int j = min_j; j < max_j; j++)
|
||||
{
|
||||
unsigned int it = j * span + min_i;
|
||||
for (int i = min_i; i < max_i; i++)
|
||||
{
|
||||
if (costmap_[it] == NO_INFORMATION){
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
if (costmap_[it] == CRITICAL_SPACE){
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
if (costmap_[it] == PREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
if (costmap_[it] == UNPREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
unsigned char old_cost = master_array[it];
|
||||
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
|
||||
int max_i, int max_j)
|
||||
{
|
||||
if (!enabled_)
|
||||
return;
|
||||
unsigned char* master = master_grid.getCharMap();
|
||||
unsigned int span = master_grid.getSizeInCellsX();
|
||||
|
||||
for (int j = min_j; j < max_j; j++)
|
||||
{
|
||||
unsigned int it = span*j+min_i;
|
||||
for (int i = min_i; i < max_i; i++)
|
||||
{
|
||||
master[it] = costmap_[it];
|
||||
it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!enabled_)
|
||||
return;
|
||||
unsigned char* master = master_grid.getCharMap();
|
||||
unsigned int span = master_grid.getSizeInCellsX();
|
||||
|
||||
for (int j = min_j; j < max_j; j++)
|
||||
{
|
||||
unsigned int it = span*j+min_i;
|
||||
for (int i = min_i; i < max_i; i++)
|
||||
{
|
||||
if (costmap_[it] != NO_INFORMATION)
|
||||
master[it] = costmap_[it];
|
||||
it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!enabled_)
|
||||
return;
|
||||
unsigned char* master_array = master_grid.getCharMap();
|
||||
unsigned int span = master_grid.getSizeInCellsX();
|
||||
|
||||
for (int j = min_j; j < max_j; j++)
|
||||
{
|
||||
unsigned int it = j * span + min_i;
|
||||
for (int i = min_i; i < max_i; i++)
|
||||
{
|
||||
if (costmap_[it] == NO_INFORMATION){
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
|
||||
unsigned char old_cost = master_array[it];
|
||||
if (old_cost == NO_INFORMATION)
|
||||
master_array[it] = costmap_[it];
|
||||
else
|
||||
{
|
||||
int sum = old_cost + costmap_[it];
|
||||
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
|
||||
else
|
||||
master_array[it] = sum;
|
||||
}
|
||||
it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace costmap_2d
|
||||
89
navigations/costmap_2d/src/costmap_math.cpp
Executable file
89
navigations/costmap_2d/src/costmap_math.cpp
Executable file
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* Copyright (c) 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 the 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.
|
||||
*/
|
||||
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
|
||||
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
|
||||
{
|
||||
double A = pX - x0;
|
||||
double B = pY - y0;
|
||||
double C = x1 - x0;
|
||||
double D = y1 - y0;
|
||||
|
||||
double dot = A * C + B * D;
|
||||
double len_sq = C * C + D * D;
|
||||
double param = dot / len_sq;
|
||||
|
||||
double xx, yy;
|
||||
|
||||
if (param < 0)
|
||||
{
|
||||
xx = x0;
|
||||
yy = y0;
|
||||
}
|
||||
else if (param > 1)
|
||||
{
|
||||
xx = x1;
|
||||
yy = y1;
|
||||
}
|
||||
else
|
||||
{
|
||||
xx = x0 + param * C;
|
||||
yy = y0 + param * D;
|
||||
}
|
||||
|
||||
return distance(pX, pY, xx, yy);
|
||||
}
|
||||
|
||||
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
|
||||
{
|
||||
bool c = false;
|
||||
int i, j, nvert = polygon.size();
|
||||
for (i = 0, j = nvert - 1; i < nvert; j = i++)
|
||||
{
|
||||
float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
|
||||
|
||||
if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
|
||||
c = !c;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
|
||||
{
|
||||
for (unsigned int i = 0; i < polygon1.size(); i++)
|
||||
if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
|
||||
{
|
||||
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
|
||||
}
|
||||
325
navigations/costmap_2d/src/footprint.cpp
Executable file
325
navigations/costmap_2d/src/footprint.cpp
Executable file
@@ -0,0 +1,325 @@
|
||||
/*
|
||||
* Copyright (c) 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 the 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.
|
||||
*/
|
||||
|
||||
#include<costmap_2d/costmap_math.h>
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/array_parser.h>
|
||||
#include<geometry_msgs/Point32.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
|
||||
{
|
||||
min_dist = std::numeric_limits<double>::max();
|
||||
max_dist = 0.0;
|
||||
|
||||
if (footprint.size() <= 2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < footprint.size() - 1; ++i)
|
||||
{
|
||||
// check the distance from the robot center point to the first vertex
|
||||
double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
|
||||
double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
|
||||
footprint[i + 1].x, footprint[i + 1].y);
|
||||
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
|
||||
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
|
||||
}
|
||||
|
||||
// we also need to do the last vertex and the first vertex
|
||||
double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
|
||||
double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
|
||||
footprint.front().x, footprint.front().y);
|
||||
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
|
||||
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
|
||||
}
|
||||
|
||||
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
|
||||
{
|
||||
geometry_msgs::Point32 point32;
|
||||
point32.x = pt.x;
|
||||
point32.y = pt.y;
|
||||
point32.z = pt.z;
|
||||
return point32;
|
||||
}
|
||||
|
||||
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
|
||||
{
|
||||
geometry_msgs::Point point;
|
||||
point.x = pt.x;
|
||||
point.y = pt.y;
|
||||
point.z = pt.z;
|
||||
return point;
|
||||
}
|
||||
|
||||
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
|
||||
{
|
||||
geometry_msgs::Polygon polygon;
|
||||
for (int i = 0; i < pts.size(); i++){
|
||||
polygon.points.push_back(toPoint32(pts[i]));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
|
||||
{
|
||||
std::vector<geometry_msgs::Point> pts;
|
||||
for (int i = 0; i < polygon.points.size(); i++)
|
||||
{
|
||||
pts.push_back(toPoint(polygon.points[i]));
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
|
||||
std::vector<geometry_msgs::Point>& oriented_footprint)
|
||||
{
|
||||
// build the oriented footprint at a given location
|
||||
oriented_footprint.clear();
|
||||
double cos_th = cos(theta);
|
||||
double sin_th = sin(theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
|
||||
{
|
||||
geometry_msgs::Point new_pt;
|
||||
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
|
||||
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
|
||||
oriented_footprint.push_back(new_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
|
||||
geometry_msgs::PolygonStamped& oriented_footprint)
|
||||
{
|
||||
// build the oriented footprint at a given location
|
||||
oriented_footprint.polygon.points.clear();
|
||||
double cos_th = cos(theta);
|
||||
double sin_th = sin(theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
|
||||
{
|
||||
geometry_msgs::Point32 new_pt;
|
||||
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
|
||||
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
|
||||
oriented_footprint.polygon.points.push_back(new_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
|
||||
{
|
||||
// pad footprint in place
|
||||
for (unsigned int i = 0; i < footprint.size(); i++)
|
||||
{
|
||||
geometry_msgs::Point& pt = footprint[ i ];
|
||||
pt.x += sign0(pt.x) * padding;
|
||||
pt.y += sign0(pt.y) * padding;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
|
||||
{
|
||||
std::vector<geometry_msgs::Point> points;
|
||||
|
||||
// Loop over 16 angles around a circle making a point each time
|
||||
int N = 16;
|
||||
geometry_msgs::Point pt;
|
||||
for (int i = 0; i < N; ++i)
|
||||
{
|
||||
double angle = i * 2 * M_PI / N;
|
||||
pt.x = cos(angle) * radius;
|
||||
pt.y = sin(angle) * radius;
|
||||
|
||||
points.push_back(pt);
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
|
||||
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
|
||||
|
||||
if (error != "")
|
||||
{
|
||||
ROS_ERROR("Error parsing footprint parameter: '%s'", error.c_str());
|
||||
ROS_ERROR(" Footprint string was '%s'.", footprint_string.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// convert vvf into points.
|
||||
if (vvf.size() < 3)
|
||||
{
|
||||
ROS_ERROR("You must specify at least three points for the robot footprint, reverting to previous footprint.");
|
||||
return false;
|
||||
}
|
||||
footprint.reserve(vvf.size());
|
||||
for (unsigned int i = 0; i < vvf.size(); i++)
|
||||
{
|
||||
if (vvf[ i ].size() == 2)
|
||||
{
|
||||
geometry_msgs::Point point;
|
||||
point.x = vvf[ i ][ 0 ];
|
||||
point.y = vvf[ i ][ 1 ];
|
||||
point.z = 0;
|
||||
footprint.push_back(point);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.",
|
||||
int(vvf[ i ].size()));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
|
||||
{
|
||||
std::string full_param_name;
|
||||
std::string full_radius_param_name;
|
||||
std::vector<geometry_msgs::Point> points;
|
||||
|
||||
if (nh.searchParam("footprint", full_param_name))
|
||||
{
|
||||
XmlRpc::XmlRpcValue footprint_xmlrpc;
|
||||
nh.getParam(full_param_name, footprint_xmlrpc);
|
||||
if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
|
||||
footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
|
||||
{
|
||||
if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
|
||||
{
|
||||
writeFootprintToParam(nh, points);
|
||||
return points;
|
||||
}
|
||||
}
|
||||
else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
|
||||
{
|
||||
points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
|
||||
writeFootprintToParam(nh, points);
|
||||
return points;
|
||||
}
|
||||
}
|
||||
|
||||
if (nh.searchParam("robot_radius", full_radius_param_name))
|
||||
{
|
||||
double robot_radius;
|
||||
nh.param(full_radius_param_name, robot_radius, 1.234);
|
||||
points = makeFootprintFromRadius(robot_radius);
|
||||
nh.setParam("robot_radius", robot_radius);
|
||||
}
|
||||
// Else neither param was found anywhere this knows about, so
|
||||
// defaults will come from dynamic_reconfigure stuff, set in
|
||||
// cfg/Costmap2D.cfg and read in this file in reconfigureCB().
|
||||
return points;
|
||||
}
|
||||
|
||||
void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
bool first = true;
|
||||
for (unsigned int i = 0; i < footprint.size(); i++)
|
||||
{
|
||||
geometry_msgs::Point p = footprint[ i ];
|
||||
if (first)
|
||||
{
|
||||
oss << "[[" << p.x << "," << p.y << "]";
|
||||
first = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
oss << ",[" << p.x << "," << p.y << "]";
|
||||
}
|
||||
}
|
||||
oss << "]";
|
||||
nh.setParam("footprint", oss.str().c_str());
|
||||
}
|
||||
|
||||
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
|
||||
{
|
||||
// Make sure that the value we're looking at is either a double or an int.
|
||||
if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
|
||||
value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
|
||||
{
|
||||
std::string& value_string = value;
|
||||
ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.",
|
||||
full_param_name.c_str(), value_string.c_str());
|
||||
throw std::runtime_error("Values in the footprint specification must be numbers");
|
||||
}
|
||||
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
|
||||
const std::string& full_param_name)
|
||||
{
|
||||
// Make sure we have an array of at least 3 elements.
|
||||
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
|
||||
footprint_xmlrpc.size() < 3)
|
||||
{
|
||||
ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
|
||||
full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
|
||||
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
|
||||
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::Point> footprint;
|
||||
geometry_msgs::Point pt;
|
||||
|
||||
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
|
||||
{
|
||||
// Make sure each element of the list is an array of size 2. (x and y coordinates)
|
||||
XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
|
||||
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
|
||||
point.size() != 2)
|
||||
{
|
||||
ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
|
||||
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
|
||||
full_param_name.c_str());
|
||||
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
|
||||
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
|
||||
}
|
||||
|
||||
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
|
||||
pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
|
||||
|
||||
footprint.push_back(pt);
|
||||
}
|
||||
return footprint;
|
||||
}
|
||||
|
||||
} // end namespace costmap_2d
|
||||
56
navigations/costmap_2d/src/layer.cpp
Executable file
56
navigations/costmap_2d/src/layer.cpp
Executable file
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (c) 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 the 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.
|
||||
*/
|
||||
|
||||
#include "costmap_2d/layer.h"
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
Layer::Layer()
|
||||
: layered_costmap_(NULL)
|
||||
, current_(false)
|
||||
, enabled_(false)
|
||||
, name_()
|
||||
, tf_(NULL)
|
||||
{}
|
||||
|
||||
void Layer::initialize(LayeredCostmap* parent, std::string name, tf2_ros::Buffer *tf)
|
||||
{
|
||||
layered_costmap_ = parent;
|
||||
name_ = name;
|
||||
tf_ = tf;
|
||||
onInitialize();
|
||||
}
|
||||
|
||||
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
|
||||
{
|
||||
return layered_costmap_->getFootprint();
|
||||
}
|
||||
|
||||
} // end namespace costmap_2d
|
||||
190
navigations/costmap_2d/src/layered_costmap.cpp
Executable file
190
navigations/costmap_2d/src/layered_costmap.cpp
Executable file
@@ -0,0 +1,190 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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>
|
||||
|
||||
using std::vector;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
|
||||
: costmap_(),
|
||||
global_frame_(global_frame),
|
||||
rolling_window_(rolling_window),
|
||||
current_(false),
|
||||
minx_(0.0),
|
||||
miny_(0.0),
|
||||
maxx_(0.0),
|
||||
maxy_(0.0),
|
||||
bx0_(0),
|
||||
bxn_(0),
|
||||
by0_(0),
|
||||
byn_(0),
|
||||
initialized_(false),
|
||||
size_locked_(false),
|
||||
circumscribed_radius_(1.0),
|
||||
inscribed_radius_(0.1)
|
||||
{
|
||||
if (track_unknown)
|
||||
costmap_.setDefaultValue(NO_INFORMATION);
|
||||
else
|
||||
costmap_.setDefaultValue(FREE_SPACE);
|
||||
}
|
||||
|
||||
LayeredCostmap::~LayeredCostmap()
|
||||
{
|
||||
while (plugins_.size() > 0)
|
||||
{
|
||||
plugins_.pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
|
||||
double origin_y, bool size_locked)
|
||||
{
|
||||
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();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->matchSize();
|
||||
}
|
||||
}
|
||||
|
||||
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
|
||||
{
|
||||
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
|
||||
// implement thread unsafe updateBounds() functions.
|
||||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
||||
|
||||
// if we're using a rolling buffer costmap... we need to update the origin using the robot's position
|
||||
if (rolling_window_)
|
||||
{
|
||||
double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
|
||||
double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
|
||||
costmap_.updateOrigin(new_origin_x, new_origin_y);
|
||||
}
|
||||
|
||||
if (plugins_.size() == 0)
|
||||
return;
|
||||
|
||||
minx_ = miny_ = 1e30;
|
||||
maxx_ = maxy_ = -1e30;
|
||||
|
||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if (!(*plugin)->isEnabled())
|
||||
continue;
|
||||
double prev_minx = minx_;
|
||||
double prev_miny = miny_;
|
||||
double prev_maxx = maxx_;
|
||||
double prev_maxy = maxy_;
|
||||
(*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)
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
|
||||
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
|
||||
prev_minx, prev_miny, prev_maxx, prev_maxy,
|
||||
minx_, miny_, maxx_, maxy_,
|
||||
(*plugin)->getName().c_str());
|
||||
}
|
||||
}
|
||||
|
||||
int x0, xn, y0, yn;
|
||||
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
|
||||
costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
|
||||
|
||||
x0 = std::max(0, x0);
|
||||
xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
|
||||
y0 = std::max(0, y0);
|
||||
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
|
||||
|
||||
ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
|
||||
|
||||
if (xn < x0 || yn < y0)
|
||||
return;
|
||||
|
||||
costmap_.resetMap(x0, y0, xn, yn);
|
||||
|
||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
|
||||
}
|
||||
|
||||
bx0_ = x0;
|
||||
bxn_ = xn;
|
||||
by0_ = y0;
|
||||
byn_ = yn;
|
||||
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
bool LayeredCostmap::isCurrent()
|
||||
{
|
||||
current_ = true;
|
||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
current_ = current_ && (*plugin)->isCurrent();
|
||||
}
|
||||
return current_;
|
||||
}
|
||||
|
||||
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
|
||||
{
|
||||
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();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->onFootprintChanged();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace costmap_2d
|
||||
251
navigations/costmap_2d/src/observation_buffer.cpp
Executable file
251
navigations/costmap_2d/src/observation_buffer.cpp
Executable file
@@ -0,0 +1,251 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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
|
||||
*********************************************************************/
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace tf2;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
|
||||
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||
double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
|
||||
string sensor_frame, double tf_tolerance) :
|
||||
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||
last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
||||
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
|
||||
{
|
||||
}
|
||||
|
||||
ObservationBuffer::~ObservationBuffer()
|
||||
{
|
||||
}
|
||||
|
||||
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||
{
|
||||
ros::Time transform_time = ros::Time::now();
|
||||
std::string tf_error;
|
||||
|
||||
geometry_msgs::TransformStamped transformStamped;
|
||||
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
|
||||
{
|
||||
ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
|
||||
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
list<Observation>::iterator obs_it;
|
||||
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
{
|
||||
try
|
||||
{
|
||||
Observation& obs = *obs_it;
|
||||
|
||||
geometry_msgs::PointStamped origin;
|
||||
origin.header.frame_id = global_frame_;
|
||||
origin.header.stamp = transform_time;
|
||||
origin.point = obs.origin_;
|
||||
|
||||
// we need to transform the origin of the observation to the new global frame
|
||||
tf2_buffer_.transform(origin, origin, new_global_frame);
|
||||
obs.origin_ = origin.point;
|
||||
|
||||
// we also need to transform the cloud of the observation to the new global frame
|
||||
tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
|
||||
}
|
||||
catch (TransformException& ex)
|
||||
{
|
||||
ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
|
||||
new_global_frame.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// now we need to update our global_frame member
|
||||
global_frame_ = new_global_frame;
|
||||
return true;
|
||||
}
|
||||
|
||||
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
||||
{
|
||||
geometry_msgs::PointStamped global_origin;
|
||||
|
||||
// create a new observation on the list to be populated
|
||||
observation_list_.push_front(Observation());
|
||||
|
||||
// check whether the origin frame has been set explicitly or whether we should get it from the cloud
|
||||
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
|
||||
|
||||
try
|
||||
{
|
||||
// given these observations come from sensors... we'll need to store the origin pt of the sensor
|
||||
geometry_msgs::PointStamped local_origin;
|
||||
local_origin.header.stamp = cloud.header.stamp;
|
||||
local_origin.header.frame_id = origin_frame;
|
||||
local_origin.point.x = 0;
|
||||
local_origin.point.y = 0;
|
||||
local_origin.point.z = 0;
|
||||
tf2_buffer_.transform(local_origin, global_origin, global_frame_);
|
||||
tf2::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
|
||||
observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2 global_frame_cloud;
|
||||
|
||||
// transform the point cloud
|
||||
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||
global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||
|
||||
// now we need to remove observations from the cloud that are below or above our height thresholds
|
||||
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||
observation_cloud.height = global_frame_cloud.height;
|
||||
observation_cloud.width = global_frame_cloud.width;
|
||||
observation_cloud.fields = global_frame_cloud.fields;
|
||||
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
|
||||
observation_cloud.point_step = global_frame_cloud.point_step;
|
||||
observation_cloud.row_step = global_frame_cloud.row_step;
|
||||
observation_cloud.is_dense = global_frame_cloud.is_dense;
|
||||
|
||||
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
||||
sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||
modifier.resize(cloud_size);
|
||||
unsigned int point_count = 0;
|
||||
|
||||
// copy over the points that are within our height bounds
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
||||
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
||||
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
||||
{
|
||||
if ((*iter_z) <= max_obstacle_height_
|
||||
&& (*iter_z) >= min_obstacle_height_)
|
||||
{
|
||||
std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
|
||||
iter_obs += global_frame_cloud.point_step;
|
||||
++point_count;
|
||||
}
|
||||
}
|
||||
|
||||
// resize the cloud for the number of legal points
|
||||
modifier.resize(point_count);
|
||||
observation_cloud.header.stamp = cloud.header.stamp;
|
||||
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
|
||||
}
|
||||
catch (TransformException& ex)
|
||||
{
|
||||
// if an exception occurs, we need to remove the empty observation from the list
|
||||
observation_list_.pop_front();
|
||||
ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
|
||||
cloud.header.frame_id.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
// if the update was successful, we want to update the last updated time
|
||||
last_updated_ = ros::Time::now();
|
||||
|
||||
// we'll also remove any stale observations from the list
|
||||
purgeStaleObservations();
|
||||
}
|
||||
|
||||
// returns a copy of the observations
|
||||
void ObservationBuffer::getObservations(vector<Observation>& observations)
|
||||
{
|
||||
// first... let's make sure that we don't have any stale observations
|
||||
purgeStaleObservations();
|
||||
|
||||
// now we'll just copy the observations for the caller
|
||||
list<Observation>::iterator obs_it;
|
||||
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
{
|
||||
observations.push_back(*obs_it);
|
||||
}
|
||||
}
|
||||
|
||||
void ObservationBuffer::purgeStaleObservations()
|
||||
{
|
||||
if (!observation_list_.empty())
|
||||
{
|
||||
list<Observation>::iterator obs_it = observation_list_.begin();
|
||||
// if we're keeping observations for no time... then we'll only keep one observation
|
||||
if (observation_keep_time_ == ros::Duration(0.0))
|
||||
{
|
||||
observation_list_.erase(++obs_it, observation_list_.end());
|
||||
return;
|
||||
}
|
||||
|
||||
// otherwise... we'll have to loop through the observations to see which ones are stale
|
||||
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
{
|
||||
Observation& obs = *obs_it;
|
||||
// check if the observation is out of date... and if it is, remove it and those that follow from the list
|
||||
if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
|
||||
{
|
||||
observation_list_.erase(obs_it, observation_list_.end());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ObservationBuffer::isCurrent() const
|
||||
{
|
||||
if (expected_update_rate_ == ros::Duration(0.0))
|
||||
return true;
|
||||
|
||||
bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
|
||||
if (!current)
|
||||
{
|
||||
ROS_WARN(
|
||||
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
|
||||
topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
void ObservationBuffer::resetLastUpdated()
|
||||
{
|
||||
last_updated_ = ros::Time::now();
|
||||
}
|
||||
} // namespace costmap_2d
|
||||
|
||||
Reference in New Issue
Block a user