created

src/Libraries/nav_2d_msgs/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.10)
+
+project(nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+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")
+
+# Tạo INTERFACE library (header-only)
+add_library(nav_2d_msgs INTERFACE)
+
+# Set include directories
+target_include_directories(nav_2d_msgs
+    INTERFACE 
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# 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
+    INTERFACE
+        std_msgs
+        geometry_msgs
+)
+
+# Cài đặt header files
+install(DIRECTORY include/nav_2d_msgs 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+# Cài đặt target
+install(TARGETS nav_2d_msgs
+        EXPORT 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)
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Headers found:")
+foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+endforeach()
+message(STATUS "Dependencies: std_msgs, geometry_msgs")
+message(STATUS "=================================")
+

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h

@@ -0,0 +1,74 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h

@@ -0,0 +1,94 @@
+// Generated by gencpp from file nav_2d_msgs/NavGridInfo.msg
+// DO NOT EDIT!
+
+
+#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
+#define NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+namespace nav_2d_msgs
+{
+template <class ContainerAllocator>
+struct NavGridInfo_
+{
+  typedef NavGridInfo_<ContainerAllocator> Type;
+
+  NavGridInfo_()
+    : width(0)
+    , height(0)
+    , resolution(0.0)
+    , frame_id()
+    , origin_x(0.0)
+    , origin_y(0.0)  {
+    }
+  NavGridInfo_(const ContainerAllocator& _alloc)
+    : width(0)
+    , height(0)
+    , resolution(0.0)
+    , frame_id(_alloc)
+    , origin_x(0.0)
+    , origin_y(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint32_t _width_type;
+  _width_type width;
+
+   typedef uint32_t _height_type;
+  _height_type height;
+
+   typedef double _resolution_type;
+  _resolution_type resolution;
+
+   typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _frame_id_type;
+  _frame_id_type frame_id;
+
+   typedef double _origin_x_type;
+  _origin_x_type origin_x;
+
+   typedef double _origin_y_type;
+  _origin_y_type origin_y;
+
+
+
+
+  typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
+  typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
+
+}; // struct NavGridInfo_
+
+typedef ::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;
+
+// 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)
+{
+  return lhs.width == rhs.width &&
+    lhs.height == rhs.height &&
+    lhs.resolution == rhs.resolution &&
+    lhs.frame_id == rhs.frame_id &&
+    lhs.origin_x == rhs.origin_x &&
+    lhs.origin_y == rhs.origin_y;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace nav_2d_msgs
+
+#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h

@@ -0,0 +1,80 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h

@@ -0,0 +1,80 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h

@@ -0,0 +1,80 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h

@@ -0,0 +1,80 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h

@@ -0,0 +1,74 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h

@@ -0,0 +1,72 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h

@@ -0,0 +1,67 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h

@@ -0,0 +1,81 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h

@@ -0,0 +1,74 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h

@@ -0,0 +1,78 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h

@@ -0,0 +1,74 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h

@@ -0,0 +1,30 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h

@@ -0,0 +1,66 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h

@@ -0,0 +1,72 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h

@@ -0,0 +1,78 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h

@@ -0,0 +1,78 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h

@@ -0,0 +1,74 @@
+// 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

src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h

@@ -0,0 +1,84 @@
+// Generated by gencpp from file nav_2d_msgs/UIntBounds.msg
+// DO NOT EDIT!
+
+
+#ifndef NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
+#define NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+
+namespace nav_2d_msgs
+{
+template <class ContainerAllocator>
+struct UIntBounds_
+{
+  typedef UIntBounds_<ContainerAllocator> Type;
+
+  UIntBounds_()
+    : min_x(0)
+    , min_y(0)
+    , max_x(0)
+    , max_y(0)  {
+    }
+  UIntBounds_(const ContainerAllocator& _alloc)
+    : min_x(0)
+    , min_y(0)
+    , max_x(0)
+    , max_y(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint32_t _min_x_type;
+  _min_x_type min_x;
+
+   typedef uint32_t _min_y_type;
+  _min_y_type min_y;
+
+   typedef uint32_t _max_x_type;
+  _max_x_type max_x;
+
+   typedef uint32_t _max_y_type;
+  _max_y_type max_y;
+
+
+
+
+  typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> > Ptr;
+  typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> const> ConstPtr;
+
+}; // struct UIntBounds_
+
+typedef ::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;
+
+// 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)
+{
+  return lhs.min_x == rhs.min_x &&
+    lhs.min_y == rhs.min_y &&
+    lhs.max_x == rhs.max_x &&
+    lhs.max_y == rhs.max_y;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace nav_2d_msgs
+
+#endif // NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H

src/Libraries/nav_2d_utils/CMakeLists.txt

@@ -0,0 +1,146 @@
+cmake_minimum_required(VERSION 3.10)
+
+project(nav_2d_utils VERSION 1.0.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Enable Position Independent Code
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# Find dependencies
+find_package(console_bridge REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(xmlrpcpp QUIET)
+if(NOT xmlrpcpp_FOUND)
+    # Try alternative package name
+    find_package(XmlRpcCpp QUIET)
+endif()
+
+# Libraries
+add_library(conversions src/conversions.cpp)
+target_include_directories(conversions
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(conversions
+    PUBLIC
+        nav_2d_msgs
+        geometry_msgs
+        nav_msgs
+        nav_grid
+        nav_core2
+    PRIVATE
+        console_bridge::console_bridge
+        Boost::system
+        Boost::thread
+)
+
+add_library(path_ops src/path_ops.cpp)
+target_include_directories(path_ops
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(path_ops
+    PUBLIC
+        nav_2d_msgs
+        geometry_msgs
+)
+
+add_library(polygons src/polygons.cpp src/footprint.cpp)
+target_include_directories(polygons
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(polygons
+    PUBLIC
+        nav_2d_msgs
+        geometry_msgs
+    PRIVATE
+        ${xmlrpcpp_LIBRARIES}
+)
+
+if(xmlrpcpp_FOUND)
+    target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
+elseif(XmlRpcCpp_FOUND)
+    target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
+    target_link_libraries(polygons PRIVATE ${XmlRpcCpp_LIBRARIES})
+endif()
+
+add_library(bounds src/bounds.cpp)
+target_include_directories(bounds
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(bounds
+    PUBLIC
+        nav_grid
+        nav_core2
+)
+
+add_library(tf_help src/tf_help.cpp)
+target_include_directories(tf_help
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(tf_help
+    PUBLIC
+        nav_2d_msgs
+        geometry_msgs
+        nav_core2
+        tf3
+)
+
+# Create an INTERFACE library that represents all nav_2d_utils libraries
+add_library(nav_2d_utils INTERFACE)
+target_include_directories(nav_2d_utils
+    INTERFACE
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(nav_2d_utils
+    INTERFACE
+        conversions
+        path_ops
+        polygons
+        bounds
+        tf_help
+)
+
+# Install header files
+install(DIRECTORY include/nav_2d_utils 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+# Install mapbox headers
+install(DIRECTORY include/mapbox
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.hpp")
+
+# Install targets
+install(TARGETS nav_2d_utils conversions path_ops polygons bounds tf_help
+        EXPORT 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)
+
+# Print configuration info
+message(STATUS "=================================")
+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 "=================================")

src/Libraries/nav_2d_utils/README.md

@@ -0,0 +1,10 @@
+# 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`
+ * Parameters - a couple ROS parameter patterns
+ * PathOps - functions for working with `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.

src/Libraries/nav_2d_utils/doc/Conversions.md

@@ -0,0 +1,54 @@
+# 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` |
+| -- | -- |
+| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
+
+## Point Transformations
+| to `nav_2d_msgs` | from `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` |
+| -- | -- |
+| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
+||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`|
+||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
+| `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
+
+## Path Transformations
+| to `nav_2d_msgs` | from `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, ros::Time)`
+
+Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
+
+## Polygon Transformations
+| to `nav_2d_msgs` | from `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` |
+| -- | -- |
+|`nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(nav_2d_msgs::NavGridInfo)`|
+
+| to `nav_grid` info | from `nav_grid` info |
+| -- | -- |
+|`nav_grid::NavGridInfo infoToInfo(nav_msgs::MapMetaData, std::string)` | `nav_msgs::MapMetaData infoToInfo(nav_grid::NavGridInfo)`
+
+| to two dimensional pose | to three dimensional pose |
+| -- | -- |
+| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
+
+## Bounds Transformations
+| to `nav_2d_msgs` | from `nav_2d_msgs` |
+| -- | -- |
+|`nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(nav_2d_msgs::UIntBounds)`|

src/Libraries/nav_2d_utils/doc/PluginMux.md

@@ -0,0 +1,39 @@
+
+# Plugin Mux
+`PluginMux` is an organizer for switching between multiple different plugins of the same type. It may be easiest to see how it operates through an example. Let's say we have multiple global planners we would like to use at different times, with only one being active at a given time.
+
+This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `nav_core` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config.
+
+```
+global_planner_namespaces:
+  - boring_nav_fn
+  - wacky_global_planner
+boring_nav_fn:
+  allow_unknown: true
+  plugin_class: navfn/NavfnROS
+wacky_global_planner:
+  allow_unknown: false
+  # default value commented out
+  # plugin_class: global_planner/GlobalPlanner
+```
+
+The namespaces are arbitrary strings, and need not reflect the name of the planner. The package and class name for the plugin will be specified by the `plugin_class` parameter. By default, the first namespace will be loaded as the current plugin.
+
+To advertise which plugin is active, we publish the namespace on a latched topic and set a parameter with the same name (`~/current_global_planner`). We can then switch among them with a `SetString` ROS service call, or the `usePlugin` C++ method.
+
+This configuration is all created by creating a plugin mux with the following parameters (parameter names shown for convenience):
+```
+PluginMux(plugin_package = "nav_core",
+          plugin_class = "BaseGlobalPlanner",
+          parameter_name = "global_planner_namespaces",
+          default_value = "global_planner/GlobalPlanner",
+          ros_name = "current_global_planner",
+          switch_service_name = "switch_global_planner");
+```
+
+If the parameter is not set or is an empty list, a namespace will be derived from the `default_value` and be loaded as the only plugin available.
+```
+global_planner_namespaces: []
+GlobalPlanner:
+  allow_unknown: true
+```

src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md

@@ -0,0 +1,52 @@
+# 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);
+```
+
+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);
+```
+
+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);
+```
+
+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);
+```
+
+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,
+                                         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);
+```
+
+Polygons can be written to parameters with
+```
+void polygontoParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
+                     bool array_of_arrays = true);
+```
+
+## Polygon Operations
+There are also a handful of methods for examining/manipulating polygons
+ * `equals` - check if two polygons are equal
+ * `movePolygonToPose` - translate and rotate a polygon
+ * `isInside` - check if a point is inside a polygon
+ * `calculateMinAndMaxDistances` - Calculate the minimum and maximum distance from the origin of a polygon
+ * `triangulate` - Decompose a polygon into a set of non-overlapping triangles using an open source implementation of the [earcut algorithm](https://github.com/mapbox/earcut.hpp)

src/Libraries/nav_2d_utils/include/mapbox/LICENSE

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

src/Libraries/nav_2d_utils/include/mapbox/NOTES

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

src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp

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

src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h

@@ -0,0 +1,91 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_BOUNDS_H
+#define NAV_2D_UTILS_BOUNDS_H
+
+#include <nav_grid/nav_grid_info.h>
+#include <nav_core2/bounds.h>
+#include <vector>
+
+/**
+ * @brief A set of utility functions for Bounds objects interacting with other messages/types
+ */
+namespace nav_2d_utils
+{
+
+/**
+ * @brief return a floating point bounds that covers the entire NavGrid
+ * @param info Info for the NavGrid
+ * @return bounds covering the entire NavGrid
+ */
+nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info);
+
+/**
+ * @brief return an integral bounds that covers the entire NavGrid
+ * @param info Info for the NavGrid
+ * @return bounds covering the entire NavGrid
+ */
+nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info);
+
+/**
+ * @brief Translate real-valued bounds to uint coordinates based on nav_grid info
+ * @param info Information used when converting the coordinates
+ * @param bounds floating point bounds object
+ * @returns corresponding UIntBounds for parameter
+ */
+nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds);
+
+/**
+ * @brief Translate uint bounds to real-valued coordinates based on nav_grid info
+ * @param info Information used when converting the coordinates
+ * @param bounds UIntBounds object
+ * @returns corresponding floating point bounds for parameter
+ */
+nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds);
+
+/**
+ * @brief divide the given bounds up into sub-bounds of roughly equal size
+ * @param original_bounds The original bounds to divide
+ * @param n_cols Positive number of columns to divide the bounds into
+ * @param n_rows Positive number of rows to divide the bounds into
+ * @return vector of a maximum of n_cols*n_rows nonempty bounds
+ * @throws std::invalid_argument when n_cols or n_rows is zero
+ */
+std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds& original_bounds,
+                                                unsigned int n_cols, unsigned int n_rows);
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_BOUNDS_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h

@@ -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 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

src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h

@@ -0,0 +1,54 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_FOOTPRINT_H
+#define NAV_2D_UTILS_FOOTPRINT_H
+
+#include <yaml-cpp/yaml.h>
+#include <nav_2d_msgs/Polygon2D.h>
+
+namespace nav_2d_utils
+{
+
+/**
+ * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius
+ *
+ * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
+ * is present.
+ */
+nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_FOOTPRINT_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_GEOMETRY_HELP_H
+#define NAV_2D_UTILS_GEOMETRY_HELP_H
+
+#include <cmath>
+
+namespace nav_2d_utils
+{
+/**
+ * @brief Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1)
+ * @param pX
+ * @param pY
+ * @param x0
+ * @param y0
+ * @param x1
+ * @param y1
+ * @return shortest distance from point to line
+ */
+inline double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
+{
+  double A = pX - x0;
+  double B = pY - y0;
+  double C = x1 - x0;
+  double D = y1 - y0;
+
+  double dot = A * C + B * D;
+  double len_sq = C * C + D * D;
+  double param = dot / len_sq;
+
+  double xx, yy;
+
+  if (param < 0)
+  {
+    xx = x0;
+    yy = y0;
+  }
+  else if (param > 1)
+  {
+    xx = x1;
+    yy = y1;
+  }
+  else
+  {
+    xx = x0 + param * C;
+    yy = y0 + param * D;
+  }
+
+  return hypot(pX - xx, pY - yy);
+}
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_GEOMETRY_HELP_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h

@@ -0,0 +1,87 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_ODOM_SUBSCRIBER_H
+#define NAV_2D_UTILS_ODOM_SUBSCRIBER_H
+
+#include <nav_2d_utils/conversions.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_2d_msgs/Twist2DStamped.h>
+#include <boost/thread/mutex.hpp>
+#include <string>
+#include <iostream>
+#include <yaml-cpp/yaml.h>
+
+namespace nav_2d_utils
+{
+
+/**
+ * @class OdomSubscriber
+ * Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
+ */
+class OdomSubscriber
+{
+public:
+  /**
+   * @brief Constructor that subscribes to an Odometry topic
+   *
+   * @param nh NodeHandle for creating subscriber
+   * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
+   */
+  explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom")
+  {
+    std::string odom_topic;
+    // nh.param("odom_topic", odom_topic, default_topic);
+    // odom_sub_ = nh.subscribe<nav_msgs::Odometry>(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1));
+  }
+
+  inline nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; }
+  inline nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; }
+
+protected:
+  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+  {
+    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_;
+};
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_ODOM_SUBSCRIBER_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h

@@ -0,0 +1,149 @@
+/*
+ * 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 <string>
+
+namespace nav_2d_utils
+{
+
+/**
+ * @brief Search for a parameter and load it, or use the default value
+ *
+ * This templated function shortens a commonly used ROS pattern in which you
+ * search for a parameter and get its value if it exists, otherwise returning a default value.
+ *
+ * @param nh NodeHandle to start the parameter search from
+ * @param param_name Name of the parameter to search for
+ * @param default_value Value to return if not found
+ * @return Value of parameter if found, otherwise the default_value
+ */
+template<class param_t>
+param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value)
+{
+  std::string resolved_name;
+  // if (nh.searchParam(param_name, resolved_name))
+  // {
+  //   param_t value = default_value;
+  //   nh.param(resolved_name, value, default_value);
+  //   return value;
+  // }
+  return default_value;
+}
+
+/**
+ * @brief Load a parameter from one of two namespaces. Complain if it uses the old name.
+ * @param nh NodeHandle to look for the parameter in
+ * @param current_name Parameter name that is current, i.e. not deprecated
+ * @param old_name Deprecated parameter name
+ * @param default_value If neither parameter is present, return this value
+ * @return The value of the parameter or the default value
+ */
+template<class param_t>
+param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name,
+                                     const std::string old_name, const param_t& default_value)
+{
+  param_t value;
+  if (nh[current_name] && nh[current_name].IsDefined())
+  {
+    value = nh[current_name].as<param_t>();
+    return value;
+  }
+  if (nh[old_name] && nh[old_name].IsDefined())
+  {
+    std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
+    value = nh[old_name].as<param_t>();
+    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 YAML::Node& nh, const std::string current_name, const std::string old_name)
+{
+  if (!nh[old_name] || !nh[old_name].IsDefined()) return;
+
+  param_t value;
+  std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
+  value = nh[old_name].as<param_t>();
+  nh[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 YAML::Node& 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

src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h

@@ -0,0 +1,88 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_PATH_OPS_H
+#define NAV_2D_UTILS_PATH_OPS_H
+
+#include <nav_2d_msgs/Path2D.h>
+
+namespace nav_2d_utils
+{
+/**
+ * @brief Calculate the linear distance between two poses
+ */
+double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1);
+
+/**
+ * @brief Calculate the length of the plan, starting from the given index
+ */
+double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index = 0);
+
+/**
+ * @brief Calculate the length of the plan from the pose on the plan closest to the given pose
+ */
+double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose);
+
+/**
+ * @brief Increase plan resolution to match that of the costmap by adding points linearly between points
+ *
+ * @param global_plan_in input plan
+ * @param resolution desired distance between waypoints
+ * @return Higher resolution plan
+ */
+nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution);
+
+/**
+ * @brief Decrease the length of the plan by eliminating colinear points
+ *
+ * Uses the Ramer Douglas Peucker algorithm. Ignores theta values
+ *
+ * @param input_path Path to compress
+ * @param epsilon maximum allowable error. Increase for greater compression.
+ * @return Path2D with possibly fewer poses
+ */
+nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon = 0.1);
+
+/**
+ * @brief Convenience function to add a pose to a path in one line.
+ * @param path Path to add to
+ * @param x x-coordinate
+ * @param y y-coordinate
+ * @param theta theta (if needed)
+ */
+void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0);
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_PATH_OPS_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h

@@ -0,0 +1,274 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_PLUGIN_MUX_H
+#define NAV_2D_UTILS_PLUGIN_MUX_H
+
+#include <ros/ros.h>
+#include <pluginlib/class_loader.h>
+#include <std_msgs/String.h>
+#include <nav_2d_msgs/SwitchPlugin.h>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace nav_2d_utils
+{
+/**
+ * @class PluginMux
+ * @brief An organizer for switching between multiple different plugins of the same type
+ *
+ * The different plugins are specified using a list of strings on the parameter server, each of which is a namespace.
+ * The specific type and additional parameters for each plugin are specified on the parameter server in that namespace.
+ * All the plugins are loaded initially, but only one is the "current" plugin at any particular time, which is published
+ * on a latched topic AND stored on the ROS parameter server. You can switch which plugin is current either through a
+ * C++ or ROS interface.
+ */
+template<class T>
+class PluginMux
+{
+public:
+  /**
+   * @brief Main Constructor - Loads plugin(s) and sets up ROS interfaces
+   *
+   * @param plugin_package The package of the plugin type
+   * @param plugin_class The class name for the plugin type
+   * @param parameter_name Name of parameter for the namespaces.
+   * @param default_value If class name is not specified, which plugin should be loaded
+   * @param ros_name ROS name for setting up topic and parameter
+   * @param switch_service_name ROS name for setting up the ROS service
+   */
+  PluginMux(const std::string& plugin_package, const std::string& plugin_class,
+            const std::string& parameter_name, const std::string& default_value,
+            const std::string& ros_name = "current_plugin", const std::string& switch_service_name = "switch_plugin");
+
+  /**
+   * @brief Create an instance of the given plugin_class_name and save it with the given plugin_name
+   * @param plugin_name Namespace for the new plugin
+   * @param plugin_class_name Class type name for the new plugin
+   */
+  void addPlugin(const std::string& plugin_name, const std::string& plugin_class_name);
+
+  /**
+   * @brief C++ Interface for switching to a given plugin
+   *
+   * @param name Namespace to set current plugin to
+   * @return true if that namespace exists and is loaded properly
+   */
+  bool usePlugin(const std::string& name)
+  {
+    // If plugin with given name doesn't exist, return false
+    if (plugins_.count(name) == 0) return false;
+
+    if (switch_callback_)
+    {
+      switch_callback_(current_plugin_, name);
+    }
+
+    // Switch Mux
+    current_plugin_ = name;
+
+    // Update ROS info
+    std_msgs::String str_msg;
+    str_msg.data = current_plugin_;
+    current_plugin_pub_.publish(str_msg);
+    private_nh_.setParam(ros_name_, current_plugin_);
+
+    return true;
+  }
+
+  /**
+   * @brief Get the Current Plugin Name
+   * @return Name of the current plugin
+   */
+  std::string getCurrentPluginName() const
+  {
+    return current_plugin_;
+  }
+
+  /**
+   * @brief Check to see if a plugin exists
+   * @param name Namespace to set current plugin to
+   * @return True if the plugin exists
+   */
+  bool hasPlugin(const std::string& name) const
+  {
+    return plugins_.find(name) != plugins_.end();
+  }
+
+  /**
+   * @brief Get the Specified Plugin
+   * @param name Name of plugin to get
+   * @return Reference to specified plugin
+   */
+  T& getPlugin(const std::string& name)
+  {
+    if (!hasPlugin(name))
+      throw std::invalid_argument("Plugin not found");
+    return *plugins_[name];
+  }
+
+  /**
+   * @brief Get the Current Plugin
+   * @return Reference to current plugin
+   */
+  T& getCurrentPlugin()
+  {
+    return getPlugin(current_plugin_);
+  }
+
+  /**
+   * @brief Get the current list of plugin names
+   */
+  std::vector<std::string> getPluginNames() const
+  {
+    std::vector<std::string> names;
+    for (auto& kv : plugins_)
+    {
+      names.push_back(kv.first);
+    }
+    return names;
+  }
+
+  /**
+   * @brief alias for the function-type of the callback fired when the plugin switches.
+   *
+   * The first parameter will be the namespace of the plugin that was previously used.
+   * The second parameter will be the namespace of the plugin that is being switched to.
+   */
+  using SwitchCallback = std::function<void(const std::string&, const std::string&)>;
+
+  /**
+   * @brief Set the callback fired when the plugin switches
+   *
+   * In addition to switching which plugin is being used via directly calling `usePlugin`
+   * a switch can also be triggered externally via ROS service. In such a case, other classes
+   * may want to know when this happens. This is accomplished using the SwitchCallback, which
+   * will be called regardless of how the plugin is switched.
+   */
+  void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; }
+
+protected:
+  /**
+   * @brief ROS Interface for Switching Plugins
+   */
+  bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp)
+  {
+    std::string name = req.new_plugin;
+    if (usePlugin(name))
+    {
+      resp.success = true;
+      resp.message = "Loaded plugin namespace " + name + ".";
+    }
+    else
+    {
+      resp.success = false;
+      resp.message = "Namespace " + name + " not configured!";
+    }
+    return true;
+  }
+
+  // Plugin Management
+  pluginlib::ClassLoader<T> plugin_loader_;
+  std::map<std::string, boost::shared_ptr<T>> plugins_;
+  std::string current_plugin_;
+
+  // ROS Interface
+  ros::ServiceServer switch_plugin_srv_;
+  ros::Publisher current_plugin_pub_;
+  YAML::Node 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

src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h

@@ -0,0 +1,199 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_POLYGONS_H
+#define NAV_2D_UTILS_POLYGONS_H
+
+#include <nav_2d_msgs/Polygon2D.h>
+#include <nav_2d_msgs/ComplexPolygon2D.h>
+#include <geometry_msgs/Pose2D.h>
+#include <xmlrpcpp/XmlRpcValue.h>
+#include <vector>
+#include <string>
+
+namespace nav_2d_utils
+{
+
+/**
+ * @class PolygonParseException
+ * @brief Exception to throw when Polygon doesn't load correctly
+ */
+class PolygonParseException: public std::runtime_error
+{
+public:
+  explicit PolygonParseException(const std::string& description) : std::runtime_error(description) {}
+};
+
+/**
+ * @brief Parse a vector of vectors of doubles from a string.
+ * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
+ *
+ * @param input The string to parse
+ * @return a vector of vectors of doubles
+ */
+std::vector<std::vector<double> > parseVVD(const std::string& input);
+
+/**
+ * @brief Create a "circular" polygon from a given radius
+ *
+ * @param radius Radius of the polygon
+ * @param num_points Number of points in the resulting polygon
+ * @return A rotationally symmetric polygon with the specified number of vertices
+ */
+nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16);
+
+/**
+ * @brief Make a polygon from the given string.
+ * Format should be bracketed array of arrays of doubles, like so: [[1.0, 2.2], [3.3, 4.2], ...]
+ *
+ * @param polygon_string The string to parse
+ * @return Polygon2D
+ */
+nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
+
+// /**
+//  * @brief Load a polygon from a parameter, whether it is a string or array, or two arrays
+//  * @param nh Node handle to load parameter from
+//  * @param parameter_name Name of the parameter
+//  * @param search Whether to search up the namespace for the parameter name
+//  * @return Loaded polygon
+//  */
+// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
+//                                          bool search = true);
+
+/**
+ * @brief Create a polygon from the given XmlRpcValue.
+ *
+ * @param polygon_xmlrpc should be an array of arrays, where the top-level array should have
+ *                       3 or more elements, and the sub-arrays should all have exactly 2 elements
+ *                       (x and y coordinates).
+ */
+nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
+
+/**
+ * @brief Create XMLRPC Value for writing the polygon to the parameter server
+ * @param polygon Polygon to convert
+ * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
+ * @return XmlRpcValue
+ */
+XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
+
+// /**
+//  * @brief Save a polygon to a parameter
+//  * @param polygon The Polygon
+//  * @param nh Node handle to save the parameter to
+//  * @param parameter_name Name of the parameter
+//  * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
+//  */
+// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
+//                      bool array_of_arrays = true);
+
+/**
+ * @brief Create a polygon from two parallel arrays
+ *
+ * @param xs Array of doubles representing x coordinates, at least three elements long
+ * @param ys Array of doubles representing y coordinates, matching length of xs
+ */
+nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
+
+/**
+ * @brief Create two parallel arrays from a polygon
+ *
+ * @param[in] polygon
+ * @param[out] xs Array of doubles representing x coordinates, to be populated
+ * @param[out] ys Array of doubles representing y coordinates, to be populated
+ */
+void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys);
+
+/**
+ * @brief check if two polygons are equal. Used in testing
+ */
+bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1);
+
+/**
+ * @brief Translate and rotate a polygon to a new pose
+ * @param polygon The polygon
+ * @param pose The x, y and theta to use when moving the polygon
+ * @return A new moved polygon
+ */
+nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
+                                         const geometry_msgs::Pose2D& pose);
+
+/**
+ * @brief Check if a given point is inside of a polygon
+ *
+ * Borders are considered outside.
+ *
+ * @param polygon Polygon to check
+ * @param x x coordinate
+ * @param y y coordinate
+ * @return true if point is inside polygon
+ */
+bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y);
+
+/**
+ * @brief Calculate the minimum and maximum distance from (0, 0) to any point on the polygon
+ * @param[in] polygon polygon to analyze
+ * @param[out] min_dist
+ * @param[out] max_dist
+ */
+void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist);
+
+/**
+ * @brief Decompose a complex polygon into a set of triangles.
+ *
+ * See https://en.wikipedia.org/wiki/Polygon_triangulation
+ *
+ * Implementation from https://github.com/mapbox/earcut.hpp
+ *
+ * @param polygon The complex polygon to deconstruct
+ * @return A vector of points where each set of three points represents a triangle
+ */
+std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon);
+
+/**
+ * @brief Decompose a simple polygon into a set of triangles.
+ *
+ * See https://en.wikipedia.org/wiki/Polygon_triangulation
+ *
+ * Implementation from https://github.com/mapbox/earcut.hpp
+ *
+ * @param polygon The polygon to deconstruct
+ * @return A vector of points where each set of three points represents a triangle
+ */
+std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::Polygon2D& polygon);
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_POLYGONS_H

src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h

@@ -0,0 +1,91 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV_2D_UTILS_TF_HELP_H
+#define NAV_2D_UTILS_TF_HELP_H
+
+#include <nav_core2/common.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+#include <string>
+
+namespace nav_2d_utils
+{
+/**
+ * @brief Transform a PoseStamped from one frame to another while catching exceptions
+ *
+ * Also returns immediately if the frames are equal.
+ * @param tf Smart pointer to TFListener
+ * @param frame Frame to transform the pose into
+ * @param in_pose Pose to transform
+ * @param out_pose Place to store the resulting transformed pose
+ * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
+ * @return True if successful transform
+ */
+ bool transformPose(const TFListenerPtr tf, const std::string frame,
+                   const geometry_msgs::PoseStamped& in_pose, geometry_msgs::PoseStamped& out_pose,
+                   const bool extrapolation_fallback = true);
+
+/**
+ * @brief Transform a Pose2DStamped from one frame to another while catching exceptions
+ *
+ * Also returns immediately if the frames are equal.
+ * @param tf Smart pointer to TFListener
+ * @param frame Frame to transform the pose into
+ * @param in_pose Pose to transform
+ * @param out_pose Place to store the resulting transformed pose
+ * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
+ * @return True if successful transform
+ */
+bool transformPose(const TFListenerPtr tf, const std::string frame,
+                   const nav_2d_msgs::Pose2DStamped& in_pose, nav_2d_msgs::Pose2DStamped& out_pose,
+                   const bool extrapolation_fallback = true);
+
+/**
+ * @brief Transform a Pose2DStamped into another frame
+ *
+ * Note that this returns a transformed pose
+ * regardless of whether the transform was successfully performed.
+ *
+ * @param tf Smart pointer to TFListener
+ * @param pose Pose to transform
+ * @param frame_id Frame to transform the pose into
+ * @return The resulting transformed pose
+ */
+geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped& pose,
+                                           const std::string& frame_id);
+
+}  // namespace nav_2d_utils
+
+#endif  // NAV_2D_UTILS_TF_HELP_H

src/Libraries/nav_2d_utils/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>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
+    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
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/nav_2d_utils</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Libraries/nav_2d_utils/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+package_info = generate_distutils_setup(
+    packages=['nav_2d_utils'],
+    package_dir={'': 'src'}
+)
+
+setup(**package_info)

src/Libraries/nav_2d_utils/src/bounds.cpp

@@ -0,0 +1,102 @@
+/*
+ * 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

src/Libraries/nav_2d_utils/src/conversions.cpp

@@ -0,0 +1,339 @@
+/*
+ * 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 ros::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 ros::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

src/Libraries/nav_2d_utils/src/footprint.cpp

@@ -0,0 +1,67 @@
+/*
+ * 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.
+ */
+
+#include <nav_2d_utils/footprint.h>
+#include <nav_2d_utils/polygons.h>
+#include <string>
+
+namespace nav_2d_utils
+{
+nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write)
+{
+  std::string full_param_name;
+  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;
+}
+
+}  // namespace nav_2d_utils

src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py

@@ -0,0 +1,131 @@
+from geometry_msgs.msg import Pose, Pose2D, Quaternion, Point
+from nav_2d_msgs.msg import Twist2D, Path2D, Pose2DStamped, Point2D
+from nav_msgs.msg import Path
+from geometry_msgs.msg import Twist, PoseStamped
+
+try:
+    from tf.transformations import euler_from_quaternion, quaternion_from_euler
+
+    def get_yaw(orientation):
+        rpy = euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w))
+        return rpy[2]
+
+    def from_yaw(yaw):
+        q = Quaternion()
+        q.x, q.y, q.z, q.w = quaternion_from_euler(0, 0, yaw)
+        return q
+except ImportError:
+    from math import sin, cos, atan2
+    # From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+    def from_yaw(yaw):
+        q = Quaternion()
+        q.z = sin(yaw * 0.5)
+        q.w = cos(yaw * 0.5)
+        return q
+
+    def get_yaw(q):
+        siny_cosp = +2.0 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        return atan2(siny_cosp, cosy_cosp)
+
+
+def twist2Dto3D(cmd_vel_2d):
+    cmd_vel = Twist()
+    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
+
+
+def twist3Dto2D(cmd_vel):
+    cmd_vel_2d = Twist2D()
+    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
+
+
+def pointToPoint3D(point_2d):
+    point = Point()
+    point.x = point_2d.x
+    point.y = point_2d.y
+    return point
+
+
+def pointToPoint2D(point):
+    point_2d = Point2D()
+    point_2d.x = point.x
+    point_2d.y = point.y
+    return point_2d
+
+
+def poseToPose2D(pose):
+    pose2d = Pose2D()
+    pose2d.x = pose.position.x
+    pose2d.y = pose.position.y
+    pose2d.theta = get_yaw(pose.orientation)
+    return pose2d
+
+
+def poseStampedToPose2DStamped(pose):
+    pose2d = Pose2DStamped()
+    pose2d.header = pose.header
+    pose2d.pose = poseToPose2D(pose.pose)
+    return pose2d
+
+
+def poseToPose2DStamped(pose, frame, stamp):
+    pose2d = Pose2DStamped()
+    pose2d.header.frame_id = frame
+    pose2d.header.stamp = stamp
+    pose2d.pose = poseToPose2D(pose)
+    return pose2d
+
+
+def pose2DToPose(pose2d):
+    pose = Pose()
+    pose.position.x = pose2d.x
+    pose.position.y = pose2d.y
+    pose.orientation = from_yaw(pose2d.theta)
+    return pose
+
+
+def pose2DStampedToPoseStamped(pose2d):
+    pose = PoseStamped()
+    pose.header = pose2d.header
+    pose.pose = pose2DToPose(pose2d.pose)
+    return pose
+
+
+def pose2DToPoseStamped(pose2d, frame, stamp):
+    pose = PoseStamped()
+    pose.header.frame_id = frame
+    pose.header.stamp = stamp
+    pose.pose.position.x = pose2d.x
+    pose.pose.position.y = pose2d.y
+    pose.pose.orientation = from_yaw(pose2d.theta)
+    return pose
+
+
+def pathToPath2D(path):
+    path2d = Path2D()
+    if len(path.poses) == 0:
+        return path
+    path2d.header.frame_id = path.poses[0].header.frame_id
+    path2d.header.stamp = path.poses[0].header.stamp
+    for pose in path.poses:
+        stamped = poseStampedToPose2DStamped(pose)
+        path2d.poses.append(stamped.pose)
+    return path2d
+
+
+def path2DToPath(path2d):
+    path = Path()
+    path.header = path2d.header
+    for pose2d in path2d.poses:
+        pose = PoseStamped()
+        pose.header = path2d.header
+        pose.pose = pose2DToPose(pose2d)
+        path.poses.append(pose)
+    return path

src/Libraries/nav_2d_utils/src/path_ops.cpp

@@ -0,0 +1,193 @@
+/*
+ * 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

src/Libraries/nav_2d_utils/src/polygons.cpp

@@ -0,0 +1,502 @@
+/*
+ * 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.
+ */
+
+#include <nav_2d_utils/polygons.h>
+#include <nav_2d_utils/geometry_help.h>
+#include <mapbox/earcut.hpp>
+#include <algorithm>
+#include <limits>
+#include <string>
+#include <vector>
+
+namespace nav_2d_utils
+{
+
+using nav_2d_msgs::Point2D;
+using nav_2d_msgs::Polygon2D;
+
+std::vector<std::vector<double> > parseVVD(const std::string& input)
+{
+  std::vector<std::vector<double> > result;
+
+  std::stringstream input_ss(input);
+  int depth = 0;
+  std::vector<double> current_vector;
+  while (input_ss.good())
+  {
+    switch (input_ss.peek())
+    {
+    case EOF:
+      break;
+    case '[':
+      depth++;
+      if (depth > 2)
+      {
+        throw PolygonParseException("Array depth greater than 2");
+      }
+      input_ss.get();
+      current_vector.clear();
+      break;
+    case ']':
+      depth--;
+      if (depth < 0)
+      {
+        throw PolygonParseException("More close ] than open [");
+      }
+      input_ss.get();
+      if (depth == 1)
+      {
+        result.push_back(current_vector);
+      }
+      break;
+    case ',':
+    case ' ':
+    case '\t':
+      input_ss.get();
+      break;
+    default:  // All other characters should be part of the numbers.
+      if (depth != 2)
+      {
+        std::stringstream err_ss;
+        err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
+        throw PolygonParseException(err_ss.str());
+      }
+      double value;
+      if (input_ss >> value)
+      {
+        current_vector.push_back(value);
+      }
+      break;
+    }
+  }
+
+  if (depth != 0)
+  {
+    throw PolygonParseException("Unterminated vector string.");
+  }
+
+  return result;
+}
+
+Polygon2D polygonFromRadius(const double radius, const unsigned int num_points)
+{
+  Polygon2D polygon;
+  Point2D pt;
+  for (unsigned int i = 0; i < num_points; ++i)
+  {
+    double angle = i * 2 * M_PI / num_points;
+    pt.x = cos(angle) * radius;
+    pt.y = sin(angle) * radius;
+    polygon.points.push_back(pt);
+  }
+
+  return polygon;
+}
+
+Polygon2D polygonFromString(const std::string& polygon_string)
+{
+  Polygon2D polygon;
+  // Will throw PolygonParseException if problematic
+  std::vector<std::vector<double> > vvd = parseVVD(polygon_string);
+
+  // convert vvd into points.
+  if (vvd.size() < 3)
+  {
+    throw PolygonParseException("You must specify at least three points for the polygon.");
+  }
+
+  polygon.points.resize(vvd.size());
+  for (unsigned int i = 0; i < vvd.size(); i++)
+  {
+    if (vvd[ i ].size() != 2)
+    {
+      std::stringstream err_ss;
+      err_ss << "Points in the polygon specification must be pairs of numbers. Point index " << i << " had ";
+      err_ss << int(vvd[ i ].size()) << " numbers.";
+      throw PolygonParseException(err_ss.str());
+    }
+
+    polygon.points[i].x = vvd[i][0];
+    polygon.points[i].y = vvd[i][1];
+  }
+
+  return polygon;
+}
+
+
+/**
+ * @brief Helper function. Convert value to double
+ */
+double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value)
+{
+  if (value.getType() == XmlRpc::XmlRpcValue::TypeInt)
+  {
+    return static_cast<double>(static_cast<int>(value));
+  }
+  else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+  {
+    return static_cast<double>(value);
+  }
+
+  std::stringstream err_ss;
+  err_ss << "Values in the polygon specification must be numbers. Found value: " << value.toXml();
+  throw PolygonParseException(err_ss.str());
+}
+
+/**
+ * @brief Helper function. Convert value to double array
+ */
+std::vector<double> getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue& value)
+{
+  if (value.getType() != XmlRpc::XmlRpcValue::TypeArray)
+  {
+    throw PolygonParseException("Subarray must have type list.");
+  }
+  std::vector<double> array;
+  for (int i = 0; i < value.size(); i++)
+  {
+    array.push_back(getNumberFromXMLRPC(value[i]));
+  }
+  return array;
+}
+
+Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
+{
+  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
+      polygon_xmlrpc != "" && polygon_xmlrpc != "[]")
+  {
+    return polygonFromString(std::string(polygon_xmlrpc));
+  }
+
+  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeStruct)
+  {
+    if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y"))
+    {
+      throw PolygonParseException("Dict-like Polygon must specify members x and y.");
+    }
+    std::vector<double> xs = getNumberVectorFromXMLRPC(polygon_xmlrpc["x"]);
+    std::vector<double> ys = getNumberVectorFromXMLRPC(polygon_xmlrpc["y"]);
+    return polygonFromParallelArrays(xs, ys);
+  }
+
+  // Make sure we have an array of at least 3 elements.
+  if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray)
+  {
+    std::stringstream err_ss;
+    err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType()
+           << " instead.";
+    throw PolygonParseException(err_ss.str());
+  }
+  else if (polygon_xmlrpc.size() < 3)
+  {
+    throw PolygonParseException("You must specify at least three points for the polygon.");
+  }
+
+  Polygon2D polygon;
+  Point2D pt;
+  for (int i = 0; i < polygon_xmlrpc.size(); ++i)
+  {
+    // Make sure each element of the list is an array of size 2. (x and y coordinates)
+    XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
+    if (point_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
+    {
+      std::stringstream err_ss;
+      err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead.";
+      throw PolygonParseException(err_ss.str());
+    }
+    else if (point_xml.size() != 2)
+    {
+      throw PolygonParseException("Each point must have two numbers (x and y).");
+    }
+
+    pt.x = getNumberFromXMLRPC(point_xml[0]);
+    pt.y = getNumberFromXMLRPC(point_xml[1]);
+    polygon.points.push_back(pt);
+  }
+  return polygon;
+}
+
+// Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, bool search)
+// {
+//   std::string full_param_name;
+//   if (search)
+//   {
+//     nh.searchParam(parameter_name, full_param_name);
+//   }
+//   else
+//   {
+//     full_param_name = parameter_name;
+//   }
+
+//   if (!nh.hasParam(full_param_name))
+//   {
+//     std::stringstream err_ss;
+//     err_ss << "Parameter " << parameter_name << "(" + nh.resolveName(parameter_name) << ") not found.";
+//     throw PolygonParseException(err_ss.str());
+//   }
+//   XmlRpc::XmlRpcValue polygon_xmlrpc;
+//   nh.getParam(full_param_name, polygon_xmlrpc);
+//   return polygonFromXMLRPC(polygon_xmlrpc);
+// }
+
+/**
+ * @brief Helper method to convert a vector of doubles
+ */
+XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
+{
+  XmlRpc::XmlRpcValue xml;
+  xml.setSize(array.size());
+  for (unsigned int i = 0; i < array.size(); ++i)
+  {
+    xml[i] = array[i];
+  }
+  return xml;
+}
+
+XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
+{
+  XmlRpc::XmlRpcValue xml;
+  if (array_of_arrays)
+  {
+    xml.setSize(polygon.points.size());
+    for (unsigned int i = 0; i < polygon.points.size(); ++i)
+    {
+      xml[i].setSize(2);
+      const Point2D& p = polygon.points[i];
+      xml[i][0] = p.x;
+      xml[i][1] = p.y;
+    }
+  }
+  else
+  {
+    std::vector<double> xs, ys;
+    polygonToParallelArrays(polygon, xs, ys);
+    xml["x"] = vectorToXMLRPC(xs);
+    xml["y"] = vectorToXMLRPC(ys);
+  }
+  return xml;
+}
+
+// void polygonToParams(const 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)
+{
+  if (xs.size() < 3)
+  {
+    throw PolygonParseException("You must specify at least three points for the polygon.");
+  }
+  else if (xs.size() != ys.size())
+  {
+    throw PolygonParseException("Length of x array does not match length of y array.");
+  }
+
+  Polygon2D polygon;
+  polygon.points.resize(xs.size());
+  for (unsigned int i = 0; i < xs.size(); i++)
+  {
+    Point2D& pt = polygon.points[i];
+    pt.x = xs[i];
+    pt.y = ys[i];
+  }
+  return polygon;
+}
+
+void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector<double>& xs, std::vector<double>& ys)
+{
+  xs.clear();
+  ys.clear();
+  for (const Point2D& pt : polygon.points)
+  {
+    xs.push_back(pt.x);
+    ys.push_back(pt.y);
+  }
+}
+
+bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1)
+{
+  if (polygon0.points.size() != polygon1.points.size())
+  {
+    return false;
+  }
+  for (unsigned int i=0; i < polygon0.points.size(); i++)
+  {
+    if (polygon0.points[i].x != polygon1.points[i].x ||polygon0.points[i].y != polygon1.points[i].y)
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
+nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon,
+                                         const geometry_msgs::Pose2D& pose)
+{
+  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];
+    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)
+{
+  // 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
+  int n = polygon.points.size();
+  int cross = 0;
+  // Loop from i = [0 ... n - 1] and j = [n - 1, 0 ... n - 2]
+  // Ensures first point connects to last point
+  for (int i = 0, j = n - 1; i < n; j = i++)
+  {
+    // Check if the line to x,y crosses this edge
+    if ( ((polygon.points[i].y > y) != (polygon.points[j].y > y))
+           && (x < (polygon.points[j].x - polygon.points[i].x) * (y - polygon.points[i].y) /
+            (polygon.points[j].y - polygon.points[i].y) + polygon.points[i].x) )
+    {
+      cross++;
+    }
+  }
+  // Return true if the number of crossings is odd
+  return cross % 2 > 0;
+}
+
+void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist)
+{
+  min_dist = std::numeric_limits<double>::max();
+  max_dist = 0.0;
+  auto& points = polygon.points;
+  if (points.size() == 0)
+  {
+    return;
+  }
+
+  for (unsigned int i = 0; i < points.size() - 1; ++i)
+  {
+    // check the distance from the robot center point to the first vertex
+    double vertex_dist = hypot(points[i].x, points[i].y);
+    double edge_dist = distanceToLine(0.0, 0.0, points[i].x, points[i].y,
+                                      points[i + 1].x, points[i + 1].y);
+    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+  }
+
+  // we also need to do the last vertex and the first vertex
+  double vertex_dist = hypot(points.back().x, points.back().y);
+  double edge_dist = distanceToLine(0.0, 0.0, points.back().x, points.back().y,
+                                      points.front().x, points.front().y);
+  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+}
+}  // namespace nav_2d_utils
+
+// Adapt Point2D for use with the triangulation library
+namespace mapbox
+{
+namespace util
+{
+template <>
+struct nth<0, nav_2d_msgs::Point2D>
+{
+  inline static double get(const nav_2d_msgs::Point2D& point)
+  {
+    return point.x;
+  };
+};
+
+template <>
+struct nth<1, nav_2d_msgs::Point2D>
+{
+  inline static double get(const nav_2d_msgs::Point2D& point)
+  {
+    return point.y;
+  };
+};
+
+}  // namespace util
+}  // namespace mapbox
+
+
+namespace nav_2d_utils
+{
+std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon)
+{
+  // Compute the triangulation
+  std::vector<std::vector<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)
+  {
+    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;
+  for (const auto& ring : rings)
+  {
+    for (const nav_2d_msgs::Point2D& point : ring)
+    {
+      points.push_back(point);
+    }
+  }
+
+  // Construct the output triangles
+  std::vector<nav_2d_msgs::Point2D> result;
+  result.reserve(indices.size());
+  for (unsigned int index : indices)
+  {
+    result.push_back(points[index]);
+  }
+  return result;
+}
+
+std::vector<nav_2d_msgs::Point2D> triangulate(const nav_2d_msgs::Polygon2D& polygon)
+{
+  nav_2d_msgs::ComplexPolygon2D complex;
+  complex.outer = polygon;
+  return triangulate(complex);
+}
+
+
+}  // namespace nav_2d_utils

src/Libraries/nav_2d_utils/src/tf_help.cpp

@@ -0,0 +1,100 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *                2022, Metro Robots
+ *  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/tf_help.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+#include <nav_2d_utils/conversions.h>
+// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <string>
+
+namespace nav_2d_utils
+{
+  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)
+    // {
+    //   out_pose = in_pose;
+    //   return true;
+    // }
+
+    // try
+    // {
+    //   tf->transform(in_pose, out_pose, frame);
+    //   return true;
+    // }
+    // catch (tf::ExtrapolationException &ex)
+    // {
+    //   if (!extrapolation_fallback)
+    //     throw;
+    //   geometry_msgs::PoseStamped latest_in_pose;
+    //   latest_in_pose.header.frame_id = in_pose.header.frame_id;
+    //   latest_in_pose.pose = in_pose.pose;
+    //   tf->transform(latest_in_pose, out_pose, frame);
+    //   return true;
+    // }
+    // catch (tf::TransformException &ex)
+    // {
+    //   ROS_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,
+                     const bool extrapolation_fallback)
+  {
+    geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
+    geometry_msgs::PoseStamped out_3d_pose;
+
+    bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, extrapolation_fallback);
+    if (ret)
+    {
+      out_pose = poseStampedToPose2D(out_3d_pose);
+    }
+    return ret;
+  }
+
+  geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const 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);
+    return local_pose.pose;
+  }
+
+} // namespace nav_2d_utils

src/Libraries/nav_2d_utils/test/bounds_test.cpp

@@ -0,0 +1,199 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <nav_2d_utils/bounds.h>
+#include <nav_grid/vector_nav_grid.h>
+#include <vector>
+
+using nav_2d_utils::divideBounds;
+using nav_core2::UIntBounds;
+
+/**
+ * @brief Count the values in a grid.
+ * @param[in] The grid
+ * @param[out] match Number of values == 1
+ * @param[out] missed Number of values == 0
+ * @param[out] multiple Number of other values
+ */
+void countValues(const nav_grid::VectorNavGrid<unsigned char>& grid,
+                 unsigned int& match, unsigned int& missed, unsigned int& multiple)
+{
+  match = 0;
+  missed = 0;
+  multiple = 0;
+
+  nav_grid::NavGridInfo info = grid.getInfo();
+
+  // No iterator to avoid tricky depenencies
+  for (unsigned int x = 0; x < info.width; x++)
+  {
+    for (unsigned int y = 0; y < info.height; y++)
+    {
+      switch (grid(x, y))
+      {
+      case 0:
+        missed++;
+        break;
+      case 1:
+        match++;
+        break;
+      default:
+        multiple++;
+        break;
+      }
+    }
+  }
+}
+
+TEST(DivideBounds, zeroes)
+{
+  UIntBounds bounds(2, 2, 5, 5);
+  // Number of rows/cols has to be positive
+  EXPECT_THROW(divideBounds(bounds, 0, 2), std::invalid_argument);
+  EXPECT_THROW(divideBounds(bounds, 2, 0), std::invalid_argument);
+  EXPECT_THROW(divideBounds(bounds, 0, 0), std::invalid_argument);
+  EXPECT_NO_THROW(divideBounds(bounds, 2, 2));
+
+  bounds.reset();
+  // check for errors with empty bounds
+  EXPECT_NO_THROW(divideBounds(bounds, 2, 2));
+}
+
+/**
+ * This test is for the divideBounds method and takes grids of various sizes
+ * (cycled through with the outer two loops) and tries to divide them into subgrids of
+ * various sizes (cycled through with the next two loops). The resulting vector of
+ * bounds should cover every cell in the original grid, so each of the divided bounds is
+ * iterated over, adding one to each grid cell. If everything works perfectly, each cell
+ * should be touched exactly once.
+ */
+TEST(DivideBounds, iterative_tests)
+{
+  nav_grid::VectorNavGrid<unsigned char> full_grid;
+  nav_grid::NavGridInfo info;
+
+  // count variables
+  unsigned int match, missed, multiple;
+
+  for (info.width = 1; info.width < 15; info.width++)
+  {
+    for (info.height = 1; info.height < 15; info.height++)
+    {
+      full_grid.setInfo(info);
+      UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
+      for (unsigned int rows = 1; rows < 11u; rows++)
+      {
+        for (unsigned int cols = 1; cols < 11u; cols++)
+        {
+          full_grid.reset();
+          std::vector<UIntBounds> divided = divideBounds(full_bounds, cols, rows);
+          ASSERT_LE(divided.size(), rows * cols) << info.width << "x" << info.height << " " << rows << "x" << cols;
+          for (const UIntBounds& sub : divided)
+          {
+            EXPECT_FALSE(sub.isEmpty());
+            // Can't use nav_grid_iterator for circular dependencies
+            for (unsigned int x = sub.getMinX(); x <= sub.getMaxX(); x++)
+            {
+              for (unsigned int y = sub.getMinY(); y <= sub.getMaxY(); y++)
+              {
+                full_grid.setValue(x, y, full_grid(x, y) + 1);
+              }
+            }
+          }
+
+          countValues(full_grid, match, missed, multiple);
+          ASSERT_EQ(match, info.width * info.height) << "Full grid: " << info.width << "x" << info.height
+                                                     << "  Requested divisions: " << rows << "x" << cols;
+          EXPECT_EQ(missed, 0u);
+          EXPECT_EQ(multiple, 0u);
+        }
+      }
+    }
+  }
+}
+
+/**
+ * This test is for the divideBounds method and calls it recursively to
+ * ensure that the method works when the minimum values in the original bounds
+ * are not zero.
+ */
+TEST(DivideBounds, recursive_tests)
+{
+  nav_grid::VectorNavGrid<unsigned char> full_grid;
+  nav_grid::NavGridInfo info;
+  info.width = 100;
+  info.height = 100;
+  full_grid.setInfo(info);
+
+  UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
+
+  std::vector<UIntBounds> level_one = divideBounds(full_bounds, 2, 2);
+  ASSERT_EQ(level_one.size(), 4u);
+  for (const UIntBounds& sub : level_one)
+  {
+    std::vector<UIntBounds> level_two = divideBounds(sub, 2, 2);
+    ASSERT_EQ(level_two.size(), 4u);
+    for (const UIntBounds& subsub : level_two)
+    {
+      EXPECT_GE(subsub.getMinX(), sub.getMinX());
+      EXPECT_LE(subsub.getMaxX(), sub.getMaxX());
+      EXPECT_GE(subsub.getMinY(), sub.getMinY());
+      EXPECT_LE(subsub.getMaxY(), sub.getMaxY());
+      // Can't use nav_grid_iterator for circular dependencies
+      for (unsigned int x = subsub.getMinX(); x <= subsub.getMaxX(); x++)
+      {
+        for (unsigned int y = subsub.getMinY(); y <= subsub.getMaxY(); y++)
+        {
+          full_grid.setValue(x, y, full_grid(x, y) + 1);
+        }
+      }
+    }
+  }
+
+  // Count values
+  unsigned int match = 0,
+               missed = 0,
+               multiple = 0;
+  countValues(full_grid, match, missed, multiple);
+  ASSERT_EQ(match, info.width * info.height);
+  EXPECT_EQ(missed, 0u);
+  EXPECT_EQ(multiple, 0u);
+}
+
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/Libraries/nav_2d_utils/test/compress_test.cpp

@@ -0,0 +1,97 @@
+/*
+ * 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.
+ */
+#include <gtest/gtest.h>
+#include <nav_2d_utils/path_ops.h>
+
+using nav_2d_utils::compressPlan;
+using nav_2d_utils::addPose;
+
+TEST(CompressTest, compress_test)
+{
+  nav_2d_msgs::Path2D path;
+  // Dataset borrowed from https://karthaus.nl/rdp/
+  addPose(path, 24, 173);
+  addPose(path, 26, 170);
+  addPose(path, 24, 166);
+  addPose(path, 27, 162);
+  addPose(path, 37, 161);
+  addPose(path, 45, 157);
+  addPose(path, 48, 152);
+  addPose(path, 46, 143);
+  addPose(path, 40, 140);
+  addPose(path, 34, 137);
+  addPose(path, 26, 134);
+  addPose(path, 24, 130);
+  addPose(path, 24, 125);
+  addPose(path, 28, 121);
+  addPose(path, 36, 118);
+  addPose(path, 46, 117);
+  addPose(path, 63, 121);
+  addPose(path, 76, 125);
+  addPose(path, 82, 120);
+  addPose(path, 86, 111);
+  addPose(path, 88, 103);
+  addPose(path, 90, 91);
+  addPose(path, 95, 87);
+  addPose(path, 107, 89);
+  addPose(path, 107, 104);
+  addPose(path, 106, 117);
+  addPose(path, 109, 129);
+  addPose(path, 119, 131);
+  addPose(path, 131, 131);
+  addPose(path, 139, 134);
+  addPose(path, 138, 143);
+  addPose(path, 131, 152);
+  addPose(path, 119, 154);
+  addPose(path, 111, 149);
+  addPose(path, 105, 143);
+  addPose(path, 91, 139);
+  addPose(path, 80, 142);
+  addPose(path, 81, 152);
+  addPose(path, 76, 163);
+  addPose(path, 67, 161);
+  addPose(path, 59, 149);
+  addPose(path, 63, 138);
+
+  EXPECT_EQ(41U, compressPlan(path, 0.1).poses.size());
+  EXPECT_EQ(34U, compressPlan(path, 1.3).poses.size());
+  EXPECT_EQ(12U, compressPlan(path, 9.5).poses.size());
+  EXPECT_EQ(8U, compressPlan(path, 19.9).poses.size());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/Libraries/nav_2d_utils/test/param_tests.cpp

@@ -0,0 +1,146 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*                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 Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Dave Hershberger
+*         David V. Lu!! (nav_2d_utils version)
+*********************************************************************/
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <nav_2d_utils/polygons.h>
+
+using nav_2d_utils::polygonFromParams;
+using nav_2d_msgs::Polygon2D;
+
+TEST(Polygon2D, unpadded_footprint_from_string_param)
+{
+  YAML::Node nh("~unpadded");
+  Polygon2D footprint = polygonFromParams(nh, "footprint");
+  ASSERT_EQ(3U, footprint.points.size());
+
+  EXPECT_EQ(1.0f, footprint.points[ 0 ].x);
+  EXPECT_EQ(1.0f, footprint.points[ 0 ].y);
+
+  EXPECT_EQ(-1.0f, footprint.points[ 1 ].x);
+  EXPECT_EQ(1.0f, footprint.points[ 1 ].y);
+
+  EXPECT_EQ(-1.0f, footprint.points[ 2 ].x);
+  EXPECT_EQ(-1.0f, footprint.points[ 2 ].y);
+}
+
+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);
+}
+
+TEST(Polygon2D, footprint_from_xmlrpc_param)
+{
+  YAML::Node nh("~xmlrpc");
+  Polygon2D footprint = polygonFromParams(nh, "footprint");
+  ASSERT_EQ(4U, footprint.points.size());
+
+  EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].x);
+  EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].y);
+
+  EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 1 ].x);
+  EXPECT_FLOAT_EQ(0.1f, footprint.points[ 1 ].y);
+
+  EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].x);
+  EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].y);
+
+  EXPECT_FLOAT_EQ(0.1f, footprint.points[ 3 ].x);
+  EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 3 ].y);
+
+  Polygon2D footprint2 = polygonFromParams(nh, "footprint2");
+  ASSERT_TRUE(nav_2d_utils::equals(footprint, footprint2));
+}
+
+TEST(Polygon2D, footprint_from_same_level_param)
+{
+  YAML::Node nh("~same_level");
+  Polygon2D footprint = polygonFromParams(nh, "footprint");
+  ASSERT_EQ(3U, footprint.points.size());
+
+  EXPECT_EQ(1.0f, footprint.points[ 0 ].x);
+  EXPECT_EQ(2.0f, footprint.points[ 0 ].y);
+
+  EXPECT_EQ(3.0f, footprint.points[ 1 ].x);
+  EXPECT_EQ(4.0f, footprint.points[ 1 ].y);
+
+  EXPECT_EQ(5.0f, footprint.points[ 2 ].x);
+  EXPECT_EQ(6.0f, footprint.points[ 2 ].y);
+}
+
+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);
+}
+
+TEST(Polygon2D, footprint_empty)
+{
+  YAML::Node nh("~empty");
+  EXPECT_THROW(polygonFromParams(nh, "footprint"), 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));
+  Polygon2D another_footprint = polygonFromParams(nh, "another_footprint");
+  EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
+
+  nh.setParam("third_footprint", nav_2d_utils::polygonToXMLRPC(footprint, false));
+  another_footprint = polygonFromParams(nh, "third_footprint");
+  EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "param_tests");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/Libraries/nav_2d_utils/test/param_tests.launch

@@ -0,0 +1,34 @@
+<launch>
+  <test time-limit="10" test-name="param_tests" pkg="nav_2d_utils" type="param_tests">
+    <param name="unpadded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
+
+    <rosparam ns="xmlrpc">
+      footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]]
+      footprint2:
+        x: [0.1, -0.1, -0.1, 0.1]
+        y: [0.1, 0.1, -0.1, -0.1]
+    </rosparam>
+
+    <rosparam ns="xmlrpc_fail"> <!-- Footprint includes a 3-value point, which should make it fail. -->
+      footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]]
+      footprint2: 1.0
+      footprint3: false
+      footprint4: [[0.1, 0.1], [0.1, 0.1]]
+      footprint5: ['x', 'y', 'z']
+      footprint6: [['x', 'y'], ['a', 'b'], ['c'], ['d']]
+      footprint7:
+       x: [0.1, -0.1, -0.1, 0.1]
+      footprint8:
+       x: 0
+       y: 0
+      footprint9:
+       x: ['a', 'b', 'c']
+       y: [d, e, f]
+    </rosparam>
+
+    <param name="same_level/footprint" value="[[1, 2], [3, 4], [5, 6]]" />
+
+    <param name="empty/empty" value="0" /> <!-- just so you can see there are no real params under "empty". -->
+  </test>
+
+</launch>

src/Libraries/nav_2d_utils/test/polygon_tests.cpp

@@ -0,0 +1,185 @@
+/*
+ * 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.
+ */
+#include <gtest/gtest.h>
+#include <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;
+
+TEST(array_parser, basic_operation)
+{
+  std::vector<std::vector<double> > vvd;
+  vvd = parseVVD("[[1, 2.2], [.3, -4e4]]");
+  EXPECT_DOUBLE_EQ(2U, vvd.size());
+  EXPECT_DOUBLE_EQ(2U, vvd[0].size());
+  EXPECT_DOUBLE_EQ(2U, vvd[1].size());
+  EXPECT_DOUBLE_EQ(1.0, vvd[0][0]);
+  EXPECT_DOUBLE_EQ(2.2, vvd[0][1]);
+  EXPECT_DOUBLE_EQ(0.3, vvd[1][0]);
+  EXPECT_DOUBLE_EQ(-40000.0, vvd[1][1]);
+}
+
+TEST(array_parser, missing_open)
+{
+  EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException);
+}
+
+TEST(array_parser, missing_close)
+{
+  EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
+}
+
+TEST(array_parser, wrong_depth)
+{
+  EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
+}
+
+TEST(Polygon2D, radius_param)
+{
+  Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0);
+  // Circular robot has 16-point footprint auto-generated.
+  ASSERT_EQ(16U, footprint.points.size());
+
+  // Check the first point
+  EXPECT_EQ(10.0, footprint.points[0].x);
+  EXPECT_EQ(0.0, footprint.points[0].y);
+
+  // Check the 4th point, which should be 90 degrees around the circle from the first.
+  EXPECT_NEAR(0.0, footprint.points[4].x, 0.0001);
+  EXPECT_NEAR(10.0, footprint.points[4].y, 0.0001);
+}
+
+TEST(Polygon2D, string_param)
+{
+  Polygon2D footprint = polygonFromString("[[1, 1], [-1, 1], [-1, -1]]");
+  ASSERT_EQ(3U, footprint.points.size());
+
+  EXPECT_EQ(1.0, footprint.points[ 0 ].x);
+  EXPECT_EQ(1.0, footprint.points[ 0 ].y);
+
+  EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
+  EXPECT_EQ(1.0, footprint.points[ 1 ].y);
+
+  EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
+  EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
+}
+
+TEST(Polygon2D, broken_string_param)
+{
+  // Not enough points
+  EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException);
+
+  // Too many numbers in point
+  EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
+
+  // Unexpected character
+  EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
+
+  // Empty String
+  EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException);
+
+  // Empty List
+  EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException);
+
+  // Empty Point
+  EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException);
+}
+
+TEST(Polygon2D, arrays)
+{
+  std::vector<double> xs = {1, -1, -1};
+  std::vector<double> ys = {1, 1, -1};
+  Polygon2D footprint = polygonFromParallelArrays(xs, ys);
+  ASSERT_EQ(3U, footprint.points.size());
+
+  EXPECT_EQ(1.0, footprint.points[ 0 ].x);
+  EXPECT_EQ(1.0, footprint.points[ 0 ].y);
+
+  EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
+  EXPECT_EQ(1.0, footprint.points[ 1 ].y);
+
+  EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
+  EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
+}
+
+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);
+}
+
+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));
+  pose.x = 15;
+  pose.y = -10;
+  pose.theta = M_PI / 4;
+  Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose);
+  ASSERT_EQ(4U, diamond.points.size());
+  double side = 1.0 / sqrt(2);
+
+  EXPECT_DOUBLE_EQ(pose.x,        diamond.points[ 0 ].x);
+  EXPECT_DOUBLE_EQ(pose.y + side, diamond.points[ 0 ].y);
+  EXPECT_DOUBLE_EQ(pose.x + side, diamond.points[ 1 ].x);
+  EXPECT_DOUBLE_EQ(pose.y,        diamond.points[ 1 ].y);
+  EXPECT_DOUBLE_EQ(pose.x,        diamond.points[ 2 ].x);
+  EXPECT_DOUBLE_EQ(pose.y - side, diamond.points[ 2 ].y);
+  EXPECT_DOUBLE_EQ(pose.x - side, diamond.points[ 3 ].x);
+  EXPECT_DOUBLE_EQ(pose.y,        diamond.points[ 3 ].y);
+}
+
+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));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/Libraries/nav_2d_utils/test/resolution_test.cpp

@@ -0,0 +1,78 @@
+/*
+ * 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.
+ */
+#include <gtest/gtest.h>
+#include <nav_2d_utils/path_ops.h>
+
+using nav_2d_utils::adjustPlanResolution;
+using nav_2d_utils::addPose;
+
+TEST(ResolutionTest, simple_example)
+{
+  nav_2d_msgs::Path2D path;
+  // Space between points is one meter
+  addPose(path, 0.0, 0.0);
+  addPose(path, 0.0, 1.0);
+
+  // resolution>=1, path won't change
+  EXPECT_EQ(2U, adjustPlanResolution(path, 2.0).poses.size());
+  EXPECT_EQ(2U, adjustPlanResolution(path, 1.0).poses.size());
+
+  // 0.5 <= resolution < 1.0, one point should be added in the middle
+  EXPECT_EQ(3U, adjustPlanResolution(path, 0.8).poses.size());
+  EXPECT_EQ(3U, adjustPlanResolution(path, 0.5).poses.size());
+
+  // 0.333 <= resolution < 0.5, two points need to be added
+  EXPECT_EQ(4U, adjustPlanResolution(path, 0.34).poses.size());
+
+  // 0.25 <= resolution < 0.333, three points need to be added
+  EXPECT_EQ(5U, adjustPlanResolution(path, 0.32).poses.size());
+}
+
+TEST(ResolutionTest, real_example)
+{
+  // This test is based on a real-world example
+  nav_2d_msgs::Path2D path;
+  addPose(path, 17.779193, -0.972024);
+  addPose(path, 17.799171, -0.950775);
+  addPose(path, 17.851942, -0.903709);
+  EXPECT_EQ(3U, adjustPlanResolution(path, 0.2).poses.size());
+  EXPECT_EQ(4U, adjustPlanResolution(path, 0.05).poses.size());
+}
+
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/Libraries/node_handle/include/robot/console.h

@@ -0,0 +1,89 @@
+#ifndef ROBOT_CONSOLE_H
+#define ROBOT_CONSOLE_H
+
+#include <cstdio>
+#include <cstdarg>
+
+namespace robot {
+
+// ANSI Color Codes
+namespace color {
+    // Reset
+    static const char* RESET = "\033[0m";
+    
+    // Text colors
+    static const char* BLACK = "\033[30m";
+    static const char* RED = "\033[31m";
+    static const char* GREEN = "\033[32m";
+    static const char* YELLOW = "\033[33m";
+    static const char* BLUE = "\033[34m";
+    static const char* MAGENTA = "\033[35m";
+    static const char* CYAN = "\033[36m";
+    static const char* WHITE = "\033[37m";
+    
+    // Bright text colors
+    static const char* BRIGHT_BLACK = "\033[90m";
+    static const char* BRIGHT_RED = "\033[91m";
+    static const char* BRIGHT_GREEN = "\033[92m";
+    static const char* BRIGHT_YELLOW = "\033[93m";
+    static const char* BRIGHT_BLUE = "\033[94m";
+    static const char* BRIGHT_MAGENTA = "\033[95m";
+    static const char* BRIGHT_CYAN = "\033[96m";
+    static const char* BRIGHT_WHITE = "\033[97m";
+    
+    // Background colors
+    static const char* BG_BLACK = "\033[40m";
+    static const char* BG_RED = "\033[41m";
+    static const char* BG_GREEN = "\033[42m";
+    static const char* BG_YELLOW = "\033[43m";
+    static const char* BG_BLUE = "\033[44m";
+    static const char* BG_MAGENTA = "\033[45m";
+    static const char* BG_CYAN = "\033[46m";
+    static const char* BG_WHITE = "\033[47m";
+    
+    // Text styles
+    static const char* BOLD = "\033[1m";
+    static const char* DIM = "\033[2m";
+    static const char* ITALIC = "\033[3m";
+    static const char* UNDERLINE = "\033[4m";
+    static const char* BLINK = "\033[5m";
+    static const char* REVERSE = "\033[7m";
+}
+
+// Check if terminal supports colors
+bool is_color_supported();
+
+// Enable/disable color output (useful for non-terminal outputs)
+void set_color_enabled(bool enabled);
+bool is_color_enabled();
+
+// Colored printf functions
+void printf_red(const char* format, ...);
+void printf_green(const char* format, ...);
+void printf_yellow(const char* format, ...);
+void printf_blue(const char* format, ...);
+void printf_cyan(const char* format, ...);
+void printf_magenta(const char* format, ...);
+void printf_white(const char* format, ...);
+
+// Colored printf with custom color
+void printf_color(const char* color_code, const char* format, ...);
+
+// Log level functions
+void log_info(const char* format, ...);
+void log_success(const char* format, ...);
+void log_warning(const char* format, ...);
+void log_error(const char* format, ...);
+void log_debug(const char* format, ...);
+
+// Print with file and line info (colored)
+void log_info_at(const char* file, int line, const char* format, ...);
+void log_success_at(const char* file, int line, const char* format, ...);
+void log_warning_at(const char* file, int line, const char* format, ...);
+void log_error_at(const char* file, int line, const char* format, ...);
+void log_debug_at(const char* file, int line, const char* format, ...);
+
+} // namespace robot
+
+#endif // ROBOT_CONSOLE_H
+

src/Libraries/node_handle/src/console.cpp

@@ -0,0 +1,207 @@
+#include "robot/console.h"
+#include <cstdlib>
+#include <cstring>
+
+namespace robot {
+
+// Global flag to enable/disable colors
+static bool color_enabled = true;
+
+bool is_color_supported() {
+    // Check if NO_COLOR environment variable is set
+    const char* no_color = std::getenv("NO_COLOR");
+    if (no_color != nullptr && strlen(no_color) > 0) {
+        return false;
+    }
+    
+    // Check if TERM environment variable suggests color support
+    const char* term = std::getenv("TERM");
+    if (term != nullptr) {
+        // Common terminals that support colors
+        if (strstr(term, "xterm") != nullptr ||
+            strstr(term, "color") != nullptr ||
+            strstr(term, "256") != nullptr ||
+            strstr(term, "screen") != nullptr ||
+            strstr(term, "tmux") != nullptr) {
+            return true;
+        }
+    }
+    
+    // Default to true for most modern terminals
+    return true;
+}
+
+void set_color_enabled(bool enabled) {
+    color_enabled = enabled && is_color_supported();
+}
+
+bool is_color_enabled() {
+    return color_enabled && is_color_supported();
+}
+
+// Helper function that accepts va_list
+static void vprintf_color(const char* color_code, const char* format, va_list args) {
+    if (is_color_enabled()) {
+        std::printf("%s", color_code);
+    }
+    
+    std::vprintf(format, args);
+    
+    if (is_color_enabled()) {
+        std::printf("%s", color::RESET);
+    }
+}
+
+void printf_color(const char* color_code, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color_code, format, args);
+    va_end(args);
+}
+
+void printf_red(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::RED, format, args);
+    va_end(args);
+}
+
+void printf_green(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::GREEN, format, args);
+    va_end(args);
+}
+
+void printf_yellow(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::YELLOW, format, args);
+    va_end(args);
+}
+
+void printf_blue(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::BLUE, format, args);
+    va_end(args);
+}
+
+void printf_cyan(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::CYAN, format, args);
+    va_end(args);
+}
+
+void printf_magenta(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::MAGENTA, format, args);
+    va_end(args);
+}
+
+void printf_white(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::WHITE, format, args);
+    va_end(args);
+}
+
+void log_info(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::BLUE, format, args);
+    va_end(args);
+}
+
+void log_success(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::GREEN, format, args);
+    va_end(args);
+}
+
+void log_warning(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::YELLOW, format, args);
+    va_end(args);
+}
+
+void log_error(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::RED, format, args);
+    va_end(args);
+}
+
+void log_debug(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::CYAN, format, args);
+    va_end(args);
+}
+
+void log_info_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[INFO]%s [%s:%d] ", color::BLUE, color::RESET, file, line);
+    } else {
+        std::printf("[INFO] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_success_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[SUCCESS]%s [%s:%d] ", color::GREEN, color::RESET, file, line);
+    } else {
+        std::printf("[SUCCESS] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_warning_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[WARNING]%s [%s:%d] ", color::YELLOW, color::RESET, file, line);
+    } else {
+        std::printf("[WARNING] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_error_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[ERROR]%s [%s:%d] ", color::RED, color::RESET, file, line);
+    } else {
+        std::printf("[ERROR] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_debug_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[DEBUG]%s [%s:%d] ", color::CYAN, color::RESET, file, line);
+    } else {
+        std::printf("[DEBUG] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+} // namespace robot
+

src/Libraries/robot_cpp/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.10)
+project(robot_cpp)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
+endif()
+
+add_library(${PROJECT_NAME}_node_handle SHARED
+  src/node_handle.cpp
+)
+
+target_include_directories(${PROJECT_NAME}_node_handle
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(${PROJECT_NAME}_node_handle
+  PUBLIC
+    yaml-cpp
+    xmlrpcpp
+)
+
+install(TARGETS ${PROJECT_NAME}_node_handle
+  EXPORT ${PROJECT_NAME}_node_handle-targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(DIRECTORY include/
+  DESTINATION include
+  FILES_MATCHING PATTERN "*.h")
+
+install(EXPORT ${PROJECT_NAME}_node_handle-targets
+  NAMESPACE robot::
+  DESTINATION lib/cmake/${PROJECT_NAME}_node_handle)
+
+# Create alias for easier usage
+add_library(robot::node_handle ALIAS ${PROJECT_NAME}_node_handle)
+
+
+add_library(${PROJECT_NAME}_console SHARED
+  src/console.cpp
+)
+
+target_include_directories(${PROJECT_NAME}_console
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+# Console library only needs standard C++ library, no external dependencies needed
+
+install(TARGETS ${PROJECT_NAME}_console
+  EXPORT ${PROJECT_NAME}_console-targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(EXPORT ${PROJECT_NAME}_console-targets
+  NAMESPACE robot::
+  DESTINATION lib/cmake/${PROJECT_NAME}_console)
+
+# Create alias for easier usage
+add_library(robot::console ALIAS ${PROJECT_NAME}_console)

src/Libraries/robot_cpp/include/robot/console.h

@@ -0,0 +1,89 @@
+#ifndef ROBOT_CONSOLE_H
+#define ROBOT_CONSOLE_H
+
+#include <cstdio>
+#include <cstdarg>
+
+namespace robot {
+
+// ANSI Color Codes
+namespace color {
+    // Reset
+    static const char* RESET = "\033[0m";
+    
+    // Text colors
+    static const char* BLACK = "\033[30m";
+    static const char* RED = "\033[31m";
+    static const char* GREEN = "\033[32m";
+    static const char* YELLOW = "\033[33m";
+    static const char* BLUE = "\033[34m";
+    static const char* MAGENTA = "\033[35m";
+    static const char* CYAN = "\033[36m";
+    static const char* WHITE = "\033[37m";
+    
+    // Bright text colors
+    static const char* BRIGHT_BLACK = "\033[90m";
+    static const char* BRIGHT_RED = "\033[91m";
+    static const char* BRIGHT_GREEN = "\033[92m";
+    static const char* BRIGHT_YELLOW = "\033[93m";
+    static const char* BRIGHT_BLUE = "\033[94m";
+    static const char* BRIGHT_MAGENTA = "\033[95m";
+    static const char* BRIGHT_CYAN = "\033[96m";
+    static const char* BRIGHT_WHITE = "\033[97m";
+    
+    // Background colors
+    static const char* BG_BLACK = "\033[40m";
+    static const char* BG_RED = "\033[41m";
+    static const char* BG_GREEN = "\033[42m";
+    static const char* BG_YELLOW = "\033[43m";
+    static const char* BG_BLUE = "\033[44m";
+    static const char* BG_MAGENTA = "\033[45m";
+    static const char* BG_CYAN = "\033[46m";
+    static const char* BG_WHITE = "\033[47m";
+    
+    // Text styles
+    static const char* BOLD = "\033[1m";
+    static const char* DIM = "\033[2m";
+    static const char* ITALIC = "\033[3m";
+    static const char* UNDERLINE = "\033[4m";
+    static const char* BLINK = "\033[5m";
+    static const char* REVERSE = "\033[7m";
+}
+
+// Check if terminal supports colors
+bool is_color_supported();
+
+// Enable/disable color output (useful for non-terminal outputs)
+void set_color_enabled(bool enabled);
+bool is_color_enabled();
+
+// Colored printf functions
+void printf_red(const char* format, ...);
+void printf_green(const char* format, ...);
+void printf_yellow(const char* format, ...);
+void printf_blue(const char* format, ...);
+void printf_cyan(const char* format, ...);
+void printf_magenta(const char* format, ...);
+void printf_white(const char* format, ...);
+
+// Colored printf with custom color
+void printf_color(const char* color_code, const char* format, ...);
+
+// Log level functions
+void log_info(const char* format, ...);
+void log_success(const char* format, ...);
+void log_warning(const char* format, ...);
+void log_error(const char* format, ...);
+void log_debug(const char* format, ...);
+
+// Print with file and line info (colored)
+void log_info_at(const char* file, int line, const char* format, ...);
+void log_success_at(const char* file, int line, const char* format, ...);
+void log_warning_at(const char* file, int line, const char* format, ...);
+void log_error_at(const char* file, int line, const char* format, ...);
+void log_debug_at(const char* file, int line, const char* format, ...);
+
+} // namespace robot
+
+#endif // ROBOT_CONSOLE_H
+

src/Libraries/robot_cpp/include/robot/node_handle.h

@@ -0,0 +1,83 @@
+#ifndef ROBOT_NODE_H_INCLUDED_H
+#define ROBOT_NODE_H_INCLUDED_H
+
+#include <yaml-cpp/yaml.h>
+#include <memory>
+#include <string>
+#include <map>
+#include <vector>
+
+namespace robot
+{
+  class NodeHandle
+  {
+  public:
+    using Ptr = std::shared_ptr<NodeHandle>;
+
+    NodeHandle();
+
+    NodeHandle(const std::string &name); 
+
+    ~NodeHandle();
+
+    bool getParam (const std::string &key, bool &b, bool default_value = false) const;
+    
+    bool getParam (const std::string &key, double &d, double default_value = 0.0) const;
+      
+    bool getParam (const std::string &key, float &f, float default_value = 0.0) const;
+      
+    bool getParam (const std::string &key, int &i, int default_value = 0) const;
+      
+    bool getParam (const std::string &key, std::map< std::string, bool > &map, std::map< std::string, bool > default_value = std::map< std::string, bool >()) const;
+      
+    bool getParam (const std::string &key, std::map< std::string, double > &map, std::map< std::string, double > default_value = std::map< std::string, double >()) const;
+
+    bool getParam (const std::string &key, std::map< std::string, float > &map, std::map< std::string, float > default_value = std::map< std::string, float >()) const;
+      
+    bool getParam (const std::string &key, std::map< std::string, int > &map, std::map< std::string, int > default_value = std::map< std::string, int >()) const;
+    
+    bool getParam (const std::string &key, std::map< std::string, std::string > &map, std::map< std::string, std::string > default_value = std::map< std::string, std::string >()) const;
+      
+    bool getParam (const std::string &key, std::string &s, std::string default_value = "") const;
+      
+    bool getParam (const std::string &key, std::vector< bool > &vec, std::vector< bool > default_value = std::vector< bool >()) const;
+    
+    bool 	getParam (const std::string &key, std::vector< double > &vec, std::vector< double > default_value = std::vector< double >()) const;
+      
+    bool getParam (const std::string &key, std::vector< float > &vec, std::vector< float > default_value = std::vector< float >()) const;
+    
+    bool getParam (const std::string &key, std::vector< int > &vec, std::vector< int > default_value = std::vector< int >()) const;
+    
+    bool getParam (const std::string &key, std::vector< std::string > &vec, std::vector< std::string > default_value = std::vector< std::string >()) const;
+    
+    bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
+
+    // Helper method to set parameters from YAML::Node
+    void setParam(const std::string &key, const YAML::Node &value);
+    
+    // Overloads for basic types
+    void setParam(const std::string &key, bool value);
+    void setParam(const std::string &key, int value);
+    void setParam(const std::string &key, double value);
+    void setParam(const std::string &key, float value);
+    void setParam(const std::string &key, const std::string &value);
+
+    // Helper method to get nested parameter by key path (e.g., "parent/child")
+    YAML::Node getParamValue(const std::string &key) const;
+
+    // Load YAML file
+    bool loadYamlFile(const std::string &filepath);
+
+    // Get namespace
+    std::string getNamespace() const;
+ 	
+  private:
+    std::string namespace_;
+    YAML::Node node_handle_;
+
+    // Helper method to split key path and get nested value
+    YAML::Node getNestedValue(const std::string &key) const;
+  };
+} // namespace robot
+
+#endif // ROBOT_NODE_H_INCLUDED_H

src/Libraries/robot_cpp/src/console.cpp

@@ -0,0 +1,207 @@
+#include "robot/console.h"
+#include <cstdlib>
+#include <cstring>
+
+namespace robot {
+
+// Global flag to enable/disable colors
+static bool color_enabled = true;
+
+bool is_color_supported() {
+    // Check if NO_COLOR environment variable is set
+    const char* no_color = std::getenv("NO_COLOR");
+    if (no_color != nullptr && strlen(no_color) > 0) {
+        return false;
+    }
+    
+    // Check if TERM environment variable suggests color support
+    const char* term = std::getenv("TERM");
+    if (term != nullptr) {
+        // Common terminals that support colors
+        if (strstr(term, "xterm") != nullptr ||
+            strstr(term, "color") != nullptr ||
+            strstr(term, "256") != nullptr ||
+            strstr(term, "screen") != nullptr ||
+            strstr(term, "tmux") != nullptr) {
+            return true;
+        }
+    }
+    
+    // Default to true for most modern terminals
+    return true;
+}
+
+void set_color_enabled(bool enabled) {
+    color_enabled = enabled && is_color_supported();
+}
+
+bool is_color_enabled() {
+    return color_enabled && is_color_supported();
+}
+
+// Helper function that accepts va_list
+static void vprintf_color(const char* color_code, const char* format, va_list args) {
+    if (is_color_enabled()) {
+        std::printf("%s", color_code);
+    }
+    
+    std::vprintf(format, args);
+    
+    if (is_color_enabled()) {
+        std::printf("%s", color::RESET);
+    }
+}
+
+void printf_color(const char* color_code, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color_code, format, args);
+    va_end(args);
+}
+
+void printf_red(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::RED, format, args);
+    va_end(args);
+}
+
+void printf_green(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::GREEN, format, args);
+    va_end(args);
+}
+
+void printf_yellow(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::YELLOW, format, args);
+    va_end(args);
+}
+
+void printf_blue(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::BLUE, format, args);
+    va_end(args);
+}
+
+void printf_cyan(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::CYAN, format, args);
+    va_end(args);
+}
+
+void printf_magenta(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::MAGENTA, format, args);
+    va_end(args);
+}
+
+void printf_white(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::WHITE, format, args);
+    va_end(args);
+}
+
+void log_info(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::BLUE, format, args);
+    va_end(args);
+}
+
+void log_success(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::GREEN, format, args);
+    va_end(args);
+}
+
+void log_warning(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::YELLOW, format, args);
+    va_end(args);
+}
+
+void log_error(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::RED, format, args);
+    va_end(args);
+}
+
+void log_debug(const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    vprintf_color(color::CYAN, format, args);
+    va_end(args);
+}
+
+void log_info_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[INFO]%s [%s:%d] ", color::BLUE, color::RESET, file, line);
+    } else {
+        std::printf("[INFO] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_success_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[SUCCESS]%s [%s:%d] ", color::GREEN, color::RESET, file, line);
+    } else {
+        std::printf("[SUCCESS] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_warning_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[WARNING]%s [%s:%d] ", color::YELLOW, color::RESET, file, line);
+    } else {
+        std::printf("[WARNING] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_error_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[ERROR]%s [%s:%d] ", color::RED, color::RESET, file, line);
+    } else {
+        std::printf("[ERROR] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+void log_debug_at(const char* file, int line, const char* format, ...) {
+    va_list args;
+    va_start(args, format);
+    if (is_color_enabled()) {
+        std::printf("%s[DEBUG]%s [%s:%d] ", color::CYAN, color::RESET, file, line);
+    } else {
+        std::printf("[DEBUG] [%s:%d] ", file, line);
+    }
+    std::vprintf(format, args);
+    va_end(args);
+}
+
+} // namespace robot
+

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -0,0 +1,631 @@
+#include <robot/node_handle.h>
+#include <sstream>
+#include <algorithm>
+#include <cctype>
+#include <fstream>
+
+namespace robot
+{
+
+NodeHandle::NodeHandle() : namespace_("")
+{
+  node_handle_ = YAML::Node();
+}
+
+NodeHandle::NodeHandle(const std::string &name) : namespace_(name)
+{
+  node_handle_ = YAML::Node();
+}
+
+NodeHandle::~NodeHandle()
+{
+}
+
+YAML::Node NodeHandle::getNestedValue(const std::string &key) const
+{
+  if (!node_handle_.IsDefined() || !node_handle_.IsMap())
+  {
+    return YAML::Node();
+  }
+
+  // Split key by '/' to handle nested keys
+  std::stringstream ss(key);
+  std::string segment;
+  std::vector<std::string> segments;
+  
+  while (std::getline(ss, segment, '/'))
+  {
+    if (!segment.empty())
+    {
+      segments.push_back(segment);
+    }
+  }
+
+  if (segments.empty())
+  {
+    return node_handle_;
+  }
+
+  YAML::Node current = node_handle_;
+  
+  for (size_t i = 0; i < segments.size(); ++i)
+  {
+    if (!current.IsMap())
+    {
+      return YAML::Node();
+    }
+    
+    if (!current[segments[i]].IsDefined())
+    {
+      return YAML::Node();
+    }
+    
+    current = current[segments[i]];
+  }
+  
+  return current;
+}
+
+YAML::Node NodeHandle::getParamValue(const std::string &key) const
+{
+  return getNestedValue(key);
+}
+
+void NodeHandle::setParam(const std::string &key, const YAML::Node &value)
+{
+  if (!node_handle_.IsMap())
+  {
+    node_handle_ = YAML::Node(YAML::NodeType::Map);
+  }
+  
+  // Split key by '/' to handle nested keys
+  std::stringstream ss(key);
+  std::string segment;
+  std::vector<std::string> segments;
+  
+  while (std::getline(ss, segment, '/'))
+  {
+    if (!segment.empty())
+    {
+      segments.push_back(segment);
+    }
+  }
+
+  if (segments.empty())
+  {
+    node_handle_ = value;
+    return;
+  }
+
+  YAML::Node current = node_handle_;
+  
+  for (size_t i = 0; i < segments.size() - 1; ++i)
+  {
+    if (!current[segments[i]].IsDefined() || !current[segments[i]].IsMap())
+    {
+      current[segments[i]] = YAML::Node(YAML::NodeType::Map);
+    }
+    current = current[segments[i]];
+  }
+  
+  // Set the final value
+  current[segments.back()] = value;
+}
+
+bool NodeHandle::loadYamlFile(const std::string &filepath)
+{
+  try
+  {
+    node_handle_ = YAML::LoadFile(filepath);
+    return true;
+  }
+  catch (const YAML::Exception &e)
+  {
+    return false;
+  }
+}
+
+std::string NodeHandle::getNamespace() const
+{
+  return namespace_;
+}
+
+void NodeHandle::setParam(const std::string &key, bool value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, int value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, double value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+void NodeHandle::setParam(const std::string &key, float value)
+{
+  setParam(key, YAML::Node(static_cast<double>(value)));
+}
+
+void NodeHandle::setParam(const std::string &key, const std::string &value)
+{
+  setParam(key, YAML::Node(value));
+}
+
+bool NodeHandle::getParam(const std::string &key, bool &b, bool default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined())
+  {
+    b = default_value;
+    return false;
+  }
+  
+  if (value.IsScalar())
+  {
+    try
+    {
+      if (value.Type() == YAML::NodeType::Scalar)
+      {
+        std::string str = value.as<std::string>();
+        std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+        if (str == "true" || str == "1" || str == "yes" || str == "on")
+        {
+          b = true;
+          return true;
+        }
+        else if (str == "false" || str == "0" || str == "no" || str == "off")
+        {
+          b = false;
+          return true;
+        }
+      }
+      b = value.as<bool>();
+      return true;
+    }
+    catch (...)
+    {
+      b = default_value;
+      return false;
+    }
+  }
+  
+  b = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, double &d, double default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsScalar())
+  {
+    d = default_value;
+    return false;
+  }
+  
+  try
+  {
+    d = value.as<double>();
+    return true;
+  }
+  catch (...)
+  {
+    d = default_value;
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, float &f, float default_value) const
+{
+  double d;
+  if (getParam(key, d, static_cast<double>(default_value)))
+  {
+    f = static_cast<float>(d);
+    return true;
+  }
+  f = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, int &i, int default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsScalar())
+  {
+    i = default_value;
+    return false;
+  }
+  
+  try
+  {
+    std::string str = value.as<std::string>();
+    // Handle hex format
+    if (str.length() > 2 && str.substr(0, 2) == "0x")
+    {
+      i = std::stoi(str, nullptr, 16);
+    }
+    else
+    {
+      i = value.as<int>();
+    }
+    return true;
+  }
+  catch (...)
+  {
+    i = default_value;
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::string &s, std::string default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsScalar())
+  {
+    s = default_value;
+    return false;
+  }
+  
+  try
+  {
+    s = value.as<std::string>();
+    return true;
+  }
+  catch (...)
+  {
+    s = default_value;
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<bool> &vec, std::vector<bool> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsSequence())
+  {
+    vec = default_value;
+    return false;
+  }
+  
+  vec.clear();
+  try
+  {
+    for (size_t i = 0; i < value.size(); ++i)
+    {
+      if (value[i].IsScalar())
+      {
+        try
+        {
+          bool b = value[i].as<bool>();
+          vec.push_back(b);
+        }
+        catch (...)
+        {
+          // Try as string and convert
+          std::string str = value[i].as<std::string>();
+          std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+          if (str == "true" || str == "1" || str == "yes" || str == "on")
+          {
+            vec.push_back(true);
+          }
+          else if (str == "false" || str == "0" || str == "no" || str == "off")
+          {
+            vec.push_back(false);
+          }
+          else
+          {
+            return false;
+          }
+        }
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<double> &vec, std::vector<double> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsSequence())
+  {
+    vec = default_value;
+    return false;
+  }
+  
+  vec.clear();
+  try
+  {
+    for (size_t i = 0; i < value.size(); ++i)
+    {
+      vec.push_back(value[i].as<double>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<float> &vec, std::vector<float> default_value) const
+{
+  std::vector<double> dvec;
+  std::vector<double> ddefault;
+  ddefault.reserve(default_value.size());
+  for (float f : default_value)
+  {
+    ddefault.push_back(static_cast<double>(f));
+  }
+  
+  if (getParam(key, dvec, ddefault))
+  {
+    vec.clear();
+    vec.reserve(dvec.size());
+    for (double d : dvec)
+    {
+      vec.push_back(static_cast<float>(d));
+    }
+    return true;
+  }
+  vec = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<int> &vec, std::vector<int> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsSequence())
+  {
+    vec = default_value;
+    return false;
+  }
+  
+  vec.clear();
+  try
+  {
+    for (size_t i = 0; i < value.size(); ++i)
+    {
+      vec.push_back(value[i].as<int>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::vector<std::string> &vec, std::vector<std::string> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsSequence())
+  {
+    vec = default_value;
+    return false;
+  }
+  
+  vec.clear();
+  try
+  {
+    for (size_t i = 0; i < value.size(); ++i)
+    {
+      vec.push_back(value[i].as<std::string>());
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, bool> &map, std::map<std::string, bool> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsMap())
+  {
+    map = default_value;
+    return false;
+  }
+  
+  map.clear();
+  try
+  {
+    for (auto it = value.begin(); it != value.end(); ++it)
+    {
+      std::string key_str = it->first.as<std::string>();
+      if (it->second.IsScalar())
+      {
+        try
+        {
+          bool b = it->second.as<bool>();
+          map[key_str] = b;
+        }
+        catch (...)
+        {
+          // Try as string and convert
+          std::string str = it->second.as<std::string>();
+          std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+          if (str == "true" || str == "1" || str == "yes" || str == "on")
+          {
+            map[key_str] = true;
+          }
+          else if (str == "false" || str == "0" || str == "no" || str == "off")
+          {
+            map[key_str] = false;
+          }
+          else
+          {
+            return false;
+          }
+        }
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, double> &map, std::map<std::string, double> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsMap())
+  {
+    map = default_value;
+    return false;
+  }
+  
+  map.clear();
+  try
+  {
+    for (auto it = value.begin(); it != value.end(); ++it)
+    {
+      std::string key_str = it->first.as<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<double>();
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, float> &map, std::map<std::string, float> default_value) const
+{
+  std::map<std::string, double> dmap;
+  std::map<std::string, double> ddefault;
+  for (const auto &pair : default_value)
+  {
+    ddefault[pair.first] = static_cast<double>(pair.second);
+  }
+  
+  if (getParam(key, dmap, ddefault))
+  {
+    map.clear();
+    for (const auto &pair : dmap)
+    {
+      map[pair.first] = static_cast<float>(pair.second);
+    }
+    return true;
+  }
+  map = default_value;
+  return false;
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, int> &map, std::map<std::string, int> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsMap())
+  {
+    map = default_value;
+    return false;
+  }
+  
+  map.clear();
+  try
+  {
+    for (auto it = value.begin(); it != value.end(); ++it)
+    {
+      std::string key_str = it->first.as<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<int>();
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, std::map<std::string, std::string> &map, std::map<std::string, std::string> default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined() || !value.IsMap())
+  {
+    map = default_value;
+    return false;
+  }
+  
+  map.clear();
+  try
+  {
+    for (auto it = value.begin(); it != value.end(); ++it)
+    {
+      std::string key_str = it->first.as<std::string>();
+      if (it->second.IsScalar())
+      {
+        map[key_str] = it->second.as<std::string>();
+      }
+      else
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
+bool NodeHandle::getParam(const std::string &key, YAML::Node &v, YAML::Node default_value) const
+{
+  YAML::Node value = getNestedValue(key);
+  
+  if (!value.IsDefined())
+  {
+    v = default_value;
+    return false;
+  }
+  
+  v = value;
+  return true;
+}
+
+} // namespace robot