HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
navigations/nav_2d_utils/include/mapbox/LICENSE Executable file
View File

@@ -0,0 +1,15 @@
ISC License
Copyright (c) 2015, Mapbox
Permission to use, copy, modify, and/or distribute this software for any purpose
with or without fee is hereby granted, provided that the above copyright notice
and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS
OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
THIS SOFTWARE.

2
navigations/nav_2d_utils/include/mapbox/NOTES Executable file
View File

@@ -0,0 +1,2 @@
A C++ port of earcut.js, a fast, header-only polygon triangulation library.
https://github.com/mapbox/earcut.hpp

776
navigations/nav_2d_utils/include/mapbox/earcut.hpp Executable file
View File

@@ -0,0 +1,776 @@
#pragma once
#include <algorithm>
#include <cassert>
#include <cmath>
#include <memory>
#include <vector>
namespace mapbox {
namespace util {
template <std::size_t I, typename T> struct nth {
inline static typename std::tuple_element<I, T>::type
get(const T& t) { return std::get<I>(t); };
};
}
namespace detail {
template <typename N = uint32_t>
class Earcut {
public:
std::vector<N> indices;
std::size_t vertices = 0;
template <typename Polygon>
void operator()(const Polygon& points);
private:
struct Node {
Node(N index, double x_, double y_) : i(index), x(x_), y(y_) {}
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
Node(Node&&) = delete;
Node& operator=(Node&&) = delete;
const N i;
const double x;
const double y;
// previous and next vertice nodes in a polygon ring
Node* prev = nullptr;
Node* next = nullptr;
// z-order curve value
int32_t z = 0;
// previous and next nodes in z-order
Node* prevZ = nullptr;
Node* nextZ = nullptr;
// indicates whether this is a steiner point
bool steiner = false;
};
template <typename Ring> Node* linkedList(const Ring& points, const bool clockwise);
Node* filterPoints(Node* start, Node* end = nullptr);
void earcutLinked(Node* ear, int pass = 0);
bool isEar(Node* ear);
bool isEarHashed(Node* ear);
Node* cureLocalIntersections(Node* start);
void splitEarcut(Node* start);
template <typename Polygon> Node* eliminateHoles(const Polygon& points, Node* outerNode);
void eliminateHole(Node* hole, Node* outerNode);
Node* findHoleBridge(Node* hole, Node* outerNode);
void indexCurve(Node* start);
Node* sortLinked(Node* list);
int32_t zOrder(const double x_, const double y_);
Node* getLeftmost(Node* start);
bool pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const;
bool isValidDiagonal(Node* a, Node* b);
double area(const Node* p, const Node* q, const Node* r) const;
bool equals(const Node* p1, const Node* p2);
bool intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2);
bool intersectsPolygon(const Node* a, const Node* b);
bool locallyInside(const Node* a, const Node* b);
bool middleInside(const Node* a, const Node* b);
Node* splitPolygon(Node* a, Node* b);
template <typename Point> Node* insertNode(std::size_t i, const Point& p, Node* last);
void removeNode(Node* p);
bool hashing;
double minX, maxX;
double minY, maxY;
double inv_size = 0;
template <typename T, typename Alloc = std::allocator<T>>
class ObjectPool {
public:
ObjectPool() { }
ObjectPool(std::size_t blockSize_) {
reset(blockSize_);
}
~ObjectPool() {
clear();
}
template <typename... Args>
T* construct(Args&&... args) {
if (currentIndex >= blockSize) {
currentBlock = alloc_traits::allocate(alloc, blockSize);
allocations.emplace_back(currentBlock);
currentIndex = 0;
}
T* object = &currentBlock[currentIndex++];
alloc_traits::construct(alloc, object, std::forward<Args>(args)...);
return object;
}
void reset(std::size_t newBlockSize) {
for (auto allocation : allocations) {
alloc_traits::deallocate(alloc, allocation, blockSize);
}
allocations.clear();
blockSize = std::max<std::size_t>(1, newBlockSize);
currentBlock = nullptr;
currentIndex = blockSize;
}
void clear() { reset(blockSize); }
private:
T* currentBlock = nullptr;
std::size_t currentIndex = 1;
std::size_t blockSize = 1;
std::vector<T*> allocations;
Alloc alloc;
typedef typename std::allocator_traits<Alloc> alloc_traits;
};
ObjectPool<Node> nodes;
};
template <typename N> template <typename Polygon>
void Earcut<N>::operator()(const Polygon& points) {
// reset
indices.clear();
vertices = 0;
if (points.empty()) return;
double x;
double y;
int threshold = 80;
std::size_t len = 0;
for (size_t i = 0; threshold >= 0 && i < points.size(); i++) {
threshold -= static_cast<int>(points[i].size());
len += points[i].size();
}
//estimate size of nodes and indices
nodes.reset(len * 3 / 2);
indices.reserve(len + points[0].size());
Node* outerNode = linkedList(points[0], true);
if (!outerNode || outerNode->prev == outerNode->next) return;
if (points.size() > 1) outerNode = eliminateHoles(points, outerNode);
// if the shape is not too simple, we'll use z-order curve hash later; calculate polygon bbox
hashing = threshold < 0;
if (hashing) {
Node* p = outerNode->next;
minX = maxX = outerNode->x;
minY = maxY = outerNode->y;
do {
x = p->x;
y = p->y;
minX = std::min<double>(minX, x);
minY = std::min<double>(minY, y);
maxX = std::max<double>(maxX, x);
maxY = std::max<double>(maxY, y);
p = p->next;
} while (p != outerNode);
// minX, minY and size are later used to transform coords into integers for z-order calculation
inv_size = std::max<double>(maxX - minX, maxY - minY);
inv_size = inv_size != .0 ? (1. / inv_size) : .0;
}
earcutLinked(outerNode);
nodes.clear();
}
// create a circular doubly linked list from polygon points in the specified winding order
template <typename N> template <typename Ring>
typename Earcut<N>::Node*
Earcut<N>::linkedList(const Ring& points, const bool clockwise) {
using Point = typename Ring::value_type;
double sum = 0;
const std::size_t len = points.size();
std::size_t i, j;
Node* last = nullptr;
// calculate original winding order of a polygon ring
for (i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) {
const auto& p1 = points[i];
const auto& p2 = points[j];
const double p20 = util::nth<0, Point>::get(p2);
const double p10 = util::nth<0, Point>::get(p1);
const double p11 = util::nth<1, Point>::get(p1);
const double p21 = util::nth<1, Point>::get(p2);
sum += (p20 - p10) * (p11 + p21);
}
// link points into circular doubly-linked list in the specified winding order
if (clockwise == (sum > 0)) {
for (i = 0; i < len; i++) last = insertNode(vertices + i, points[i], last);
} else {
for (i = len; i-- > 0;) last = insertNode(vertices + i, points[i], last);
}
if (last && equals(last, last->next)) {
removeNode(last);
last = last->next;
}
vertices += len;
return last;
}
// eliminate colinear or duplicate points
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::filterPoints(Node* start, Node* end) {
if (!end) end = start;
Node* p = start;
bool again;
do {
again = false;
if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) {
removeNode(p);
p = end = p->prev;
if (p == p->next) break;
again = true;
} else {
p = p->next;
}
} while (again || p != end);
return end;
}
// main ear slicing loop which triangulates a polygon (given as a linked list)
template <typename N>
void Earcut<N>::earcutLinked(Node* ear, int pass) {
if (!ear) return;
// interlink polygon nodes in z-order
if (!pass && hashing) indexCurve(ear);
Node* stop = ear;
Node* prev;
Node* next;
int iterations = 0;
// iterate through ears, slicing them one by one
while (ear->prev != ear->next) {
iterations++;
prev = ear->prev;
next = ear->next;
if (hashing ? isEarHashed(ear) : isEar(ear)) {
// cut off the triangle
indices.emplace_back(prev->i);
indices.emplace_back(ear->i);
indices.emplace_back(next->i);
removeNode(ear);
// skipping the next vertice leads to less sliver triangles
ear = next->next;
stop = next->next;
continue;
}
ear = next;
// if we looped through the whole remaining polygon and can't find any more ears
if (ear == stop) {
// try filtering points and slicing again
if (!pass) earcutLinked(filterPoints(ear), 1);
// if this didn't work, try curing all small self-intersections locally
else if (pass == 1) {
ear = cureLocalIntersections(ear);
earcutLinked(ear, 2);
// as a last resort, try splitting the remaining polygon into two
} else if (pass == 2) splitEarcut(ear);
break;
}
}
}
// check whether a polygon node forms a valid ear with adjacent nodes
template <typename N>
bool Earcut<N>::isEar(Node* ear) {
const Node* a = ear->prev;
const Node* b = ear;
const Node* c = ear->next;
if (area(a, b, c) >= 0) return false; // reflex, can't be an ear
// now make sure we don't have other points inside the potential ear
Node* p = ear->next->next;
while (p != ear->prev) {
if (pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) &&
area(p->prev, p, p->next) >= 0) return false;
p = p->next;
}
return true;
}
template <typename N>
bool Earcut<N>::isEarHashed(Node* ear) {
const Node* a = ear->prev;
const Node* b = ear;
const Node* c = ear->next;
if (area(a, b, c) >= 0) return false; // reflex, can't be an ear
// triangle bbox; min & max are calculated like this for speed
const double minTX = std::min<double>(a->x, std::min<double>(b->x, c->x));
const double minTY = std::min<double>(a->y, std::min<double>(b->y, c->y));
const double maxTX = std::max<double>(a->x, std::max<double>(b->x, c->x));
const double maxTY = std::max<double>(a->y, std::max<double>(b->y, c->y));
// z-order range for the current triangle bbox;
const int32_t minZ = zOrder(minTX, minTY);
const int32_t maxZ = zOrder(maxTX, maxTY);
// first look for points inside the triangle in increasing z-order
Node* p = ear->nextZ;
while (p && p->z <= maxZ) {
if (p != ear->prev && p != ear->next &&
pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) &&
area(p->prev, p, p->next) >= 0) return false;
p = p->nextZ;
}
// then look for points in decreasing z-order
p = ear->prevZ;
while (p && p->z >= minZ) {
if (p != ear->prev && p != ear->next &&
pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) &&
area(p->prev, p, p->next) >= 0) return false;
p = p->prevZ;
}
return true;
}
// go through all polygon nodes and cure small local self-intersections
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::cureLocalIntersections(Node* start) {
Node* p = start;
do {
Node* a = p->prev;
Node* b = p->next->next;
// a self-intersection where edge (v[i-1],v[i]) intersects (v[i+1],v[i+2])
if (!equals(a, b) && intersects(a, p, p->next, b) && locallyInside(a, b) && locallyInside(b, a)) {
indices.emplace_back(a->i);
indices.emplace_back(p->i);
indices.emplace_back(b->i);
// remove two nodes involved
removeNode(p);
removeNode(p->next);
p = start = b;
}
p = p->next;
} while (p != start);
return p;
}
// try splitting polygon into two and triangulate them independently
template <typename N>
void Earcut<N>::splitEarcut(Node* start) {
// look for a valid diagonal that divides the polygon into two
Node* a = start;
do {
Node* b = a->next->next;
while (b != a->prev) {
if (a->i != b->i && isValidDiagonal(a, b)) {
// split the polygon in two by the diagonal
Node* c = splitPolygon(a, b);
// filter colinear points around the cuts
a = filterPoints(a, a->next);
c = filterPoints(c, c->next);
// run earcut on each half
earcutLinked(a);
earcutLinked(c);
return;
}
b = b->next;
}
a = a->next;
} while (a != start);
}
// link every hole into the outer loop, producing a single-ring polygon without holes
template <typename N> template <typename Polygon>
typename Earcut<N>::Node*
Earcut<N>::eliminateHoles(const Polygon& points, Node* outerNode) {
const size_t len = points.size();
std::vector<Node*> queue;
for (size_t i = 1; i < len; i++) {
Node* list = linkedList(points[i], false);
if (list) {
if (list == list->next) list->steiner = true;
queue.push_back(getLeftmost(list));
}
}
std::sort(queue.begin(), queue.end(), [](const Node* a, const Node* b) {
return a->x < b->x;
});
// process holes from left to right
for (size_t i = 0; i < queue.size(); i++) {
eliminateHole(queue[i], outerNode);
outerNode = filterPoints(outerNode, outerNode->next);
}
return outerNode;
}
// find a bridge between vertices that connects hole with an outer ring and and link it
template <typename N>
void Earcut<N>::eliminateHole(Node* hole, Node* outerNode) {
outerNode = findHoleBridge(hole, outerNode);
if (outerNode) {
Node* b = splitPolygon(outerNode, hole);
filterPoints(b, b->next);
}
}
// David Eberly's algorithm for finding a bridge between hole and outer polygon
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::findHoleBridge(Node* hole, Node* outerNode) {
Node* p = outerNode;
double hx = hole->x;
double hy = hole->y;
double qx = -std::numeric_limits<double>::infinity();
Node* m = nullptr;
// find a segment intersected by a ray from the hole's leftmost Vertex to the left;
// segment's endpoint with lesser x will be potential connection Vertex
do {
if (hy <= p->y && hy >= p->next->y && p->next->y != p->y) {
double x = p->x + (hy - p->y) * (p->next->x - p->x) / (p->next->y - p->y);
if (x <= hx && x > qx) {
qx = x;
if (x == hx) {
if (hy == p->y) return p;
if (hy == p->next->y) return p->next;
}
m = p->x < p->next->x ? p : p->next;
}
}
p = p->next;
} while (p != outerNode);
if (!m) return 0;
if (hx == qx) return m->prev;
// look for points inside the triangle of hole Vertex, segment intersection and endpoint;
// if there are no points found, we have a valid connection;
// otherwise choose the Vertex of the minimum angle with the ray as connection Vertex
const Node* stop = m;
double tanMin = std::numeric_limits<double>::infinity();
double tanCur = 0;
p = m->next;
double mx = m->x;
double my = m->y;
while (p != stop) {
if (hx >= p->x && p->x >= mx && hx != p->x &&
pointInTriangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x, p->y)) {
tanCur = std::abs(hy - p->y) / (hx - p->x); // tangential
if ((tanCur < tanMin || (tanCur == tanMin && p->x > m->x)) && locallyInside(p, hole)) {
m = p;
tanMin = tanCur;
}
}
p = p->next;
}
return m;
}
// interlink polygon nodes in z-order
template <typename N>
void Earcut<N>::indexCurve(Node* start) {
assert(start);
Node* p = start;
do {
p->z = p->z ? p->z : zOrder(p->x, p->y);
p->prevZ = p->prev;
p->nextZ = p->next;
p = p->next;
} while (p != start);
p->prevZ->nextZ = nullptr;
p->prevZ = nullptr;
sortLinked(p);
}
// Simon Tatham's linked list merge sort algorithm
// http://www.chiark.greenend.org.uk/~sgtatham/algorithms/listsort.html
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::sortLinked(Node* list) {
assert(list);
Node* p;
Node* q;
Node* e;
Node* tail;
int i, numMerges, pSize, qSize;
int inSize = 1;
for (;;) {
p = list;
list = nullptr;
tail = nullptr;
numMerges = 0;
while (p) {
numMerges++;
q = p;
pSize = 0;
for (i = 0; i < inSize; i++) {
pSize++;
q = q->nextZ;
if (!q) break;
}
qSize = inSize;
while (pSize > 0 || (qSize > 0 && q)) {
if (pSize == 0) {
e = q;
q = q->nextZ;
qSize--;
} else if (qSize == 0 || !q) {
e = p;
p = p->nextZ;
pSize--;
} else if (p->z <= q->z) {
e = p;
p = p->nextZ;
pSize--;
} else {
e = q;
q = q->nextZ;
qSize--;
}
if (tail) tail->nextZ = e;
else list = e;
e->prevZ = tail;
tail = e;
}
p = q;
}
tail->nextZ = nullptr;
if (numMerges <= 1) return list;
inSize *= 2;
}
}
// z-order of a Vertex given coords and size of the data bounding box
template <typename N>
int32_t Earcut<N>::zOrder(const double x_, const double y_) {
// coords are transformed into non-negative 15-bit integer range
int32_t x = static_cast<int32_t>(32767.0 * (x_ - minX) * inv_size);
int32_t y = static_cast<int32_t>(32767.0 * (y_ - minY) * inv_size);
x = (x | (x << 8)) & 0x00FF00FF;
x = (x | (x << 4)) & 0x0F0F0F0F;
x = (x | (x << 2)) & 0x33333333;
x = (x | (x << 1)) & 0x55555555;
y = (y | (y << 8)) & 0x00FF00FF;
y = (y | (y << 4)) & 0x0F0F0F0F;
y = (y | (y << 2)) & 0x33333333;
y = (y | (y << 1)) & 0x55555555;
return x | (y << 1);
}
// find the leftmost node of a polygon ring
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::getLeftmost(Node* start) {
Node* p = start;
Node* leftmost = start;
do {
if (p->x < leftmost->x || (p->x == leftmost->x && p->y < leftmost->y))
leftmost = p;
p = p->next;
} while (p != start);
return leftmost;
}
// check if a point lies within a convex triangle
template <typename N>
bool Earcut<N>::pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const {
return (cx - px) * (ay - py) - (ax - px) * (cy - py) >= 0 &&
(ax - px) * (by - py) - (bx - px) * (ay - py) >= 0 &&
(bx - px) * (cy - py) - (cx - px) * (by - py) >= 0;
}
// check if a diagonal between two polygon nodes is valid (lies in polygon interior)
template <typename N>
bool Earcut<N>::isValidDiagonal(Node* a, Node* b) {
return a->next->i != b->i && a->prev->i != b->i && !intersectsPolygon(a, b) &&
locallyInside(a, b) && locallyInside(b, a) && middleInside(a, b);
}
// signed area of a triangle
template <typename N>
double Earcut<N>::area(const Node* p, const Node* q, const Node* r) const {
return (q->y - p->y) * (r->x - q->x) - (q->x - p->x) * (r->y - q->y);
}
// check if two points are equal
template <typename N>
bool Earcut<N>::equals(const Node* p1, const Node* p2) {
return p1->x == p2->x && p1->y == p2->y;
}
// check if two segments intersect
template <typename N>
bool Earcut<N>::intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2) {
if ((equals(p1, q1) && equals(p2, q2)) ||
(equals(p1, q2) && equals(p2, q1))) return true;
return (area(p1, q1, p2) > 0) != (area(p1, q1, q2) > 0) &&
(area(p2, q2, p1) > 0) != (area(p2, q2, q1) > 0);
}
// check if a polygon diagonal intersects any polygon segments
template <typename N>
bool Earcut<N>::intersectsPolygon(const Node* a, const Node* b) {
const Node* p = a;
do {
if (p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i &&
intersects(p, p->next, a, b)) return true;
p = p->next;
} while (p != a);
return false;
}
// check if a polygon diagonal is locally inside the polygon
template <typename N>
bool Earcut<N>::locallyInside(const Node* a, const Node* b) {
return area(a->prev, a, a->next) < 0 ?
area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 :
area(a, b, a->prev) < 0 || area(a, a->next, b) < 0;
}
// check if the middle Vertex of a polygon diagonal is inside the polygon
template <typename N>
bool Earcut<N>::middleInside(const Node* a, const Node* b) {
const Node* p = a;
bool inside = false;
double px = (a->x + b->x) / 2;
double py = (a->y + b->y) / 2;
do {
if (((p->y > py) != (p->next->y > py)) && p->next->y != p->y &&
(px < (p->next->x - p->x) * (py - p->y) / (p->next->y - p->y) + p->x))
inside = !inside;
p = p->next;
} while (p != a);
return inside;
}
// link two polygon vertices with a bridge; if the vertices belong to the same ring, it splits
// polygon into two; if one belongs to the outer ring and another to a hole, it merges it into a
// single ring
template <typename N>
typename Earcut<N>::Node*
Earcut<N>::splitPolygon(Node* a, Node* b) {
Node* a2 = nodes.construct(a->i, a->x, a->y);
Node* b2 = nodes.construct(b->i, b->x, b->y);
Node* an = a->next;
Node* bp = b->prev;
a->next = b;
b->prev = a;
a2->next = an;
an->prev = a2;
b2->next = a2;
a2->prev = b2;
bp->next = b2;
b2->prev = bp;
return b2;
}
// create a node and util::optionally link it with previous one (in a circular doubly linked list)
template <typename N> template <typename Point>
typename Earcut<N>::Node*
Earcut<N>::insertNode(std::size_t i, const Point& pt, Node* last) {
Node* p = nodes.construct(static_cast<N>(i), util::nth<0, Point>::get(pt), util::nth<1, Point>::get(pt));
if (!last) {
p->prev = p;
p->next = p;
} else {
assert(last);
p->next = last->next;
p->prev = last;
last->next->prev = p;
last->next = p;
}
return p;
}
template <typename N>
void Earcut<N>::removeNode(Node* p) {
p->next->prev = p->prev;
p->prev->next = p->next;
if (p->prevZ) p->prevZ->nextZ = p->nextZ;
if (p->nextZ) p->nextZ->prevZ = p->prevZ;
}
}
template <typename N = uint32_t, typename Polygon>
std::vector<N> earcut(const Polygon& poly) {
mapbox::detail::Earcut<N> earcut;
earcut(poly);
return std::move(earcut.indices);
}
}

91
navigations/nav_2d_utils/include/nav_2d_utils/bounds.h Executable file
View File

@@ -0,0 +1,91 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_BOUNDS_H
#define NAV_2D_UTILS_BOUNDS_H
#include <nav_grid/nav_grid_info.h>
#include <nav_core2/bounds.h>
#include <vector>
/**
* @brief A set of utility functions for Bounds objects interacting with other messages/types
*/
namespace nav_2d_utils
{
/**
* @brief return a floating point bounds that covers the entire NavGrid
* @param info Info for the NavGrid
* @return bounds covering the entire NavGrid
*/
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info);
/**
* @brief return an integral bounds that covers the entire NavGrid
* @param info Info for the NavGrid
* @return bounds covering the entire NavGrid
*/
nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info);
/**
* @brief Translate real-valued bounds to uint coordinates based on nav_grid info
* @param info Information used when converting the coordinates
* @param bounds floating point bounds object
* @returns corresponding UIntBounds for parameter
*/
nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds);
/**
* @brief Translate uint bounds to real-valued coordinates based on nav_grid info
* @param info Information used when converting the coordinates
* @param bounds UIntBounds object
* @returns corresponding floating point bounds for parameter
*/
nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds);
/**
* @brief divide the given bounds up into sub-bounds of roughly equal size
* @param original_bounds The original bounds to divide
* @param n_cols Positive number of columns to divide the bounds into
* @param n_rows Positive number of rows to divide the bounds into
* @return vector of a maximum of n_cols*n_rows nonempty bounds
* @throws std::invalid_argument when n_cols or n_rows is zero
*/
std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds& original_bounds,
unsigned int n_cols, unsigned int n_rows);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_BOUNDS_H

107
navigations/nav_2d_utils/include/nav_2d_utils/conversions.h Executable file
View File

@@ -0,0 +1,107 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_CONVERSIONS_H
#define NAV_2D_UTILS_CONVERSIONS_H
#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Point2D.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <nav_2d_msgs/Polygon2DStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/NavGridInfo.h>
#include <nav_2d_msgs/UIntBounds.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/Path.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/bounds.h>
#include <tf/tf.h>
#include <vector>
#include <string>
namespace nav_2d_utils
{
// Twist Transformations
geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d);
nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel);
// Point Transformations
nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point);
nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point);
geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point);
geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point);
// Pose Transformations
nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose);
nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose);
geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d);
geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d);
geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
const std::string& frame, const ros::Time& stamp);
// Path Transformations
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path);
nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped>& poses);
nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped>& poses);
nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
const std::string& frame, const ros::Time& stamp);
nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d);
// Polygon Transformations
geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d);
nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d);
geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d);
nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d);
// Info Transformations
nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info);
nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg);
geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info);
geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info);
nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame);
nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info);
// Bounds Transformations
nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds);
nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_CONVERSIONS_H

54
navigations/nav_2d_utils/include/nav_2d_utils/footprint.h Executable file
View File

@@ -0,0 +1,54 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_FOOTPRINT_H
#define NAV_2D_UTILS_FOOTPRINT_H
#include <ros/ros.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_utils
{
/**
* @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius
*
* Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
* is present.
*/
nav_2d_msgs::Polygon2D footprintFromParams(ros::NodeHandle& nh, bool write = true);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_FOOTPRINT_H

85
navigations/nav_2d_utils/include/nav_2d_utils/geometry_help.h Executable file
View File

@@ -0,0 +1,85 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_GEOMETRY_HELP_H
#define NAV_2D_UTILS_GEOMETRY_HELP_H
#include <cmath>
namespace nav_2d_utils
{
/**
* @brief Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1)
* @param pX
* @param pY
* @param x0
* @param y0
* @param x1
* @param y1
* @return shortest distance from point to line
*/
inline 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 hypot(pX - xx, pY - yy);
}
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_GEOMETRY_HELP_H

87
navigations/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h Executable file
View File

@@ -0,0 +1,87 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_ODOM_SUBSCRIBER_H
#define NAV_2D_UTILS_ODOM_SUBSCRIBER_H
#include <ros/ros.h>
#include <nav_2d_utils/conversions.h>
#include <nav_msgs/Odometry.h>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <boost/thread/mutex.hpp>
#include <string>
namespace nav_2d_utils
{
/**
* @class OdomSubscriber
* Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
*/
class OdomSubscriber
{
public:
/**
* @brief Constructor that subscribes to an Odometry topic
*
* @param nh NodeHandle for creating subscriber
* @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
*/
explicit OdomSubscriber(ros::NodeHandle& nh, std::string default_topic = "odom")
{
std::string odom_topic;
nh.param("odom_topic", odom_topic, default_topic);
odom_sub_ = nh.subscribe<nav_msgs::Odometry>(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1));
}
inline nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; }
inline nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; }
protected:
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
ROS_INFO_ONCE("odom received!");
boost::mutex::scoped_lock lock(odom_mutex_);
odom_vel_.header = msg->header;
odom_vel_.velocity = twist3Dto2D(msg->twist.twist);
}
ros::Subscriber odom_sub_;
nav_2d_msgs::Twist2DStamped odom_vel_;
boost::mutex odom_mutex_;
};
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_ODOM_SUBSCRIBER_H

150
navigations/nav_2d_utils/include/nav_2d_utils/parameters.h Executable file
View File

@@ -0,0 +1,150 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_PARAMETERS_H
#define NAV_2D_UTILS_PARAMETERS_H
#include <ros/ros.h>
#include <string>
namespace nav_2d_utils
{
/**
* @brief Search for a parameter and load it, or use the default value
*
* This templated function shortens a commonly used ROS pattern in which you
* search for a parameter and get its value if it exists, otherwise returning a default value.
*
* @param nh NodeHandle to start the parameter search from
* @param param_name Name of the parameter to search for
* @param default_value Value to return if not found
* @return Value of parameter if found, otherwise the default_value
*/
template<class param_t>
param_t searchAndGetParam(const ros::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
{
std::string resolved_name;
if (nh.searchParam(param_name, resolved_name))
{
param_t value = default_value;
nh.param(resolved_name, value, default_value);
return value;
}
return default_value;
}
/**
* @brief Load a parameter from one of two namespaces. Complain if it uses the old name.
* @param nh NodeHandle to look for the parameter in
* @param current_name Parameter name that is current, i.e. not deprecated
* @param old_name Deprecated parameter name
* @param default_value If neither parameter is present, return this value
* @return The value of the parameter or the default value
*/
template<class param_t>
param_t loadParameterWithDeprecation(const ros::NodeHandle& nh, const std::string current_name,
const std::string old_name, const param_t& default_value)
{
param_t value;
if (nh.hasParam(current_name))
{
nh.getParam(current_name, value);
return value;
}
if (nh.hasParam(old_name))
{
ROS_WARN("Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
nh.getParam(old_name, value);
return value;
}
return default_value;
}
/**
* @brief If a deprecated parameter exists, complain and move to its new location
* @param nh NodeHandle to look for the parameter in
* @param current_name Parameter name that is current, i.e. not deprecated
* @param old_name Deprecated parameter name
*/
template<class param_t>
void moveDeprecatedParameter(const ros::NodeHandle& nh, const std::string current_name, const std::string old_name)
{
if (!nh.hasParam(old_name)) return;
param_t value;
ROS_WARN("Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
nh.getParam(old_name, value);
nh.setParam(current_name, value);
}
/**
* @brief Move a parameter from one place to another
*
* For use loading old parameter structures into new places.
*
* If the new name already has a value, don't move the old value there.
*
* @param nh NodeHandle for loading/saving params
* @param old_name Parameter name to move/remove
* @param current_name Destination parameter name
* @param default_value Value to save in the new name if old parameter is not found.
* @param should_delete If true, whether to delete the parameter from the old name
*/
template<class param_t>
void moveParameter(const ros::NodeHandle& nh, std::string old_name,
std::string current_name, param_t default_value, bool should_delete = true)
{
if (nh.hasParam(current_name))
{
if (should_delete)
nh.deleteParam(old_name);
return;
}
XmlRpc::XmlRpcValue value;
if (nh.hasParam(old_name))
{
nh.getParam(old_name, value);
if (should_delete) nh.deleteParam(old_name);
}
else
value = default_value;
nh.setParam(current_name, value);
}
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_PARAMETERS_H

88
navigations/nav_2d_utils/include/nav_2d_utils/path_ops.h Executable file
View File

@@ -0,0 +1,88 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_PATH_OPS_H
#define NAV_2D_UTILS_PATH_OPS_H
#include <nav_2d_msgs/Path2D.h>
namespace nav_2d_utils
{
/**
* @brief Calculate the linear distance between two poses
*/
double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1);
/**
* @brief Calculate the length of the plan, starting from the given index
*/
double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index = 0);
/**
* @brief Calculate the length of the plan from the pose on the plan closest to the given pose
*/
double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose);
/**
* @brief Increase plan resolution to match that of the costmap by adding points linearly between points
*
* @param global_plan_in input plan
* @param resolution desired distance between waypoints
* @return Higher resolution plan
*/
nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution);
/**
* @brief Decrease the length of the plan by eliminating colinear points
*
* Uses the Ramer Douglas Peucker algorithm. Ignores theta values
*
* @param input_path Path to compress
* @param epsilon maximum allowable error. Increase for greater compression.
* @return Path2D with possibly fewer poses
*/
nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon = 0.1);
/**
* @brief Convenience function to add a pose to a path in one line.
* @param path Path to add to
* @param x x-coordinate
* @param y y-coordinate
* @param theta theta (if needed)
*/
void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_PATH_OPS_H

274
navigations/nav_2d_utils/include/nav_2d_utils/plugin_mux.h Executable file
View File

@@ -0,0 +1,274 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_PLUGIN_MUX_H
#define NAV_2D_UTILS_PLUGIN_MUX_H
#include <ros/ros.h>
#include <pluginlib/class_loader.h>
#include <std_msgs/String.h>
#include <nav_2d_msgs/SwitchPlugin.h>
#include <map>
#include <string>
#include <vector>
namespace nav_2d_utils
{
/**
* @class PluginMux
* @brief An organizer for switching between multiple different plugins of the same type
*
* The different plugins are specified using a list of strings on the parameter server, each of which is a namespace.
* The specific type and additional parameters for each plugin are specified on the parameter server in that namespace.
* All the plugins are loaded initially, but only one is the "current" plugin at any particular time, which is published
* on a latched topic AND stored on the ROS parameter server. You can switch which plugin is current either through a
* C++ or ROS interface.
*/
template<class T>
class PluginMux
{
public:
/**
* @brief Main Constructor - Loads plugin(s) and sets up ROS interfaces
*
* @param plugin_package The package of the plugin type
* @param plugin_class The class name for the plugin type
* @param parameter_name Name of parameter for the namespaces.
* @param default_value If class name is not specified, which plugin should be loaded
* @param ros_name ROS name for setting up topic and parameter
* @param switch_service_name ROS name for setting up the ROS service
*/
PluginMux(const std::string& plugin_package, const std::string& plugin_class,
const std::string& parameter_name, const std::string& default_value,
const std::string& ros_name = "current_plugin", const std::string& switch_service_name = "switch_plugin");
/**
* @brief Create an instance of the given plugin_class_name and save it with the given plugin_name
* @param plugin_name Namespace for the new plugin
* @param plugin_class_name Class type name for the new plugin
*/
void addPlugin(const std::string& plugin_name, const std::string& plugin_class_name);
/**
* @brief C++ Interface for switching to a given plugin
*
* @param name Namespace to set current plugin to
* @return true if that namespace exists and is loaded properly
*/
bool usePlugin(const std::string& name)
{
// If plugin with given name doesn't exist, return false
if (plugins_.count(name) == 0) return false;
if (switch_callback_)
{
switch_callback_(current_plugin_, name);
}
// Switch Mux
current_plugin_ = name;
// Update ROS info
std_msgs::String str_msg;
str_msg.data = current_plugin_;
current_plugin_pub_.publish(str_msg);
private_nh_.setParam(ros_name_, current_plugin_);
return true;
}
/**
* @brief Get the Current Plugin Name
* @return Name of the current plugin
*/
std::string getCurrentPluginName() const
{
return current_plugin_;
}
/**
* @brief Check to see if a plugin exists
* @param name Namespace to set current plugin to
* @return True if the plugin exists
*/
bool hasPlugin(const std::string& name) const
{
return plugins_.find(name) != plugins_.end();
}
/**
* @brief Get the Specified Plugin
* @param name Name of plugin to get
* @return Reference to specified plugin
*/
T& getPlugin(const std::string& name)
{
if (!hasPlugin(name))
throw std::invalid_argument("Plugin not found");
return *plugins_[name];
}
/**
* @brief Get the Current Plugin
* @return Reference to current plugin
*/
T& getCurrentPlugin()
{
return getPlugin(current_plugin_);
}
/**
* @brief Get the current list of plugin names
*/
std::vector<std::string> getPluginNames() const
{
std::vector<std::string> names;
for (auto& kv : plugins_)
{
names.push_back(kv.first);
}
return names;
}
/**
* @brief alias for the function-type of the callback fired when the plugin switches.
*
* The first parameter will be the namespace of the plugin that was previously used.
* The second parameter will be the namespace of the plugin that is being switched to.
*/
using SwitchCallback = std::function<void(const std::string&, const std::string&)>;
/**
* @brief Set the callback fired when the plugin switches
*
* In addition to switching which plugin is being used via directly calling `usePlugin`
* a switch can also be triggered externally via ROS service. In such a case, other classes
* may want to know when this happens. This is accomplished using the SwitchCallback, which
* will be called regardless of how the plugin is switched.
*/
void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; }
protected:
/**
* @brief ROS Interface for Switching Plugins
*/
bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp)
{
std::string name = req.new_plugin;
if (usePlugin(name))
{
resp.success = true;
resp.message = "Loaded plugin namespace " + name + ".";
}
else
{
resp.success = false;
resp.message = "Namespace " + name + " not configured!";
}
return true;
}
// Plugin Management
pluginlib::ClassLoader<T> plugin_loader_;
std::map<std::string, boost::shared_ptr<T>> plugins_;
std::string current_plugin_;
// ROS Interface
ros::ServiceServer switch_plugin_srv_;
ros::Publisher current_plugin_pub_;
ros::NodeHandle private_nh_;
std::string ros_name_;
// Switch Callback
SwitchCallback switch_callback_;
};
// *********************************************************************************************************************
// ****************** Implementation of Templated Methods **************************************************************
// *********************************************************************************************************************
template<class T>
PluginMux<T>::PluginMux(const std::string& plugin_package, const std::string& plugin_class,
const std::string& parameter_name, const std::string& default_value,
const std::string& ros_name, const std::string& switch_service_name)
: plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr)
{
// Create the latched publisher
current_plugin_pub_ = private_nh_.advertise<std_msgs::String>(ros_name_, 1, true);
// Load Plugins
std::string plugin_class_name;
std::vector<std::string> plugin_namespaces;
private_nh_.getParam(parameter_name, plugin_namespaces);
if (plugin_namespaces.size() == 0)
{
// If no namespaces are listed, use the name of the default class as the singular namespace.
std::string plugin_name = plugin_loader_.getName(default_value);
plugin_namespaces.push_back(plugin_name);
}
for (const std::string& the_namespace : plugin_namespaces)
{
// Load the class name from namespace/plugin_class, or use default value
private_nh_.param(std::string(the_namespace + "/plugin_class"), plugin_class_name, default_value);
addPlugin(the_namespace, plugin_class_name);
}
// By default, use the first one as current
usePlugin(plugin_namespaces[0]);
// Now that the plugins are created, advertise the service if there are multiple
if (plugin_namespaces.size() > 1)
{
switch_plugin_srv_ = private_nh_.advertiseService(switch_service_name, &PluginMux::switchPluginService, this);
}
}
template<class T>
void PluginMux<T>::addPlugin(const std::string& plugin_name, const std::string& plugin_class_name)
{
try
{
plugins_[plugin_name] = plugin_loader_.createInstance(plugin_class_name);
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL_NAMED("PluginMux",
"Failed to load the plugin: %s. Exception: %s", plugin_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
}
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_PLUGIN_MUX_H

199
navigations/nav_2d_utils/include/nav_2d_utils/polygons.h Executable file
View File

@@ -0,0 +1,199 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_POLYGONS_H
#define NAV_2D_UTILS_POLYGONS_H
#include <ros/ros.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <nav_2d_msgs/ComplexPolygon2D.h>
#include <geometry_msgs/Pose2D.h>
#include <vector>
#include <string>
namespace nav_2d_utils
{
/**
* @class PolygonParseException
* @brief Exception to throw when Polygon doesn't load correctly
*/
class PolygonParseException: public std::runtime_error
{
public:
explicit PolygonParseException(const std::string& description) : std::runtime_error(description) {}
};
/**
* @brief Parse a vector of vectors of doubles from a string.
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
*
* @param input The string to parse
* @return a vector of vectors of doubles
*/
std::vector<std::vector<double> > parseVVD(const std::string& input);
/**
* @brief Create a "circular" polygon from a given radius
*
* @param radius Radius of the polygon
* @param num_points Number of points in the resulting polygon
* @return A rotationally symmetric polygon with the specified number of vertices
*/
nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16);
/**
* @brief Make a polygon from the given string.
* Format should be bracketed array of arrays of doubles, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
* @param polygon_string The string to parse
* @return Polygon2D
*/
nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
/**
* @brief Load a polygon from a parameter, whether it is a string or array, or two arrays
* @param nh Node handle to load parameter from
* @param parameter_name Name of the parameter
* @param search Whether to search up the namespace for the parameter name
* @return Loaded polygon
*/
nav_2d_msgs::Polygon2D polygonFromParams(const ros::NodeHandle& nh, const std::string parameter_name,
bool search = true);
/**
* @brief Create a polygon from the given XmlRpcValue.
*
* @param polygon_xmlrpc should be an array of arrays, where the top-level array should have
* 3 or more elements, and the sub-arrays should all have exactly 2 elements
* (x and y coordinates).
*/
nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
/**
* @brief Create XMLRPC Value for writing the polygon to the parameter server
* @param polygon Polygon to convert
* @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
* @return XmlRpcValue
*/
XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
/**
* @brief Save a polygon to a parameter
* @param polygon The Polygon
* @param nh Node handle to save the parameter to
* @param parameter_name Name of the parameter
* @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
*/
void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const ros::NodeHandle& nh, const std::string parameter_name,
bool array_of_arrays = true);
/**
* @brief Create a polygon from two parallel arrays
*
* @param xs Array of doubles representing x coordinates, at least three elements long
* @param ys Array of doubles representing y coordinates, matching length of xs
*/
nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
/**
* @brief Create two parallel arrays from a polygon
*
* @param[in] polygon
* @param[out] xs Array of doubles representing x coordinates, to be populated
* @param[out] ys Array of doubles representing y coordinates, to be populated
*/
void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys);
/**
* @brief check if two polygons are equal. Used in testing
*/
bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1);
/**
* @brief Translate and rotate a polygon to a new pose
* @param polygon The polygon
* @param pose The x, y and theta to use when moving the polygon
* @return A new moved polygon
*/
nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
const geometry_msgs::Pose2D& pose);
/**
* @brief Check if a given point is inside of a polygon
*
* Borders are considered outside.
*
* @param polygon Polygon to check
* @param x x coordinate
* @param y y coordinate
* @return true if point is inside polygon
*/
bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y);
/**
* @brief Calculate the minimum and maximum distance from (0, 0) to any point on the polygon
* @param[in] polygon polygon to analyze
* @param[out] min_dist
* @param[out] max_dist
*/
void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist);
/**
* @brief Decompose a complex polygon into a set of triangles.
*
* See https://en.wikipedia.org/wiki/Polygon_triangulation
*
* Implementation from https://github.com/mapbox/earcut.hpp
*
* @param polygon The complex polygon to deconstruct
* @return A vector of points where each set of three points represents a triangle
*/
std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon);
/**
* @brief Decompose a simple polygon into a set of triangles.
*
* See https://en.wikipedia.org/wiki/Polygon_triangulation
*
* Implementation from https://github.com/mapbox/earcut.hpp
*
* @param polygon The polygon to deconstruct
* @return A vector of points where each set of three points represents a triangle
*/
std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::Polygon2D& polygon);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_POLYGONS_H

91
navigations/nav_2d_utils/include/nav_2d_utils/tf_help.h Executable file
View File

@@ -0,0 +1,91 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#ifndef NAV_2D_UTILS_TF_HELP_H
#define NAV_2D_UTILS_TF_HELP_H
#include <nav_core2/common.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <string>
namespace nav_2d_utils
{
/**
* @brief Transform a PoseStamped from one frame to another while catching exceptions
*
* Also returns immediately if the frames are equal.
* @param tf Smart pointer to TFListener
* @param frame Frame to transform the pose into
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
* @return True if successful transform
*/
bool transformPose(const TFListenerPtr tf, const std::string frame,
const geometry_msgs::PoseStamped& in_pose, geometry_msgs::PoseStamped& out_pose,
const bool extrapolation_fallback = true);
/**
* @brief Transform a Pose2DStamped from one frame to another while catching exceptions
*
* Also returns immediately if the frames are equal.
* @param tf Smart pointer to TFListener
* @param frame Frame to transform the pose into
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
* @return True if successful transform
*/
bool transformPose(const TFListenerPtr tf, const std::string frame,
const nav_2d_msgs::Pose2DStamped& in_pose, nav_2d_msgs::Pose2DStamped& out_pose,
const bool extrapolation_fallback = true);
/**
* @brief Transform a Pose2DStamped into another frame
*
* Note that this returns a transformed pose
* regardless of whether the transform was successfully performed.
*
* @param tf Smart pointer to TFListener
* @param pose Pose to transform
* @param frame_id Frame to transform the pose into
* @return The resulting transformed pose
*/
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped& pose,
const std::string& frame_id);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS_TF_HELP_H