changed name robot_nav_2d_utils robot_nav_2d_msgs
This commit is contained in:
@@ -1,74 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/ComplexPolygon2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
#define NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <nav_2d_msgs/Polygon2D.h>
|
||||
#include <nav_2d_msgs/Polygon2D.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct ComplexPolygon2D_
|
||||
{
|
||||
typedef ComplexPolygon2D_<ContainerAllocator> Type;
|
||||
|
||||
ComplexPolygon2D_()
|
||||
: outer()
|
||||
, inner() {
|
||||
}
|
||||
ComplexPolygon2D_(const ContainerAllocator& _alloc)
|
||||
: outer(_alloc)
|
||||
, inner(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::nav_2d_msgs::Polygon2D_<ContainerAllocator> _outer_type;
|
||||
_outer_type outer;
|
||||
|
||||
typedef std::vector< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> >> _inner_type;
|
||||
_inner_type inner;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct ComplexPolygon2D_
|
||||
|
||||
typedef ::nav_2d_msgs::ComplexPolygon2D_<std::allocator<void> > ComplexPolygon2D;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.outer == rhs.outer &&
|
||||
lhs.inner == rhs.inner;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
@@ -1,80 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/NavGridOfChars.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <nav_2d_msgs/NavGridInfo.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfChars_
|
||||
{
|
||||
typedef NavGridOfChars_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfChars_()
|
||||
: stamp()
|
||||
, info()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfChars_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, info(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
|
||||
_info_type info;
|
||||
|
||||
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfChars_
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridOfChars_<std::allocator<void> > NavGridOfChars;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.info == rhs.info &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
@@ -1,80 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/NavGridOfCharsUpdate.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <nav_2d_msgs/UIntBounds.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfCharsUpdate_
|
||||
{
|
||||
typedef NavGridOfCharsUpdate_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfCharsUpdate_()
|
||||
: stamp()
|
||||
, bounds()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfCharsUpdate_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, bounds(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
|
||||
_bounds_type bounds;
|
||||
|
||||
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfCharsUpdate_
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridOfCharsUpdate_<std::allocator<void> > NavGridOfCharsUpdate;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.bounds == rhs.bounds &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
@@ -1,80 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/NavGridOfDoubles.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <nav_2d_msgs/NavGridInfo.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfDoubles_
|
||||
{
|
||||
typedef NavGridOfDoubles_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfDoubles_()
|
||||
: stamp()
|
||||
, info()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfDoubles_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, info(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
|
||||
_info_type info;
|
||||
|
||||
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfDoubles_
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridOfDoubles_<std::allocator<void> > NavGridOfDoubles;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.info == rhs.info &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
@@ -1,80 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/NavGridOfDoublesUpdate.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <nav_2d_msgs/UIntBounds.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfDoublesUpdate_
|
||||
{
|
||||
typedef NavGridOfDoublesUpdate_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfDoublesUpdate_()
|
||||
: stamp()
|
||||
, bounds()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, bounds(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
|
||||
_bounds_type bounds;
|
||||
|
||||
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfDoublesUpdate_
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridOfDoublesUpdate_<std::allocator<void> > NavGridOfDoublesUpdate;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.bounds == rhs.bounds &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
@@ -1,74 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Path2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
#define NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <nav_2d_msgs/Pose2DStamped.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Path2D_
|
||||
{
|
||||
typedef Path2D_<ContainerAllocator> Type;
|
||||
|
||||
Path2D_()
|
||||
: header()
|
||||
, poses() {
|
||||
}
|
||||
Path2D_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, poses(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef std::vector< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;
|
||||
_poses_type poses;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Path2D_
|
||||
|
||||
typedef ::nav_2d_msgs::Path2D_<std::allocator<void> > Path2D;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Path2D > Path2DPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Path2D const> Path2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.poses == rhs.poses;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
@@ -1,72 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Point2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Point2D_
|
||||
{
|
||||
typedef Point2D_<ContainerAllocator> Type;
|
||||
|
||||
Point2D_()
|
||||
: x(0.0)
|
||||
, y(0.0) {
|
||||
}
|
||||
Point2D_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef double _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef double _y_type;
|
||||
_y_type y;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Point2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Point2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Point2D_
|
||||
|
||||
typedef ::nav_2d_msgs::Point2D_<std::allocator<void> > Point2D;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Point2D > Point2DPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Point2D const> Point2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
@@ -1,67 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Polygon2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <nav_2d_msgs/Point2D.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2D_
|
||||
{
|
||||
typedef Polygon2D_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2D_()
|
||||
: points() {
|
||||
}
|
||||
Polygon2D_(const ContainerAllocator& _alloc)
|
||||
: points(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef std::vector< ::nav_2d_msgs::Point2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Point2D_<ContainerAllocator> >> _points_type;
|
||||
_points_type points;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2D_
|
||||
|
||||
typedef ::nav_2d_msgs::Polygon2D_<std::allocator<void> > Polygon2D;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D > Polygon2DPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D const> Polygon2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.points == rhs.points;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
@@ -1,81 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Polygon2DCollection.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <nav_2d_msgs/ComplexPolygon2D.h>
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2DCollection_
|
||||
{
|
||||
typedef Polygon2DCollection_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2DCollection_()
|
||||
: header()
|
||||
, polygons()
|
||||
, colors() {
|
||||
}
|
||||
Polygon2DCollection_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, polygons(_alloc)
|
||||
, colors(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef std::vector< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
|
||||
_polygons_type polygons;
|
||||
|
||||
typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
|
||||
_colors_type colors;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2DCollection_
|
||||
|
||||
typedef ::nav_2d_msgs::Polygon2DCollection_<std::allocator<void> > Polygon2DCollection;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.polygons == rhs.polygons &&
|
||||
lhs.colors == rhs.colors;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
@@ -1,74 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Polygon2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <nav_2d_msgs/Polygon2D.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2DStamped_
|
||||
{
|
||||
typedef Polygon2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2DStamped_()
|
||||
: header()
|
||||
, polygon() {
|
||||
}
|
||||
Polygon2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, polygon(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::nav_2d_msgs::Polygon2D_<ContainerAllocator> _polygon_type;
|
||||
_polygon_type polygon;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2DStamped_
|
||||
|
||||
typedef ::nav_2d_msgs::Polygon2DStamped_<std::allocator<void> > Polygon2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.polygon == rhs.polygon;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
@@ -1,78 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Pose2D32.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Pose2D32_
|
||||
{
|
||||
typedef Pose2D32_<ContainerAllocator> Type;
|
||||
|
||||
Pose2D32_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Pose2D32_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef float _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef float _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef float _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Pose2D32_
|
||||
|
||||
typedef ::nav_2d_msgs::Pose2D32_<std::allocator<void> > Pose2D32;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 > Pose2D32Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
@@ -1,74 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Pose2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
#define NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Pose2DStamped_
|
||||
{
|
||||
typedef Pose2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Pose2DStamped_()
|
||||
: header()
|
||||
, pose() {
|
||||
}
|
||||
Pose2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, pose() {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::geometry_msgs::Pose2D _pose_type;
|
||||
_pose_type pose;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Pose2DStamped_
|
||||
|
||||
typedef ::nav_2d_msgs::Pose2DStamped_<std::allocator<void> > Pose2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped > Pose2DStampedPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped const> Pose2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.pose == rhs.pose;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
@@ -1,30 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/SwitchPlugin.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
|
||||
|
||||
#include <nav_2d_msgs/SwitchPluginRequest.h>
|
||||
#include <nav_2d_msgs/SwitchPluginResponse.h>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
|
||||
struct SwitchPlugin
|
||||
{
|
||||
|
||||
typedef SwitchPluginRequest Request;
|
||||
typedef SwitchPluginResponse Response;
|
||||
Request request;
|
||||
Response response;
|
||||
|
||||
typedef Request RequestType;
|
||||
typedef Response ResponseType;
|
||||
|
||||
}; // struct SwitchPlugin
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
@@ -1,66 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/SwitchPluginRequest.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct SwitchPluginRequest_
|
||||
{
|
||||
typedef SwitchPluginRequest_<ContainerAllocator> Type;
|
||||
|
||||
SwitchPluginRequest_()
|
||||
: new_plugin() {
|
||||
}
|
||||
SwitchPluginRequest_(const ContainerAllocator& _alloc)
|
||||
: new_plugin(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _new_plugin_type;
|
||||
_new_plugin_type new_plugin;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct SwitchPluginRequest_
|
||||
|
||||
typedef ::nav_2d_msgs::SwitchPluginRequest_<std::allocator<void> > SwitchPluginRequest;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest > SwitchPluginRequestPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest const> SwitchPluginRequestConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.new_plugin == rhs.new_plugin;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
@@ -1,72 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/SwitchPluginResponse.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct SwitchPluginResponse_
|
||||
{
|
||||
typedef SwitchPluginResponse_<ContainerAllocator> Type;
|
||||
|
||||
SwitchPluginResponse_()
|
||||
: success(false)
|
||||
, message() {
|
||||
}
|
||||
SwitchPluginResponse_(const ContainerAllocator& _alloc)
|
||||
: success(false)
|
||||
, message(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _success_type;
|
||||
_success_type success;
|
||||
|
||||
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _message_type;
|
||||
_message_type message;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct SwitchPluginResponse_
|
||||
|
||||
typedef ::nav_2d_msgs::SwitchPluginResponse_<std::allocator<void> > SwitchPluginResponse;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse > SwitchPluginResponsePtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse const> SwitchPluginResponseConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.success == rhs.success &&
|
||||
lhs.message == rhs.message;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
@@ -1,78 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Twist2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
#define NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2D_
|
||||
{
|
||||
typedef Twist2D_<ContainerAllocator> Type;
|
||||
|
||||
Twist2D_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Twist2D_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef double _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef double _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef double _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2D_
|
||||
|
||||
typedef ::nav_2d_msgs::Twist2D_<std::allocator<void> > Twist2D;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D > Twist2DPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D const> Twist2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
@@ -1,78 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Twist2D32.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
#define NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2D32_
|
||||
{
|
||||
typedef Twist2D32_<ContainerAllocator> Type;
|
||||
|
||||
Twist2D32_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Twist2D32_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef float _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef float _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef float _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2D32_
|
||||
|
||||
typedef ::nav_2d_msgs::Twist2D32_<std::allocator<void> > Twist2D32;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 > Twist2D32Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 const> Twist2D32ConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
@@ -1,74 +0,0 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/Twist2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
#define NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2DStamped_
|
||||
{
|
||||
typedef Twist2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Twist2DStamped_()
|
||||
: header()
|
||||
, velocity() {
|
||||
}
|
||||
Twist2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, velocity(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
|
||||
_velocity_type velocity;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2DStamped_
|
||||
|
||||
typedef ::nav_2d_msgs::Twist2DStamped_<std::allocator<void> > Twist2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped > Twist2DStampedPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped const> Twist2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.velocity == rhs.velocity;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
@@ -1,91 +0,0 @@
|
||||
/*
|
||||
* 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
|
||||
@@ -1,106 +0,0 @@
|
||||
/*
|
||||
* 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 <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 robot::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 robot::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
|
||||
@@ -1,153 +0,0 @@
|
||||
/*
|
||||
* 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 <robot/node_handle.h>
|
||||
#include <robot/console.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
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 robot::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;
|
||||
robot::NodeHandle nh_private = robot::NodeHandle("~");
|
||||
nh_private.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 robot::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, default_value);
|
||||
return value;
|
||||
}
|
||||
if (nh.hasParam(old_name))
|
||||
{
|
||||
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
|
||||
nh.getParam(old_name, value, default_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 robot::NodeHandle& nh, const std::string current_name, const std::string old_name)
|
||||
{
|
||||
if (!nh.hasParam(old_name)) return;
|
||||
|
||||
param_t value;
|
||||
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
|
||||
value = nh.param<param_t>(old_name);
|
||||
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 robot::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
|
||||
@@ -1,88 +0,0 @@
|
||||
/*
|
||||
* 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
|
||||
@@ -1,274 +0,0 @@
|
||||
/*
|
||||
* 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
|
||||
robot::ServiceServer switch_plugin_srv_;
|
||||
robot::Publisher current_plugin_pub_;
|
||||
robot::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
|
||||
@@ -1,91 +0,0 @@
|
||||
/*
|
||||
* 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
|
||||
@@ -1,102 +0,0 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <nav_2d_utils/bounds.h>
|
||||
#include <nav_grid/coordinate_conversion.h>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info)
|
||||
{
|
||||
return nav_core2::Bounds(info.origin_x, info.origin_y,
|
||||
info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height);
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info)
|
||||
{
|
||||
// bounds are inclusive, so we subtract one
|
||||
return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds)
|
||||
{
|
||||
unsigned int g_min_x, g_min_y, g_max_x, g_max_y;
|
||||
worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y);
|
||||
worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y);
|
||||
return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
|
||||
}
|
||||
|
||||
nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds)
|
||||
{
|
||||
double min_x, min_y, max_x, max_y;
|
||||
gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y);
|
||||
gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y);
|
||||
return nav_core2::Bounds(min_x, min_y, max_x, max_y);
|
||||
}
|
||||
|
||||
std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds& original_bounds,
|
||||
unsigned int n_cols, unsigned int n_rows)
|
||||
{
|
||||
if (n_cols * n_rows == 0)
|
||||
{
|
||||
throw std::invalid_argument("Number of rows and columns must be positive (not zero)");
|
||||
}
|
||||
unsigned int full_width = original_bounds.getWidth(),
|
||||
full_height = original_bounds.getHeight();
|
||||
|
||||
unsigned int small_width = static_cast<unsigned int>(ceil(static_cast<double>(full_width) / n_cols)),
|
||||
small_height = static_cast<unsigned int>(ceil(static_cast<double>(full_height) / n_rows));
|
||||
|
||||
std::vector<nav_core2::UIntBounds> divided;
|
||||
|
||||
for (unsigned int row = 0; row < n_rows; row++)
|
||||
{
|
||||
unsigned int min_y = original_bounds.getMinY() + small_height * row;
|
||||
unsigned int max_y = std::min(min_y + small_height - 1, original_bounds.getMaxY());
|
||||
|
||||
for (unsigned int col = 0; col < n_cols; col++)
|
||||
{
|
||||
unsigned int min_x = original_bounds.getMinX() + small_width * col;
|
||||
unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX());
|
||||
nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
|
||||
if (!sub.isEmpty())
|
||||
divided.push_back(sub);
|
||||
}
|
||||
}
|
||||
return divided;
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
@@ -1,339 +0,0 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
|
||||
geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d)
|
||||
{
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = cmd_vel_2d.x;
|
||||
cmd_vel.linear.y = cmd_vel_2d.y;
|
||||
cmd_vel.angular.z = cmd_vel_2d.theta;
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
|
||||
nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel)
|
||||
{
|
||||
nav_2d_msgs::Twist2D cmd_vel_2d;
|
||||
cmd_vel_2d.x = cmd_vel.linear.x;
|
||||
cmd_vel_2d.y = cmd_vel.linear.y;
|
||||
cmd_vel_2d.theta = cmd_vel.angular.z;
|
||||
return cmd_vel_2d;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point)
|
||||
{
|
||||
nav_2d_msgs::Point2D output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point)
|
||||
{
|
||||
nav_2d_msgs::Point2D output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point)
|
||||
{
|
||||
geometry_msgs::Point output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point)
|
||||
{
|
||||
geometry_msgs::Point32 output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
// nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose)
|
||||
// {
|
||||
// nav_2d_msgs::Pose2DStamped pose2d;
|
||||
// pose2d.header.stamp = pose.stamp_;
|
||||
// pose2d.header.frame_id = pose.frame_id_;
|
||||
// pose2d.pose.x = pose.getOrigin().getX();
|
||||
// pose2d.pose.y = pose.getOrigin().getY();
|
||||
// pose2d.pose.theta = tf::getYaw(pose.getRotation());
|
||||
// return pose2d;
|
||||
// }
|
||||
|
||||
nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose)
|
||||
{
|
||||
nav_2d_msgs::Pose2DStamped pose2d;
|
||||
pose2d.header = pose.header;
|
||||
pose2d.pose.x = pose.pose.position.x;
|
||||
pose2d.pose.y = pose.pose.position.y;
|
||||
// pose2d.pose.theta = tf::getYaw(pose.pose.orientation);
|
||||
return pose2d;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d)
|
||||
{
|
||||
geometry_msgs::Pose pose;
|
||||
pose.position.x = pose2d.x;
|
||||
pose.position.y = pose2d.y;
|
||||
// pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
|
||||
return pose;
|
||||
}
|
||||
|
||||
geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d)
|
||||
{
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.header = pose2d.header;
|
||||
pose.pose = pose2DToPose(pose2d.pose);
|
||||
return pose;
|
||||
}
|
||||
|
||||
// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
|
||||
// const std::string& frame, const robot::Time& stamp)
|
||||
// {
|
||||
// geometry_msgs::PoseStamped pose;
|
||||
// pose.header.frame_id = frame;
|
||||
// pose.header.stamp = stamp;
|
||||
// pose.pose.position.x = pose2d.x;
|
||||
// pose.pose.position.y = pose2d.y;
|
||||
// // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
|
||||
// return pose;
|
||||
// }
|
||||
|
||||
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path)
|
||||
{
|
||||
nav_2d_msgs::Path2D path2d;
|
||||
path2d.header = path.header;
|
||||
nav_2d_msgs::Pose2DStamped stamped_2d;
|
||||
path2d.poses.resize(path.poses.size());
|
||||
for (unsigned int i = 0; i < path.poses.size(); i++)
|
||||
{
|
||||
stamped_2d = poseStampedToPose2D(path.poses[i]);
|
||||
path2d.poses[i] = stamped_2d;
|
||||
}
|
||||
return path2d;
|
||||
}
|
||||
|
||||
nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped>& poses)
|
||||
{
|
||||
nav_msgs::Path path;
|
||||
if (poses.empty())
|
||||
return path;
|
||||
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = poses[0].header.frame_id;
|
||||
path.header.stamp = poses[0].header.stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++)
|
||||
{
|
||||
path.poses[i] = poses[i];
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped>& poses)
|
||||
{
|
||||
nav_2d_msgs::Path2D path;
|
||||
if (poses.empty())
|
||||
return path;
|
||||
|
||||
nav_2d_msgs::Pose2DStamped pose;
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = poses[0].header.frame_id;
|
||||
path.header.stamp = poses[0].header.stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++)
|
||||
{
|
||||
pose = poseStampedToPose2D(poses[i]);
|
||||
path.poses[i] = pose;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
|
||||
// nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
|
||||
// const std::string& frame, const robot::Time& stamp)
|
||||
// {
|
||||
// nav_msgs::Path path;
|
||||
// path.poses.resize(poses.size());
|
||||
// path.header.frame_id = frame;
|
||||
// path.header.stamp = stamp;
|
||||
// for (unsigned int i = 0; i < poses.size(); i++)
|
||||
// {
|
||||
// path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
|
||||
// }
|
||||
// return path;
|
||||
// }
|
||||
|
||||
nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d)
|
||||
{
|
||||
nav_msgs::Path path;
|
||||
path.header = path2d.header;
|
||||
path.poses.resize(path2d.poses.size());
|
||||
for (unsigned int i = 0; i < path.poses.size(); i++)
|
||||
{
|
||||
path.poses[i].header = path2d.header;
|
||||
path.poses[i].pose = pose2DToPose(path2d.poses[i].pose);
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d)
|
||||
{
|
||||
geometry_msgs::Polygon polygon;
|
||||
polygon.points.reserve(polygon_2d.points.size());
|
||||
for (const auto& pt : polygon_2d.points)
|
||||
{
|
||||
polygon.points.push_back(pointToPoint32(pt));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d)
|
||||
{
|
||||
nav_2d_msgs::Polygon2D polygon;
|
||||
polygon.points.reserve(polygon_3d.points.size());
|
||||
for (const auto& pt : polygon_3d.points)
|
||||
{
|
||||
polygon.points.push_back(pointToPoint2D(pt));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d)
|
||||
{
|
||||
geometry_msgs::PolygonStamped polygon;
|
||||
polygon.header = polygon_2d.header;
|
||||
polygon.polygon = polygon2Dto3D(polygon_2d.polygon);
|
||||
return polygon;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d)
|
||||
{
|
||||
nav_2d_msgs::Polygon2DStamped polygon;
|
||||
polygon.header = polygon_3d.header;
|
||||
polygon.polygon = polygon3Dto2D(polygon_3d.polygon);
|
||||
return polygon;
|
||||
}
|
||||
|
||||
nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info)
|
||||
{
|
||||
nav_2d_msgs::NavGridInfo msg;
|
||||
msg.width = info.width;
|
||||
msg.height = info.height;
|
||||
msg.resolution = info.resolution;
|
||||
msg.frame_id = info.frame_id;
|
||||
msg.origin_x = info.origin_x;
|
||||
msg.origin_y = info.origin_y;
|
||||
return msg;
|
||||
}
|
||||
|
||||
nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg)
|
||||
{
|
||||
nav_grid::NavGridInfo info;
|
||||
info.width = msg.width;
|
||||
info.height = msg.height;
|
||||
info.resolution = msg.resolution;
|
||||
info.frame_id = msg.frame_id;
|
||||
info.origin_x = msg.origin_x;
|
||||
info.origin_y = msg.origin_y;
|
||||
return info;
|
||||
}
|
||||
|
||||
nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame)
|
||||
{
|
||||
nav_grid::NavGridInfo info;
|
||||
info.frame_id = frame;
|
||||
info.resolution = metadata.resolution;
|
||||
info.width = metadata.width;
|
||||
info.height = metadata.height;
|
||||
info.origin_x = metadata.origin.position.x;
|
||||
info.origin_y = metadata.origin.position.y;
|
||||
// if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5)
|
||||
// {
|
||||
// std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl;
|
||||
// }
|
||||
return info;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info)
|
||||
{
|
||||
geometry_msgs::Pose origin;
|
||||
origin.position.x = info.origin_x;
|
||||
origin.position.y = info.origin_y;
|
||||
origin.orientation.w = 1.0;
|
||||
return origin;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info)
|
||||
{
|
||||
geometry_msgs::Pose2D origin;
|
||||
origin.x = info.origin_x;
|
||||
origin.y = info.origin_y;
|
||||
return origin;
|
||||
}
|
||||
|
||||
nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info)
|
||||
{
|
||||
nav_msgs::MapMetaData metadata;
|
||||
metadata.resolution = info.resolution;
|
||||
metadata.width = info.width;
|
||||
metadata.height = info.height;
|
||||
metadata.origin = getOrigin3D(info);
|
||||
return metadata;
|
||||
}
|
||||
|
||||
nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds)
|
||||
{
|
||||
nav_2d_msgs::UIntBounds msg;
|
||||
msg.min_x = bounds.getMinX();
|
||||
msg.min_y = bounds.getMinY();
|
||||
msg.max_x = bounds.getMaxX();
|
||||
msg.max_y = bounds.getMaxY();
|
||||
return msg;
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg)
|
||||
{
|
||||
return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
@@ -1,193 +0,0 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <nav_2d_utils/path_ops.h>
|
||||
#include <nav_2d_utils/geometry_help.h>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
|
||||
double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1)
|
||||
{
|
||||
return hypot(pose0.x - pose1.x, pose0.y - pose1.y);
|
||||
}
|
||||
|
||||
double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index)
|
||||
{
|
||||
double length = 0.0;
|
||||
for (unsigned int i = start_index + 1; i < plan.poses.size(); i++)
|
||||
{
|
||||
length += poseDistance(plan.poses[i - 1].pose, plan.poses[i].pose);
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose)
|
||||
{
|
||||
if (plan.poses.size() == 0) return 0.0;
|
||||
|
||||
unsigned int closest_index = 0;
|
||||
double closest_distance = poseDistance(plan.poses[0].pose, query_pose);
|
||||
for (unsigned int i = 1; i < plan.poses.size(); i++)
|
||||
{
|
||||
double distance = poseDistance(plan.poses[i].pose, query_pose);
|
||||
if (closest_distance > distance)
|
||||
{
|
||||
closest_index = i;
|
||||
closest_distance = distance;
|
||||
}
|
||||
}
|
||||
return getPlanLength(plan, closest_index);
|
||||
}
|
||||
|
||||
nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution)
|
||||
{
|
||||
nav_2d_msgs::Path2D global_plan_out;
|
||||
global_plan_out.header = global_plan_in.header;
|
||||
if (global_plan_in.poses.size() == 0)
|
||||
{
|
||||
return global_plan_out;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Pose2DStamped last_stp = global_plan_in.poses[0];
|
||||
global_plan_out.poses.push_back(last_stp);
|
||||
|
||||
double sq_resolution = resolution * resolution;
|
||||
geometry_msgs::Pose2D last = last_stp.pose;
|
||||
for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i)
|
||||
{
|
||||
geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose;
|
||||
double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
|
||||
if (sq_dist > sq_resolution)
|
||||
{
|
||||
// add points in-between
|
||||
double diff = sqrt(sq_dist) - resolution;
|
||||
double steps_double = ceil(diff / resolution) + 1.0;
|
||||
int steps = static_cast<int>(steps_double);
|
||||
|
||||
double delta_x = (loop.x - last.x) / steps_double;
|
||||
double delta_y = (loop.y - last.y) / steps_double;
|
||||
double delta_t = (loop.theta - last.theta) / steps_double;
|
||||
|
||||
for (int j = 1; j < steps; ++j)
|
||||
{
|
||||
nav_2d_msgs::Pose2DStamped pose;
|
||||
pose.header = last_stp.header;
|
||||
pose.pose.x = last.x + j * delta_x;
|
||||
pose.pose.y = last.y + j * delta_y;
|
||||
pose.pose.theta = last.theta + j * delta_t;
|
||||
global_plan_out.poses.push_back(pose);
|
||||
}
|
||||
}
|
||||
global_plan_out.poses.push_back(global_plan_in.poses[i]);
|
||||
last.x = loop.x;
|
||||
last.y = loop.y;
|
||||
}
|
||||
return global_plan_out;
|
||||
}
|
||||
|
||||
using PoseList = std::vector<nav_2d_msgs::Pose2DStamped>;
|
||||
|
||||
/**
|
||||
* @brief Helper function for other version of compressPlan.
|
||||
*
|
||||
* Uses the Ramer Douglas Peucker algorithm. Ignores theta values
|
||||
*
|
||||
* @param input Full list of poses
|
||||
* @param start_index Index of first pose (inclusive)
|
||||
* @param end_index Index of last pose (inclusive)
|
||||
* @param epsilon maximum allowable error. Increase for greater compression.
|
||||
* @param list of poses possibly compressed for the poses[start_index, end_index]
|
||||
*/
|
||||
PoseList compressPlan(const PoseList& input, unsigned int start_index, unsigned int end_index, double epsilon)
|
||||
{
|
||||
// Skip if only two points
|
||||
if (end_index - start_index <= 1)
|
||||
{
|
||||
PoseList::const_iterator first = input.begin() + start_index;
|
||||
PoseList::const_iterator last = input.begin() + end_index + 1;
|
||||
return PoseList(first, last);
|
||||
}
|
||||
|
||||
// Find the point with the maximum distance to the line from start to end
|
||||
const nav_2d_msgs::Pose2DStamped& start = input[start_index],
|
||||
end = input[end_index];
|
||||
double max_distance = 0.0;
|
||||
unsigned int max_index = 0;
|
||||
for (unsigned int i = start_index + 1; i < end_index; i++)
|
||||
{
|
||||
const nav_2d_msgs::Pose2DStamped& pose = input[i];
|
||||
double d = distanceToLine(pose.pose.x, pose.pose.y, start.pose.x, start.pose.y, end.pose.x, end.pose.y);
|
||||
if (d > max_distance)
|
||||
{
|
||||
max_index = i;
|
||||
max_distance = d;
|
||||
}
|
||||
}
|
||||
|
||||
// If max distance is less than epsilon, eliminate all the points between start and end
|
||||
if (max_distance <= epsilon)
|
||||
{
|
||||
PoseList outer;
|
||||
outer.push_back(start);
|
||||
outer.push_back(end);
|
||||
return outer;
|
||||
}
|
||||
|
||||
// If max distance is greater than epsilon, recursively simplify
|
||||
PoseList first_part = compressPlan(input, start_index, max_index, epsilon);
|
||||
PoseList second_part = compressPlan(input, max_index, end_index, epsilon);
|
||||
first_part.insert(first_part.end(), second_part.begin() + 1, second_part.end());
|
||||
return first_part;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon)
|
||||
{
|
||||
nav_2d_msgs::Path2D results;
|
||||
results.header = input_path.header;
|
||||
results.poses = compressPlan(input_path.poses, 0, input_path.poses.size() - 1, epsilon);
|
||||
return results;
|
||||
}
|
||||
|
||||
void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta)
|
||||
{
|
||||
nav_2d_msgs::Pose2DStamped pose;
|
||||
pose.pose.x = x;
|
||||
pose.pose.y = y;
|
||||
pose.pose.theta = theta;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
@@ -1,6 +1,6 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
|
||||
project(nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX)
|
||||
project(robot_nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX)
|
||||
|
||||
# Chuẩn C++
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
@@ -8,13 +8,13 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
# Tìm tất cả header files
|
||||
file(GLOB_RECURSE HEADERS "include/nav_2d_msgs/*.h")
|
||||
file(GLOB_RECURSE HEADERS "include/robot_nav_2d_msgs/*.h")
|
||||
|
||||
# Tạo INTERFACE library (header-only)
|
||||
add_library(nav_2d_msgs INTERFACE)
|
||||
add_library(robot_nav_2d_msgs INTERFACE)
|
||||
|
||||
# Set include directories
|
||||
target_include_directories(nav_2d_msgs
|
||||
target_include_directories(robot_nav_2d_msgs
|
||||
INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
@@ -22,30 +22,30 @@ target_include_directories(nav_2d_msgs
|
||||
|
||||
# Link dependencies (header-only, chỉ cần include paths)
|
||||
# Các dependencies này cũng là header-only từ common_msgs
|
||||
target_link_libraries(nav_2d_msgs
|
||||
target_link_libraries(robot_nav_2d_msgs
|
||||
INTERFACE
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
# Cài đặt header files
|
||||
install(DIRECTORY include/nav_2d_msgs
|
||||
install(DIRECTORY include/robot_nav_2d_msgs
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.h")
|
||||
|
||||
# Cài đặt target
|
||||
install(TARGETS nav_2d_msgs
|
||||
EXPORT nav_2d_msgs-targets
|
||||
install(TARGETS robot_nav_2d_msgs
|
||||
EXPORT robot_nav_2d_msgs-targets
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin)
|
||||
|
||||
# Export targets
|
||||
# Bây giờ có thể export dependencies vì std_msgs và geometry_msgs đã được export
|
||||
install(EXPORT nav_2d_msgs-targets
|
||||
FILE nav_2d_msgs-targets.cmake
|
||||
NAMESPACE nav_2d_msgs::
|
||||
DESTINATION lib/cmake/nav_2d_msgs)
|
||||
install(EXPORT robot_nav_2d_msgs-targets
|
||||
FILE robot_nav_2d_msgs-targets.cmake
|
||||
NAMESPACE robot_nav_2d_msgs::
|
||||
DESTINATION lib/cmake/robot_nav_2d_msgs)
|
||||
|
||||
# In thông tin cấu hình
|
||||
message(STATUS "=================================")
|
||||
@@ -0,0 +1,74 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/ComplexPolygon2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct ComplexPolygon2D_
|
||||
{
|
||||
typedef ComplexPolygon2D_<ContainerAllocator> Type;
|
||||
|
||||
ComplexPolygon2D_()
|
||||
: outer()
|
||||
, inner() {
|
||||
}
|
||||
ComplexPolygon2D_(const ContainerAllocator& _alloc)
|
||||
: outer(_alloc)
|
||||
, inner(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> _outer_type;
|
||||
_outer_type outer;
|
||||
|
||||
typedef std::vector< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> >> _inner_type;
|
||||
_inner_type inner;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct ComplexPolygon2D_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::ComplexPolygon2D_<std::allocator<void> > ComplexPolygon2D;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.outer == rhs.outer &&
|
||||
lhs.inner == rhs.inner;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
|
||||
@@ -1,15 +1,15 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/NavGridInfo.msg
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/NavGridInfo.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
#define NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
namespace nav_2d_msgs
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridInfo_
|
||||
@@ -57,22 +57,22 @@ struct NavGridInfo_
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridInfo_
|
||||
|
||||
typedef ::nav_2d_msgs::NavGridInfo_<std::allocator<void> > NavGridInfo;
|
||||
typedef ::robot_nav_2d_msgs::NavGridInfo_<std::allocator<void> > NavGridInfo;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo > NavGridInfoPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo > NavGridInfoPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
|
||||
bool operator==(const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.width == rhs.width &&
|
||||
lhs.height == rhs.height &&
|
||||
@@ -83,12 +83,12 @@ bool operator==(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, co
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
|
||||
bool operator!=(const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
|
||||
@@ -0,0 +1,80 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfChars.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <robot_nav_2d_msgs/NavGridInfo.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfChars_
|
||||
{
|
||||
typedef NavGridOfChars_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfChars_()
|
||||
: stamp()
|
||||
, info()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfChars_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, info(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
|
||||
_info_type info;
|
||||
|
||||
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfChars_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridOfChars_<std::allocator<void> > NavGridOfChars;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.info == rhs.info &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
|
||||
@@ -0,0 +1,80 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfCharsUpdate.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <robot_nav_2d_msgs/UIntBounds.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfCharsUpdate_
|
||||
{
|
||||
typedef NavGridOfCharsUpdate_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfCharsUpdate_()
|
||||
: stamp()
|
||||
, bounds()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfCharsUpdate_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, bounds(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
|
||||
_bounds_type bounds;
|
||||
|
||||
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfCharsUpdate_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<std::allocator<void> > NavGridOfCharsUpdate;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.bounds == rhs.bounds &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
|
||||
@@ -0,0 +1,80 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfDoubles.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <robot_nav_2d_msgs/NavGridInfo.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfDoubles_
|
||||
{
|
||||
typedef NavGridOfDoubles_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfDoubles_()
|
||||
: stamp()
|
||||
, info()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfDoubles_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, info(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
|
||||
_info_type info;
|
||||
|
||||
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfDoubles_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridOfDoubles_<std::allocator<void> > NavGridOfDoubles;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.info == rhs.info &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
|
||||
@@ -0,0 +1,80 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfDoublesUpdate.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot/time.h>
|
||||
#include <robot_nav_2d_msgs/UIntBounds.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct NavGridOfDoublesUpdate_
|
||||
{
|
||||
typedef NavGridOfDoublesUpdate_<ContainerAllocator> Type;
|
||||
|
||||
NavGridOfDoublesUpdate_()
|
||||
: stamp()
|
||||
, bounds()
|
||||
, data() {
|
||||
}
|
||||
NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc)
|
||||
: stamp()
|
||||
, bounds(_alloc)
|
||||
, data(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef robot::Time _stamp_type;
|
||||
_stamp_type stamp;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
|
||||
_bounds_type bounds;
|
||||
|
||||
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
|
||||
_data_type data;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct NavGridOfDoublesUpdate_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<std::allocator<void> > NavGridOfDoublesUpdate;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.stamp == rhs.stamp &&
|
||||
lhs.bounds == rhs.bounds &&
|
||||
lhs.data == rhs.data;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
|
||||
@@ -0,0 +1,69 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Path2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Path2D_
|
||||
{
|
||||
typedef Path2D_<ContainerAllocator> Type;
|
||||
|
||||
Path2D_()
|
||||
: header()
|
||||
, poses() {
|
||||
}
|
||||
Path2D_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, poses(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef std::vector< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;
|
||||
_poses_type poses;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Path2D_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Path2D_<std::allocator<void> > Path2D;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D > Path2DPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D const> Path2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.poses == rhs.poses;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H
|
||||
@@ -0,0 +1,72 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Point2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Point2D_
|
||||
{
|
||||
typedef Point2D_<ContainerAllocator> Type;
|
||||
|
||||
Point2D_()
|
||||
: x(0.0)
|
||||
, y(0.0) {
|
||||
}
|
||||
Point2D_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef double _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef double _y_type;
|
||||
_y_type y;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Point2D_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Point2D_<std::allocator<void> > Point2D;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D > Point2DPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D const> Point2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H
|
||||
@@ -0,0 +1,67 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Polygon2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <robot_nav_2d_msgs/Point2D.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2D_
|
||||
{
|
||||
typedef Polygon2D_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2D_()
|
||||
: points() {
|
||||
}
|
||||
Polygon2D_(const ContainerAllocator& _alloc)
|
||||
: points(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef std::vector< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> >> _points_type;
|
||||
_points_type points;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2D_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Polygon2D_<std::allocator<void> > Polygon2D;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D > Polygon2DPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D const> Polygon2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.points == rhs.points;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H
|
||||
@@ -0,0 +1,81 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Polygon2DCollection.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <robot_nav_2d_msgs/ComplexPolygon2D.h>
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2DCollection_
|
||||
{
|
||||
typedef Polygon2DCollection_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2DCollection_()
|
||||
: header()
|
||||
, polygons()
|
||||
, colors() {
|
||||
}
|
||||
Polygon2DCollection_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, polygons(_alloc)
|
||||
, colors(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef std::vector< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
|
||||
_polygons_type polygons;
|
||||
|
||||
typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
|
||||
_colors_type colors;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2DCollection_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Polygon2DCollection_<std::allocator<void> > Polygon2DCollection;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.polygons == rhs.polygons &&
|
||||
lhs.colors == rhs.colors;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
|
||||
@@ -0,0 +1,74 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Polygon2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Polygon2DStamped_
|
||||
{
|
||||
typedef Polygon2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Polygon2DStamped_()
|
||||
: header()
|
||||
, polygon() {
|
||||
}
|
||||
Polygon2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, polygon(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> _polygon_type;
|
||||
_polygon_type polygon;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Polygon2DStamped_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Polygon2DStamped_<std::allocator<void> > Polygon2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.polygon == rhs.polygon;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
|
||||
@@ -0,0 +1,78 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Pose2D32.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Pose2D32_
|
||||
{
|
||||
typedef Pose2D32_<ContainerAllocator> Type;
|
||||
|
||||
Pose2D32_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Pose2D32_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef float _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef float _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef float _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Pose2D32_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Pose2D32_<std::allocator<void> > Pose2D32;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32 > Pose2D32Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H
|
||||
@@ -0,0 +1,74 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Pose2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Pose2DStamped_
|
||||
{
|
||||
typedef Pose2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Pose2DStamped_()
|
||||
: header()
|
||||
, pose() {
|
||||
}
|
||||
Pose2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, pose() {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::geometry_msgs::Pose2D _pose_type;
|
||||
_pose_type pose;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Pose2DStamped_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Pose2DStamped_<std::allocator<void> > Pose2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped > Pose2DStampedPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped const> Pose2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.pose == rhs.pose;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H
|
||||
@@ -0,0 +1,30 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/SwitchPlugin.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
|
||||
|
||||
#include <robot_nav_2d_msgs/SwitchPluginRequest.h>
|
||||
#include <robot_nav_2d_msgs/SwitchPluginResponse.h>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
|
||||
struct SwitchPlugin
|
||||
{
|
||||
|
||||
typedef SwitchPluginRequest Request;
|
||||
typedef SwitchPluginResponse Response;
|
||||
Request request;
|
||||
Response response;
|
||||
|
||||
typedef Request RequestType;
|
||||
typedef Response ResponseType;
|
||||
|
||||
}; // struct SwitchPlugin
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H
|
||||
@@ -0,0 +1,66 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/SwitchPluginRequest.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct SwitchPluginRequest_
|
||||
{
|
||||
typedef SwitchPluginRequest_<ContainerAllocator> Type;
|
||||
|
||||
SwitchPluginRequest_()
|
||||
: new_plugin() {
|
||||
}
|
||||
SwitchPluginRequest_(const ContainerAllocator& _alloc)
|
||||
: new_plugin(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _new_plugin_type;
|
||||
_new_plugin_type new_plugin;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct SwitchPluginRequest_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::SwitchPluginRequest_<std::allocator<void> > SwitchPluginRequest;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest > SwitchPluginRequestPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest const> SwitchPluginRequestConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.new_plugin == rhs.new_plugin;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H
|
||||
@@ -0,0 +1,72 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/SwitchPluginResponse.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct SwitchPluginResponse_
|
||||
{
|
||||
typedef SwitchPluginResponse_<ContainerAllocator> Type;
|
||||
|
||||
SwitchPluginResponse_()
|
||||
: success(false)
|
||||
, message() {
|
||||
}
|
||||
SwitchPluginResponse_(const ContainerAllocator& _alloc)
|
||||
: success(false)
|
||||
, message(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef uint8_t _success_type;
|
||||
_success_type success;
|
||||
|
||||
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _message_type;
|
||||
_message_type message;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct SwitchPluginResponse_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::SwitchPluginResponse_<std::allocator<void> > SwitchPluginResponse;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse > SwitchPluginResponsePtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse const> SwitchPluginResponseConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.success == rhs.success &&
|
||||
lhs.message == rhs.message;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H
|
||||
@@ -0,0 +1,78 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Twist2D.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2D_
|
||||
{
|
||||
typedef Twist2D_<ContainerAllocator> Type;
|
||||
|
||||
Twist2D_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Twist2D_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef double _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef double _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef double _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2D_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Twist2D_<std::allocator<void> > Twist2D;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D > Twist2DPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D const> Twist2DConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H
|
||||
@@ -0,0 +1,78 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Twist2D32.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2D32_
|
||||
{
|
||||
typedef Twist2D32_<ContainerAllocator> Type;
|
||||
|
||||
Twist2D32_()
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
}
|
||||
Twist2D32_(const ContainerAllocator& _alloc)
|
||||
: x(0.0)
|
||||
, y(0.0)
|
||||
, theta(0.0) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef float _x_type;
|
||||
_x_type x;
|
||||
|
||||
typedef float _y_type;
|
||||
_y_type y;
|
||||
|
||||
typedef float _theta_type;
|
||||
_theta_type theta;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2D32_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Twist2D32_<std::allocator<void> > Twist2D32;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32 > Twist2D32Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32 const> Twist2D32ConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.x == rhs.x &&
|
||||
lhs.y == rhs.y &&
|
||||
lhs.theta == rhs.theta;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H
|
||||
@@ -0,0 +1,74 @@
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/Twist2DStamped.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include <std_msgs/Header.h>
|
||||
#include <robot_nav_2d_msgs/Twist2D.h>
|
||||
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct Twist2DStamped_
|
||||
{
|
||||
typedef Twist2DStamped_<ContainerAllocator> Type;
|
||||
|
||||
Twist2DStamped_()
|
||||
: header()
|
||||
, velocity() {
|
||||
}
|
||||
Twist2DStamped_(const ContainerAllocator& _alloc)
|
||||
: header()
|
||||
, velocity(_alloc) {
|
||||
(void)_alloc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
typedef ::std_msgs::Header _header_type;
|
||||
_header_type header;
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
|
||||
_velocity_type velocity;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct Twist2DStamped_
|
||||
|
||||
typedef ::robot_nav_2d_msgs::Twist2DStamped_<std::allocator<void> > Twist2DStamped;
|
||||
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped > Twist2DStampedPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped const> Twist2DStampedConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.header == rhs.header &&
|
||||
lhs.velocity == rhs.velocity;
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H
|
||||
@@ -1,9 +1,9 @@
|
||||
// Generated by gencpp from file nav_2d_msgs/UIntBounds.msg
|
||||
// Generated by gencpp from file robot_nav_2d_msgs/UIntBounds.msg
|
||||
// DO NOT EDIT!
|
||||
|
||||
|
||||
#ifndef NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
#define NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
#define ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
|
||||
|
||||
#include <string>
|
||||
@@ -11,7 +11,7 @@
|
||||
#include <memory>
|
||||
|
||||
|
||||
namespace nav_2d_msgs
|
||||
namespace robot_nav_2d_msgs
|
||||
{
|
||||
template <class ContainerAllocator>
|
||||
struct UIntBounds_
|
||||
@@ -49,22 +49,22 @@ struct UIntBounds_
|
||||
|
||||
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> const> ConstPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> > Ptr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> const> ConstPtr;
|
||||
|
||||
}; // struct UIntBounds_
|
||||
|
||||
typedef ::nav_2d_msgs::UIntBounds_<std::allocator<void> > UIntBounds;
|
||||
typedef ::robot_nav_2d_msgs::UIntBounds_<std::allocator<void> > UIntBounds;
|
||||
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds > UIntBoundsPtr;
|
||||
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds const> UIntBoundsConstPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds > UIntBoundsPtr;
|
||||
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds const> UIntBoundsConstPtr;
|
||||
|
||||
// constants requiring out of line definition
|
||||
|
||||
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator==(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
|
||||
bool operator==(const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return lhs.min_x == rhs.min_x &&
|
||||
lhs.min_y == rhs.min_y &&
|
||||
@@ -73,12 +73,12 @@ bool operator==(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, con
|
||||
}
|
||||
|
||||
template<typename ContainerAllocator1, typename ContainerAllocator2>
|
||||
bool operator!=(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
|
||||
bool operator!=(const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_msgs
|
||||
} // namespace robot_nav_2d_msgs
|
||||
|
||||
#endif // NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
|
||||
@@ -1,6 +1,6 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
|
||||
project(nav_2d_utils VERSION 1.0.0 LANGUAGES CXX)
|
||||
project(robot_nav_2d_utils VERSION 1.0.0 LANGUAGES CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
@@ -27,7 +27,7 @@ target_include_directories(conversions
|
||||
)
|
||||
target_link_libraries(conversions
|
||||
PUBLIC
|
||||
nav_2d_msgs
|
||||
robot_nav_2d_msgs
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
nav_grid
|
||||
@@ -51,7 +51,7 @@ target_include_directories(path_ops
|
||||
)
|
||||
target_link_libraries(path_ops
|
||||
PUBLIC
|
||||
nav_2d_msgs
|
||||
robot_nav_2d_msgs
|
||||
geometry_msgs
|
||||
robot_cpp
|
||||
)
|
||||
@@ -66,17 +66,17 @@ target_include_directories(polygons
|
||||
if(xmlrpcpp_FOUND)
|
||||
target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
|
||||
target_link_libraries(polygons
|
||||
PUBLIC nav_2d_msgs geometry_msgs robot_cpp
|
||||
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
|
||||
PRIVATE ${xmlrpcpp_LIBRARIES})
|
||||
elseif(XmlRpcCpp_FOUND)
|
||||
target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
|
||||
target_link_libraries(polygons
|
||||
PUBLIC nav_2d_msgs geometry_msgs robot_cpp
|
||||
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
|
||||
PRIVATE ${XmlRpcCpp_LIBRARIES})
|
||||
else()
|
||||
target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
|
||||
target_link_libraries(polygons
|
||||
PUBLIC nav_2d_msgs geometry_msgs robot_cpp
|
||||
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
|
||||
PRIVATE ${xmlrpcpp_LIBRARIES})
|
||||
endif()
|
||||
|
||||
@@ -109,7 +109,7 @@ target_include_directories(tf_help
|
||||
)
|
||||
target_link_libraries(tf_help
|
||||
PUBLIC
|
||||
nav_2d_msgs
|
||||
robot_nav_2d_msgs
|
||||
geometry_msgs
|
||||
nav_core2
|
||||
tf3
|
||||
@@ -120,14 +120,14 @@ set_target_properties(tf_help PROPERTIES
|
||||
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||
)
|
||||
|
||||
# Create an INTERFACE library that represents all nav_2d_utils libraries
|
||||
add_library(nav_2d_utils INTERFACE)
|
||||
target_include_directories(nav_2d_utils
|
||||
# Create an INTERFACE library that represents all robot_nav_2d_utils libraries
|
||||
add_library(robot_nav_2d_utils INTERFACE)
|
||||
target_include_directories(robot_nav_2d_utils
|
||||
INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_link_libraries(nav_2d_utils
|
||||
target_link_libraries(robot_nav_2d_utils
|
||||
INTERFACE
|
||||
conversions
|
||||
path_ops
|
||||
@@ -138,7 +138,7 @@ target_link_libraries(nav_2d_utils
|
||||
)
|
||||
|
||||
# Install header files
|
||||
install(DIRECTORY include/nav_2d_utils
|
||||
install(DIRECTORY include/robot_nav_2d_utils
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.h")
|
||||
|
||||
@@ -148,17 +148,17 @@ install(DIRECTORY include/mapbox
|
||||
FILES_MATCHING PATTERN "*.hpp")
|
||||
|
||||
# Install targets
|
||||
install(TARGETS nav_2d_utils conversions path_ops polygons bounds tf_help
|
||||
EXPORT nav_2d_utils-targets
|
||||
install(TARGETS robot_nav_2d_utils conversions path_ops polygons bounds tf_help
|
||||
EXPORT robot_nav_2d_utils-targets
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin)
|
||||
|
||||
# Export targets
|
||||
install(EXPORT nav_2d_utils-targets
|
||||
FILE nav_2d_utils-targets.cmake
|
||||
NAMESPACE nav_2d_utils::
|
||||
DESTINATION lib/cmake/nav_2d_utils)
|
||||
install(EXPORT robot_nav_2d_utils-targets
|
||||
FILE robot_nav_2d_utils-targets.cmake
|
||||
NAMESPACE robot_nav_2d_utils::
|
||||
DESTINATION lib/cmake/robot_nav_2d_utils)
|
||||
|
||||
# Print configuration info
|
||||
message(STATUS "=================================")
|
||||
@@ -166,5 +166,5 @@ message(STATUS "Project: ${PROJECT_NAME}")
|
||||
message(STATUS "Version: ${PROJECT_VERSION}")
|
||||
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
|
||||
message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help")
|
||||
message(STATUS "Dependencies: nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
|
||||
message(STATUS "Dependencies: robot_nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
|
||||
message(STATUS "=================================")
|
||||
@@ -1,10 +1,10 @@
|
||||
# nav_2d_utils
|
||||
# robot_nav_2d_utils
|
||||
A handful of useful utility functions for nav_core2 packages.
|
||||
* Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds.
|
||||
* [Conversions](doc/Conversions.md) - Tools for converting between `nav_2d_msgs` and other types.
|
||||
* OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `nav_2d_msgs::Twist`
|
||||
* [Conversions](doc/Conversions.md) - Tools for converting between `robot_nav_2d_msgs` and other types.
|
||||
* OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `robot_nav_2d_msgs::Twist`
|
||||
* Parameters - a couple ROS parameter patterns
|
||||
* PathOps - functions for working with `nav_2d_msgs::Path2D` objects (beyond strict conversion)
|
||||
* PathOps - functions for working with `robot_nav_2d_msgs::Path2D` objects (beyond strict conversion)
|
||||
* [Plugin Mux](doc/PluginMux.md) - tool for switching between multiple `pluginlib` plugins
|
||||
* [Polygons and Footprints](doc/PolygonsAndFootprints.md) - functions for working with `Polygon2D` objects
|
||||
* TF Help - Tools for transforming `nav_2d_msgs` and other common operations.
|
||||
* TF Help - Tools for transforming `robot_nav_2d_msgs` and other common operations.
|
||||
@@ -1,20 +1,20 @@
|
||||
# nav_2d_utils Conversions
|
||||
# robot_nav_2d_utils Conversions
|
||||
|
||||
(note: exact syntax differs from below for conciseness, leaving out `const` and `&`)
|
||||
|
||||
## Twist Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
|
||||
|
||||
## Point Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
| `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)`
|
||||
| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)`
|
||||
|
||||
## Pose Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
|
||||
||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
|
||||
@@ -22,7 +22,7 @@
|
||||
| `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
|
||||
|
||||
## Path Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
| `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
|
||||
| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
|
||||
@@ -30,15 +30,15 @@
|
||||
Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
|
||||
|
||||
## Polygon Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
| `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
|
||||
| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
|
||||
|
||||
## Info Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
|`nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(nav_2d_msgs::NavGridInfo)`|
|
||||
|`robot_nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(robot_nav_2d_msgs::NavGridInfo)`|
|
||||
|
||||
| to `nav_grid` info | from `nav_grid` info |
|
||||
| -- | -- |
|
||||
@@ -49,6 +49,6 @@ Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
|
||||
| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
|
||||
|
||||
## Bounds Transformations
|
||||
| to `nav_2d_msgs` | from `nav_2d_msgs` |
|
||||
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
|
||||
| -- | -- |
|
||||
|`nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(nav_2d_msgs::UIntBounds)`|
|
||||
|`robot_nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`|
|
||||
@@ -1,22 +1,22 @@
|
||||
# nav_2d_utils Polygons and Footprints
|
||||
# robot_nav_2d_utils Polygons and Footprints
|
||||
This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
|
||||
|
||||
## Polygons and the Parameter Server
|
||||
There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with
|
||||
```
|
||||
nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16);
|
||||
```
|
||||
|
||||
The second two ways involve specifying the points of the polygon individually. This can be done with either a string representing a bracketed array of arrays of doubles, `"[[1.0, 2.2], [3.3, 4.2], ...]"`. This can be read with
|
||||
|
||||
```
|
||||
nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
|
||||
```
|
||||
|
||||
Alternatively, with ROS, you can read the points directly from the parameter server in the form of an `XmlRpcValue`, which should be an array of arrays of doubles, which is read with
|
||||
|
||||
```
|
||||
nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
|
||||
```
|
||||
|
||||
If the `XmlRpcValue` is a string, it will call the `polygonFromString` method.
|
||||
@@ -24,22 +24,22 @@ If the `XmlRpcValue` is a string, it will call the `polygonFromString` method.
|
||||
The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
|
||||
|
||||
```
|
||||
nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
|
||||
```
|
||||
|
||||
All of the above methods (except the radius one) can be loaded as appropriate from the parameter server with
|
||||
```
|
||||
nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
|
||||
bool search = true);
|
||||
```
|
||||
to include the radius, use the logic in `footprint.h` which either uses "footprint" or "robot_radius"
|
||||
```
|
||||
nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
|
||||
robot_nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
|
||||
```
|
||||
|
||||
Polygons can be written to parameters with
|
||||
```
|
||||
void polygontoParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
|
||||
void polygontoParams(const robot_nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
|
||||
bool array_of_arrays = true);
|
||||
```
|
||||
|
||||
92
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
Executable file
92
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
Executable file
@@ -0,0 +1,92 @@
|
||||
/*
|
||||
* 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 ROBOT_NAV_2D_UTILS_BOUNDS_H
|
||||
#define ROBOT_NAV_2D_UTILS_BOUNDS_H
|
||||
|
||||
#include <nav_grid/nav_grid_info.h>
|
||||
#include <nav_core2/bounds.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace robot;
|
||||
/**
|
||||
* @brief A set of utility functions for Bounds objects interacting with other messages/types
|
||||
*/
|
||||
namespace robot_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 robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_BOUNDS_H
|
||||
106
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
Executable file
106
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
Executable file
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* 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 ROBOT_NAV_2D_UTILS_CONVERSIONS_H
|
||||
#define ROBOT_NAV_2D_UTILS_CONVERSIONS_H
|
||||
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
#include <robot_nav_2d_msgs/Twist2D.h>
|
||||
#include <robot_nav_2d_msgs/Path2D.h>
|
||||
#include <robot_nav_2d_msgs/Point2D.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2DStamped.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <robot_nav_2d_msgs/NavGridInfo.h>
|
||||
#include <robot_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 robot_nav_2d_utils
|
||||
{
|
||||
// Twist Transformations
|
||||
::geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
|
||||
::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::geometry_msgs::Twist &cmd_vel);
|
||||
|
||||
// Point Transformations
|
||||
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point &point);
|
||||
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point);
|
||||
::geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
|
||||
::geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
|
||||
|
||||
// Pose Transformations
|
||||
// ::robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose);
|
||||
::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::geometry_msgs::PoseStamped &pose);
|
||||
::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d);
|
||||
::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
|
||||
::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::geometry_msgs::Pose2D &pose2d,
|
||||
const ::std::string &frame, const robot::Time &stamp);
|
||||
|
||||
// Path Transformations
|
||||
::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path);
|
||||
nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
|
||||
::robot_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 robot::Time &stamp);
|
||||
nav_msgs::Path pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d);
|
||||
|
||||
// Polygon Transformations
|
||||
::geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
|
||||
::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::geometry_msgs::Polygon &polygon_3d);
|
||||
::geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
|
||||
::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::geometry_msgs::PolygonStamped &polygon_3d);
|
||||
|
||||
// Info Transformations
|
||||
::robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info);
|
||||
nav_grid::NavGridInfo fromMsg(const ::robot_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
|
||||
::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds);
|
||||
robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg);
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_CONVERSIONS_H
|
||||
@@ -32,23 +32,23 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS_FOOTPRINT_H
|
||||
#define NAV_2D_UTILS_FOOTPRINT_H
|
||||
#ifndef ROBOT_NAV_2D_UTILS_FOOTPRINT_H
|
||||
#define ROBOT_NAV_2D_UTILS_FOOTPRINT_H
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <nav_2d_msgs/Polygon2D.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_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(robot::NodeHandle& nh, bool write = true);
|
||||
/**
|
||||
* @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.
|
||||
*/
|
||||
robot_nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle &nh, bool write = true);
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS_FOOTPRINT_H
|
||||
#endif // ROBOT_NAV_2D_UTILS_FOOTPRINT_H
|
||||
@@ -32,54 +32,54 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
#define NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
#ifndef ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
#define ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_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)
|
||||
/**
|
||||
* @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)
|
||||
{
|
||||
xx = x0;
|
||||
yy = y0;
|
||||
}
|
||||
else if (param > 1)
|
||||
{
|
||||
xx = x1;
|
||||
yy = y1;
|
||||
}
|
||||
else
|
||||
{
|
||||
xx = x0 + param * C;
|
||||
yy = y0 + param * D;
|
||||
}
|
||||
double A = pX - x0;
|
||||
double B = pY - y0;
|
||||
double C = x1 - x0;
|
||||
double D = y1 - y0;
|
||||
|
||||
return hypot(pX - xx, pY - yy);
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
double dot = A * C + B * D;
|
||||
double len_sq = C * C + D * D;
|
||||
double param = dot / len_sq;
|
||||
|
||||
#endif // NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
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 robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H
|
||||
@@ -32,56 +32,56 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
#define NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
#ifndef ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
#define ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <robot_nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <robot/node_handle.h>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_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.
|
||||
* @class OdomSubscriber
|
||||
* Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
|
||||
*/
|
||||
explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom")
|
||||
class OdomSubscriber
|
||||
{
|
||||
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));
|
||||
}
|
||||
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(robot::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_; }
|
||||
inline robot_nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; }
|
||||
inline robot_nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; }
|
||||
|
||||
protected:
|
||||
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
|
||||
{
|
||||
std::cout << "odom received!" << std::endl;
|
||||
boost::mutex::scoped_lock lock(odom_mutex_);
|
||||
odom_vel_.header = msg->header;
|
||||
odom_vel_.velocity = twist3Dto2D(msg->twist.twist);
|
||||
}
|
||||
protected:
|
||||
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
std::cout << "odom received!" << std::endl;
|
||||
boost::mutex::scoped_lock lock(odom_mutex_);
|
||||
odom_vel_.header = msg->header;
|
||||
odom_vel_.velocity = twist3Dto2D(msg->twist.twist);
|
||||
}
|
||||
|
||||
nav_2d_msgs::Twist2DStamped odom_vel_;
|
||||
boost::mutex odom_mutex_;
|
||||
};
|
||||
robot_nav_2d_msgs::Twist2DStamped odom_vel_;
|
||||
boost::mutex odom_mutex_;
|
||||
};
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
#endif // ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H
|
||||
153
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h
Executable file
153
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h
Executable file
@@ -0,0 +1,153 @@
|
||||
/*
|
||||
* 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 ROBOT_NAV_2D_UTILS_PARAMETERS_H
|
||||
#define ROBOT_NAV_2D_UTILS_PARAMETERS_H
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot/console.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
namespace robot_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 robot::NodeHandle &nh, const std::string ¶m_name, const param_t &default_value)
|
||||
{
|
||||
std::string resolved_name;
|
||||
if (nh.searchParam(param_name, resolved_name))
|
||||
{
|
||||
param_t value = default_value;
|
||||
robot::NodeHandle nh_private = robot::NodeHandle("~");
|
||||
nh_private.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 robot::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, default_value);
|
||||
return value;
|
||||
}
|
||||
if (nh.hasParam(old_name))
|
||||
{
|
||||
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
|
||||
nh.getParam(old_name, value, default_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 robot::NodeHandle &nh, const std::string current_name, const std::string old_name)
|
||||
{
|
||||
if (!nh.hasParam(old_name))
|
||||
return;
|
||||
|
||||
param_t value;
|
||||
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
|
||||
value = nh.param<param_t>(old_name);
|
||||
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 robot::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 robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_PARAMETERS_H
|
||||
88
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
Executable file
88
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
Executable 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 ROBOT_NAV_2D_UTILS_PATH_OPS_H
|
||||
#define ROBOT_NAV_2D_UTILS_PATH_OPS_H
|
||||
|
||||
#include <robot_nav_2d_msgs/Path2D.h>
|
||||
|
||||
namespace robot_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 robot_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 robot_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
|
||||
*/
|
||||
robot_nav_2d_msgs::Path2D adjustPlanResolution(const robot_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
|
||||
*/
|
||||
robot_nav_2d_msgs::Path2D compressPlan(const robot_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(robot_nav_2d_msgs::Path2D &path, double x, double y, double theta = 0.0);
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_PATH_OPS_H
|
||||
275
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
Executable file
275
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
Executable file
@@ -0,0 +1,275 @@
|
||||
/*
|
||||
* 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 ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H
|
||||
#define ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <robot_nav_2d_msgs/SwitchPlugin.h>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_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 ¶meter_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(robot_nav_2d_msgs::SwitchPlugin::Request &req, robot_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
|
||||
robot::ServiceServer switch_plugin_srv_;
|
||||
robot::Publisher current_plugin_pub_;
|
||||
robot::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 ¶meter_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 robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H
|
||||
@@ -32,17 +32,17 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS_POLYGONS_H
|
||||
#define NAV_2D_UTILS_POLYGONS_H
|
||||
#ifndef ROBOT_NAV_2D_UTILS_POLYGONS_H
|
||||
#define ROBOT_NAV_2D_UTILS_POLYGONS_H
|
||||
|
||||
#include <nav_2d_msgs/Polygon2D.h>
|
||||
#include <nav_2d_msgs/ComplexPolygon2D.h>
|
||||
#include <robot_nav_2d_msgs/Polygon2D.h>
|
||||
#include <robot_nav_2d_msgs/ComplexPolygon2D.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
#include <xmlrpcpp/XmlRpcValue.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
|
||||
/**
|
||||
@@ -71,7 +71,7 @@ std::vector<std::vector<double> > parseVVD(const std::string& input);
|
||||
* @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);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16);
|
||||
|
||||
/**
|
||||
* @brief Make a polygon from the given string.
|
||||
@@ -80,7 +80,7 @@ nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int
|
||||
* @param polygon_string The string to parse
|
||||
* @return Polygon2D
|
||||
*/
|
||||
nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
|
||||
robot_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
|
||||
@@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
|
||||
// * @param search Whether to search up the namespace for the parameter name
|
||||
// * @return Loaded polygon
|
||||
// */
|
||||
// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name,
|
||||
// robot_nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name,
|
||||
// bool search = true);
|
||||
|
||||
/**
|
||||
@@ -99,7 +99,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
|
||||
* 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);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
|
||||
|
||||
/**
|
||||
* @brief Create XMLRPC Value for writing the polygon to the parameter server
|
||||
@@ -107,7 +107,7 @@ nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
|
||||
* @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);
|
||||
XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
|
||||
|
||||
// /**
|
||||
// * @brief Save a polygon to a parameter
|
||||
@@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
|
||||
// * @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 robot::NodeHandle& nh, const std::string parameter_name,
|
||||
// void polygonToParams(const robot_nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name,
|
||||
// bool array_of_arrays = true);
|
||||
|
||||
/**
|
||||
@@ -125,7 +125,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
|
||||
* @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);
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
|
||||
|
||||
/**
|
||||
* @brief Create two parallel arrays from a polygon
|
||||
@@ -134,12 +134,12 @@ nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs,
|
||||
* @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);
|
||||
void polygonToParallelArrays(const robot_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);
|
||||
bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msgs::Polygon2D& polygon1);
|
||||
|
||||
/**
|
||||
* @brief Translate and rotate a polygon to a new pose
|
||||
@@ -147,7 +147,7 @@ bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D
|
||||
* @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,
|
||||
robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
|
||||
const geometry_msgs::Pose2D& pose);
|
||||
|
||||
/**
|
||||
@@ -160,7 +160,7 @@ nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
|
||||
* @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);
|
||||
bool isInside(const robot_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
|
||||
@@ -168,7 +168,7 @@ bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const doubl
|
||||
* @param[out] min_dist
|
||||
* @param[out] max_dist
|
||||
*/
|
||||
void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist);
|
||||
void calculateMinAndMaxDistances(const robot_nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist);
|
||||
|
||||
/**
|
||||
* @brief Decompose a complex polygon into a set of triangles.
|
||||
@@ -180,7 +180,7 @@ void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double&
|
||||
* @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);
|
||||
std::vector<robot_nav_2d_msgs::Point2D> triangulate(const robot_nav_2d_msgs::ComplexPolygon2D& polygon);
|
||||
|
||||
/**
|
||||
* @brief Decompose a simple polygon into a set of triangles.
|
||||
@@ -192,8 +192,8 @@ std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2
|
||||
* @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);
|
||||
std::vector<robot_nav_2d_msgs::Point2D> triangulate(const robot_nav_2d_msgs::Polygon2D& polygon);
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS_POLYGONS_H
|
||||
#endif // ROBOT_NAV_2D_UTILS_POLYGONS_H
|
||||
91
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
Executable file
91
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
Executable 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 ROBOT_NAV_2D_UTILS_TF_HELP_H
|
||||
#define ROBOT_NAV_2D_UTILS_TF_HELP_H
|
||||
|
||||
#include <nav_core2/common.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <string>
|
||||
|
||||
namespace robot_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 ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_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 ::robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const ::std::string &frame_id);
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
#endif // ROBOT_NAV_2D_UTILS_TF_HELP_H
|
||||
@@ -1,9 +1,9 @@
|
||||
<package>
|
||||
<name>nav_2d_utils</name>
|
||||
<name>robot_nav_2d_utils</name>
|
||||
<version>0.7.10</version>
|
||||
<description>
|
||||
nav_2d_utils is the second generation of the transform library, which lets
|
||||
the user keep track of multiple coordinate frames over time. nav_2d_utils
|
||||
robot_nav_2d_utils is the second generation of the transform library, which lets
|
||||
the user keep track of multiple coordinate frames over time. robot_nav_2d_utils
|
||||
maintains the relationship between coordinate frames in a tree
|
||||
structure buffered in time, and lets the user transform points,
|
||||
vectors, etc between any two coordinate frames at any desired
|
||||
@@ -15,7 +15,7 @@
|
||||
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://www.ros.org/wiki/nav_2d_utils</url>
|
||||
<url type="website">http://www.ros.org/wiki/robot_nav_2d_utils</url>
|
||||
|
||||
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
|
||||
|
||||
@@ -4,7 +4,7 @@ from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
package_info = generate_distutils_setup(
|
||||
packages=['nav_2d_utils'],
|
||||
packages=['robot_nav_2d_utils'],
|
||||
package_dir={'': 'src'}
|
||||
)
|
||||
|
||||
103
src/Libraries/robot_nav_2d_utils/src/bounds.cpp
Executable file
103
src/Libraries/robot_nav_2d_utils/src/bounds.cpp
Executable file
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <robot_nav_2d_utils/bounds.h>
|
||||
#include <nav_grid/coordinate_conversion.h>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
using namespace robot;
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
return nav_core2::Bounds(info.origin_x, info.origin_y,
|
||||
info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height);
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
// bounds are inclusive, so we subtract one
|
||||
return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds)
|
||||
{
|
||||
unsigned int g_min_x, g_min_y, g_max_x, g_max_y;
|
||||
worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y);
|
||||
worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y);
|
||||
return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
|
||||
}
|
||||
|
||||
nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds)
|
||||
{
|
||||
double min_x, min_y, max_x, max_y;
|
||||
gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y);
|
||||
gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y);
|
||||
return nav_core2::Bounds(min_x, min_y, max_x, max_y);
|
||||
}
|
||||
|
||||
std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds &original_bounds,
|
||||
unsigned int n_cols, unsigned int n_rows)
|
||||
{
|
||||
if (n_cols * n_rows == 0)
|
||||
{
|
||||
throw std::invalid_argument("Number of rows and columns must be positive (not zero)");
|
||||
}
|
||||
unsigned int full_width = original_bounds.getWidth(),
|
||||
full_height = original_bounds.getHeight();
|
||||
|
||||
unsigned int small_width = static_cast<unsigned int>(ceil(static_cast<double>(full_width) / n_cols)),
|
||||
small_height = static_cast<unsigned int>(ceil(static_cast<double>(full_height) / n_rows));
|
||||
|
||||
std::vector<nav_core2::UIntBounds> divided;
|
||||
|
||||
for (unsigned int row = 0; row < n_rows; row++)
|
||||
{
|
||||
unsigned int min_y = original_bounds.getMinY() + small_height * row;
|
||||
unsigned int max_y = std::min(min_y + small_height - 1, original_bounds.getMaxY());
|
||||
|
||||
for (unsigned int col = 0; col < n_cols; col++)
|
||||
{
|
||||
unsigned int min_x = original_bounds.getMinX() + small_width * col;
|
||||
unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX());
|
||||
nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
|
||||
if (!sub.isEmpty())
|
||||
divided.push_back(sub);
|
||||
}
|
||||
}
|
||||
return divided;
|
||||
}
|
||||
} // namespace robot_nav_2d_utils
|
||||
336
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
Executable file
336
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
Executable file
@@ -0,0 +1,336 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
|
||||
geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d)
|
||||
{
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = cmd_vel_2d.x;
|
||||
cmd_vel.linear.y = cmd_vel_2d.y;
|
||||
cmd_vel.angular.z = cmd_vel_2d.theta;
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist &cmd_vel)
|
||||
{
|
||||
robot_nav_2d_msgs::Twist2D cmd_vel_2d;
|
||||
cmd_vel_2d.x = cmd_vel.linear.x;
|
||||
cmd_vel_2d.y = cmd_vel.linear.y;
|
||||
cmd_vel_2d.theta = cmd_vel.angular.z;
|
||||
return cmd_vel_2d;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point &point)
|
||||
{
|
||||
robot_nav_2d_msgs::Point2D output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32 &point)
|
||||
{
|
||||
robot_nav_2d_msgs::Point2D output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point)
|
||||
{
|
||||
geometry_msgs::Point output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point)
|
||||
{
|
||||
geometry_msgs::Point32 output;
|
||||
output.x = point.x;
|
||||
output.y = point.y;
|
||||
return output;
|
||||
}
|
||||
|
||||
// robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose)
|
||||
// {
|
||||
// robot_nav_2d_msgs::Pose2DStamped pose2d;
|
||||
// pose2d.header.stamp = pose.stamp_;
|
||||
// pose2d.header.frame_id = pose.frame_id_;
|
||||
// pose2d.pose.x = pose.getOrigin().getX();
|
||||
// pose2d.pose.y = pose.getOrigin().getY();
|
||||
// pose2d.pose.theta = tf::getYaw(pose.getRotation());
|
||||
// return pose2d;
|
||||
// }
|
||||
|
||||
robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
|
||||
{
|
||||
robot_nav_2d_msgs::Pose2DStamped pose2d;
|
||||
pose2d.header = pose.header;
|
||||
pose2d.pose.x = pose.pose.position.x;
|
||||
pose2d.pose.y = pose.pose.position.y;
|
||||
// pose2d.pose.theta = tf::getYaw(pose.pose.orientation);
|
||||
return pose2d;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D &pose2d)
|
||||
{
|
||||
geometry_msgs::Pose pose;
|
||||
pose.position.x = pose2d.x;
|
||||
pose.position.y = pose2d.y;
|
||||
// pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
|
||||
return pose;
|
||||
}
|
||||
|
||||
geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d)
|
||||
{
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.header = pose2d.header;
|
||||
pose.pose = pose2DToPose(pose2d.pose);
|
||||
return pose;
|
||||
}
|
||||
|
||||
// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
|
||||
// const std::string& frame, const robot::Time& stamp)
|
||||
// {
|
||||
// geometry_msgs::PoseStamped pose;
|
||||
// pose.header.frame_id = frame;
|
||||
// pose.header.stamp = stamp;
|
||||
// pose.pose.position.x = pose2d.x;
|
||||
// pose.pose.position.y = pose2d.y;
|
||||
// // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
|
||||
// return pose;
|
||||
// }
|
||||
|
||||
robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
|
||||
{
|
||||
robot_nav_2d_msgs::Path2D path2d;
|
||||
path2d.header = path.header;
|
||||
robot_nav_2d_msgs::Pose2DStamped stamped_2d;
|
||||
path2d.poses.resize(path.poses.size());
|
||||
for (unsigned int i = 0; i < path.poses.size(); i++)
|
||||
{
|
||||
stamped_2d = poseStampedToPose2D(path.poses[i]);
|
||||
path2d.poses[i] = stamped_2d;
|
||||
}
|
||||
return path2d;
|
||||
}
|
||||
|
||||
nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped> &poses)
|
||||
{
|
||||
nav_msgs::Path path;
|
||||
if (poses.empty())
|
||||
return path;
|
||||
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = poses[0].header.frame_id;
|
||||
path.header.stamp = poses[0].header.stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++)
|
||||
{
|
||||
path.poses[i] = poses[i];
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped> &poses)
|
||||
{
|
||||
robot_nav_2d_msgs::Path2D path;
|
||||
if (poses.empty())
|
||||
return path;
|
||||
|
||||
robot_nav_2d_msgs::Pose2DStamped pose;
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = poses[0].header.frame_id;
|
||||
path.header.stamp = poses[0].header.stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++)
|
||||
{
|
||||
pose = poseStampedToPose2D(poses[i]);
|
||||
path.poses[i] = pose;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
// nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
|
||||
// const std::string& frame, const robot::Time& stamp)
|
||||
// {
|
||||
// nav_msgs::Path path;
|
||||
// path.poses.resize(poses.size());
|
||||
// path.header.frame_id = frame;
|
||||
// path.header.stamp = stamp;
|
||||
// for (unsigned int i = 0; i < poses.size(); i++)
|
||||
// {
|
||||
// path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
|
||||
// }
|
||||
// return path;
|
||||
// }
|
||||
|
||||
nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d)
|
||||
{
|
||||
nav_msgs::Path path;
|
||||
path.header = path2d.header;
|
||||
path.poses.resize(path2d.poses.size());
|
||||
for (unsigned int i = 0; i < path.poses.size(); i++)
|
||||
{
|
||||
path.poses[i].header = path2d.header;
|
||||
path.poses[i].pose = pose2DToPose(path2d.poses[i].pose);
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d)
|
||||
{
|
||||
geometry_msgs::Polygon polygon;
|
||||
polygon.points.reserve(polygon_2d.points.size());
|
||||
for (const auto &pt : polygon_2d.points)
|
||||
{
|
||||
polygon.points.push_back(pointToPoint32(pt));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon &polygon_3d)
|
||||
{
|
||||
robot_nav_2d_msgs::Polygon2D polygon;
|
||||
polygon.points.reserve(polygon_3d.points.size());
|
||||
for (const auto &pt : polygon_3d.points)
|
||||
{
|
||||
polygon.points.push_back(pointToPoint2D(pt));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d)
|
||||
{
|
||||
geometry_msgs::PolygonStamped polygon;
|
||||
polygon.header = polygon_2d.header;
|
||||
polygon.polygon = polygon2Dto3D(polygon_2d.polygon);
|
||||
return polygon;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped &polygon_3d)
|
||||
{
|
||||
robot_nav_2d_msgs::Polygon2DStamped polygon;
|
||||
polygon.header = polygon_3d.header;
|
||||
polygon.polygon = polygon3Dto2D(polygon_3d.polygon);
|
||||
return polygon;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
robot_nav_2d_msgs::NavGridInfo msg;
|
||||
msg.width = info.width;
|
||||
msg.height = info.height;
|
||||
msg.resolution = info.resolution;
|
||||
msg.frame_id = info.frame_id;
|
||||
msg.origin_x = info.origin_x;
|
||||
msg.origin_y = info.origin_y;
|
||||
return msg;
|
||||
}
|
||||
|
||||
nav_grid::NavGridInfo fromMsg(const robot_nav_2d_msgs::NavGridInfo &msg)
|
||||
{
|
||||
nav_grid::NavGridInfo info;
|
||||
info.width = msg.width;
|
||||
info.height = msg.height;
|
||||
info.resolution = msg.resolution;
|
||||
info.frame_id = msg.frame_id;
|
||||
info.origin_x = msg.origin_x;
|
||||
info.origin_y = msg.origin_y;
|
||||
return info;
|
||||
}
|
||||
|
||||
nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const std::string &frame)
|
||||
{
|
||||
nav_grid::NavGridInfo info;
|
||||
info.frame_id = frame;
|
||||
info.resolution = metadata.resolution;
|
||||
info.width = metadata.width;
|
||||
info.height = metadata.height;
|
||||
info.origin_x = metadata.origin.position.x;
|
||||
info.origin_y = metadata.origin.position.y;
|
||||
// if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5)
|
||||
// {
|
||||
// std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl;
|
||||
// }
|
||||
return info;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
geometry_msgs::Pose origin;
|
||||
origin.position.x = info.origin_x;
|
||||
origin.position.y = info.origin_y;
|
||||
origin.orientation.w = 1.0;
|
||||
return origin;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
geometry_msgs::Pose2D origin;
|
||||
origin.x = info.origin_x;
|
||||
origin.y = info.origin_y;
|
||||
return origin;
|
||||
}
|
||||
|
||||
nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info)
|
||||
{
|
||||
nav_msgs::MapMetaData metadata;
|
||||
metadata.resolution = info.resolution;
|
||||
metadata.width = info.width;
|
||||
metadata.height = info.height;
|
||||
metadata.origin = getOrigin3D(info);
|
||||
return metadata;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds)
|
||||
{
|
||||
robot_nav_2d_msgs::UIntBounds msg;
|
||||
msg.min_x = bounds.getMinX();
|
||||
msg.min_y = bounds.getMinY();
|
||||
msg.max_x = bounds.getMaxX();
|
||||
msg.max_y = bounds.getMaxY();
|
||||
return msg;
|
||||
}
|
||||
|
||||
robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
|
||||
{
|
||||
return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
|
||||
}
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
@@ -31,37 +31,37 @@
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <nav_2d_utils/footprint.h>
|
||||
#include <nav_2d_utils/polygons.h>
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot_nav_2d_utils/footprint.h>
|
||||
#include <robot_nav_2d_utils/polygons.h>
|
||||
#include <string>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write)
|
||||
{
|
||||
std::string full_param_name;
|
||||
nav_2d_msgs::Polygon2D footprint;
|
||||
robot_nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node &nh, bool write)
|
||||
{
|
||||
std::string full_param_name;
|
||||
robot_nav_2d_msgs::Polygon2D footprint;
|
||||
|
||||
// if (nh.searchParam("footprint", full_param_name))
|
||||
// {
|
||||
// // footprint = polygonFromParams(nh, full_param_name, false);
|
||||
// if (write)
|
||||
// {
|
||||
// nh.setParam("footprint", polygonToXMLRPC(footprint));
|
||||
// }
|
||||
// }
|
||||
// else if (nh.searchParam("robot_radius", full_param_name))
|
||||
// {
|
||||
// double robot_radius = 0.0;
|
||||
// nh.getParam(full_param_name, robot_radius);
|
||||
// footprint = polygonFromRadius(robot_radius);
|
||||
// if (write)
|
||||
// {
|
||||
// nh.setParam("robot_radius", robot_radius);
|
||||
// }
|
||||
// }
|
||||
return footprint;
|
||||
}
|
||||
// if (nh.searchParam("footprint", full_param_name))
|
||||
// {
|
||||
// // footprint = polygonFromParams(nh, full_param_name, false);
|
||||
// if (write)
|
||||
// {
|
||||
// nh.setParam("footprint", polygonToXMLRPC(footprint));
|
||||
// }
|
||||
// }
|
||||
// else if (nh.searchParam("robot_radius", full_param_name))
|
||||
// {
|
||||
// double robot_radius = 0.0;
|
||||
// nh.getParam(full_param_name, robot_radius);
|
||||
// footprint = polygonFromRadius(robot_radius);
|
||||
// if (write)
|
||||
// {
|
||||
// nh.setParam("robot_radius", robot_radius);
|
||||
// }
|
||||
// }
|
||||
return footprint;
|
||||
}
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
194
src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
Executable file
194
src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
Executable file
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <robot_nav_2d_utils/path_ops.h>
|
||||
#include <robot_nav_2d_utils/geometry_help.h>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
|
||||
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
|
||||
{
|
||||
return hypot(pose0.x - pose1.x, pose0.y - pose1.y);
|
||||
}
|
||||
|
||||
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const unsigned int start_index)
|
||||
{
|
||||
double length = 0.0;
|
||||
for (unsigned int i = start_index + 1; i < plan.poses.size(); i++)
|
||||
{
|
||||
length += poseDistance(plan.poses[i - 1].pose, plan.poses[i].pose);
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose)
|
||||
{
|
||||
if (plan.poses.size() == 0)
|
||||
return 0.0;
|
||||
|
||||
unsigned int closest_index = 0;
|
||||
double closest_distance = poseDistance(plan.poses[0].pose, query_pose);
|
||||
for (unsigned int i = 1; i < plan.poses.size(); i++)
|
||||
{
|
||||
double distance = poseDistance(plan.poses[i].pose, query_pose);
|
||||
if (closest_distance > distance)
|
||||
{
|
||||
closest_index = i;
|
||||
closest_distance = distance;
|
||||
}
|
||||
}
|
||||
return getPlanLength(plan, closest_index);
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Path2D adjustPlanResolution(const robot_nav_2d_msgs::Path2D &global_plan_in, double resolution)
|
||||
{
|
||||
robot_nav_2d_msgs::Path2D global_plan_out;
|
||||
global_plan_out.header = global_plan_in.header;
|
||||
if (global_plan_in.poses.size() == 0)
|
||||
{
|
||||
return global_plan_out;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Pose2DStamped last_stp = global_plan_in.poses[0];
|
||||
global_plan_out.poses.push_back(last_stp);
|
||||
|
||||
double sq_resolution = resolution * resolution;
|
||||
geometry_msgs::Pose2D last = last_stp.pose;
|
||||
for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i)
|
||||
{
|
||||
geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose;
|
||||
double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
|
||||
if (sq_dist > sq_resolution)
|
||||
{
|
||||
// add points in-between
|
||||
double diff = sqrt(sq_dist) - resolution;
|
||||
double steps_double = ceil(diff / resolution) + 1.0;
|
||||
int steps = static_cast<int>(steps_double);
|
||||
|
||||
double delta_x = (loop.x - last.x) / steps_double;
|
||||
double delta_y = (loop.y - last.y) / steps_double;
|
||||
double delta_t = (loop.theta - last.theta) / steps_double;
|
||||
|
||||
for (int j = 1; j < steps; ++j)
|
||||
{
|
||||
robot_nav_2d_msgs::Pose2DStamped pose;
|
||||
pose.header = last_stp.header;
|
||||
pose.pose.x = last.x + j * delta_x;
|
||||
pose.pose.y = last.y + j * delta_y;
|
||||
pose.pose.theta = last.theta + j * delta_t;
|
||||
global_plan_out.poses.push_back(pose);
|
||||
}
|
||||
}
|
||||
global_plan_out.poses.push_back(global_plan_in.poses[i]);
|
||||
last.x = loop.x;
|
||||
last.y = loop.y;
|
||||
}
|
||||
return global_plan_out;
|
||||
}
|
||||
|
||||
using PoseList = std::vector<robot_nav_2d_msgs::Pose2DStamped>;
|
||||
|
||||
/**
|
||||
* @brief Helper function for other version of compressPlan.
|
||||
*
|
||||
* Uses the Ramer Douglas Peucker algorithm. Ignores theta values
|
||||
*
|
||||
* @param input Full list of poses
|
||||
* @param start_index Index of first pose (inclusive)
|
||||
* @param end_index Index of last pose (inclusive)
|
||||
* @param epsilon maximum allowable error. Increase for greater compression.
|
||||
* @param list of poses possibly compressed for the poses[start_index, end_index]
|
||||
*/
|
||||
PoseList compressPlan(const PoseList &input, unsigned int start_index, unsigned int end_index, double epsilon)
|
||||
{
|
||||
// Skip if only two points
|
||||
if (end_index - start_index <= 1)
|
||||
{
|
||||
PoseList::const_iterator first = input.begin() + start_index;
|
||||
PoseList::const_iterator last = input.begin() + end_index + 1;
|
||||
return PoseList(first, last);
|
||||
}
|
||||
|
||||
// Find the point with the maximum distance to the line from start to end
|
||||
const robot_nav_2d_msgs::Pose2DStamped &start = input[start_index],
|
||||
end = input[end_index];
|
||||
double max_distance = 0.0;
|
||||
unsigned int max_index = 0;
|
||||
for (unsigned int i = start_index + 1; i < end_index; i++)
|
||||
{
|
||||
const robot_nav_2d_msgs::Pose2DStamped &pose = input[i];
|
||||
double d = distanceToLine(pose.pose.x, pose.pose.y, start.pose.x, start.pose.y, end.pose.x, end.pose.y);
|
||||
if (d > max_distance)
|
||||
{
|
||||
max_index = i;
|
||||
max_distance = d;
|
||||
}
|
||||
}
|
||||
|
||||
// If max distance is less than epsilon, eliminate all the points between start and end
|
||||
if (max_distance <= epsilon)
|
||||
{
|
||||
PoseList outer;
|
||||
outer.push_back(start);
|
||||
outer.push_back(end);
|
||||
return outer;
|
||||
}
|
||||
|
||||
// If max distance is greater than epsilon, recursively simplify
|
||||
PoseList first_part = compressPlan(input, start_index, max_index, epsilon);
|
||||
PoseList second_part = compressPlan(input, max_index, end_index, epsilon);
|
||||
first_part.insert(first_part.end(), second_part.begin() + 1, second_part.end());
|
||||
return first_part;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Path2D compressPlan(const robot_nav_2d_msgs::Path2D &input_path, double epsilon)
|
||||
{
|
||||
robot_nav_2d_msgs::Path2D results;
|
||||
results.header = input_path.header;
|
||||
results.poses = compressPlan(input_path.poses, 0, input_path.poses.size() - 1, epsilon);
|
||||
return results;
|
||||
}
|
||||
|
||||
void addPose(robot_nav_2d_msgs::Path2D &path, double x, double y, double theta)
|
||||
{
|
||||
robot_nav_2d_msgs::Pose2DStamped pose;
|
||||
pose.pose.x = x;
|
||||
pose.pose.y = y;
|
||||
pose.pose.theta = theta;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
} // namespace robot_nav_2d_utils
|
||||
@@ -32,8 +32,8 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <nav_2d_utils/polygons.h>
|
||||
#include <nav_2d_utils/geometry_help.h>
|
||||
#include <robot_nav_2d_utils/polygons.h>
|
||||
#include <robot_nav_2d_utils/geometry_help.h>
|
||||
#include <mapbox/earcut.hpp>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
@@ -41,11 +41,11 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
|
||||
using nav_2d_msgs::Point2D;
|
||||
using nav_2d_msgs::Polygon2D;
|
||||
using robot_nav_2d_msgs::Point2D;
|
||||
using robot_nav_2d_msgs::Polygon2D;
|
||||
|
||||
std::vector<std::vector<double> > parseVVD(const std::string& input)
|
||||
{
|
||||
@@ -285,7 +285,7 @@ XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
|
||||
return xml;
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
|
||||
XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
|
||||
{
|
||||
XmlRpc::XmlRpcValue xml;
|
||||
if (array_of_arrays)
|
||||
@@ -309,13 +309,13 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
|
||||
return xml;
|
||||
}
|
||||
|
||||
// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
|
||||
// void polygonToParams(const robot_nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
|
||||
// bool array_of_arrays)
|
||||
// {
|
||||
// nh.setParam(parameter_name, polygonToXMLRPC(polygon, array_of_arrays));
|
||||
// }
|
||||
|
||||
nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys)
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys)
|
||||
{
|
||||
if (xs.size() < 3)
|
||||
{
|
||||
@@ -337,7 +337,7 @@ nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs,
|
||||
return polygon;
|
||||
}
|
||||
|
||||
void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys)
|
||||
void polygonToParallelArrays(const robot_nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys)
|
||||
{
|
||||
xs.clear();
|
||||
ys.clear();
|
||||
@@ -348,7 +348,7 @@ void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<d
|
||||
}
|
||||
}
|
||||
|
||||
bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1)
|
||||
bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msgs::Polygon2D& polygon1)
|
||||
{
|
||||
if (polygon0.points.size() != polygon1.points.size())
|
||||
{
|
||||
@@ -364,23 +364,23 @@ bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D
|
||||
return true;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
|
||||
robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
|
||||
const geometry_msgs::Pose2D& pose)
|
||||
{
|
||||
nav_2d_msgs::Polygon2D new_polygon;
|
||||
robot_nav_2d_msgs::Polygon2D new_polygon;
|
||||
new_polygon.points.resize(polygon.points.size());
|
||||
double cos_th = cos(pose.theta);
|
||||
double sin_th = sin(pose.theta);
|
||||
for (unsigned int i = 0; i < polygon.points.size(); ++i)
|
||||
{
|
||||
nav_2d_msgs::Point2D& new_pt = new_polygon.points[i];
|
||||
robot_nav_2d_msgs::Point2D& new_pt = new_polygon.points[i];
|
||||
new_pt.x = pose.x + polygon.points[i].x * cos_th - polygon.points[i].y * sin_th;
|
||||
new_pt.y = pose.y + polygon.points[i].x * sin_th + polygon.points[i].y * cos_th;
|
||||
}
|
||||
return new_polygon;
|
||||
}
|
||||
|
||||
bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y)
|
||||
bool isInside(const robot_nav_2d_msgs::Polygon2D& polygon, const double x, const double y)
|
||||
{
|
||||
// Determine if the given point is inside the polygon using the number of crossings method
|
||||
// https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
|
||||
@@ -402,7 +402,7 @@ bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const doubl
|
||||
return cross % 2 > 0;
|
||||
}
|
||||
|
||||
void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist)
|
||||
void calculateMinAndMaxDistances(const robot_nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist)
|
||||
{
|
||||
min_dist = std::numeric_limits<double>::max();
|
||||
max_dist = 0.0;
|
||||
@@ -429,7 +429,7 @@ void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double&
|
||||
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
|
||||
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
// Adapt Point2D for use with the triangulation library
|
||||
namespace mapbox
|
||||
@@ -437,18 +437,18 @@ namespace mapbox
|
||||
namespace util
|
||||
{
|
||||
template <>
|
||||
struct nth<0, nav_2d_msgs::Point2D>
|
||||
struct nth<0, robot_nav_2d_msgs::Point2D>
|
||||
{
|
||||
inline static double get(const nav_2d_msgs::Point2D& point)
|
||||
inline static double get(const robot_nav_2d_msgs::Point2D& point)
|
||||
{
|
||||
return point.x;
|
||||
};
|
||||
};
|
||||
|
||||
template <>
|
||||
struct nth<1, nav_2d_msgs::Point2D>
|
||||
struct nth<1, robot_nav_2d_msgs::Point2D>
|
||||
{
|
||||
inline static double get(const nav_2d_msgs::Point2D& point)
|
||||
inline static double get(const robot_nav_2d_msgs::Point2D& point)
|
||||
{
|
||||
return point.y;
|
||||
};
|
||||
@@ -458,32 +458,32 @@ struct nth<1, nav_2d_msgs::Point2D>
|
||||
} // namespace mapbox
|
||||
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon)
|
||||
std::vector<robot_nav_2d_msgs::Point2D> triangulate(const robot_nav_2d_msgs::ComplexPolygon2D& polygon)
|
||||
{
|
||||
// Compute the triangulation
|
||||
std::vector<std::vector<nav_2d_msgs::Point2D>> rings;
|
||||
std::vector<std::vector<robot_nav_2d_msgs::Point2D>> rings;
|
||||
rings.reserve(1 + polygon.inner.size());
|
||||
rings.push_back(polygon.outer.points);
|
||||
for (const nav_2d_msgs::Polygon2D& inner : polygon.inner)
|
||||
for (const robot_nav_2d_msgs::Polygon2D& inner : polygon.inner)
|
||||
{
|
||||
rings.push_back(inner.points);
|
||||
}
|
||||
std::vector<unsigned int> indices = mapbox::earcut<unsigned int>(rings);
|
||||
|
||||
// Create a sequential point index. The triangulation results are indices in this vector.
|
||||
std::vector<nav_2d_msgs::Point2D> points;
|
||||
std::vector<robot_nav_2d_msgs::Point2D> points;
|
||||
for (const auto& ring : rings)
|
||||
{
|
||||
for (const nav_2d_msgs::Point2D& point : ring)
|
||||
for (const robot_nav_2d_msgs::Point2D& point : ring)
|
||||
{
|
||||
points.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Construct the output triangles
|
||||
std::vector<nav_2d_msgs::Point2D> result;
|
||||
std::vector<robot_nav_2d_msgs::Point2D> result;
|
||||
result.reserve(indices.size());
|
||||
for (unsigned int index : indices)
|
||||
{
|
||||
@@ -492,12 +492,12 @@ std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::Polygon2D& polygon)
|
||||
std::vector<robot_nav_2d_msgs::Point2D> triangulate(const robot_nav_2d_msgs::Polygon2D& polygon)
|
||||
{
|
||||
nav_2d_msgs::ComplexPolygon2D complex;
|
||||
robot_nav_2d_msgs::ComplexPolygon2D complex;
|
||||
complex.outer = polygon;
|
||||
return triangulate(complex);
|
||||
}
|
||||
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
@@ -32,17 +32,17 @@
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <robot/console.h>
|
||||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <string>
|
||||
|
||||
namespace nav_2d_utils
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
bool transformPose(const TFListenerPtr tf, const std::string frame,
|
||||
const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped &out_pose,
|
||||
bool transformPose(const TFListenerPtr tf, const ::std::string frame,
|
||||
const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose,
|
||||
const bool extrapolation_fallback)
|
||||
{
|
||||
// if (in_pose.header.frame_id == frame)
|
||||
@@ -56,7 +56,7 @@ namespace nav_2d_utils
|
||||
// tf->transform(in_pose, out_pose, frame);
|
||||
// return true;
|
||||
// }
|
||||
// catch (tf::ExtrapolationException &ex)
|
||||
// catch (tf3::ExtrapolationException &ex)
|
||||
// {
|
||||
// if (!extrapolation_fallback)
|
||||
// throw;
|
||||
@@ -66,16 +66,16 @@ namespace nav_2d_utils
|
||||
// tf->transform(latest_in_pose, out_pose, frame);
|
||||
// return true;
|
||||
// }
|
||||
// catch (tf::TransformException &ex)
|
||||
// catch (tf3::TransformException &ex)
|
||||
// {
|
||||
// ROS_ERROR("Exception in transformPose: %s", ex.what());
|
||||
// robot::log_error("Exception in transformPose: %s", ex.what());
|
||||
// return false;
|
||||
// }
|
||||
return false;
|
||||
}
|
||||
|
||||
bool transformPose(const TFListenerPtr tf, const std::string frame,
|
||||
const nav_2d_msgs::Pose2DStamped &in_pose, nav_2d_msgs::Pose2DStamped &out_pose,
|
||||
bool transformPose(const TFListenerPtr tf, const ::std::string frame,
|
||||
const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose,
|
||||
const bool extrapolation_fallback)
|
||||
{
|
||||
geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
|
||||
@@ -89,12 +89,12 @@ namespace nav_2d_utils
|
||||
return ret;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose,
|
||||
const std::string &frame_id)
|
||||
::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const ::std::string &frame_id)
|
||||
{
|
||||
nav_2d_msgs::Pose2DStamped local_pose;
|
||||
nav_2d_utils::transformPose(tf, frame_id, pose, local_pose);
|
||||
robot_nav_2d_msgs::Pose2DStamped local_pose;
|
||||
robot_nav_2d_utils::transformPose(tf, frame_id, pose, local_pose);
|
||||
return local_pose.pose;
|
||||
}
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
} // namespace robot_nav_2d_utils
|
||||
@@ -32,11 +32,11 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <gtest/gtest.h>
|
||||
#include <nav_2d_utils/bounds.h>
|
||||
#include <robot_nav_2d_utils/bounds.h>
|
||||
#include <nav_grid/vector_nav_grid.h>
|
||||
#include <vector>
|
||||
|
||||
using nav_2d_utils::divideBounds;
|
||||
using robot_nav_2d_utils::divideBounds;
|
||||
using nav_core2::UIntBounds;
|
||||
|
||||
/**
|
||||
@@ -111,7 +111,7 @@ TEST(DivideBounds, iterative_tests)
|
||||
for (info.height = 1; info.height < 15; info.height++)
|
||||
{
|
||||
full_grid.setInfo(info);
|
||||
UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
|
||||
UIntBounds full_bounds = robot_nav_2d_utils::getFullUIntBounds(info);
|
||||
for (unsigned int rows = 1; rows < 11u; rows++)
|
||||
{
|
||||
for (unsigned int cols = 1; cols < 11u; cols++)
|
||||
@@ -156,7 +156,7 @@ TEST(DivideBounds, recursive_tests)
|
||||
info.height = 100;
|
||||
full_grid.setInfo(info);
|
||||
|
||||
UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
|
||||
UIntBounds full_bounds = robot_nav_2d_utils::getFullUIntBounds(info);
|
||||
|
||||
std::vector<UIntBounds> level_one = divideBounds(full_bounds, 2, 2);
|
||||
ASSERT_EQ(level_one.size(), 4u);
|
||||
@@ -32,14 +32,14 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <gtest/gtest.h>
|
||||
#include <nav_2d_utils/path_ops.h>
|
||||
#include <robot_nav_2d_utils/path_ops.h>
|
||||
|
||||
using nav_2d_utils::compressPlan;
|
||||
using nav_2d_utils::addPose;
|
||||
using robot_nav_2d_utils::compressPlan;
|
||||
using robot_nav_2d_utils::addPose;
|
||||
|
||||
TEST(CompressTest, compress_test)
|
||||
{
|
||||
nav_2d_msgs::Path2D path;
|
||||
robot_nav_2d_msgs::Path2D path;
|
||||
// Dataset borrowed from https://karthaus.nl/rdp/
|
||||
addPose(path, 24, 173);
|
||||
addPose(path, 26, 170);
|
||||
@@ -34,14 +34,14 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Dave Hershberger
|
||||
* David V. Lu!! (nav_2d_utils version)
|
||||
* David V. Lu!! (robot_nav_2d_utils version)
|
||||
*********************************************************************/
|
||||
#include <gtest/gtest.h>
|
||||
#include <ros/ros.h>
|
||||
#include <nav_2d_utils/polygons.h>
|
||||
#include <robot_nav_2d_utils/polygons.h>
|
||||
|
||||
using nav_2d_utils::polygonFromParams;
|
||||
using nav_2d_msgs::Polygon2D;
|
||||
using robot_nav_2d_utils::polygonFromParams;
|
||||
using robot_nav_2d_msgs::Polygon2D;
|
||||
|
||||
TEST(Polygon2D, unpadded_footprint_from_string_param)
|
||||
{
|
||||
@@ -64,7 +64,7 @@ TEST(Polygon2D, check_search_capabilities)
|
||||
YAML::Node nh("~unpadded/unneccessarily/long_namespace");
|
||||
Polygon2D footprint = polygonFromParams(nh, "footprint");
|
||||
ASSERT_EQ(3U, footprint.points.size());
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint", false), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint", false), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, footprint_from_xmlrpc_param)
|
||||
@@ -86,7 +86,7 @@ TEST(Polygon2D, footprint_from_xmlrpc_param)
|
||||
EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 3 ].y);
|
||||
|
||||
Polygon2D footprint2 = polygonFromParams(nh, "footprint2");
|
||||
ASSERT_TRUE(nav_2d_utils::equals(footprint, footprint2));
|
||||
ASSERT_TRUE(robot_nav_2d_utils::equals(footprint, footprint2));
|
||||
}
|
||||
|
||||
TEST(Polygon2D, footprint_from_same_level_param)
|
||||
@@ -108,34 +108,34 @@ TEST(Polygon2D, footprint_from_same_level_param)
|
||||
TEST(Polygon2D, footprint_from_xmlrpc_param_failure)
|
||||
{
|
||||
YAML::Node nh("~xmlrpc_fail");
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint2"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint3"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint4"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint5"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint6"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint7"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint8"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint9"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint2"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint3"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint4"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint5"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint6"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint7"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint8"), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint9"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, footprint_empty)
|
||||
{
|
||||
YAML::Node nh("~empty");
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParams(nh, "footprint"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, test_write)
|
||||
{
|
||||
YAML::Node nh("~unpadded");
|
||||
Polygon2D footprint = polygonFromParams(nh, "footprint");
|
||||
nh.setParam("another_footprint", nav_2d_utils::polygonToXMLRPC(footprint));
|
||||
nh.setParam("another_footprint", robot_nav_2d_utils::polygonToXMLRPC(footprint));
|
||||
Polygon2D another_footprint = polygonFromParams(nh, "another_footprint");
|
||||
EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
|
||||
EXPECT_TRUE(robot_nav_2d_utils::equals(footprint, another_footprint));
|
||||
|
||||
nh.setParam("third_footprint", nav_2d_utils::polygonToXMLRPC(footprint, false));
|
||||
nh.setParam("third_footprint", robot_nav_2d_utils::polygonToXMLRPC(footprint, false));
|
||||
another_footprint = polygonFromParams(nh, "third_footprint");
|
||||
EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
|
||||
EXPECT_TRUE(robot_nav_2d_utils::equals(footprint, another_footprint));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
@@ -1,5 +1,5 @@
|
||||
<launch>
|
||||
<test time-limit="10" test-name="param_tests" pkg="nav_2d_utils" type="param_tests">
|
||||
<test time-limit="10" test-name="param_tests" pkg="robot_nav_2d_utils" type="param_tests">
|
||||
<param name="unpadded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
|
||||
|
||||
<rosparam ns="xmlrpc">
|
||||
@@ -32,13 +32,13 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <gtest/gtest.h>
|
||||
#include <nav_2d_utils/polygons.h>
|
||||
#include <robot_nav_2d_utils/polygons.h>
|
||||
#include <vector>
|
||||
|
||||
using nav_2d_utils::parseVVD;
|
||||
using nav_2d_msgs::Polygon2D;
|
||||
using nav_2d_utils::polygonFromString;
|
||||
using nav_2d_utils::polygonFromParallelArrays;
|
||||
using robot_nav_2d_utils::parseVVD;
|
||||
using robot_nav_2d_msgs::Polygon2D;
|
||||
using robot_nav_2d_utils::polygonFromString;
|
||||
using robot_nav_2d_utils::polygonFromParallelArrays;
|
||||
|
||||
TEST(array_parser, basic_operation)
|
||||
{
|
||||
@@ -55,22 +55,22 @@ TEST(array_parser, basic_operation)
|
||||
|
||||
TEST(array_parser, missing_open)
|
||||
{
|
||||
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(array_parser, missing_close)
|
||||
{
|
||||
EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(array_parser, wrong_depth)
|
||||
{
|
||||
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, radius_param)
|
||||
{
|
||||
Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0);
|
||||
Polygon2D footprint = robot_nav_2d_utils::polygonFromRadius(10.0);
|
||||
// Circular robot has 16-point footprint auto-generated.
|
||||
ASSERT_EQ(16U, footprint.points.size());
|
||||
|
||||
@@ -101,22 +101,22 @@ TEST(Polygon2D, string_param)
|
||||
TEST(Polygon2D, broken_string_param)
|
||||
{
|
||||
// Not enough points
|
||||
EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), robot_nav_2d_utils::PolygonParseException);
|
||||
|
||||
// Too many numbers in point
|
||||
EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), robot_nav_2d_utils::PolygonParseException);
|
||||
|
||||
// Unexpected character
|
||||
EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), robot_nav_2d_utils::PolygonParseException);
|
||||
|
||||
// Empty String
|
||||
EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString(""), robot_nav_2d_utils::PolygonParseException);
|
||||
|
||||
// Empty List
|
||||
EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString("[]"), robot_nav_2d_utils::PolygonParseException);
|
||||
|
||||
// Empty Point
|
||||
EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromString("[[]]"), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, arrays)
|
||||
@@ -141,20 +141,20 @@ TEST(Polygon2D, broken_arrays)
|
||||
std::vector<double> shorty = {1, -1};
|
||||
std::vector<double> three = {1, 1, -1};
|
||||
std::vector<double> four = {1, 1, -1, -1};
|
||||
EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParallelArrays(three, four), nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), robot_nav_2d_utils::PolygonParseException);
|
||||
EXPECT_THROW(polygonFromParallelArrays(three, four), robot_nav_2d_utils::PolygonParseException);
|
||||
}
|
||||
|
||||
TEST(Polygon2D, test_move)
|
||||
{
|
||||
Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
|
||||
geometry_msgs::Pose2D pose;
|
||||
Polygon2D square2 = nav_2d_utils::movePolygonToPose(square, pose);
|
||||
EXPECT_TRUE(nav_2d_utils::equals(square, square2));
|
||||
Polygon2D square2 = robot_nav_2d_utils::movePolygonToPose(square, pose);
|
||||
EXPECT_TRUE(robot_nav_2d_utils::equals(square, square2));
|
||||
pose.x = 15;
|
||||
pose.y = -10;
|
||||
pose.theta = M_PI / 4;
|
||||
Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose);
|
||||
Polygon2D diamond = robot_nav_2d_utils::movePolygonToPose(square, pose);
|
||||
ASSERT_EQ(4U, diamond.points.size());
|
||||
double side = 1.0 / sqrt(2);
|
||||
|
||||
@@ -171,11 +171,11 @@ TEST(Polygon2D, test_move)
|
||||
TEST(Polygon2D, inside)
|
||||
{
|
||||
Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
|
||||
EXPECT_TRUE(nav_2d_utils::isInside(square, 0.00, 0.00));
|
||||
EXPECT_TRUE(nav_2d_utils::isInside(square, 0.45, 0.45));
|
||||
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.50, 0.50));
|
||||
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.00, 0.50));
|
||||
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.55, 0.55));
|
||||
EXPECT_TRUE(robot_nav_2d_utils::isInside(square, 0.00, 0.00));
|
||||
EXPECT_TRUE(robot_nav_2d_utils::isInside(square, 0.45, 0.45));
|
||||
EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.50, 0.50));
|
||||
EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.00, 0.50));
|
||||
EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.55, 0.55));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
@@ -32,14 +32,14 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <gtest/gtest.h>
|
||||
#include <nav_2d_utils/path_ops.h>
|
||||
#include <robot_nav_2d_utils/path_ops.h>
|
||||
|
||||
using nav_2d_utils::adjustPlanResolution;
|
||||
using nav_2d_utils::addPose;
|
||||
using robot_nav_2d_utils::adjustPlanResolution;
|
||||
using robot_nav_2d_utils::addPose;
|
||||
|
||||
TEST(ResolutionTest, simple_example)
|
||||
{
|
||||
nav_2d_msgs::Path2D path;
|
||||
robot_nav_2d_msgs::Path2D path;
|
||||
// Space between points is one meter
|
||||
addPose(path, 0.0, 0.0);
|
||||
addPose(path, 0.0, 1.0);
|
||||
@@ -62,7 +62,7 @@ TEST(ResolutionTest, simple_example)
|
||||
TEST(ResolutionTest, real_example)
|
||||
{
|
||||
// This test is based on a real-world example
|
||||
nav_2d_msgs::Path2D path;
|
||||
robot_nav_2d_msgs::Path2D path;
|
||||
addPose(path, 17.779193, -0.972024);
|
||||
addPose(path, 17.799171, -0.950775);
|
||||
addPose(path, 17.851942, -0.903709);
|
||||
Reference in New Issue
Block a user