created

src/APIs/c_api/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.10)
+project(nav_c_api VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Dependencies
+set(PACKAGES_DIR
+    move_base_core
+    move_base
+    tf3
+    robot_time
+    geometry_msgs
+)
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+)
+
+# Tìm tất cả file source
+file(GLOB SOURCES "src/*.cpp")
+file(GLOB HEADERS "include/*.h")
+
+# Tạo thư viện shared (.so)
+add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
+
+# Link libraries
+target_link_libraries(nav_c_api
+    PUBLIC
+        ${PACKAGES_DIR}
+)
+
+# Set include directories
+target_include_directories(nav_c_api
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Cài đặt header files
+install(DIRECTORY include/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+)
+
+# Cài đặt library
+install(TARGETS nav_c_api
+    EXPORT nav_c_api-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Export targets
+install(EXPORT nav_c_api-targets
+    FILE nav_c_api-targets.cmake
+    DESTINATION lib/cmake/nav_c_api
+)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "=================================")
+

src/APIs/c_api/include/nav_c_api.h

@@ -0,0 +1,349 @@
+#ifndef NAVIGATION_C_API_H
+#define NAVIGATION_C_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+// Forward declarations
+typedef void* NavigationHandle;
+typedef void* TFListenerHandle;
+
+// ============================================================================
+// Enums
+// ============================================================================
+
+/**
+ * @brief Navigation states, including planning and controller status
+ */
+typedef enum {
+    NAV_STATE_PENDING = 0,
+    NAV_STATE_ACTIVE = 1,
+    NAV_STATE_PREEMPTED = 2,
+    NAV_STATE_SUCCEEDED = 3,
+    NAV_STATE_ABORTED = 4,
+    NAV_STATE_REJECTED = 5,
+    NAV_STATE_PREEMPTING = 6,
+    NAV_STATE_RECALLING = 7,
+    NAV_STATE_RECALLED = 8,
+    NAV_STATE_LOST = 9,
+    NAV_STATE_PLANNING = 10,
+    NAV_STATE_CONTROLLING = 11,
+    NAV_STATE_CLEARING = 12,
+    NAV_STATE_PAUSED = 13
+} NavigationState;
+
+// ============================================================================
+// Structures
+// ============================================================================
+
+/**
+ * @brief Point structure (x, y, z)
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Point;
+
+/**
+ * @brief Pose2D structure (x, y, theta)
+ */
+typedef struct {
+    double x;
+    double y;
+    double theta;
+} Pose2D;
+
+/**
+ * @brief Quaternion structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+    double w;
+} Quaternion;
+
+/**
+ * @brief Position structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Position;
+
+/**
+ * @brief Pose structure
+ */
+typedef struct {
+    Position position;
+    Quaternion orientation;
+} Pose;
+
+/**
+ * @brief Header structure
+ */
+typedef struct {
+    uint32_t seq;
+    int64_t sec;
+    uint32_t nsec;
+    char* frame_id;
+} Header;
+
+/**
+ * @brief PoseStamped structure
+ */
+typedef struct {
+    Header header;
+    Pose pose;
+} PoseStamped;
+
+/**
+ * @brief Vector3 structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Vector3;
+
+/**
+ * @brief Navigation feedback structure
+ */
+typedef struct {
+    NavigationState navigation_state;
+    char* feed_back_str;
+    Pose2D current_pose;
+    bool goal_checked;
+    bool is_ready;
+} NavFeedback;
+
+// ============================================================================
+// String Management
+// ============================================================================
+
+/**
+ * @brief Free a string allocated by the library
+ * @param str String to free
+ */
+void nav_c_api_free_string(char* str);
+
+// ============================================================================
+// State Conversion
+// ============================================================================
+
+/**
+ * @brief Convert a State enum to its string representation
+ * @param state Enum value of NavigationState
+ * @return String representation (caller must free with nav_c_api_free_string)
+ */
+char* navigation_state_to_string(NavigationState state);
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+/**
+ * @brief Creates a target pose by offsetting a given 2D pose along its heading direction
+ * @param pose_x X coordinate of the original pose
+ * @param pose_y Y coordinate of the original pose
+ * @param pose_theta Heading angle in radians
+ * @param frame_id The coordinate frame ID (null-terminated string)
+ * @param offset_distance Distance to offset along heading (positive = forward, negative = backward)
+ * @param out_goal Output parameter for the offset pose
+ * @return true on success, false on failure
+ */
+bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
+                               const char* frame_id, double offset_distance,
+                               PoseStamped* out_goal);
+
+/**
+ * @brief Creates an offset target pose from a given PoseStamped
+ * @param in_pose Input pose
+ * @param offset_distance Distance to offset along heading direction
+ * @param out_goal Output parameter for the offset pose
+ * @return true on success, false on failure
+ */
+bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
+                                     PoseStamped* out_goal);
+
+// ============================================================================
+// Navigation Handle Management
+// ============================================================================
+
+/**
+ * @brief Create a new navigation instance
+ * @return Navigation handle, or NULL on failure
+ */
+NavigationHandle navigation_create(void);
+
+/**
+ * @brief Destroy a navigation instance
+ * @param handle Navigation handle to destroy
+ */
+void navigation_destroy(NavigationHandle handle);
+
+// ============================================================================
+// TF Listener Management
+// ============================================================================
+
+/**
+ * @brief Create a TF listener instance
+ * @return TF listener handle, or NULL on failure
+ */
+TFListenerHandle tf_listener_create(void);
+
+/**
+ * @brief Destroy a TF listener instance
+ * @param handle TF listener handle to destroy
+ */
+void tf_listener_destroy(TFListenerHandle handle);
+
+// ============================================================================
+// Navigation Interface Methods
+// ============================================================================
+
+/**
+ * @brief Initialize the navigation system
+ * @param handle Navigation handle
+ * @param tf_handle TF listener handle
+ * @return true on success, false on failure
+ */
+bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+
+/**
+ * @brief Set the robot's footprint (outline shape)
+ * @param handle Navigation handle
+ * @param points Array of points representing the footprint polygon
+ * @param point_count Number of points in the array
+ * @return true on success, false on failure
+ */
+bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
+
+/**
+ * @brief Send a goal for the robot to navigate to
+ * @param handle Navigation handle
+ * @param goal Target pose in the global frame
+ * @param xy_goal_tolerance Acceptable error in X/Y (meters)
+ * @param yaw_goal_tolerance Acceptable angular error (radians)
+ * @return true if goal was accepted and sent successfully
+ */
+bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
+                        double xy_goal_tolerance, double yaw_goal_tolerance);
+
+/**
+ * @brief Send a docking goal to a predefined marker
+ * @param handle Navigation handle
+ * @param marker Marker name or ID (null-terminated string)
+ * @param goal Target pose for docking
+ * @param xy_goal_tolerance Acceptable XY error (meters)
+ * @param yaw_goal_tolerance Acceptable heading error (radians)
+ * @return true if docking command succeeded
+ */
+bool navigation_dock_to(NavigationHandle handle, const char* marker,
+                        const PoseStamped* goal,
+                        double xy_goal_tolerance, double yaw_goal_tolerance);
+
+/**
+ * @brief Move straight toward the target position
+ * @param handle Navigation handle
+ * @param goal Target pose
+ * @param xy_goal_tolerance Acceptable positional error (meters)
+ * @return true if command issued successfully
+ */
+bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+                                  double xy_goal_tolerance);
+
+/**
+ * @brief Rotate in place to align with target orientation
+ * @param handle Navigation handle
+ * @param goal Pose containing desired heading (only Z-axis used)
+ * @param yaw_goal_tolerance Acceptable angular error (radians)
+ * @return true if rotation command was sent successfully
+ */
+bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+                         double yaw_goal_tolerance);
+
+/**
+ * @brief Pause the robot's movement
+ * @param handle Navigation handle
+ */
+void navigation_pause(NavigationHandle handle);
+
+/**
+ * @brief Resume motion after a pause
+ * @param handle Navigation handle
+ */
+void navigation_resume(NavigationHandle handle);
+
+/**
+ * @brief Cancel the current goal and stop the robot
+ * @param handle Navigation handle
+ */
+void navigation_cancel(NavigationHandle handle);
+
+/**
+ * @brief Send limited linear velocity command
+ * @param handle Navigation handle
+ * @param linear_x Linear velocity in X direction
+ * @param linear_y Linear velocity in Y direction
+ * @param linear_z Linear velocity in Z direction
+ * @return true if the command was accepted
+ */
+bool navigation_set_twist_linear(NavigationHandle handle,
+                                  double linear_x, double linear_y, double linear_z);
+
+/**
+ * @brief Send limited angular velocity command
+ * @param handle Navigation handle
+ * @param angular_x Angular velocity around X axis
+ * @param angular_y Angular velocity around Y axis
+ * @param angular_z Angular velocity around Z axis
+ * @return true if the command was accepted
+ */
+bool navigation_set_twist_angular(NavigationHandle handle,
+                                   double angular_x, double angular_y, double angular_z);
+
+/**
+ * @brief Get the robot's pose as a PoseStamped
+ * @param handle Navigation handle
+ * @param out_pose Output parameter with the robot's current pose
+ * @return true if pose was successfully retrieved
+ */
+bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose);
+
+/**
+ * @brief Get the robot's pose as a 2D pose
+ * @param handle Navigation handle
+ * @param out_pose Output parameter with the robot's current 2D pose
+ * @return true if pose was successfully retrieved
+ */
+bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
+
+/**
+ * @brief Get navigation feedback
+ * @param handle Navigation handle
+ * @param out_feedback Output parameter with navigation feedback
+ * @return true if feedback was successfully retrieved
+ * @note The feed_back_str field must be freed using nav_c_api_free_string
+ */
+bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback);
+
+/**
+ * @brief Free navigation feedback structure
+ * @param feedback Feedback structure to free
+ */
+void navigation_free_feedback(NavFeedback* feedback);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NAVIGATION_C_API_H
+

src/APIs/c_api/src/nav_c_api.cpp

@@ -0,0 +1,432 @@
+#include "nav_c_api.h"
+#include <move_base_core/navigation.h>
+#include <move_base_core/common.h>
+#include <move_base/move_base.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Vector3.h>
+#include <tf3/buffer_core.h>
+#include <string>
+#include <vector>
+#include <cstring>
+#include <cstdlib>
+#include <boost/dll/import.hpp>
+
+
+// ============================================================================
+// Internal Helper Functions
+// ============================================================================
+
+namespace {
+    // Convert C PoseStamped to C++ PoseStamped
+    geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
+        geometry_msgs::PoseStamped cpp_pose;
+        cpp_pose.header.seq = c_pose->header.seq;
+        cpp_pose.header.stamp.sec = c_pose->header.sec;
+        cpp_pose.header.stamp.nsec = c_pose->header.nsec;
+        if (c_pose->header.frame_id) {
+            cpp_pose.header.frame_id = c_pose->header.frame_id;
+        }
+        cpp_pose.pose.position.x = c_pose->pose.position.x;
+        cpp_pose.pose.position.y = c_pose->pose.position.y;
+        cpp_pose.pose.position.z = c_pose->pose.position.z;
+        cpp_pose.pose.orientation.x = c_pose->pose.orientation.x;
+        cpp_pose.pose.orientation.y = c_pose->pose.orientation.y;
+        cpp_pose.pose.orientation.z = c_pose->pose.orientation.z;
+        cpp_pose.pose.orientation.w = c_pose->pose.orientation.w;
+        return cpp_pose;
+    }
+
+    // Convert C++ PoseStamped to C PoseStamped
+    void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
+        c_pose->header.seq = cpp_pose.header.seq;
+        c_pose->header.sec = cpp_pose.header.stamp.sec;
+        c_pose->header.nsec = cpp_pose.header.stamp.nsec;
+        if (!cpp_pose.header.frame_id.empty()) {
+            c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str());
+        } else {
+            c_pose->header.frame_id = nullptr;
+        }
+        c_pose->pose.position.x = cpp_pose.pose.position.x;
+        c_pose->pose.position.y = cpp_pose.pose.position.y;
+        c_pose->pose.position.z = cpp_pose.pose.position.z;
+        c_pose->pose.orientation.x = cpp_pose.pose.orientation.x;
+        c_pose->pose.orientation.y = cpp_pose.pose.orientation.y;
+        c_pose->pose.orientation.z = cpp_pose.pose.orientation.z;
+        c_pose->pose.orientation.w = cpp_pose.pose.orientation.w;
+    }
+
+    // Convert C++ Pose2D to C Pose2D
+    void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
+        c_pose->x = cpp_pose.x;
+        c_pose->y = cpp_pose.y;
+        c_pose->theta = cpp_pose.theta;
+    }
+
+    // Convert C++ NavFeedback to C NavFeedback
+    void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
+        c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
+        if (!cpp_feedback.feed_back_str.empty()) {
+            c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str());
+        } else {
+            c_feedback->feed_back_str = nullptr;
+        }
+        c_feedback->current_pose.x = cpp_feedback.current_pose.x;
+        c_feedback->current_pose.y = cpp_feedback.current_pose.y;
+        c_feedback->current_pose.theta = cpp_feedback.current_pose.theta;
+        c_feedback->goal_checked = cpp_feedback.goal_checked;
+        c_feedback->is_ready = cpp_feedback.is_ready;
+    }
+}
+
+// ============================================================================
+// String Management
+// ============================================================================
+
+extern "C" void nav_c_api_free_string(char* str) {
+    if (str) {
+        free(str);
+    }
+}
+
+// ============================================================================
+// State Conversion
+// ============================================================================
+
+extern "C" char* navigation_state_to_string(NavigationState state) {
+    move_base_core::State cpp_state = static_cast<move_base_core::State>(static_cast<int>(state));
+    std::string str = move_base_core::toString(cpp_state);
+    return strdup(str.c_str());
+}
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
+                                          const char* frame_id, double offset_distance,
+                                          PoseStamped* out_goal) {
+    if (!out_goal || !frame_id) {
+        return false;
+    }
+
+    geometry_msgs::Pose2D pose;
+    pose.x = pose_x;
+    pose.y = pose_y;
+    pose.theta = pose_theta;
+
+    geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
+    cpp_to_c_pose_stamped(result, out_goal);
+    return true;
+}
+
+extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
+                                                PoseStamped* out_goal) {
+    if (!in_pose || !out_goal) {
+        return false;
+    }
+
+    geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
+    geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance);
+    cpp_to_c_pose_stamped(result, out_goal);
+    return true;
+}
+
+// ============================================================================
+// Navigation Handle Management
+// ============================================================================
+
+extern "C" NavigationHandle navigation_create(void) {
+    try {
+        // Create a concrete MoveBase instance
+        // Using default constructor - initialization will be done via navigation_initialize()
+        move_base::MoveBase* move_base = new move_base::MoveBase();
+        return static_cast<NavigationHandle>(move_base);
+    } catch (const std::exception& e) {
+        // Log error if possible (in production, use proper logging)
+        return nullptr;
+    } catch (...) {
+        return nullptr;
+    }
+}
+
+extern "C" void navigation_destroy(NavigationHandle handle) {
+    if (handle) {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        delete nav;
+    }
+}
+
+// ============================================================================
+// TF Listener Management
+// ============================================================================
+
+extern "C" TFListenerHandle tf_listener_create(void) {
+    try {
+        auto tf = std::make_shared<tf3::BufferCore>();
+        return new std::shared_ptr<tf3::BufferCore>(tf);
+    } catch (...) {
+        return nullptr;
+    }
+}
+
+extern "C" void tf_listener_destroy(TFListenerHandle handle) {
+    if (handle) {
+        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(handle);
+        delete tf_ptr;
+    }
+}
+
+// ============================================================================
+// Navigation Interface Methods
+// ============================================================================
+
+extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) 
+{
+    printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__);
+    if (!handle || !tf_handle) {
+        printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__);
+        return false;
+    }
+    try {
+        printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__);
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
+        
+        auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
+            "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so",
+            "MoveBase", 
+            boost::dll::load_mode::append_decorations);
+            
+        move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
+        printf("[%s:%d] Navigation created\n", __FILE__, __LINE__);
+        if (nav_ptr == nullptr) {
+            printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__);
+            return false;
+        }
+        nav_ptr->initialize(*tf_ptr);
+    
+        return true;    
+    } catch (const std::exception &e) {
+        printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
+        return false;
+    } catch (...) {
+        printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__);
+        return false;
+    }
+}
+
+extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) {
+    if (!handle || !points || point_count == 0) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        std::vector<geometry_msgs::Point> footprint;
+        footprint.reserve(point_count);
+        for (size_t i = 0; i < point_count; ++i) {
+            geometry_msgs::Point pt;
+            pt.x = points[i].x;
+            pt.y = points[i].y;
+            pt.z = points[i].z;
+            footprint.push_back(pt);
+        }
+        nav->setRobotFootprint(footprint);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
+                                    double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
+                                     const PoseStamped* goal,
+                                     double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !marker || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+                                             double xy_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+                                      double yaw_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_pause(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->pause();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" void navigation_resume(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->resume();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" void navigation_cancel(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->cancel();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
+                                             double linear_x, double linear_y, double linear_z) {
+    if (!handle) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Vector3 linear;
+        linear.x = linear_x;
+        linear.y = linear_y;
+        linear.z = linear_z;
+        return nav->setTwistLinear(linear);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
+                                              double angular_x, double angular_y, double angular_z) {
+    if (!handle) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Vector3 angular;
+        angular.x = angular_x;
+        angular.y = angular_y;
+        angular.z = angular_z;
+        return nav->setTwistAngular(angular);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) {
+    if (!handle || !out_pose) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_pose;
+        if (nav->getRobotPose(cpp_pose)) {
+            cpp_to_c_pose_stamped(cpp_pose, out_pose);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) {
+    if (!handle || !out_pose) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Pose2D cpp_pose;
+        if (nav->getRobotPose(cpp_pose)) {
+            cpp_to_c_pose_2d(cpp_pose, out_pose);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
+    if (!handle || !out_feedback) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        if (nav->nav_feedback_) {
+            cpp_to_c_nav_feedback(*nav->nav_feedback_, out_feedback);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_free_feedback(NavFeedback* feedback) {
+    if (feedback) {
+        nav_c_api_free_string(feedback->feed_back_str);
+        feedback->feed_back_str = nullptr;
+    }
+}
+

src/Algorithms/Cores/score_algorithm/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.10)
+
+# Tên dự án
+project(score_algorithm VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+set(PACKAGES_DIR nav_2d_msgs nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles)
+
+find_package(Boost REQUIRED COMPONENTS filesystem system)
+
+# Thư mục include
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+# Tạo thư viện shared (.so)
+
+add_library(score_algorithm src/score_algorithm.cpp)
+target_link_libraries(score_algorithm 
+  PUBLIC
+    ${PACKAGES_DIR}
+  PRIVATE
+    Boost::filesystem
+    Boost::system
+)
+target_include_directories(score_algorithm PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+# Cài đặt header files
+install(DIRECTORY include/score_algorithm 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+# Cài đặt library
+install(TARGETS score_algorithm  
+        EXPORT score_algorithm-targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+# Export targets
+install(EXPORT score_algorithm-targets
+        FILE score_algorithm-targets.cmake
+        DESTINATION lib/cmake/score_algorithm)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+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 "=================================")

src/Algorithms/Cores/score_algorithm/include/angles/angles.h

@@ -0,0 +1,26 @@
+#ifndef ANGLES_ANGLES_H
+#define ANGLES_ANGLES_H
+
+#include <cmath>
+
+namespace angles
+{
+
+/**
+ * @brief Normalize an angle to the range [-π, π]
+ * @param angle The angle in radians to normalize
+ * @return The normalized angle in the range [-π, π]
+ */
+inline double normalize_angle(double angle)
+{
+  while (angle > M_PI)
+    angle -= 2.0 * M_PI;
+  while (angle < -M_PI)
+    angle += 2.0 * M_PI;
+  return angle;
+}
+
+} // namespace angles
+
+#endif // ANGLES_ANGLES_H
+

src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h

@@ -0,0 +1,56 @@
+#ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__
+#define _SCORE_GOAL_CHECKER_H_INCLUDED__
+
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_2d_msgs/Twist2D.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+#include <nav_2d_msgs/Path2D.h>
+#include <memory>
+
+namespace score_algorithm
+{
+
+  /**
+   * @class GoalChecker
+   * @brief Function-object for checking whether a goal has been reached
+   *
+   * This class defines the plugin interface for determining whether you have reached
+   * the goal state. This primarily consists of checking the relative positions of two poses
+   * (which are presumed to be in the same frame). It can also check the velocity, as some
+   * applications require that robot be stopped to be considered as having reached the goal.
+   */
+  class GoalChecker
+  {
+  public:
+    using Ptr = std::shared_ptr<score_algorithm::GoalChecker>;
+
+    virtual ~GoalChecker() {}
+
+    /**
+     * @brief Initialize any parameters from the NodeHandle
+     * @param nh NodeHandle for grabbing parameters
+     */
+    virtual void initialize(const ros::NodeHandle& nh) = 0;
+
+    /**
+     * @brief Reset the state of the goal checker (if any)
+     */
+    virtual void reset() {}
+
+    /**
+     * @brief Check whether the goal should be considered reached
+     * @param query_pose The pose to check
+     * @param goal_pose The pose to check against
+     *
+     * @param velocity The robot's current velocity
+     * @return True if goal is reached
+     */
+    virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose,
+      const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D& velocity) = 0;
+  };
+
+}  // namespace score_algorithm
+
+#endif  // _SCORE_GOAL_CHECKER_H_INCLUDED__

src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h

@@ -0,0 +1,169 @@
+#ifndef _SCORE_ALGORITHM_H_INCLUDED__
+#define _SCORE_ALGORITHM_H_INCLUDED__
+
+#include <yaml-cpp/yaml.h>
+#include <iostream>
+#include <nav_2d_msgs/Twist2DStamped.h>
+#include <nav_2d_msgs/Path2D.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+
+#include <nav_core2/common.h>
+#include <nav_core2/exceptions.h>
+#include <nav_core2/costmap.h>
+
+#include <costmap_2d/costmap_2d_robot.h>
+#include <geometry_msgs/PoseArray.h>
+#include <mkt_msgs/Trajectory2D.h>
+#include <score_algorithm/trajectory_generator.h>
+
+#define FORWARD 1
+#define BACKWARD -1
+
+namespace score_algorithm
+{
+  class ScoreAlgorithm
+  {
+  public:
+    using Ptr = boost::shared_ptr<score_algorithm::ScoreAlgorithm>;
+
+    inline double smoothstep(double x)
+    {
+      x = std::clamp(x, 0.0, 1.0);
+      return x * x * (3 - 2 * x);
+    }
+
+    /**
+     * @brief Initialize parameters as needed
+     * @param name Namespace for this planner
+     * @param tf TFListener pointer
+     * @param costmap_ros Costmap pointer
+     */
+    virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros,
+                            const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
+  
+    /**
+     * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+     *
+     * Subclasses may overwrite. Return false in case there is any error.
+     *
+     * @param pose Current pose (costmap frame)
+     * @param velocity Current velocity
+     * @param goal The final goal (costmap frame)
+     * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+     */
+    virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
+                         const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
+                         double &x_direction, double &y_direction, double &theta_direction) = 0;
+
+    /**
+     * @brief Calculating algorithm
+     * @param pose
+     * @param velocity
+     * @param traj
+     */
+    virtual mkt_msgs::Trajectory2D calculator(
+        const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
+
+    /**
+     * @brief Reset all data
+     */
+    virtual void reset() {};
+
+    /**
+     * @brief Stoping move navigation
+     */
+    virtual void stop() {};
+
+    /**
+     * @brief resume move navigation after stopped
+     */
+    virtual void resume() {};
+
+    std::string getName()
+    {
+      return name_;
+    }
+
+  protected:
+    double sign(double x)
+    {
+      return x < 0.0 ? -1.0 : 1.0;
+    }
+
+    /**
+     * @brief Get goal index
+     * @param robot_pose
+     * @param plan
+     * @param distance
+     * @param start_index
+     * @param last_valid_index
+     * @return goal index
+     */
+    virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<nav_2d_msgs::Pose2DStamped> &plan,
+                                      const double distance, unsigned int start_index, unsigned int last_valid_index) const;
+
+    /**
+     * @brief Get goal pose
+     * @param robot_pose
+     * @param velocity
+     * @param global_plan
+     * @param sub_goal
+     * @param S
+     * @return true if goal pose is found, false otherwise
+     */
+    virtual bool computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S,
+                                    const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result);
+
+    /**
+     * @brief Find sub goal index
+     * @param plan
+     * @param index_s
+     * @return true if sub goal index is found, false otherwise
+     */
+    virtual bool findSubGoalIndex(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes);
+
+    /**
+     * @brief Calculate journey
+     * @param plan
+     * @param start_index
+     * @param last_valid_index
+     * @return journey
+     */
+    double journey(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
+
+    /**
+     * @brief  Check whether the robot is stopped or not
+     * @param velocity The current odometry information for the robot
+     * @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
+     * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
+     * @return True if the robot is stopped, false otherwise
+     */
+    bool stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity);
+
+    void clear();
+
+    std::string name_;
+    TFListenerPtr tf_;
+    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    TrajectoryGenerator::Ptr traj_;
+
+    int index_samples_;
+    bool follow_step_path_;
+    double xy_local_goal_tolerance_;
+    double old_xy_goal_tolerance_;
+    double angle_threshold_;
+
+    bool enable_publish_;
+    // ros::Publisher reached_intermediate_goals_pub_;
+    // ros::Publisher current_goal_pub_;
+    // ros::Publisher closet_robot_goal_pub_;
+    // ros::Publisher transformed_plan_pub_, compute_plan_pub_;
+
+    std::vector<nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
+    std::vector<unsigned int> start_index_saved_vt_;
+    unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
+    unsigned int sub_start_index_saved_, sub_start_seq_saved_;
+  };
+} // namespace score_algorithm
+
+#endif // SCORE_ALGORITHM_H_INCLUDED__

src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h

@@ -0,0 +1,136 @@
+#ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
+#define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
+
+#include <yaml-cpp/yaml.h>
+#include <mkt_msgs/Trajectory2D.h>
+#include <nav_2d_msgs/Path2D.h>
+#include <nav_2d_msgs/Twist2D.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+#include <memory>
+#include <vector>
+#include <geometry_msgs/Vector3.h>
+
+namespace score_algorithm
+{
+
+  /**
+   * @class TrajectoryGenerator
+   * @brief Interface for iterating through possible velocities and creating trajectories
+   *
+   * This class defines the plugin interface for two separate but related components.
+   *
+   * First, this class provides an iterator interface for exploring all of the velocities
+   * to search, given the current velocity.
+   *
+   * Second, the class gives an independent interface for creating a trajectory from a twist,
+   * i.e. projecting it out in time and space.
+   *
+   * Both components rely heavily on the robot's kinematic model, and can share many parameters,
+   * which is why they are grouped into a singular class.
+   */
+  class TrajectoryGenerator
+  {
+  public:
+    using Ptr = boost::shared_ptr<score_algorithm::TrajectoryGenerator>;
+
+    virtual ~TrajectoryGenerator() {}
+
+    /**
+     * @brief Initialize parameters as needed
+     * @param nh NodeHandle to read parameters from
+     */
+    virtual void initialize(const YAML::Node &nh) = 0;
+
+    /**
+     * @brief Reset the state (if any) when the planner gets a new goal
+     */
+    virtual void reset() {}
+
+    /**
+     * @brief Start a new iteration based on the current velocity
+     * @param current_velocity
+     */
+    virtual void startNewIteration(const nav_2d_msgs::Twist2D &current_velocity) = 0;
+
+    /**
+     * @brief Set direct of robot based on the current velocity
+     * @param direct
+     */
+    virtual void setDirect(int *xytheta_direct) = 0;
+
+    /**
+     * @brief set velocity for x, y asix of the robot
+     * @param linear the velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual bool setTwistLinear(geometry_msgs::Vector3 linear) = 0;
+
+    /**
+     * @brief get velocity for x, y asix of the robot
+     * @param linear The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
+
+    /**
+     * @brief Set velocity theta for z asix of the robot
+     * @param angular the command velocity
+     * @return True if set is done, false otherwise
+     */
+    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) = 0;
+
+    /**
+     * @brief Get velocity theta for z asix of the robot
+     * @param direct The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
+
+    /**
+     * @brief Test to see whether there are more twists to test
+     * @return True if more twists, false otherwise
+     */
+    virtual bool hasMoreTwists() = 0;
+
+    /**
+     * @brief Return the next twist and advance the iteration
+     * @return The Twist!
+     */
+    virtual nav_2d_msgs::Twist2D nextTwist() = 0;
+
+    /**
+     * @brief Get all the twists for an iteration.
+     *
+     * Note: Resets the iterator if one is in process
+     *
+     * @param current_velocity
+     * @return all the twists
+     */
+    virtual std::vector<nav_2d_msgs::Twist2D> getTwists(const nav_2d_msgs::Twist2D &current_velocity)
+    {
+      std::vector<nav_2d_msgs::Twist2D> twists;
+      startNewIteration(current_velocity);
+      while (hasMoreTwists())
+      {
+        twists.push_back(nextTwist());
+      }
+      return twists;
+    }
+
+    /**
+     * @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
+     * @param start_pose Current robot location
+     * @param start_vel Current robot velocity
+     * @param cmd_vel The desired command velocity
+     */
+    virtual mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose,
+                                                      const nav_2d_msgs::Path2D &transformed_plan,
+                                                      const nav_2d_msgs::Twist2D &start_vel,
+                                                      const nav_2d_msgs::Twist2D &cmd_vel) = 0;
+
+    virtual YAML::Node getNodeHandle() = 0;
+  };
+
+} // namespace score_algorithm
+
+#endif // _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H

src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp

@@ -0,0 +1,470 @@
+#include <score_algorithm/score_algorithm.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/conversions.h>
+#include <angles/angles.h>
+
+unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<nav_2d_msgs::Pose2DStamped> &plan,
+                                                           const double distance, unsigned int start_index, unsigned int last_valid_index) const
+{
+  if (start_index >= last_valid_index)
+    return last_valid_index;
+
+  unsigned int goal_index = start_index + 1;
+  const double start_direction_x = plan[goal_index].pose.x - plan[start_index].pose.x;
+  const double start_direction_y = plan[goal_index].pose.y - plan[start_index].pose.y;
+  if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
+  { // make sure that atan2 is defined
+    double start_angle = atan2(start_direction_y, start_direction_x);
+    double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2));
+    // double angle_threshold = 0.0;
+    for (unsigned int i = start_index + 1; i <= last_valid_index; i++)
+    {
+      const double current_direction_x = plan[i].pose.x - plan[i - 1].pose.x;
+      const double current_direction_y = plan[i].pose.y - plan[i - 1].pose.y;
+      const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2));
+      S += dd;
+      if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
+      {
+        double current_angle = atan2(current_direction_y, current_direction_x);
+        goal_index = i;
+        if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance)
+        {
+          break;
+        }
+      }
+    }
+  }
+  if (last_valid_index - goal_index == 1)
+    goal_index = last_valid_index;
+  return goal_index;
+}
+
+bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes)
+{
+  if (plan.empty())
+  {
+    return false;
+  }
+  double x_direction_saved = 0.0;
+  geometry_msgs::PoseArray poseArrayMsg;
+  poseArrayMsg.header.stamp = robot::Time::now();
+  poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
+
+  nav_2d_msgs::Pose2DStamped p1, p2, p3;
+
+  if (plan.size() < 3)
+  {
+    seq_s.push_back(plan.back().header.seq);
+    mutes.push_back(plan.back());
+  }
+  else
+  {
+    unsigned int i = 0;
+    p1 = plan[i];
+    for (i = 0; i < (unsigned int)plan.size() - 2; i++)
+    {
+      double egde1_angle, egde2_angle;
+      p2 = plan[i + 1];
+      if ((fabs(p2.pose.y - p1.pose.y) > 1e-9 || fabs(p2.pose.x - p1.pose.x) > 1e-9))
+      {
+        egde1_angle = atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x);
+      }
+      else
+        continue;
+
+      const double target_direction = cos(fabs(egde1_angle - p2.pose.theta));
+
+      if (target_direction * x_direction_saved < 0.0)
+      {
+        if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end())
+        {
+          seq_s.push_back(plan[i].header.seq);
+          mutes.push_back(plan[i]);
+          geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
+          poseArrayMsg.poses.push_back(pose);
+        }
+      }
+      x_direction_saved = target_direction;
+
+      p3 = plan[i + 2];
+      if ((fabs(p3.pose.y - p2.pose.y) > 1e-9 || fabs(p3.pose.x - p2.pose.x) > 1e-9))
+      {
+        egde2_angle = atan2(p3.pose.y - p2.pose.y, p3.pose.x - p2.pose.x);
+        p1 = p2;
+      }
+      else
+        continue;
+
+      double thresh_angle = 0.3;
+      double angle = fabs(remainder(egde1_angle - egde2_angle, 2 * M_PI));
+      if (angle >= thresh_angle || angle >= M_PI - thresh_angle)
+      {
+        if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end())
+        {
+          seq_s.push_back(plan[i + 1].header.seq);
+          mutes.push_back(plan[i + 1]);
+          geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
+          poseArrayMsg.poses.push_back(pose);
+        }
+        p2 = p3;
+      }
+    }
+  }
+
+  if (seq_s.empty())
+  {
+    seq_s.push_back(plan.back().header.seq);
+    mutes.push_back(plan.back());
+    geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
+    poseArrayMsg.poses.push_back(pose);
+  }
+  else
+  {
+    if (seq_s.back() < plan.back().header.seq)
+    {
+      seq_s.push_back(plan.back().header.seq);
+      mutes.push_back(plan.back());
+      geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
+      poseArrayMsg.poses.push_back(pose);
+    }
+  }
+  return true;
+}
+
+bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S,
+                                                         const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result)
+{
+  result.poses.clear();
+  if (global_plan.poses.empty())
+  {
+     std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl; 
+    return false;
+  }
+
+  std::vector<unsigned int> seq_s;
+  std::vector<nav_2d_msgs::Pose2DStamped> mutes;
+  if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes))
+  {
+    std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl;
+    return false;
+  }
+
+  unsigned int sub_goal_index;
+  unsigned int sub_start_index;
+
+  bool found_sub_goal_index_saved = false, found_sub_start_index_saved = false;
+  for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i)
+  {
+    if (global_plan.poses[i].header.seq == sub_goal_seq_saved_)
+    {
+      sub_goal_index_saved_ = i;
+      found_sub_goal_index_saved = true;
+    }
+    if (global_plan.poses[i].header.seq == sub_start_seq_saved_)
+    {
+      sub_start_index_saved_ = i;
+      found_sub_start_index_saved = true;
+    }
+    if (found_sub_goal_index_saved && found_sub_start_index_saved)
+    {
+      break;
+    }
+  }
+
+  sub_goal_index = !found_sub_goal_index_saved ? (unsigned int)global_plan.poses.size() : sub_goal_index_saved_;
+  sub_start_index = !found_sub_start_index_saved ? 0 : sub_start_index_saved_;
+
+  std::vector<unsigned int> index_s;
+  for (auto seq : seq_s)
+  {
+    for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i)
+    {
+      if (global_plan.poses[i].header.seq == seq)
+      {
+        index_s.push_back(i);
+        break;
+      }
+    }
+  }
+
+  // Handle empty seq_s
+  if (index_s.empty())
+  {
+    sub_goal_index = (unsigned int)global_plan.poses.size();
+    sub_goal_seq_saved_ = global_plan.poses.back().header.seq;
+    std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl;
+  }
+  else
+  {
+    // Update sub_goal_index if index_s.front() is greater and valid
+    if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
+    {
+      sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
+      sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
+      std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
+    }
+
+    // Process index_s with multiple elements
+    if (index_s.size() > 1)
+    {
+      for (size_t i = 0; i < index_s.size(); ++i)
+      {
+        if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
+        {
+          std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl;
+          continue;
+        }
+
+        double dx = robot_pose.pose.x - global_plan.poses[index_s[i - 1]].pose.x;
+        double dy = robot_pose.pose.y - global_plan.poses[index_s[i - 1]].pose.y;
+        double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[index_s[i - 1]].pose.theta);
+        double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
+        double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
+
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        {
+          double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+          if (fabs(tolerance) <= xy_local_goal_tolerance_)
+          {
+            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta);
+            // ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+            if (index_s[i] > sub_goal_index_saved_)
+            {
+              // ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size());
+              // ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size());
+              sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
+              sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
+              old_xy_goal_tolerance_ = 0.0;
+              if (follow_step_path_)
+              {
+                sub_start_index = index_s[i - 1];
+                sub_start_seq_saved_ = global_plan.poses[sub_start_index].header.seq;
+              }
+              break;
+            }
+          }
+        }
+        else
+          old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+      }
+    }
+    else
+    {
+      // Validate sub_goal_index
+      if (sub_goal_index >= (unsigned int)global_plan.poses.size() - 1)
+      {
+        sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
+        sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
+        std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
+      }
+      else
+      {
+        double dx = robot_pose.pose.x - global_plan.poses[sub_goal_index].pose.x;
+        double dy = robot_pose.pose.y - global_plan.poses[sub_goal_index].pose.y;
+        double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
+        double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
+        double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        {
+          double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+          if (fabs(tolerance) <= xy_local_goal_tolerance_)
+          {
+            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta);
+            // ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+            sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
+            sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
+            old_xy_goal_tolerance_ = 0.0;
+            std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl;
+          }
+        }
+        else
+          old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+      }
+    }
+  }
+  if (sub_start_index >= sub_goal_index)
+  {
+    std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl;
+    return false;
+  }
+
+  // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
+  // Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
+  unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi
+  unsigned int closet_index = 0;
+  double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
+  bool started_path = false;
+  for (unsigned int i = sub_start_index; i < sub_goal_index; ++i)
+  {
+    // Still on the costmap. Continue.
+    double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[i].pose, robot_pose.pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
+    if (distance_to_start > distance)
+    {
+      start_index = i;
+      closet_index = start_index;
+      distance_to_start = distance;
+      started_path = true;
+    }
+  }
+
+  // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
+  if (!started_path)
+  {
+    std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl;
+    return false;
+  }
+  std::stringstream ss;
+  start_index_saved_vt_.push_back(global_plan.poses[start_index].header.seq);
+  if (start_index_saved_vt_.size() > index_samples_)
+    start_index_saved_vt_.erase(start_index_saved_vt_.begin());
+
+  unsigned num = 0;
+  if (!start_index_saved_vt_.empty())
+  {
+    for (int i = 0; i < (int)start_index_saved_vt_.size() - 1; i++)
+    {
+      if (start_index_saved_vt_[i] == start_index_saved_vt_[i + 1])
+        num++;
+      ss << start_index_saved_vt_[i] << " ";
+    }
+  }
+  if (num < index_samples_ - 1 && !start_index_saved_vt_.empty())
+  {
+    auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end()));
+    unsigned int max_start_index = start_index;
+    for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); i++)
+    {
+      if (global_plan.poses[i].header.seq == max_iter)
+      {
+        max_start_index = i;
+        break;
+      }
+    }
+    double S1 = journey(global_plan.poses, start_index, (unsigned int)global_plan.poses.size() - 1);
+    double S2 = journey(global_plan.poses, max_start_index, (unsigned int)global_plan.poses.size() - 1);
+
+    if (S2 - S1)
+    {
+      start_index = max_start_index;
+    }
+  }
+  // ROS_INFO_STREAM_THROTTLE(0.1, ss.str() << "\n");
+
+  // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
+  // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
+  if (start_index < 1)
+    start_index = 1;
+  // unsigned int last_valid_index = (unsigned int)global_plan.poses.size() - 1;
+  unsigned int last_valid_index = sub_goal_index;
+
+  // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
+  // i.e. is within angle_threshold_ of the starting direction.
+  // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
+  unsigned int goal_index = start_index;
+  while (goal_index < last_valid_index)
+  {
+    // Tìm kiếm vị trí điểm mục tiêu phù hợp
+    goal_index = getGoalIndex(robot_pose.pose, global_plan.poses, S, start_index, last_valid_index);
+
+    // check if goal already reached
+    bool goal_already_reached = false;
+
+    // geometry_msgs::PoseArray poseArrayMsg;
+    // poseArrayMsg.header.stamp = ros::Time::now();
+    // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
+    // int c = 0;
+    for (auto &reached_intermediate_goal : reached_intermediate_goals_)
+    {
+      double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
+
+      // geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
+      // poseArrayMsg.poses.push_back(pose);
+
+      if (distance < xy_local_goal_tolerance_)
+      {
+        goal_already_reached = true;
+        // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
+        // (start_index is now > goal_index)
+        for (start_index = goal_index; start_index < last_valid_index; ++start_index)
+        {
+          distance = nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[start_index].pose);
+          if (distance >= xy_local_goal_tolerance_)
+            break;
+        }
+        break;
+      }
+    }
+
+    if (!goal_already_reached)
+    {
+      // new goal not in reached_intermediate_goals_
+      double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[start_index].pose, global_plan.poses[goal_index].pose));
+      if (distance < xy_local_goal_tolerance_)
+      {
+        // the robot has currently reached the goal
+        reached_intermediate_goals_.push_back(global_plan.poses[goal_index]);
+        std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl;
+      }
+      else
+      {
+        // we've found a new goal!
+        break;
+      }
+    }
+  }
+
+  if (start_index > goal_index)
+    start_index = goal_index;
+  if (goal_index > last_valid_index)
+    goal_index = last_valid_index;
+  if ((unsigned int)global_plan.poses.size() == 2 && !global_plan.poses.empty())
+  {
+    start_index = 0;
+    goal_index = 1;
+  }
+
+  if (goal_index == (unsigned int)global_plan.poses.size() - 1)
+    start_index = (unsigned int)global_plan.poses.size() - 2;
+
+  start_index_global_plan = start_index;
+  goal_index_global_plan = goal_index;
+
+  nav_2d_msgs::Pose2DStamped sub_pose;
+  sub_pose = global_plan.poses[closet_index];
+
+  nav_2d_msgs::Pose2DStamped sub_goal;
+  sub_goal = global_plan.poses[goal_index];
+
+  result.header = global_plan.header;
+  for (int i = closet_index; i <= goal_index; i++)
+  {
+    result.poses.push_back(global_plan.poses[i]);
+  }
+  return true;
+}
+
+double score_algorithm::ScoreAlgorithm::journey(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
+{
+  double S = 0;
+  if (last_valid_index - start_index > 1)
+  {
+    for (int i = start_index; i < last_valid_index; i++)
+    {
+      S += nav_2d_utils::poseDistance(plan[i].pose, plan[i + 1].pose);
+    }
+  }
+  return S;
+}
+
+bool score_algorithm::ScoreAlgorithm::stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
+{
+  return fabs(velocity.theta) <= rot_stopped_velocity && fabs(velocity.x) <= trans_stopped_velocity && fabs(velocity.y) <= trans_stopped_velocity;
+}
+
+void score_algorithm::ScoreAlgorithm::clear()
+{
+  reached_intermediate_goals_.clear();
+  start_index_saved_vt_.clear();
+  sub_goal_index_saved_ = sub_goal_seq_saved_ = 0;
+  sub_start_index_saved_, sub_start_seq_saved_ = 0;
+}

src/Algorithms/Libraries/angles/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.10)
+project(angles VERSION 1.0.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# Create interface library (header-only)
+add_library(angles INTERFACE)
+
+# Set include directories
+target_include_directories(angles INTERFACE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Install headers
+install(DIRECTORY include/angles
+  DESTINATION include
+  FILES_MATCHING PATTERN "*.h"
+)
+
+# Install target
+install(TARGETS angles
+  EXPORT angles-targets
+  INCLUDES DESTINATION include
+)
+
+# Export targets
+install(EXPORT angles-targets
+  FILE angles-targets.cmake
+  DESTINATION lib/cmake/angles
+)
+

src/Algorithms/Libraries/angles/include/angles/angles.h

@@ -0,0 +1,26 @@
+#ifndef ANGLES_ANGLES_H
+#define ANGLES_ANGLES_H
+
+#include <cmath>
+
+namespace angles
+{
+
+/**
+ * @brief Normalize an angle to the range [-π, π]
+ * @param angle The angle in radians to normalize
+ * @return The normalized angle in the range [-π, π]
+ */
+inline double normalize_angle(double angle)
+{
+  while (angle > M_PI)
+    angle -= 2.0 * M_PI;
+  while (angle < -M_PI)
+    angle += 2.0 * M_PI;
+  return angle;
+}
+
+} // namespace angles
+
+#endif // ANGLES_ANGLES_H
+

src/Algorithms/Libraries/kalman/CMakeLists.txt

@@ -0,0 +1,92 @@
+cmake_minimum_required(VERSION 3.10)
+project(kalman VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Find dependencies
+find_package(Eigen3 REQUIRED)
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+    ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Tìm tất cả file source
+file(GLOB SOURCES "src/kalman.cpp")
+file(GLOB HEADERS "include/kalman/kalman.h")
+
+# Tạo thư viện shared (.so)
+add_library(kalman SHARED ${SOURCES} ${HEADERS})
+
+# Link libraries
+target_link_libraries(kalman
+    PUBLIC
+        Eigen3::Eigen
+)
+
+# Set include directories
+target_include_directories(kalman
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+        ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Tạo executable cho test
+add_executable(kalman_node src/kalman-test.cpp)
+target_link_libraries(kalman_node
+    PRIVATE
+        kalman
+        Eigen3::Eigen
+)
+
+# Cài đặt header files
+install(DIRECTORY include/kalman
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+)
+
+# Cài đặt library
+install(TARGETS kalman
+    EXPORT kalman-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Cài đặt executable (optional)
+install(TARGETS kalman_node
+    RUNTIME DESTINATION bin
+)
+
+# Export targets
+install(EXPORT kalman-targets
+    FILE kalman-targets.cmake
+    DESTINATION lib/cmake/kalman
+)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Eigen3 found: ${EIGEN3_FOUND}")
+message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIRS}")
+message(STATUS "=================================")

src/Algorithms/Libraries/kalman/README.md

@@ -0,0 +1,18 @@
+Kalman Filter
+=============
+This is a basic Kalman filter implementation in C++ using the
+[Eigen](http://eigen.tuxfamily.org/) library. It implements the algorithm
+directly as found in [An Introduction to the Kalman Filter](http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf).
+
+There is a test program that estimates the motion of a projectile based on
+noisy observations. To run it, use CMake:
+
+    cd kalman-cpp
+    mkdir build
+    cd build
+    cmake ..
+    make
+    ./kalman-test
+
+Note: You may have to specify the path to your Eigen library in
+`CMakeLists.txt`.

src/Algorithms/Libraries/kalman/include/kalman/kalman.h

@@ -0,0 +1,91 @@
+/**
+* Kalman filter implementation using Eigen. Based on the following
+* introductory paper:
+*
+*     http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
+*
+* @author: Hayk Martirosyan
+* @date: 2014.11.15
+*/
+
+#include <Eigen/Dense>
+
+#pragma once
+
+class KalmanFilter {
+
+public:
+
+  /**
+  * Create a Kalman filter with the specified matrices.
+  *   A - System dynamics matrix
+  *   C - Output matrix
+  *   Q - Process noise covariance
+  *   R - Measurement noise covariance
+  *   P - Estimate error covariance
+  */
+  KalmanFilter(
+      double dt,
+      const Eigen::MatrixXd& A,
+      const Eigen::MatrixXd& C,
+      const Eigen::MatrixXd& Q,
+      const Eigen::MatrixXd& R,
+      const Eigen::MatrixXd& P
+  );
+
+  /**
+  * Create a blank estimator.
+  */
+  KalmanFilter();
+
+  /**
+  * Initialize the filter with initial states as zero.
+  */
+  void init();
+
+  /**
+  * Initialize the filter with a guess for initial states.
+  */
+  void init(double t0, const Eigen::VectorXd& x0);
+
+  /**
+  * Update the estimated state based on measured values. The
+  * time step is assumed to remain constant.
+  */
+  void update(const Eigen::VectorXd& y);
+
+  /**
+  * Update the estimated state based on measured values,
+  * using the given time step and dynamics matrix.
+  */
+  void update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A);
+
+  /**
+  * Return the current state and time.
+  */
+  Eigen::VectorXd state() { return x_hat; };
+  double time() { return t; };
+
+private:
+
+  // Matrices for computation
+  Eigen::MatrixXd A, C, Q, R, P, K, P0;
+
+  // System dimensions
+  int m, n;
+
+  // Initial and current time
+  double t0, t;
+
+  // Discrete time step
+  double dt;
+
+  // Is the filter initialized?
+  bool initialized;
+
+  // n-size identity
+  Eigen::MatrixXd I;
+
+  // Estimated states
+  Eigen::VectorXd x_hat, x_hat_new;
+};

src/Algorithms/Libraries/kalman/src/kalman-test.cpp

@@ -0,0 +1,77 @@
+/**
+ * Test for the KalmanFilter class with 1D projectile motion.
+ *
+ * @author: Hayk Martirosyan
+ * @date: 2014.11.15
+ */
+
+#include <iostream>
+#include <vector>
+#include <Eigen/Dense>
+
+#include <kalman/kalman.h>
+
+int main(int argc, char* argv[]) {
+
+  int n = 3; // Number of states
+  int m = 1; // Number of measurements
+
+  double dt = 1.0/30; // Time step
+
+  Eigen::MatrixXd A(n, n); // System dynamics matrix
+  Eigen::MatrixXd C(m, n); // Output matrix
+  Eigen::MatrixXd Q(n, n); // Process noise covariance
+  Eigen::MatrixXd R(m, m); // Measurement noise covariance
+  Eigen::MatrixXd P(n, n); // Estimate error covariance
+
+  // Discrete LTI projectile motion, measuring position only
+  A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
+  C << 1, 0, 0;
+
+  // Reasonable covariance matrices
+  Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
+  R << 5;
+  P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
+
+  std::cout << "A: \n" << A << std::endl;
+  std::cout << "C: \n" << C << std::endl;
+  std::cout << "Q: \n" << Q << std::endl;
+  std::cout << "R: \n" << R << std::endl;
+  std::cout << "P: \n" << P << std::endl;
+
+  // Construct the filter
+  KalmanFilter kf(dt,A, C, Q, R, P);
+
+  // List of noisy position measurements (y)
+  std::vector<double> measurements = {
+      1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,
+      1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,
+      2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,
+      2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,
+      2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,
+      2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,
+      2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,
+      1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,
+      0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813
+  };
+
+  // Best guess of initial states
+  Eigen::VectorXd x0(n);
+  double t = 0;
+  x0 << measurements[0], 0, -9.81;
+  kf.init(t, x0);
+
+  // Feed measurements into filter, output estimated states
+
+  Eigen::VectorXd y(m);
+  std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl;
+  for(int i = 0; i < measurements.size(); i++) {
+    t += dt;
+    y << measurements[i];
+    kf.update(y);
+    std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose()
+        << ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl;
+  }
+
+  return 0;
+}

src/Algorithms/Libraries/kalman/src/kalman.cpp

@@ -0,0 +1,65 @@
+/**
+* Implementation of KalmanFilter class.
+*
+* @author: Hayk Martirosyan
+* @date: 2014.11.15
+*/
+
+#include <iostream>
+#include <stdexcept>
+
+#include <kalman/kalman.h>
+
+KalmanFilter::KalmanFilter(
+    double dt,
+    const Eigen::MatrixXd& A,
+    const Eigen::MatrixXd& C,
+    const Eigen::MatrixXd& Q,
+    const Eigen::MatrixXd& R,
+    const Eigen::MatrixXd& P)
+  : A(A), C(C), Q(Q), R(R), P0(P),
+    m(C.rows()), n(A.rows()), dt(dt), initialized(false),
+    I(n, n), x_hat(n), x_hat_new(n)
+{
+  I.setIdentity();
+}
+
+KalmanFilter::KalmanFilter() {}
+
+void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
+  x_hat = x0;
+  P = P0;
+  this->t0 = t0;
+  t = t0;
+  initialized = true;
+}
+
+void KalmanFilter::init() {
+  x_hat.setZero();
+  P = P0;
+  t0 = 0;
+  t = t0;
+  initialized = true;
+}
+
+void KalmanFilter::update(const Eigen::VectorXd& y) {
+
+  if(!initialized)
+    throw std::runtime_error("Filter is not initialized!");
+
+  x_hat_new = A * x_hat;
+  P = A*P*A.transpose() + Q;
+  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
+  x_hat_new += K * (y - C*x_hat_new);
+  P = (I - K*C)*P;
+  x_hat = x_hat_new;
+
+  t += dt;
+}
+
+void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {
+
+  this->A = A;
+  this->dt = dt;
+  update(y);
+}

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -0,0 +1,110 @@
+cmake_minimum_required(VERSION 3.10)
+project(mkt_algorithm VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Find dependencies
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(Eigen3 REQUIRED)
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+    ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Dependencies packages
+set(PACKAGES_DIR
+  geometry_msgs
+  score_algorithm
+  nav_2d_msgs
+  nav_2d_utils
+  kalman
+    angles
+    nav_grid
+  costmap_2d
+    sensor_msgs
+  visualization_msgs
+    std_msgs
+)
+
+# Tìm tất cả file source cho diff library
+file(GLOB DIFF_SOURCES 
+    "src/diff/diff_predictive_trajectory.cpp"
+    "src/diff/diff_rotate_to_goal.cpp"
+    "src/diff/diff_go_straight.cpp"
+)
+
+file(GLOB DIFF_HEADERS 
+    "include/mkt_algorithm/diff/*.h"
+)
+
+# Tạo thư viện shared cho diff
+add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
+
+# Link libraries
+target_link_libraries(mkt_algorithm_diff
+    PUBLIC
+        ${PACKAGES_DIR}
+        Boost::system
+        Eigen3::Eigen
+)
+
+# Set include directories
+target_include_directories(mkt_algorithm_diff
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+        ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Cài đặt header files
+install(DIRECTORY include/mkt_algorithm
+    DESTINATION include
+  FILES_MATCHING PATTERN "*.h"
+)
+
+# Cài đặt library
+install(TARGETS mkt_algorithm_diff
+    EXPORT mkt_algorithm-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Cài đặt plugins.xml (nếu cần cho pluginlib)
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
+    install(FILES plugins.xml
+        DESTINATION share/${PROJECT_NAME}
+)
+endif()
+
+# Export targets
+install(EXPORT mkt_algorithm-targets
+    FILE mkt_algorithm-targets.cmake
+    DESTINATION lib/cmake/mkt_algorithm
+)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Eigen3 found: ${EIGEN3_FOUND}")
+message(STATUS "=================================")

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h

@@ -0,0 +1,108 @@
+#ifndef _NAV_ALGORITHM_BICYCLE_H_INCLUDED__
+#define _NAV_ALGORITHM_BICYCLE_H_INCLUDED__
+
+#include <score_algorithm/score_algorithm.h>
+#include <mkt_algorithm/equation_line.h>
+#include <pluginlib/class_loader.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <kalman/kalman.h>
+
+namespace mkt_algorithm
+{
+
+/**
+ * @class Bicycle
+ * @brief Standard Bicycle-like ScoreAlgorithm
+*/
+class Bicycle : public score_algorithm::ScoreAlgorithm
+{
+public:
+  
+  Bicycle() {}; //: line_generator_(nullptr) {};
+  
+  virtual ~Bicycle() {};
+
+  /**
+   * @brief Initialize parameters as needed
+   * @param nh NodeHandle to read parameters from
+   */
+  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+
+  /**
+   * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+   *
+   * Subclasses may overwrite. Return false in case there is any error.
+   *
+   * @param pose Current pose (costmap frame)
+   * @param velocity Current velocity
+   * @param goal The final goal (costmap frame)
+   * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+   */
+  virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) override;
+
+  /**
+   * @brief Reset all data
+  */
+  virtual void reset() override;
+
+  /**
+   * @brief Calculating algorithm 
+   * @param pose  
+   * @param velocity 
+  */
+  virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
+
+
+protected:
+
+  unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+                            const double distance, unsigned int start_index, unsigned int last_valid_index) const;
+
+  double journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
+
+
+  bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, 
+                   const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+                   double &target_direction);
+                         
+  bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
+
+  ros::NodeHandle nh_priv_, nh_;
+  std::string frame_id_path_;
+  ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
+
+  std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
+  geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
+  geometry_msgs::Pose2D goal_;
+  nav_2d_msgs::Path2D global_plan_;
+
+  unsigned int start_index_saved_;
+  unsigned int sub_goal_index_saved_, sub_start_index_saved_;
+  std::vector<unsigned int> start_index_saved_vt_;
+  int index_samples_;
+
+  double steering_fix_wheel_distance_x_;
+  double steering_fix_wheel_distance_y_;
+  double xy_local_goal_tolerance_;
+  double xy_goal_tolerance_;
+  double angle_threshold_;
+  double acc_lim_theta_;
+  double decel_lim_theta_;
+  double x_direction_, y_direction_, theta_direction_;
+  double k_d_reduce_, distance_rg_, k_ld_, ld_min_, ld_max_;
+  double controller_frequency_param_name_;
+
+  bool follow_step_path_;
+  bool is_filter_;
+  boost::shared_ptr<KalmanFilter> kf_;
+  ros::Publisher cmd_raw_pub_;
+  ros::Time last_actuator_update_;
+}; // class Bicycle 
+
+} // namespace mkt_algorithm
+
+
+#endif //_NAV_ALGORITHM_BICYCLE_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h

@@ -0,0 +1,74 @@
+#ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
+#define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+#include <iostream>
+
+namespace mkt_algorithm
+{
+
+/**
+ * @class NoSolution
+ */
+class NoSolution : public std::exception {
+  public:
+    const char * what () const throw () {
+      return "Robot cannot calculate error line!";
+    }
+}; // class NoSolution
+
+/**
+ * @class LineGenerator
+ */
+class LineGenerator 
+{
+  /* data */
+  struct LineEquation {
+    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
+  };
+
+  /* Save data present position */
+  struct PresentPosition {
+    float Gxh, Gyh;             /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot  */ 
+    float Yaw_degree;           /* Angle of Robot (degree)              */
+    float Yaw_rad;              /* Angle of Robot (degree)              */
+    float error_line;           /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2)       */
+    double distance_to_goal;    /* The distance from query pose to goal */
+    double distance_to_start;   /* The distance from query pose to start*/
+    double distace_start_goal;
+  };
+public:
+
+  /**
+   * @brief To caculated the equation of the line 
+   * @param start_point   start point of the line
+   * @param end_point     end point of the line
+   */
+  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+
+  /**
+   * @brief Calculating error 
+   * @param[in] Lm          The distance from the center of the wheel axis to the center line
+   * @param[in] pose
+   * @param[in] rev         
+   */
+  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+
+  /**
+   * @brief Calculating tolerance 
+   * @param query_pose The pose to check
+   * @param goal_pose The pose to check against
+   */
+  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+
+  /* Properties */
+  LineEquation      Leq_; 
+  PresentPosition   present_pose_;
+
+}; // class LineGenerator 
+
+} // namespace mkt_algorithm
+
+#endif // __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h

@@ -0,0 +1,70 @@
+#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
+#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
+
+#include <score_algorithm/score_algorithm.h>
+#include <pluginlib/class_loader.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace mkt_algorithm
+{
+
+/**
+ * @class GoStraight
+ * @brief Standard Bicycle-like ScoreAlgorithm
+*/
+class GoStraight : public score_algorithm::ScoreAlgorithm
+{
+public:
+  
+  GoStraight() {};
+  
+  virtual ~GoStraight() {};
+
+  /**
+   * @brief Initialize parameters as needed
+   * @param nh NodeHandle to read parameters from
+   */
+  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+
+  /**
+   * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+   *
+   * Subclasses may overwrite. Return false in case there is any error.
+   *
+   * @param pose Current pose (costmap frame)
+   * @param velocity Current velocity
+   * @param goal The final goal (costmap frame)
+   * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+   */
+  virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) override;
+
+  /**
+   * @brief Reset all data
+  */
+  virtual void reset() override;
+
+  /**
+   * @brief Calculating algorithm 
+   * @param pose  
+   * @param velocity 
+  */
+  virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
+
+
+protected:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_kinematics_;
+
+  double steering_fix_wheel_distance_x_;
+  double steering_fix_wheel_distance_y_;
+  geometry_msgs::Pose2D goal_;
+  ros::Time time_last_cmd_;
+
+}; // class GoStraight 
+
+} // namespace mkt_algorithm
+
+#endif

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h

@@ -0,0 +1,45 @@
+#ifndef _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__
+#define _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__
+
+#include <score_algorithm/goal_checker.h>
+#include <mkt_algorithm/equation_line.h>
+
+namespace mkt_algorithm
+{
+class GoalChecker : public score_algorithm::GoalChecker
+{
+public:
+
+  /**
+   * @brief Initialize any parameters from the NodeHandle
+   * @param nh NodeHandle for grabbing parameters
+   */
+  virtual void initialize(const ros::NodeHandle& nh) override;
+
+  /**
+   * @brief Reset the state of the goal checker (if any)
+   */
+  virtual void reset() override;
+
+  /**
+   * @brief Check whether the goal should be considered reached
+   * @param query_pose The pose to check
+   * @param goal_pose The pose to check against
+   * @param velocity The robot's current velocity
+   * @return True if goal is reached
+   */
+  bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, 
+                             const std::vector<geometry_msgs::PoseStamped>& path, const nav_2d_msgs::Twist2D& velocity) override;
+
+protected:
+
+  boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
+  double yaw_goal_tolerance_;
+  double xy_goal_tolerance_;
+  double old_xy_goal_tolerance_;
+  bool setup_;
+};
+
+} // namespace mkt_algorithm
+
+#endif // _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h

@@ -0,0 +1,60 @@
+#ifndef _NAV_ALGORITHM_PTA_H_INCLUDED__
+#define _NAV_ALGORITHM_PTA_H_INCLUDED__
+
+#include <score_algorithm/score_algorithm.h>
+#include <mkt_algorithm/equation_line.h>
+
+namespace mkt_algorithm
+{
+
+/**
+ * @class PTA
+ * @brief Standard PTA-like ScoreAlgorithm
+*/
+class PTA : public score_algorithm::ScoreAlgorithm 
+{
+public:
+  
+  PTA() : line_generator_(nullptr) {};
+  
+  virtual ~PTA() {};
+
+// Standard ScoreAlgorithm Interface
+  /**
+   * @brief Initialize parameters as needed
+   * @param nh NodeHandle to read parameters from
+   */
+  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+
+  /**
+   * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+   *
+   * Subclasses may overwrite. Return false in case there is any error.
+   *
+   * @param pose Current pose (costmap frame)
+   * @param velocity Current velocity
+   * @param goal The final goal (costmap frame)
+   * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+   */
+  virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) override;
+
+  /**
+   * @brief Calculating algorithm 
+   * @param pose  
+   * @param velocity 
+  */
+  virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
+
+protected:
+
+  boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
+  double pta_amplitude_max_;  // (Default: 0.5 - Range: 0.1 -> 1.0) Cai dat bien do (mot phia - m) cho phep robot di chuyen trong do
+
+}; // class PTA 
+
+} // namespace mkt_algorithm
+
+
+#endif //_NAV_ALGORITHM_PTA_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h

@@ -0,0 +1,74 @@
+#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
+#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
+
+#include <score_algorithm/score_algorithm.h>
+#include <pluginlib/class_loader.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace mkt_algorithm
+{
+
+/**
+ * @class GoStraight
+ * @brief Standard Bicycle-like ScoreAlgorithm
+*/
+class RotateToGoal : public score_algorithm::ScoreAlgorithm
+{
+public:
+  
+  RotateToGoal() {};
+  
+  virtual ~RotateToGoal() {};
+
+  /**
+   * @brief Initialize parameters as needed
+   * @param nh NodeHandle to read parameters from
+   */
+  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+
+  /**
+   * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+   *
+   * Subclasses may overwrite. Return false in case there is any error.
+   *
+   * @param pose Current pose (costmap frame)
+   * @param velocity Current velocity
+   * @param goal The final goal (costmap frame)
+   * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+   */
+  virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) override;
+
+  /**
+   * @brief Reset all data
+  */
+  virtual void reset() override;
+
+  /**
+   * @brief Calculating algorithm 
+   * @param pose  
+   * @param velocity 
+  */
+  virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
+
+
+protected:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_kinematics_;
+  geometry_msgs::Pose2D sub_goal_;
+
+  double steering_fix_wheel_distance_x_;
+  double steering_fix_wheel_distance_y_;
+  double angle_threshold_;
+  double current_direction_;
+  double velocity_rotate_min_, velocity_rotate_max_;
+  geometry_msgs::Pose2D goal_;
+  ros::Time time_last_cmd_;
+
+}; // class RotateToGoal 
+
+} // namespace mkt_algorithm
+
+#endif

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h

@@ -0,0 +1,43 @@
+#ifndef _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__
+#define _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__
+
+#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
+
+namespace mkt_algorithm
+{
+
+  namespace diff
+  {
+    /**
+     * @class GoStraight
+     * @brief Standard Bicycle-like ScoreAlgorithm
+     */
+    class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
+    {
+    public:
+      GoStraight() {};
+
+      virtual ~GoStraight() {};
+
+      /**
+       * @brief Initialize parameters as needed
+       * @param nh NodeHandle to read parameters from
+       */
+      virtual void initialize(
+          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+
+      /**
+       * @brief Calculating algorithm
+       * @param pose
+       * @param velocity
+       * @param traj
+       */
+      virtual mkt_msgs::Trajectory2D calculator(
+          const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    }; // class GoStraight
+  } // namespace diff
+
+} // namespace mkt_algorithm
+
+#endif // _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -0,0 +1,325 @@
+#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
+#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
+
+#include <score_algorithm/score_algorithm.h>
+
+#include <boost/dll/import.hpp>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+// #include <visualization_msgs/Marker.h>
+// #include <ddynamic_reconfigure/ddynamic_reconfigure.h>
+
+// #include <base_local_planner/costmap_model.h>
+// #include <base_local_planner/world_model.h>
+// #include <base_local_planner/trajectory_planner.h>
+// #include <base_local_planner/map_grid_visualizer.h>
+// #include <base_local_planner/BaseLocalPlannerConfig.h>
+// #include <teb_local_planner/pose_se2.h>
+
+#include <nav_core2/exceptions.h>
+#include <nav_core2/costmap.h>
+#include <nav_grid/coordinate_conversion.h>
+#include <angles/angles.h>
+#include <tf3/utils.h>
+#include <nav_msgs/Path.h>
+#include <kalman/kalman.h>
+#include <vector>
+
+#include <nav_2d_utils/parameters.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/conversions.h>
+
+namespace mkt_algorithm
+{
+  namespace diff
+  {
+    /**
+     * @class PredictiveTrajectory
+     * @brief Standard PredictiveTrajectory-like ScoreAlgorithm
+     */
+    class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
+    {
+    public:
+      PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
+
+      virtual ~PredictiveTrajectory();
+
+      // Standard ScoreAlgorithm Interface
+      /**
+       * @brief Initialize parameters as needed
+       * @param nh NodeHandle to read parameters from
+       */
+      virtual void initialize(
+          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+
+      /**
+       * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+       *
+       * Subclasses may overwrite. Return false in case there is any error.
+       *
+       * @param pose Current pose (costmap frame)
+       * @param velocity Current velocity
+       * @param goal The final goal (costmap frame)
+       * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+       */
+      virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
+                           const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
+                           double &x_direction, double &y_direction, double &theta_direction) override;
+
+      /**
+       * @brief Calculating algorithm
+       * @param pose
+       * @param velocity
+       * @param traj
+       */
+      virtual mkt_msgs::Trajectory2D calculator(
+          const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+      /**
+       * @brief Reset all data
+       */
+      virtual void reset() override;
+
+      /**
+       * @brief Stoping move navigation
+       */
+      virtual void stop() override;
+
+      /**
+       * @brief resume move navigation after stopped
+       */
+      virtual void resume() override;
+
+    protected:
+      inline double sign(double x)
+      {
+        return x < 0.0 ? -1.0 : 1.0;
+      }
+
+      /**
+       * @brief Initialize parameters
+       */
+      virtual void getParams();
+
+      /**
+       * @brief Initialize dynamic reconfigure
+       * @param nh NodeHandle to read parameters from
+       */
+      virtual void initDynamicReconfigure(const ros::NodeHandle &nh);
+
+      /**
+       * @brief Dynamically adjust look ahead distance based on the speed
+       * @param velocity
+       * @return look ahead distance
+       */
+      double getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity);
+
+      /**
+       * @brief Get lookahead point on the global plan
+       * @param lookahead_dist
+       * @param global_plan
+       * @return lookahead point
+       */
+      std::vector<nav_2d_msgs::Pose2DStamped>::iterator
+      getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan);
+
+      /**
+       * @brief Create carrot message
+       * @param carrot_pose
+       * @return carrot message
+       */
+      geometry_msgs::PointStamped createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose);
+
+      /**
+       * @brief Prune global plan
+       * @param tf
+       * @param pose
+       * @param global_plan
+       * @param dist_behind_robot
+       * @return true if plan is pruned, false otherwise
+       */
+      bool pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose,
+                           nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);
+
+      /**
+       * @brief  Transforms the global plan of the robot from the planner frame to the local frame (modified).
+       *
+       * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
+       * such that the index of the current goal pose is returned as well as
+       * the transformation between the global plan and the planning frame.
+       * @param tf A reference to a tf buffer
+       * @param global_plan The plan to be transformed
+       * @param pose The pose of the robot
+       * @param costmap A reference to the costmap being used so the window size for transforming can be computed
+       * @param global_frame The frame to transform the plan to
+       * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
+       * @param[out] transformed_plan Populated with the transformed plan
+       * @return \c true if the global plan is transformed, \c false otherwise
+       */
+      bool transformGlobalPlan(
+          TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
+          const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
+          nav_2d_msgs::Path2D &transformed_plan);
+
+      /**
+       * @brief Should rotate to path
+       * @param carrot_pose
+       * @param angle_to_path
+       * @return true if should rotate, false otherwise
+       */
+      bool shouldRotateToPath(
+          const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);
+
+      /**
+       * @brief Rotate to heading
+       * @param  angle_to_path
+       * @param  velocity The velocity of the robot
+       * @param  cmd_vel The velocity commands to be filled
+       */
+      void rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel);
+
+      /**
+       * @brief  the robot is moving acceleration limits
+       * @param  velocity The velocity of the robot
+       * @param  cmd_vel The velocity commands
+       * @param  cmd_vel_result The velocity commands result
+       */
+      void moveWithAccLimits(
+          const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result);
+
+      /**
+       * @brief Stop the robot taking into account acceleration limits
+       * @param  pose The pose of the robot in the global frame
+       * @param  velocity The velocity of the robot
+       * @param  cmd_vel The velocity commands to be filled
+       * @return  True if a valid trajectory was found, false otherwise
+       */
+      bool stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel);
+
+      /**
+       * @brief Apply constraints
+       * @param dist_error
+       * @param lookahead_dist
+       * @param curvature
+       * @param curr_speed
+       * @param pose_cost
+       * @param linear_vel
+       * @param sign
+       */
+      void applyConstraints(
+          const double &dist_error, const double &lookahead_dist,
+          const double &curvature, const nav_2d_msgs::Twist2D &curr_speed,
+          const double &pose_cost, double &linear_vel, double &sign_x);
+
+      std::vector<geometry_msgs::Point> interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution);
+
+      /**
+       * @brief Cost at pose
+       * @param x
+       * @param y
+       * @return cost
+       */
+      double costAtPose(const double &x, const double &y);
+
+      void updateCostAtOffset(
+          TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose,
+          double x_offset, double y_offset, double &cost_left, double &cost_right);
+
+      double computeDecelerationFactor(double remaining_distance, double decel_distance);
+
+      double getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
+
+      bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle, 
+                                    double amplitude_threshold = 0.3, size_t window_size = 20);
+
+      void publishMarkers(nav_2d_msgs::Pose2DStamped pose);
+
+      std::vector<double> angle_history_;
+      size_t window_size_;
+
+      bool initialized_;
+      bool nav_stop_, avoid_obstacles_;
+      ros::NodeHandle nh_, nh_priv_;
+      std::string frame_id_path_;
+      std::string robot_base_frame_;
+      ros::Publisher carrot_pub_;
+
+      nav_2d_msgs::Pose2DStamped goal_;
+      nav_2d_msgs::Path2D global_plan_;
+      nav_2d_msgs::Path2D compute_plan_;
+      nav_2d_msgs::Path2D transform_plan_;
+      nav_2d_msgs::Twist2D prevous_drive_cmd_;
+      ros::Publisher drive_pub_;
+
+      double x_direction_, y_direction_, theta_direction_;
+      double max_robot_pose_search_dist_;
+      double global_plan_prune_distance_{1.0};
+
+      // Lookahead
+      bool use_velocity_scaled_lookahead_dist_;
+      double lookahead_time_;
+      double lookahead_dist_;
+      double min_lookahead_dist_;
+      double max_lookahead_dist_;
+
+      // journey
+      double min_journey_squared_{1e9};
+      double max_journey_squared_{1e9};
+
+      // Rotate to heading
+      bool use_rotate_to_heading_;
+      double rotate_to_heading_min_angle_;
+
+      double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
+      double min_path_distance_, max_path_distance_;
+
+      // Regulated linear velocity scaling
+      bool use_regulated_linear_velocity_scaling_;
+      double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
+      double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
+
+      double rot_stopped_velocity_, trans_stopped_velocity_;
+
+      double min_approach_linear_velocity_;
+      double regulated_linear_scaling_min_radius_;
+      double regulated_linear_scaling_min_speed_;
+
+      bool use_cost_regulated_linear_velocity_scaling_;
+      double inflation_cost_scaling_factor_;
+      double cost_scaling_dist_, cost_scaling_gain_;
+
+      double cost_left_goal_, cost_right_goal_;
+      double cost_left_side_ , cost_right_side_;
+      double center_cost_;
+
+      // Control frequency
+      double control_duration_;
+
+      std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddr_;
+      base_local_planner::BaseLocalPlannerConfig config_;
+
+      base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use
+      base_local_planner::TrajectoryPlanner *tc_;   ///< @brief The trajectory controller
+      base_local_planner::MapGridVisualizer map_viz_;
+      std::vector<geometry_msgs::Point> footprint_spec_;
+
+      ros::Time last_actuator_update_;
+      boost::shared_ptr<KalmanFilter> kf_;
+      int m_, n_;
+      Eigen::MatrixXd A;
+      Eigen::MatrixXd C;
+      Eigen::MatrixXd Q;
+      Eigen::MatrixXd R;
+      Eigen::MatrixXd P;
+
+      ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_;
+      // visualization_msgs::Marker L_, R_;
+    }; // class PredictiveTrajectory
+
+  } // namespace diff
+
+} // namespace mkt_algorithm
+
+#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h

@@ -0,0 +1,68 @@
+#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__
+#define _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__
+
+#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
+
+namespace mkt_algorithm
+{
+  namespace diff
+  {
+    /**
+     * @class GoStraight
+     * @brief Standard Bicycle-like ScoreAlgorithm
+     */
+    class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
+    {
+    public:
+      RotateToGoal() {};
+
+      virtual ~RotateToGoal() {};
+
+      /**
+       * @brief Initialize parameters as needed
+       * @param nh NodeHandle to read parameters from
+       */
+      virtual void initialize(
+          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+
+      /**
+       * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
+       *
+       * Subclasses may overwrite. Return false in case there is any error.
+       *
+       * @param pose Current pose (costmap frame)
+       * @param velocity Current velocity
+       * @param goal The final goal (costmap frame)
+       * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
+       */
+      virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
+                           const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
+                           double &x_direction, double &y_direction, double &theta_direction) override;
+
+      /**
+       * @brief Reset all data
+       */
+      virtual void reset() override;
+
+      /**
+       * @brief Calculating algorithm
+       * @param pose
+       * @param velocity
+       * @param traj
+       */
+      virtual mkt_msgs::Trajectory2D calculator(
+          const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    protected:
+      /**
+       * @brief Initialize parameters
+       */
+      virtual void getParams() override;
+
+    }; // class RotateToGoalDiff
+
+  } // namespace diff
+
+} // namespace mkt_algorithm
+
+#endif // _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__

src/Algorithms/Libraries/mkt_algorithm/package.xml

@@ -0,0 +1,94 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mkt_algorithm</name>
+  <version>0.0.0</version>
+  <description>The mkt_algorithm package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mkt_algorithm</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>score_algorithm</build_depend>
+  <build_depend>nav_2d_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_2d_utils</build_depend>
+  <build_depend>kalman</build_depend>
+  <build_depend>ddynamic_reconfigure</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>base_local_planner</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>score_algorithm</build_export_depend>
+  <build_export_depend>nav_2d_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>nav_2d_utils</build_export_depend>
+  <build_export_depend>kalman</build_export_depend>
+  <build_export_depend>ddynamic_reconfigure</build_export_depend>
+  <build_export_depend>costmap_2d</build_export_depend>
+  <build_export_depend>base_local_planner</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>score_algorithm</exec_depend>
+  <exec_depend>nav_2d_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>nav_2d_utils</exec_depend>
+  <exec_depend>kalman</exec_depend>
+  <exec_depend>ddynamic_reconfigure</exec_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>base_local_planner</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+
+  <depend>pluginlib</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <score_algorithm plugin="${prefix}/plugins.xml"/>
+  </export>
+</package>

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp

@@ -0,0 +1,68 @@
+#include <mkt_algorithm/go_straight.h>
+#include <nav_2d_utils/parameters.h>
+#include <pluginlib/class_list_macros.h>
+#include <angles/angles.h>
+
+namespace  mkt_algorithm
+{
+
+void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+{
+  nh_       = ros::NodeHandle(nh, name);
+  name_     = name;
+  tf_       = tf;
+  costmap_ros_ = costmap_ros;
+
+  steering_fix_wheel_distance_x_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
+  steering_fix_wheel_distance_y_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
+}
+
+void GoStraight::reset()
+{
+  time_last_cmd_ = ros::Time::now();
+}
+
+bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) 
+{
+  goal_ = goal;
+
+  const double rb_direction_x  = goal_.x - pose.x;
+  const double rb_direction_y  = goal_.y - pose.y;
+  const double rb_angle = atan2(rb_direction_y, rb_direction_x);
+  double rb_dir = cos(rb_angle- pose.theta);
+  x_direction = y_direction = theta_direction = rb_dir;
+  
+  return  true;
+}
+            
+nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
+{
+  nav_2d_msgs::Twist2DStamped result;
+  result.header.stamp = ros::Time::now();
+
+  geometry_msgs::Pose2D steer_to_baselink_pose;
+  
+  if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
+    return result;
+  double odom_alpha = steer_to_baselink_pose.theta;
+  if(fabs(odom_alpha) > 0.03) return result;
+
+  double delta_x = goal_.x - pose.x;
+  double delta_y = goal_.y - pose.y;
+
+  double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); 
+  double d_reduce  = std::min(std::max(fabs(velocity.x), 0.6), 0.9);  
+  double v_min = 0.03 * velocity.x / (fabs(velocity.x) + 1e-9); 
+
+// Van toc robot
+  double vr =  d_rg > d_reduce?  velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; 
+  result.velocity.x = fabs(cos(fabs(0.0 - odom_alpha))) * vr / cos(std::min(M_PI/3,  fabs(odom_alpha)));
+  // ROS_INFO("d_rg %f d_reduce %f V = %f %f", d_rg, d_reduce,velocity.x, vr);
+  return result;
+}
+
+}; // namespace  mkt_algorithm
+
+PLUGINLIB_EXPORT_CLASS(mkt_algorithm::GoStraight, score_algorithm::ScoreAlgorithm)

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp

@@ -0,0 +1,641 @@
+#include <mkt_algorithm/bicycle.h>
+#include <pluginlib/class_list_macros.h>
+#include <nav_grid/coordinate_conversion.h>
+#include <nav_2d_utils/parameters.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <angles/angles.h>
+
+
+namespace  mkt_algorithm
+{
+
+void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+{
+  nh_priv_  = ros::NodeHandle(nh, name);
+  nh_       = ros::NodeHandle("~");
+  name_     = name;
+  tf_       = tf;
+  costmap_ros_ = costmap_ros;
+  last_actuator_update_ = ros::Time(0);
+
+  nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
+  nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
+  nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
+  nh_priv_.param("index_samples", index_samples_, 0);
+  nh_priv_.param("filter", is_filter_, false);
+
+  steering_fix_wheel_distance_x_    = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1);
+  steering_fix_wheel_distance_y_    = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.);
+  xy_goal_tolerance_                = nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1);
+  acc_lim_theta_                    = nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1);
+  decel_lim_theta_                  = nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1);
+  controller_frequency_param_name_  = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
+  if(controller_frequency_param_name_ <= 0.0) 
+  {
+    ROS_WARN("controller_frequency is not setting lower 0.0 so will set to 1.0");
+    controller_frequency_param_name_ = 1.0;
+  }
+
+  reached_intermediate_goals_pub_   = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
+  current_goal_pub_                 = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
+  closet_robot_goal_pub_            = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
+  cmd_raw_pub_                      = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 1);
+  x_direction_ = y_direction_= theta_direction_ = 0;
+
+// kalman
+  int n = 3; // Number of states
+  int m = 1; // Number of measurements
+  double dt = 1.0/controller_frequency_param_name_; // Time step
+  // ROS_INFO("controller_frequency_param_name %f", controller_frequency_param_name);
+  Eigen::MatrixXd A(n, n); // System dynamics matrix
+  Eigen::MatrixXd C(m, n); // Output matrix
+  Eigen::MatrixXd Q(n, n); // Process noise covariance
+  Eigen::MatrixXd R(m, m); // Measurement noise covariance
+  Eigen::MatrixXd P(n, n); // Estimate error covariance
+
+  // Discrete LTI projectile motion, measuring position only
+  A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
+  C << 1, 0, 0;
+
+  // Reasonable covariance matrices
+  double q = 0.1;
+  double r = 0.6;
+  double p = 0.2;
+  Q << q, q, 0.0, q, q, 0.0, 0.0, 0.0, 0.0;
+  R << r;
+  P << p, p, p, p, 10.0, 1, p, 1, 10.0;
+
+  // Construct the filter
+  kf_ = boost::make_shared<KalmanFilter>(dt,A, C, Q, R, P); 
+  // Best guess of initial states
+  Eigen::VectorXd x0(n);
+  x0 << 0.0, 0.0, 0.0;
+  kf_->init(ros::Time::now().toSec(), x0);
+}
+
+void Bicycle::reset()
+{
+  reached_intermediate_goals_.clear();
+  start_index_saved_vt_.clear();
+  start_index_saved_ = 0;
+  sub_goal_index_saved_ = 0;
+  sub_start_index_saved_ = 0;
+  x_direction_ = y_direction_= theta_direction_ = 0;
+  follow_step_path_ = false;
+  last_actuator_update_ = ros::Time(0);
+}
+
+bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) 
+{
+  nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
+  nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
+  nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
+  nh_priv_.param("index_samples", index_samples_, 0);
+  nh_priv_.param("distance_rg", distance_rg_, 0.1);
+  nh_priv_.param("k_ld", k_ld_, 1.0);
+  nh_priv_.param("k_d_reduce", k_d_reduce_, 1.0);
+  nh_priv_.param("ld_min", ld_min_, 0.2);
+  nh_priv_.param("ld_max", ld_max_, 1.1);
+  nh_priv_.param("follow_step_path", follow_step_path_, false);
+
+  // ROS_INFO("%f %f %f %d", xy_local_goal_tolerance_, xy_local_goal_tolerance_offset_, angle_threshold_, index_samples_);
+  frame_id_path_ = global_plan.header.frame_id;
+  goal_ = goal;
+  global_plan_ = global_plan;
+  double rb_direction;
+  if(!getGoalPose(pose, velocity,global_plan, sub_goal_, rb_direction))
+  {
+    ROS_ERROR("getGoalPose Failed");
+    return false;
+  }
+  x_direction_= fabs(rb_direction)/rb_direction;
+  x_direction = x_direction_;
+  y_direction = y_direction_ = 0;
+  theta_direction = theta_direction_;
+  return true;
+}
+            
+nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
+{
+  nav_2d_msgs::Twist2DStamped result;
+  result.header.stamp = ros::Time::now();
+  
+  geometry_msgs::Pose2D steer_to_baselink_pose;
+  
+  if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
+    return result;
+  double actual_phi_steer = steer_to_baselink_pose.theta;
+
+  const double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
+  const double delta_x = sub_goal_.x - pose.x;
+  const double delta_y = sub_goal_.y - pose.y;
+  const double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); 
+
+// Van toc robot
+  double d_reduce  = std::min(std::max(fabs(velocity.x) * k_d_reduce_, 0.6), 3.0);  
+  double v_min = 0.05 * velocity.x / (fabs(velocity.x) + 1e-9); 
+  double vr =  d_rg > d_reduce?  velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; 
+
+// Góc lái của bánh lái là: α(t)
+  double alpha =  atan2(delta_y,delta_x) - pose.theta;
+  double ld = std::min(std::max(fabs(velocity.x)*k_ld_, ld_min_), ld_max_);
+  // double ld = std::min(std::max(d_rg*k_ld_, ld_min_), ld_max_);
+
+  double phi_steer = atan2(2 * L * sin(alpha), ld);
+  if(d_rg <= distance_rg_ && global_plan_.poses.size() > 1) 
+  {
+    const double d_x = goal_.x - sub_goal_.x;
+    const double d_y = goal_.y - sub_goal_.y;
+    const double dd  = sqrt(d_x * d_x + d_y * d_y);
+    const double d_theta = goal_.theta - sub_goal_.theta;
+    if(dd < 1e-5)
+    {
+      geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
+      alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
+      phi_steer = atan2(2 * L * sin(alpha), 1.0);
+    }
+  }
+
+  double alpha_reduce  = std::min(std::max(fabs(vr), 0.6), 1.0); 
+  double cos_actual_phi_steer = cos( std::min(M_PI/2.3, fabs(actual_phi_steer)));
+
+  // limit W
+  double result_W_max = 0.25;
+  // double result_W_min = 0.13;
+  double result_W = vr * fabs(cos(alpha)) * tan(actual_phi_steer) / L;
+  double V_s = std::max(alpha_reduce, fabs(cos(alpha))) * vr / cos_actual_phi_steer;
+  std::stringstream ss;
+  if(fabs(result_W) > 0.02)
+  {
+    double d_w = fabs(result_W) - fabs(velocity.theta);
+    double a = fabs(velocity.theta) > fabs(result_W) ? fabs(acc_lim_theta_/controller_frequency_param_name_) : fabs(decel_lim_theta_/controller_frequency_param_name_);
+    // ss << d_w << " " << a << " ";
+    double result;
+    if(fabs(d_w) >= a) 
+      result =  fabs(velocity.theta) + d_w / fabs(d_w + 1e-9) * a;
+    else if (fabs(d_w) < fabs(a))
+      result = fabs(velocity.theta);
+    // ss << result_W << " " << velocity.theta << " ";
+    result_W = result_W / (fabs(result_W) + 1e-9) * result;
+    // ss << result_W << " ";
+    // ROS_INFO_STREAM(ss.str());
+
+    double d_v = fabs(result_W * L / tan(actual_phi_steer)) - fabs(vr);
+    if(fabs(d_v) >= a) 
+      result =  velocity.x / (fabs(velocity.x) + 1e-9)* (fabs(vr) + d_v / fabs(d_v + 1e-9) * a);
+    else if (fabs(d_v) < fabs(a))
+      result = velocity.x / (fabs(velocity.x) + 1e-9) * fabs(vr);
+
+    V_s = std::max(alpha_reduce, fabs(cos(alpha)))*result/ cos_actual_phi_steer;
+  }
+  
+  if(fabs(result_W) > result_W_max) 
+    V_s = ( result_W / (fabs(result_W) + 1e-9) * result_W_max * L / tan(actual_phi_steer)) / cos_actual_phi_steer;
+
+  if(is_filter_)
+  {
+    geometry_msgs::Twist msg;
+    msg.linear.x = result.velocity.x;
+    msg.angular.z = phi_steer;
+    cmd_raw_pub_.publish(msg);
+   
+    Eigen::VectorXd y(1);
+    y << phi_steer;
+    ros::Time current_time = ros::Time::now();
+    double dt = ros::Duration(current_time - last_actuator_update_).toSec();
+    last_actuator_update_ = current_time;
+    Eigen::MatrixXd A(3, 3); // System dynamics matrix
+    A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
+    kf_->update(y, dt, A);
+    phi_steer = kf_->state().transpose()[0];
+  }
+  theta_direction_ = phi_steer !=0 ? phi_steer/(fabs(phi_steer) + 1e-9) * velocity.x/((fabs(velocity.x) + 1e-9)) : 0;
+
+  result.velocity.theta = phi_steer;
+  result.velocity.x = fabs(cos(phi_steer - actual_phi_steer)) * V_s;
+  return result;
+}
+
+unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+                                   const double distance, unsigned int start_index, unsigned int last_valid_index) const
+{
+  if (start_index >= last_valid_index)
+    return last_valid_index;
+
+  unsigned int goal_index = start_index;
+  const double start_direction_x = plan[start_index + 1].x - plan[start_index].x;
+  const double start_direction_y = plan[start_index + 1].y - plan[start_index].y;
+  if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) {  // make sure that atan2 is defined
+    double start_angle = atan2(start_direction_y, start_direction_x);
+    double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2));
+    // double angle_threshold = 0.0;
+    for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
+    {
+      const double current_direction_x = plan[i].x - plan[i - 1].x;
+      const double current_direction_y = plan[i].y - plan[i - 1].y;
+      const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2));
+      S += dd;
+      if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
+      {
+        double current_angle = atan2(current_direction_y, current_direction_x);
+        if(fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance)
+        {
+          break;
+        }
+        goal_index = i;
+      }
+    }
+  }
+  return goal_index;
+}
+
+bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, 
+                          const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+                          double &target_direction)
+{
+  const nav_core2::Costmap &costmap = *costmap_;
+  const nav_grid::NavGridInfo &info = costmap.getInfo();
+
+  if (global_plan.poses.empty())
+  {
+    ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
+    return false;
+  }
+  // Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
+  std::vector<geometry_msgs::Pose2D> plan = global_plan.poses;
+
+  std::vector<unsigned int> index_s;
+  if(!findSubGoalIndex(plan, index_s))
+  {
+    ROS_ERROR_NAMED("PathProgressCritic", "Can not find SubGoal Index");
+    return false;
+  }
+  // else 
+  // {
+  //   std::stringstream ss;
+  //   for(auto i : index_s) ss << i << " ";
+  //   ROS_INFO_STREAM(ss.str());
+  // }
+
+  unsigned int sub_goal_index = sub_goal_index_saved_;
+  unsigned int sub_start_index = sub_start_index_saved_;
+  if(index_s.empty() || index_s.size() < 1)
+  {
+    sub_goal_index = (unsigned int)plan.size();
+    sub_goal_index_saved_ = sub_goal_index;
+  }
+  else
+  {
+    if(index_s.front() > sub_goal_index_saved_) 
+    {
+      sub_goal_index = (unsigned int)(index_s.front()) + 1;
+      sub_goal_index_saved_ = sub_goal_index;
+    }
+    if(index_s.size() > 1)
+    {
+      for(int i = 1; i < index_s.size(); ++i)
+      {
+        // double tolerance = line_generator->calculateTolerance(robot_pose, plan[index_s[i-1]]);
+        double distance = fabs(nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]]));
+        // double dx           = plan[index_s[i-1]].x - robot_pose.x;
+        // double dy           = plan[index_s[i-1]].y - robot_pose.y;
+        // if(std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_goal_tolerance_ + 0.15)
+        if (distance < xy_local_goal_tolerance_offset_)
+        {
+          if(index_s[i] > sub_goal_index_saved_) 
+          {
+            if(i < index_s.size()-1)
+            {
+              sub_goal_index = index_s[i] + 1;
+            }
+            else
+            {
+              sub_goal_index = (unsigned int)plan.size();
+            }
+            if(follow_step_path_) 
+            {
+              sub_start_index = index_s[i-1];
+              sub_start_index_saved_ = sub_start_index;
+            }
+            ROS_INFO("Tang nac path tu %d den %d boi khoang cach", sub_goal_index_saved_, sub_goal_index);
+            // ROS_WARN("i %d follow_step_path_ = %x start = %d sub_goal = %d size = %d", i, follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
+            sub_goal_index_saved_ = sub_goal_index;
+            break;
+          }
+        }
+      }
+    }
+    else
+    {
+      double distance = fabs(nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose)); 
+      if(distance <= xy_local_goal_tolerance_ )
+      {
+        sub_goal_index = (unsigned int)plan.size();
+        sub_goal_index_saved_ = sub_goal_index;
+      }
+    }
+    
+  }
+
+  // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
+  // Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
+  unsigned int start_index = 0;                      // số vị trí pose trong kế hoạch đường đi
+  double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
+  bool started_path = false; 
+  // for (unsigned int i = 0; i < plan.size(); i++)
+  // ROS_INFO_THROTTLE(1.0, "follow_step_path_ = %x start = %d sub_goal = %d size = %d", follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
+  for (unsigned int i = sub_start_index; i < sub_goal_index; i++)
+  {
+    
+    // double g_x = plan[i].x;
+    // double g_y = plan[i].y;
+    // unsigned int map_x, map_y;
+    // if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
+    //     && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
+    // { 
+      // Still on the costmap. Continue.
+      double distance = fabs(nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
+      if (distance_to_start > distance) 
+      {
+        start_index = i;
+        distance_to_start = distance;
+        started_path = true;
+      }
+      // else
+      // { 
+      //   break;
+      // }
+    // else if (started_path)
+    // {
+    //   // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
+    //   // even closer to the robot, but then we would skip over parts of the plan.
+    //   break;
+    // }
+    // else, we have not yet found a point on the costmap, so we just continue
+  }
+  // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
+  if (!started_path)
+  {
+    ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
+    return false;
+  }
+
+  // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
+  // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
+  if(start_index < 1) start_index == 1;
+  // unsigned int last_valid_index = plan.size() - 1;
+  unsigned int last_valid_index = sub_goal_index - 1;
+  // for (unsigned int i = start_index + 1; i < sub_goal_index; i++)
+  // {
+  //   double g_x = plan[i].x;
+  //   double g_y = plan[i].y;
+  //   unsigned int map_x, map_y;
+  //   if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
+  //   {
+  //     // Still on the costmap. Continue.
+  //     last_valid_index = i;
+  //   }
+  //   else
+  //   {
+  //     // Off the costmap after being on the costmap.
+  //     break;
+  //   }
+  // }
+    
+  // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
+  // i.e. is within angle_threshold_ of the starting direction.
+  // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
+
+  unsigned int goal_index = start_index;
+  ros::Rate r(30);
+  while (goal_index < last_valid_index && ros::ok())
+  {
+    // Tìm kiếm vị trí điểm mục tiêu phù hợp
+    double S;
+    if(velocity.x > 0) S = std::min(4.0, std::max(0.9, fabs(velocity.x)*k_d_reduce_));
+    // else if(velocity.x < 0) S = 0.5;
+    else if(velocity.x < 0) S = std::min(0.7, std::max(0.5, fabs(velocity.x)*k_d_reduce_));
+    else S = 0.5;
+    goal_index = getGoalIndex(robot_pose, plan, S, start_index, last_valid_index);
+
+    // check if goal already reached
+    bool goal_already_reached = false;
+
+    geometry_msgs::PoseArray poseArrayMsg;
+    poseArrayMsg.header.stamp = ros::Time::now();
+    poseArrayMsg.header.frame_id = frame_id_path_;
+	  // std::stringstream ss;
+    for (auto &reached_intermediate_goal : reached_intermediate_goals_)
+    {
+      double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
+      // ss << distance << " ";
+      geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal);
+      poseArrayMsg.poses.push_back(pose);
+    }
+    reached_intermediate_goals_pub_.publish(poseArrayMsg);
+    // if(!reached_intermediate_goals_.empty()) 
+    // {
+    //   ROS_INFO_STREAM(ss.str());
+    // }
+
+    for (auto &reached_intermediate_goal : reached_intermediate_goals_)
+    {
+      double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
+      if (distance < xy_local_goal_tolerance_)
+      {
+        goal_already_reached = true;
+        // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
+        // (start_index is now > goal_index)
+        for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
+        {
+          distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
+          if ( distance >= xy_local_goal_tolerance_offset_)
+          {
+            break;
+          }
+        }
+        // start_index_saved_ = start_index;
+        break;
+      }
+    }
+
+  //try hiep
+    unsigned num = 0;
+    start_index_saved_vt_.insert(start_index_saved_vt_.begin(),start_index);
+    if(start_index_saved_vt_.size() > index_samples_) start_index_saved_vt_.pop_back();
+    std::stringstream ss;
+    for(int i = 0; i < start_index_saved_vt_.size() - 1; i++)
+    {
+      double edge = 0;
+      if(start_index_saved_vt_[i] != start_index_saved_vt_[i+1]) 
+        edge = journey(plan, start_index_saved_vt_[i], start_index_saved_vt_[i+1]);
+      if(edge < xy_local_goal_tolerance_) num++;
+      ss << start_index_saved_vt_[i] << " ";
+    }
+    // ss << start_index << " ";
+
+    if(num < index_samples_ - 1 && !start_index_saved_vt_.empty())
+    {
+      auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end()));
+      start_index_saved_ = start_index_saved_ >= max_iter? start_index_saved_ : max_iter;
+      double S1 = journey(plan, start_index, plan.size()-1 );
+      double S2 = journey(plan, start_index_saved_, plan.size()-1 );
+
+      // ROS_INFO_STREAM(ss.str());
+      if(S2 - S1 < -fabs(xy_local_goal_tolerance_))
+      {
+        start_index = start_index_saved_;
+      }
+      else start_index_saved_ = start_index;
+      // ROS_INFO("  %d %d %f %d %f %f", max_iter, start_index, S1, start_index_saved_, S2, S2 - S1);
+    } 
+    else start_index_saved_ = start_index;
+
+    if(start_index >= goal_index) goal_index = getGoalIndex(robot_pose, plan, S,start_index, last_valid_index);
+
+    if (!goal_already_reached)
+    {
+      // new goal not in reached_intermediate_goals_
+      double distance = fabs(nav_2d_utils::poseDistance(plan[goal_index], plan[start_index]));
+      if (distance < xy_local_goal_tolerance_)
+      {
+        // ROS_INFO("goal_index %d distance %f", goal_index, distance);
+        // the robot has currently reached the goal
+        reached_intermediate_goals_.push_back(plan[goal_index]);
+        ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
+      }
+      else
+      {
+        // we've found a new goal!
+        break;
+      }
+    }
+
+    r.sleep();
+    ros::spinOnce();
+  }
+
+  if (start_index > goal_index)
+    start_index = goal_index;
+  ROS_ASSERT(goal_index <= last_valid_index);
+  if((unsigned int)plan.size() == 2 && !plan.empty()) 
+  {
+    start_index = 0;
+    goal_index = 1;
+  }
+
+  // Tham chiếu lại đối số đầu vao
+  sub_goal = plan[goal_index];
+  if(goal_index > 0) rev_sub_goal_ = plan[goal_index - 1];
+  else rev_sub_goal_ = plan[start_index];
+
+  target_direction = x_direction_;
+  if(goal_index > 0 && (unsigned int)plan.size() > 1)
+  {
+    if(goal_index < (unsigned int)(plan.size() -1))
+    {
+      const geometry_msgs::Pose2D p1 = plan[start_index];
+      int index;
+      for(index = start_index+1; index < goal_index; index++)
+      {
+        geometry_msgs::Pose2D pose = plan[index];
+        if(nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_)  break;
+      }      
+      const geometry_msgs::Pose2D p2 = plan[index];
+      if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
+      {
+        const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
+        const double dir_path = cos(fabs(path_angle - p2.theta));
+        if(fabs(dir_path) > 1e-9) 
+          target_direction = dir_path; 
+        // ROS_INFO("%f %f %f %f", p1.x ,p1.y, p2.x ,p2.y);
+        // ROS_INFO("%d  %0.2f %0.2f %0.2f", goal_index, path_angle, p2.theta, target_direction);
+      }
+    }
+    else 
+    {
+      const double rb_direction_x  = sub_goal.x - robot_pose.x;
+      const double rb_direction_y  = sub_goal.y - robot_pose.y;
+      const double rb_angle = atan2(rb_direction_y, rb_direction_x);
+      double dir_rg = cos(rb_angle - robot_pose.theta);
+      const double angle_threshold = 0.6;
+      if(fabs(dir_rg) > angle_threshold) 
+        target_direction = dir_rg; 
+    }
+  }
+
+  // Publish start_index
+  geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, ros::Time::now());
+  closet_robot_goal_pub_.publish(pose_st);
+
+  // Publish goal_index
+  geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, ros::Time::now());
+  current_goal_pub_.publish(pose_g);
+ 
+  return true;
+}
+
+bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
+{
+  if (plan.empty())
+  {
+    return false;
+  }
+  double x_direction_saved = 0.0;
+  geometry_msgs::PoseArray poseArrayMsg;
+  poseArrayMsg.header.stamp = ros::Time::now();
+  poseArrayMsg.header.frame_id = frame_id_path_;
+
+  for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
+  {
+    geometry_msgs::Pose2D p1 = plan[i];
+    geometry_msgs::Pose2D p2 = plan[i+1];
+    // geometry_msgs::Pose2D p3 = plan[i+2];
+    double angle1;
+    if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
+      angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
+    else continue;
+    // const double angle2 = atan2(p3.y - p2.y, p3.x - p2.x);
+    const double target_direction = cos(fabs(angle1 - p2.theta));
+
+    if(target_direction * x_direction_saved < 0.0)
+    {
+      index_s.push_back(i);
+      geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(plan[i]);
+      poseArrayMsg.poses.push_back(pose);
+    }
+    x_direction_saved = target_direction;
+  }
+
+  if(index_s.empty()) 
+    index_s.push_back((unsigned int)plan.size() - 1);
+  else
+  {
+    if(index_s.back() < (unsigned int)plan.size() - 1)
+      index_s.push_back((unsigned int)plan.size() - 1);
+  }
+  reached_intermediate_goals_pub_.publish(poseArrayMsg);
+  return true;
+}
+
+double Bicycle::journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
+{
+  double S = 0; 
+  if(last_valid_index - start_index > 1)
+  {
+    for(int i= start_index; i < last_valid_index; i++)
+    {
+      S += nav_2d_utils::poseDistance(plan[i], plan[i+1]);
+    }
+  }
+  return S;
+}
+
+
+} // namespace  mkt_algorithm
+
+PLUGINLIB_EXPORT_CLASS(mkt_algorithm::Bicycle, score_algorithm::ScoreAlgorithm)

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp

@@ -0,0 +1,78 @@
+#include <mkt_algorithm/rotate_to_goal.h>
+#include <nav_2d_utils/parameters.h>
+#include <pluginlib/class_list_macros.h>
+#include <angles/angles.h>
+
+namespace  mkt_algorithm
+{
+
+void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+{
+  nh_       = ros::NodeHandle(nh, name);
+  name_     = name;
+  tf_       = tf;
+  costmap_ros_ = costmap_ros;
+
+  steering_fix_wheel_distance_x_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
+  steering_fix_wheel_distance_y_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
+  nh_.param("velocity_rotate_min", velocity_rotate_min_, 0.1);
+  nh_.param("velocity_rotate_max", velocity_rotate_max_, 0.6);
+  nh_.param("angle_pass_rotate", angle_threshold_, 0.02);
+}
+
+void RotateToGoal::reset()
+{
+  time_last_cmd_ = ros::Time::now();
+}
+
+bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
+                       const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
+                       double& x_direction, double& y_direction, double& theta_direction) 
+{
+  goal_ = goal;
+  double angle = angles::shortest_angular_distance(pose.theta, goal_.theta);
+
+  if(angle > 0) current_direction_ = 1;
+  else if(angle < 0) current_direction_ = -1;
+  x_direction = y_direction = theta_direction = current_direction_;
+  return  true;
+}
+            
+nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
+{
+  nav_2d_msgs::Twist2DStamped result;
+  result.header.stamp = ros::Time::now();
+
+  geometry_msgs::Pose2D steer_to_baselink_pose;
+  
+  if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
+    return result;
+  double actual_phi_steer = steer_to_baselink_pose.theta;
+
+  double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
+
+  // ros::Time t = ros::Time::now();
+  // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec();
+  // time_last_cmd_ = ros::Time::now();
+
+  double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta);
+  double angle_reduce  = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_);
+  double w_min = velocity.theta / (fabs(velocity.theta) + 1e-9) * velocity_rotate_min_;
+  double w_r =  fabs(angle_rg) > angle_reduce? velocity.theta : (velocity.theta - w_min)*sin((fabs(angle_rg) / angle_reduce) * (M_PI/2)) + w_min; 
+  
+  int direct_rotate = (angle_rg/fabs(angle_rg));
+  double target_phi_steer = direct_rotate * M_PI/2;
+  if( fabs(target_phi_steer - actual_phi_steer) <= angle_threshold_)
+  {
+    double target_v = fabs(w_r * L);
+    result.velocity.x = std::min(velocity_rotate_max_ * L, fabs(target_v)); 
+  }
+  else result.velocity.x = 0.0;
+  
+  result.velocity.theta = target_phi_steer; 
+  return result;
+}
+
+}; // namespace  mkt_algorithm
+
+PLUGINLIB_EXPORT_CLASS(mkt_algorithm::RotateToGoal, score_algorithm::ScoreAlgorithm)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -0,0 +1,243 @@
+#include <mkt_algorithm/diff/diff_go_straight.h>
+
+// other
+// #include <pluginlib/class_list_macros.h>
+#include <boost/dll/alias.hpp>
+
+void mkt_algorithm::diff::GoStraight::initialize(
+    ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+{
+  if (!initialized_)
+  {
+    nh_ = ros::NodeHandle("~/");
+    nh_priv_ = ros::NodeHandle("~/" + name);
+    name_ = name;
+    tf_ = tf;
+    traj_ = traj;
+    costmap_ros_ = costmap_ros;
+
+    this->getParams();
+    // this->initDynamicReconfigure(nh_priv_);
+    nh_.param("publish_topic", enable_publish_, false);
+    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
+
+
+    carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
+    reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
+    current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
+    closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
+    transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
+    compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
+
+    std::vector<geometry_msgs::Point> footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+    if (footprint.size() > 1)
+    {
+      double min_length = 1e10;
+      double max_length = 0.0;
+      for (size_t i = 0; i < footprint.size(); ++i)
+      {
+        const geometry_msgs::Point &p1 = footprint[i];
+        const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+        double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
+        if (len > max_length)
+        {
+          max_length = len;
+        }
+        if (len < min_length)
+        {
+          min_length = len;
+        }
+      }
+      this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
+      this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
+    }
+
+    // kalman
+    last_actuator_update_ = ros::Time::now();
+    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
+    m_ = 2; // measurements: x, y, theta
+    double dt = control_duration_;
+
+    // Khởi tạo ma trận
+    A = Eigen::MatrixXd::Identity(n_, n_);
+    C = Eigen::MatrixXd::Zero(m_, n_);
+    Q = Eigen::MatrixXd::Zero(n_, n_);
+    R = Eigen::MatrixXd::Identity(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_);
+
+    for (int i = 0; i < n_; i += 3)
+    {
+      A(i, i + 1) = dt;
+      A(i, i + 2) = 0.5 * dt * dt;
+      A(i + 1, i + 2) = dt;
+    }
+
+    C(0, 0) = 1;
+    C(1, 3) = 1;
+    Q(2, 2) = 0.1;
+    Q(5, 5) = 0.6;
+
+    R(0, 0) = 0.1;
+    R(1, 1) = 0.2;
+
+    P(3, 3) = 0.4;
+    P(4, 4) = 0.4;
+    P(5, 5) = 0.4;
+
+    kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
+    Eigen::VectorXd x0(n_);
+    x0 << 0, 0, 0, 0, 0, 0;
+    kf_->init(ros::Time::now().toSec(), x0);
+
+    x_direction_ = y_direction_ = theta_direction_ = 0;
+    initialized_ = true;
+    ROS_INFO("GoStraight Initialized successfully");
+  }
+}
+
+mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
+    const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  mkt_msgs::Trajectory2D result;
+  result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
+  if (!traj_)
+    return result;
+  if (compute_plan_.poses.size() < 2)
+  {
+    ROS_WARN_THROTTLE(2.0, "Local compute plan is available");
+    return result;
+  }
+
+  ros::Time current_time = ros::Time::now();
+  double dt = (current_time - last_actuator_update_).toSec();
+  last_actuator_update_ = current_time;
+
+  nav_2d_msgs::Twist2D drive_cmd;
+  double sign_x = x_direction_;
+
+  nav_2d_msgs::Twist2D twist;
+  traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
+  ros::Rate r(50);
+  while (ros::ok() && traj_->hasMoreTwists())
+  {
+    twist = traj_->nextTwist();
+    // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
+    r.sleep();
+  }
+  
+  drive_cmd.x = sqrt(twist.x * twist.x);
+  nav_2d_msgs::Path2D transformed_plan = transform_plan_;
+  if (transformed_plan.poses.empty())
+  {
+    ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
+    return result;
+  }
+
+  if (enable_publish_)
+    transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
+
+  // Dynamically adjust look ahead distance based on the speed
+  double lookahead_dist = this->getLookAheadDistance(twist);
+  // Return false if the transformed global plan is empty
+  geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
+  lookahead_dist += fabs(front.y);
+
+  // Get lookahead point and publish for visualization
+  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+
+  if (enable_publish_)
+    carrot_pub_.publish(this->createCarrotMsg(carrot_pose));
+
+  // Carrot distance squared
+  const double carrot_dist2 =
+      (carrot_pose.pose.x * carrot_pose.pose.x) +
+      (carrot_pose.pose.y * carrot_pose.pose.y);
+
+  // Find curvature of circle (k = 1 / R)
+  double curvature = 0.0;
+  if (carrot_dist2 > 2e-2)
+  {
+    curvature = 2.0 * carrot_pose.pose.y / carrot_dist2;
+  }
+
+  // Constrain linear velocity
+  this->applyConstraints(
+      0.0, lookahead_dist, curvature, twist,
+      this->costAtPose(pose.pose.x, pose.pose.y),
+      drive_cmd.x, sign_x);
+
+
+  if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_)
+  {
+    nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
+    nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
+    double dx = end.pose.x - start.pose.x;
+    double dy = end.pose.y - start.pose.y;
+    drive_cmd.theta = atan2(dy, dx);
+
+    //[-π/2, π/2]
+    if (drive_cmd.theta > M_PI / 2)
+      drive_cmd.theta -= M_PI;
+    else if (drive_cmd.theta < -M_PI / 2)
+      drive_cmd.theta += M_PI;
+
+    drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_);
+  }
+  else
+    drive_cmd.theta = 0.0;
+
+  // populate and return message
+  if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9)
+    drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)));
+    
+  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double vel_x_reduce = std::min(fabs(v_max), 0.3);
+  double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
+  const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
+  const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
+  double d_reduce = std::clamp(journey_plan, min_S, max_S);
+  double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
+  double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
+  double v_min = 
+    journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
+  v_min *= sign_x;
+  double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
+  double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
+  journey_plan += max_path_distance_;
+  drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min;
+
+  // drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min;
+
+  Eigen::VectorXd y(2);
+  y << drive_cmd.x, drive_cmd.theta;
+
+  // Cập nhật lại A nếu dt thay đổi
+  for (int i = 0; i < n_; ++i)
+    for (int j = 0; j < n_; ++j)
+      A(i, j) = (i == j ? 1.0 : 0.0);
+  for (int i = 0; i < n_; i += 3)
+  {
+    A(i, i + 1) = dt;
+    A(i, i + 2) = 0.5 * dt * dt;
+    A(i + 1, i + 2) = dt;
+  }
+  kf_->update(y, dt, A);
+
+  if (drive_cmd.x != v_max)
+  {
+    double min = drive_cmd.x < v_max ? drive_cmd.x : v_max;
+    double max = drive_cmd.x > v_max ? drive_cmd.x : v_max;
+    drive_cmd.x = std::clamp(kf_->state()[0], min, max);
+  }
+  else
+  {
+    drive_cmd.x = std::clamp(kf_->state()[0], v_max, v_max);
+  }
+
+  drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
+  drive_cmd.theta = kf_->state()[3];
+  result.velocity = drive_cmd;
+  return result;
+}
+
+PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp

@@ -0,0 +1,131 @@
+#include <mkt_algorithm/diff/diff_rotate_to_goal.h>
+
+// other
+#include <nav_2d_utils/parameters.h>
+#include <pluginlib/class_list_macros.h>
+#include <angles/angles.h>
+
+void mkt_algorithm::diff::RotateToGoal::initialize(
+    ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+{
+  if (!initialized_)
+  {
+    nh_ = ros::NodeHandle(nh, name);
+    name_ = name;
+    tf_ = tf;
+    traj_ = traj;
+    costmap_ros_ = costmap_ros;
+    this->getParams();
+
+    x_direction_ = y_direction_ = theta_direction_ = 0;
+    this->initialized_ = true;
+    ROS_INFO("RotateToGoal Initialized successfully");
+  }
+}
+
+void mkt_algorithm::diff::RotateToGoal::getParams()
+{
+  robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
+  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
+  nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
+  nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
+  nh_priv_.param<int>("index_samples", index_samples_, 0);
+  nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);
+
+  nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
+  nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
+  nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
+  nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
+  nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
+  nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
+  nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());
+
+  nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
+  nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
+
+  nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
+  nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
+  nh_priv_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
+
+  // Regulated linear velocity scaling
+  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
+  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
+  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
+
+  // Inflation cost scaling (Limit velocity by proximity to obstacles)
+  nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
+  nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
+  nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
+  nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
+  if (inflation_cost_scaling_factor_ <= 0.0)
+  {
+    ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
+             "it should be >0. Disabling cost regulated linear velocity scaling.");
+    use_cost_regulated_linear_velocity_scaling_ = false;
+  }
+  double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
+  control_duration_ = 1.0 / control_frequency;
+  if (traj_)
+  {
+    traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);
+
+    traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);
+
+    traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
+    traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
+  }
+}
+
+void mkt_algorithm::diff::RotateToGoal::reset()
+{
+  this->clear();
+}
+
+bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
+                                                const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
+                                                double &x_direction, double &y_direction, double &theta_direction)
+{
+
+  if (!initialized_)
+  {
+    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+    return false;
+  }
+  this->getParams();
+  global_plan_ = global_plan;
+
+  goal_ = goal;
+  double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
+  x_direction = y_direction = theta_direction = sign(angle);
+  ;
+  return true;
+}
+
+mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
+    const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  mkt_msgs::Trajectory2D result;
+  nav_2d_msgs::Twist2D drive_cmd;
+  result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
+  if (!traj_)
+    return result;
+
+  const double dt = control_duration_;
+  double vel_yaw = velocity.theta;
+  double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
+  rotateToHeading(ang_diff, velocity, drive_cmd);
+  drive_cmd.x = 0.0;
+  drive_cmd.y = 0.0;
+  result.velocity = drive_cmd;
+  return result;
+}
+
+PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm)

src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.10)
+
+# Tên dự án
+project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Cho phép các project khác include được header của mkt_msgs
+set(mkt_msgs_INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    PARENT_SCOPE
+)
+
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+
+# Tạo INTERFACE library (header-only)
+add_library(mkt_msgs INTERFACE)
+target_link_libraries(mkt_msgs INTERFACE nav_2d_msgs geometry_msgs)
+
+# Set include directories
+target_include_directories(mkt_msgs
+    INTERFACE 
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Cài đặt header files
+install(DIRECTORY include/mkt_msgs 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+install(TARGETS mkt_msgs 
+        EXPORT mkt_msgs-targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+# Export targets
+install(EXPORT mkt_msgs-targets
+         FILE mkt_msgs-targets.cmake
+        DESTINATION lib/cmake/mkt_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 "=================================")

src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h

@@ -0,0 +1,66 @@
+// Generated by gencpp from file mkt_msgs/Trajectory2D.msg
+// DO NOT EDIT!
+
+#ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H
+#define MKT_MSGS_MESSAGE_TRAJECTORY2D_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <nav_2d_msgs/Twist2D.h>
+#include <geometry_msgs/Pose2D.h>
+
+namespace mkt_msgs
+{
+  template <class ContainerAllocator>
+  struct Trajectory2D_
+  {
+    typedef Trajectory2D_<ContainerAllocator> Type;
+
+    Trajectory2D_()
+        : velocity(), poses(), time_offsets()
+    {
+    }
+    Trajectory2D_(const ContainerAllocator &_alloc)
+        : velocity(_alloc), poses(_alloc), time_offsets(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
+    _velocity_type velocity;
+
+    typedef std::vector<geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<geometry_msgs::Pose2D>> _poses_type;
+    _poses_type poses;
+
+    typedef std::vector<robot::Duration, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot::Duration>> _time_offsets_type;
+    _time_offsets_type time_offsets;
+
+    typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct Trajectory2D_
+
+  typedef ::mkt_msgs::Trajectory2D_<std::allocator<void>> Trajectory2D;
+
+  typedef boost::shared_ptr<::mkt_msgs::Trajectory2D> Trajectory2DPtr;
+  typedef boost::shared_ptr<::mkt_msgs::Trajectory2D const> Trajectory2DConstPtr;
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::mkt_msgs::Trajectory2D_<ContainerAllocator1> &lhs, const ::mkt_msgs::Trajectory2D_<ContainerAllocator2> &rhs)
+  {
+    return lhs.velocity == rhs.velocity &&
+           lhs.poses == rhs.poses &&
+           lhs.time_offsets == rhs.time_offsets;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::mkt_msgs::Trajectory2D_<ContainerAllocator1> &lhs, const ::mkt_msgs::Trajectory2D_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace mkt_msgs
+
+#endif // MKT_MSGS_MESSAGE_TRAJECTORY2D_H

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

src/Navigations/Cores/move_base_core/.gitignore

@@ -0,0 +1,414 @@
+# ---> VisualStudio
+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
+
+# User-specific files
+*.rsuser
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Mono auto generated files
+mono_crash.*
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+[Ww][Ii][Nn]32/
+[Aa][Rr][Mm]/
+[Aa][Rr][Mm]64/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+[Ll]ogs/
+
+# Visual Studio 2015/2017 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# Visual Studio 2017 auto generated files
+Generated\ Files/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUnit
+*.VisualState.xml
+TestResult.xml
+nunit-*.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+
+# ASP.NET Scaffolding
+ScaffoldingReadMe.txt
+
+# StyleCop
+StyleCopReport.xml
+
+# Files built by Visual Studio
+*_i.c
+*_p.c
+*_h.h
+*.ilk
+*.meta
+*.obj
+*.iobj
+*.pch
+*.pdb
+*.ipdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*_wpftmp.csproj
+*.log
+*.tlog
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# Visual Studio Trace Files
+*.e2e
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Coverlet is a free, cross platform Code Coverage Tool
+coverage*.json
+coverage*.xml
+coverage*.info
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# NuGet Symbol Packages
+*.snupkg
+# The packages folder can be ignored because of Package Restore
+**/[Pp]ackages/*
+# except build/, which is used as an MSBuild target.
+!**/[Pp]ackages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/[Pp]ackages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+*.appxbundle
+*.appxupload
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!?*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Including strong name files can present a security risk
+# (https://github.com/github/gitignore/pull/2483#issue-259490424)
+#*.snk
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+ServiceFabricBackup/
+*.rptproj.bak
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+*.rptproj.rsuser
+*- [Bb]ackup.rdl
+*- [Bb]ackup ([0-9]).rdl
+*- [Bb]ackup ([0-9][0-9]).rdl
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio 6 auto-generated project file (contains which files were open etc.)
+*.vbp
+
+# Visual Studio 6 workspace and project file (working project files containing files to include in project)
+*.dsw
+*.dsp
+
+# Visual Studio 6 technical files
+*.ncb
+*.aps
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# CodeRush personal settings
+.cr/personal
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+# OpenCover UI analysis results
+OpenCover/
+
+# Azure Stream Analytics local run output
+ASALocalRun/
+
+# MSBuild Binary and Structured Log
+*.binlog
+
+# NVidia Nsight GPU debugger configuration file
+*.nvuser
+
+# MFractors (Xamarin productivity tool) working folder
+.mfractor/
+
+# Local History for Visual Studio
+.localhistory/
+
+# Visual Studio History (VSHistory) files
+.vshistory/
+
+# BeatPulse healthcheck temp database
+healthchecksdb
+
+# Backup folder for Package Reference Convert tool in Visual Studio 2017
+MigrationBackup/
+
+# Ionide (cross platform F# VS Code tools) working folder
+.ionide/
+
+# Fody - auto-generated XML schema
+FodyWeavers.xsd
+
+# VS Code files for those working on multiple tools
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/
+
+# Windows Installer files from build outputs
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# JetBrains Rider
+*.sln.iml
+
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+

src/Navigations/Cores/move_base_core/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.10)
+
+# Tên dự án
+project(move_base_core VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Cho phép các project khác include được header của move_base_core
+set(MOVE_BASE_CORE_INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    PARENT_SCOPE
+)
+
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+
+# Tìm tất cả header files
+file(GLOB HEADERS "include/move_base_core/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(move_base_core INTERFACE)
+target_link_libraries(move_base_core INTERFACE tf3 robot_time geometry_msgs)
+
+# Set include directories
+target_include_directories(move_base_core
+    INTERFACE 
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Cài đặt header files
+install(DIRECTORY include/move_base_core 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+# Cài đặt target
+install(TARGETS move_base_core 
+        EXPORT move_base_core-targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+# Export targets
+install(EXPORT move_base_core-targets
+         FILE move_base_core-targets.cmake
+        DESTINATION lib/cmake/move_base_core)
+
+# 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 "=================================")