add move_base

.gitignore

@@ -201,6 +201,8 @@ PublishScripts/
 **/[Pp]ackages/*
 # except build/, which is used as an MSBuild target.
 !**/[Pp]ackages/build/
+# except move_base package in Navigations
+!src/Navigations/Packages/move_base/
 # 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

src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md

@@ -0,0 +1,142 @@
+# Move Base - Dual Build System Support
+
+Package `move_base` có thể được build bằng cả **Catkin** và **Standalone CMake**.
+
+## Cách Build
+
+### 1. Build với Catkin (ROS Workspace)
+
+**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_nav_core`) có `CMakeLists.txt` ở root, 
+nên **KHÔNG THỂ** chạy `catkin_make` trực tiếp trong thư mục đó. Catkin workspace không được 
+phép có `CMakeLists.txt` ở root.
+
+**Giải pháp**: Tạo một catkin workspace riêng và link packages vào đó.
+
+#### Cách 1: Sử dụng script tự động (Khuyến nghị)
+
+```bash
+# Chạy script setup từ thư mục gốc của project
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
+./setup_catkin_workspace.sh
+
+# Script sẽ tạo workspace và link các packages có package.xml
+# Sau đó build:
+cd ../pnkx_nav_catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+#### Cách 2: Tạo workspace thủ công
+
+```bash
+# Tạo catkin workspace
+mkdir -p ~/pnkx_nav_catkin_ws/src
+cd ~/pnkx_nav_catkin_ws/src
+
+# Link package move_base vào workspace
+ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Navigations/Packages/move_base move_base
+
+# Link các dependencies cần thiết (nếu có package.xml)
+ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/tf3 tf3
+ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/nav_2d_utils nav_2d_utils
+
+# Build với catkin
+cd ~/pnkx_nav_catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+**Lưu ý**: Khi build với catkin, package sẽ tự động detect và sử dụng:
+- `catkin_package()` để export dependencies
+- Catkin installation paths
+- ROS message dependencies
+
+### 2. Build với Standalone CMake
+
+```bash
+cd /path/to/move_base
+mkdir -p build
+cd build
+cmake ..
+make
+```
+
+**Lưu ý**: Khi build standalone, bạn cần đảm bảo:
+- Tất cả dependencies đã được build trước
+- CMake có thể tìm thấy các internal packages (move_base_core, nav_core, etc.)
+- ROS packages (nếu cần) được cài đặt và có trong CMAKE_PREFIX_PATH
+
+## Cách CMakeLists.txt Detect Build Mode
+
+CMakeLists.txt tự động detect build mode bằng cách kiểm tra:
+- `CATKIN_DEVEL_PREFIX` - biến môi trường của catkin
+- `CATKIN_TOPLEVEL` - biến môi trường của catkin
+
+Nếu một trong hai biến này được định nghĩa, package sẽ build ở **Catkin mode**.
+Ngược lại, sẽ build ở **Standalone CMake mode**.
+
+## Dependencies
+
+### ROS Packages (khi build với Catkin):
+- roscpp, rospy
+- std_msgs, geometry_msgs, nav_msgs
+- tf2, tf2_ros
+- actionlib, dynamic_reconfigure
+
+### Internal Packages (cần build trước):
+- move_base_core
+- nav_core
+- costmap_2d
+- xmlrpcpp
+- node_handle
+- tf3_sensor_msgs
+- tf3_geometry_msgs
+
+### System Libraries:
+- Boost
+- yaml-cpp
+- pthread, dl
+
+## Kiểm tra Build Mode
+
+Khi chạy `cmake`, bạn sẽ thấy message:
+```
+Building move_base with Catkin
+```
+hoặc
+```
+Building move_base with Standalone CMake
+```
+
+## Troubleshooting
+
+### Lỗi: "catkin_make must be invoked in the root of workspace"
+**Nguyên nhân**: Thư mục hiện tại có `CMakeLists.txt` ở root, catkin không cho phép điều này.
+
+**Giải pháp**: 
+- Tạo catkin workspace riêng (xem hướng dẫn ở trên)
+- Hoặc đổi tên/ẩn `CMakeLists.txt` ở root khi build với catkin (không khuyến nghị)
+
+### Lỗi: "Cannot find package X"
+- **Với Catkin**: 
+  - Đảm bảo package X đã được build trong cùng workspace
+  - Đảm bảo package X có `package.xml` và được link vào `src/` của workspace
+  - Kiểm tra dependencies trong `package.xml`
+- **Với Standalone**: 
+  - Đảm bảo package X đã được build và cài đặt
+  - Thêm vào `CMAKE_PREFIX_PATH` nếu cần
+
+### Lỗi: "catkin_package() not found"
+- Đảm bảo bạn đang build trong catkin workspace (không phải thư mục gốc có CMakeLists.txt)
+- Kiểm tra `CATKIN_DEVEL_PREFIX` có được set không
+- Source ROS setup: `source /opt/ros/noetic/setup.bash`
+
+### Lỗi: "Cannot find ROS packages"
+- Cài đặt ROS Noetic: `sudo apt-get install ros-noetic-<package-name>`
+- Source ROS setup: `source /opt/ros/noetic/setup.bash`
+- Kiểm tra `ROS_PACKAGE_PATH` environment variable
+
+### Lỗi: Internal packages không tìm thấy (move_base_core, nav_core, etc.)
+- **Với Catkin**: Các internal packages cần có `package.xml` và được link vào workspace
+- **Với Standalone**: Đảm bảo các packages đã được build trước trong project chính
+

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -0,0 +1,257 @@
+cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building move_base with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building move_base with Standalone CMake")
+endif()
+
+# Tên dự án
+project(move_base 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()
+
+# ========================================================
+# Find Packages
+# ========================================================
+find_package(Boost REQUIRED)
+
+# If building with Catkin, use catkin_package to find dependencies
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    std_msgs
+    geometry_msgs
+    nav_msgs
+    tf2
+    tf2_ros
+    actionlib
+    dynamic_reconfigure
+  )
+  
+  # Find internal packages (these should be built in the same workspace)
+  # Catkin will handle these through workspace dependencies
+  find_package(yaml-cpp REQUIRED)
+endif()
+
+# ========================================================
+# Source Files
+# ========================================================
+file(GLOB SOURCES "src/move_base.cpp")
+file(GLOB HEADERS "include/move_base/move_base.h")
+
+# ========================================================
+# Include Directories
+# ========================================================
+include_directories(
+  ${PROJECT_SOURCE_DIR}/include
+)
+
+if(BUILDING_WITH_CATKIN)
+  include_directories(
+    ${catkin_INCLUDE_DIRS}
+  )
+endif()
+
+# ========================================================
+# RPATH settings: ưu tiên thư viện build tại chỗ
+# ========================================================
+set(CMAKE_SKIP_BUILD_RPATH FALSE)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
+set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+# ========================================================
+# Create Library
+# ========================================================
+add_library(move_base SHARED ${SOURCES} ${HEADERS})
+
+# ========================================================
+# Dependencies and Link Libraries
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+  # Catkin mode: use catkin dependencies
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES move_base
+    CATKIN_DEPENDS 
+      roscpp 
+      rospy 
+      std_msgs 
+      geometry_msgs 
+      nav_msgs 
+      tf2 
+      tf2_ros 
+      actionlib 
+      dynamic_reconfigure
+    DEPENDS 
+      yaml-cpp
+      Boost
+  )
+  
+  # Link against catkin packages
+  target_link_libraries(move_base 
+    ${catkin_LIBRARIES}
+    ${Boost_LIBRARIES}
+    yaml-cpp
+  )
+  
+  # Internal packages (assumed to be in same workspace)
+  # These will be linked via catkin workspace dependencies
+  target_link_libraries(move_base
+    move_base_core
+    nav_core
+    costmap_2d
+    xmlrpcpp
+    robot::node_handle
+    robot::console
+    tf3_sensor_msgs
+    tf3_geometry_msgs
+    dl
+    pthread
+  )
+  
+  # Set RPATH để ưu tiên thư viện build cục bộ
+  set_target_properties(move_base PROPERTIES
+    BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
+    INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
+    LINK_FLAGS "-Wl,--disable-new-dtags"
+  )
+else()
+  # Standalone CMake mode: link all dependencies manually
+  set(PACKAGES_DIR 
+    geometry_msgs 
+    std_msgs 
+    move_base_core 
+    nav_core 
+    costmap_2d 
+    yaml-cpp
+    xmlrpcpp
+    tf3_sensor_msgs
+    tf3_geometry_msgs
+    data_convert
+    dl
+    pthread
+  )
+  
+  target_link_libraries(move_base 
+    PUBLIC ${PACKAGES_DIR} 
+    PUBLIC robot::node_handle robot::console
+    PRIVATE Boost::boost
+  )
+  
+  # Set RPATH để ưu tiên thư viện build cục bộ
+  set_target_properties(move_base PROPERTIES
+    BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
+    INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
+    LINK_FLAGS "-Wl,--disable-new-dtags"
+  )
+endif()
+
+# ========================================================
+# Include Directories for Target
+# ========================================================
+target_include_directories(move_base
+  PUBLIC 
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+if(BUILDING_WITH_CATKIN)
+  target_include_directories(move_base
+    PUBLIC ${catkin_INCLUDE_DIRS}
+  )
+endif()
+
+# ========================================================
+# Executable
+# ========================================================
+add_executable(move_base_main src/move_base_main.cpp)
+target_link_libraries(move_base_main 
+  PRIVATE move_base 
+  PRIVATE Boost::boost
+)
+
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(move_base_main
+    PRIVATE ${catkin_LIBRARIES}
+  )
+endif()
+
+# ========================================================
+# Installation
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+  # Catkin installation
+  install(DIRECTORY include/move_base 
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+  )
+  
+  install(TARGETS move_base move_base_main
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+else()
+  # Standalone CMake installation
+  install(DIRECTORY include/move_base 
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+  )
+  
+  install(TARGETS move_base move_base_main
+    EXPORT move_base-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+  
+  install(EXPORT move_base-targets
+    FILE move_base-targets.cmake
+    DESTINATION lib/cmake/move_base
+  )
+endif()
+
+# ========================================================
+# Build Options
+# ========================================================
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# ========================================================
+# Compiler Flags
+# ========================================================
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# ========================================================
+# Status Messages
+# ========================================================
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "Build mode: ${BUILDING_WITH_CATKIN}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+message(STATUS "Headers found:")
+foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+endforeach()
+message(STATUS "=================================")

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -0,0 +1,354 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Author: Kevin Lee
+ *********************************************************************/
+#ifndef NAV_MOVE_BASE_ACTION_H_
+#define NAV_MOVE_BASE_ACTION_H_
+
+#include <vector>
+#include <string>
+#include <tf3/buffer_core.h>
+#include <yaml-cpp/yaml.h>
+
+
+// interfaces headers
+#include <move_base_core/navigation.h>
+#include <nav_core/base_global_planner.h>
+#include <nav_core/base_local_planner.h>
+#include <nav_core/recovery_behavior.h>
+
+// boost headers
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/recursive_mutex.hpp>
+
+#include <costmap_2d/costmap_2d_robot.h>
+#include <costmap_2d/costmap_2d.h>
+#include <nav_msgs/GetPlan.h>
+
+#include <robot/node_handle.h>
+#include <robot/console.h>
+
+namespace move_base
+{
+
+  enum MoveBaseState
+  {
+    PLANNING,
+    CONTROLLING,
+    CLEARING
+  };
+
+  enum RecoveryTrigger
+  {
+    PLANNING_R,
+    CONTROLLING_R,
+    OSCILLATION_R
+  };
+
+  struct MoveBaseConfig 
+  {
+    std::string base_global_planner;
+  };
+
+  /**
+   * @class MoveBase
+   * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
+   */
+  class MoveBase : public move_base_core::BaseNavigation
+  {
+  public:
+    /**
+     * @brief  Constructor for the actions
+     * @param name The name of the action
+     * @param tf A reference to a TransformListener
+     */
+    MoveBase(TFListenerPtr tf);
+    MoveBase();
+
+    /**
+     * @brief Initialize the navigation system.
+     * @param tf Shared pointer to the TF listener or buffer.
+     */
+    virtual void initialize(TFListenerPtr tf) override;
+
+    /**
+     * @brief Set the robot's footprint (outline shape) in the global frame.
+     *        This can be used for planning or collision checking.
+     *
+     * @param fprt A vector of points representing the robot's footprint polygon.
+     *             The points should be ordered counter-clockwise.
+     */
+    virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) override;
+
+    /**
+     * @brief Send a goal for the robot to navigate to.
+     * @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.
+     */
+    virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
+                        double xy_goal_tolerance = 0.0,
+                        double yaw_goal_tolerance = 0.0) override;
+
+    /**
+     * @brief Send a docking goal to a predefined marker (e.g. charger).
+     * @param maker             Marker name or ID.
+     * @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.
+     */
+    virtual bool dockTo(const std::string &maker,
+                        const geometry_msgs::PoseStamped &goal,
+                        double xy_goal_tolerance = 0.0,
+                        double yaw_goal_tolerance = 0.0) override;
+
+    /**
+     * @brief Move straight toward the target position (X-axis).
+     * @param goal              Target pose.
+     * @param xy_goal_tolerance Acceptable positional error (meters).
+     * @return True if command issued successfully.
+     */
+    virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
+                                double xy_goal_tolerance = 0.0) override;
+
+    /**
+     * @brief Rotate in place to align with target orientation.
+     * @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.
+     */
+    virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
+                          double yaw_goal_tolerance = 0.0) override;
+
+    /**
+     * @brief Pause the robot's movement (e.g. during obstacle avoidance).
+     */
+    virtual void pause() override;
+
+    /**
+     * @brief Resume motion after a pause.
+     */
+    virtual void resume() override;
+
+    /**
+     * @brief Cancel the current goal and stop the robot.
+     */
+    virtual void cancel() override;
+
+    /**
+     * @brief Send direct linear velocity command.
+     * @param linear Linear velocity vector (x, y, z).
+     * @return True if the command was accepted.
+     */
+    virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) override;
+
+    /**
+     * @brief Send limited angular velocity command.
+     * @param angular Angular velocity vector (x, y, z).
+     * @return True if the command was accepted.
+     */
+    virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) override;
+
+    /**
+     * @brief Get the robot’s pose as a PoseStamped.
+     * @param pose Output parameter with the robot’s current pose.
+     * @return True if pose was successfully retrieved.
+     */
+    virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) override;
+
+    /**
+     * @brief Get the robot’s pose as a 2D pose.
+     * @param pose Output parameter with the robot’s current 2D pose.
+     * @return True if pose was successfully retrieved.
+     */
+    virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override;
+
+    /**
+     * @brief  Destructor - Cleans up
+     */
+    virtual ~MoveBase();
+
+    /**
+     * @brief  Performs a control cycle
+     * @param goal A reference to the goal to pursue
+     * @return True if processing of the goal is done, false otherwise
+     */
+    bool executeCycle(geometry_msgs::PoseStamped &goal);
+
+    /**
+     * @brief Create a new MoveBase
+     * @return A shared pointer to the new MoveBase
+     */
+    static move_base_core::BaseNavigation::Ptr create();
+
+  private:
+
+    /**
+     * @brief Set velocity for yaw goal tolerance of the robot
+     * @param yaw_goal_tolerance the value command
+     */
+    void setYawGoalTolerance(double yaw_goal_tolerance);
+
+    /**
+     * @brief Set velocity for yaw goal tolerance of the robot
+     * @param xy_goal_tolerance the value command
+     */
+    void setXyGoalTolerance(double xy_goal_tolerance);
+
+    /**
+     * @brief Swaps the current global planner with a new one specified by name
+     * @param base_global_planner The name of the new global planner to use
+     */
+    void swapPlanner(std::string base_global_planner);
+
+    // /**
+    //  * @brief  A service call that clears the costmaps of obstacles
+    //  * @param req The service request
+    //  * @param resp The service response
+    //  * @return True if the service call succeeds, false otherwise
+    //  */
+    // bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
+
+    // /**
+    //  * @brief  A service call that can be made when the action is inactive that will return a plan
+    //  * @param  req The goal request
+    //  * @param  resp The plan request
+    //  * @return True if planning succeeded, false otherwise
+    //  */
+    // bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
+
+    /**
+     * @brief  Make a new global plan
+     * @param  goal The goal to plan to
+     * @param  plan Will be filled in with the plan made by the planner
+     * @return  True if planning succeeds, false otherwise
+     */
+    bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);
+
+    /**
+     * @brief  Load the recovery behaviors for the navigation stack from the parameter server
+     * @return True if the recovery behaviors were loaded successfully, false otherwise
+     */
+    bool loadRecoveryBehaviors(const robot::NodeHandle &node);
+
+    /**
+     * @brief  Loads the default recovery behaviors for the navigation stack
+     */
+    void loadDefaultRecoveryBehaviors();
+
+    /**
+     * @brief  Clears obstacles within a window around the robot
+     * @param size_x The x size of the window
+     * @param size_y The y size of the window
+     */
+    void clearCostmapWindows(double size_x, double size_y);
+
+    /**
+     * @brief  Publishes a velocity command of zero to the base
+     */
+    void publishZeroVelocity();
+
+    /**
+     * @brief  Reset the state of the move_base action and send a zero velocity command to the base
+     */
+    void resetState();
+
+    // void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
+
+    void planThread();
+
+    // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
+
+    bool isQuaternionValid(const geometry_msgs::Quaternion &q);
+
+    bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
+
+    double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2);
+
+    geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
+
+    // /**
+    //  * @brief This is used to wake the planner at periodic intervals.
+    //  */
+    // void wakePlanner(const robot::TimerEvent &event);
+
+  private:
+    bool initialized_;
+    TFListenerPtr tf_;
+    robot::NodeHandle private_nh_;
+
+    boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
+    boost::function<nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
+    boost::function<nav_core::RecoveryBehavior::Ptr()> recovery_loader_;
+
+    nav_core::BaseLocalPlanner::Ptr tc_;
+    nav_core::BaseGlobalPlanner::Ptr planner_;
+    std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
+    
+    costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
+    costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
+    std::string robot_base_frame_, global_frame_;
+    std::vector<std::string> recovery_behavior_names_;
+    unsigned int recovery_index_;
+
+    geometry_msgs::PoseStamped global_pose_;
+    double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
+    double planner_patience_, controller_patience_;
+    int32_t max_planning_retries_;
+    uint32_t planning_retries_;
+    double conservative_reset_dist_, clearing_radius_;
+    // ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
+    // ros::Subscriber goal_sub_;
+    // ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
+    bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
+    bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
+    double oscillation_timeout_, oscillation_distance_;
+
+    MoveBaseState state_;
+    RecoveryTrigger recovery_trigger_;
+
+    robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
+    geometry_msgs::PoseStamped oscillation_pose_;
+
+    // set up plan triple buffer
+    std::vector<geometry_msgs::PoseStamped> *planner_plan_;
+    std::vector<geometry_msgs::PoseStamped> *latest_plan_;
+    std::vector<geometry_msgs::PoseStamped> *controller_plan_;
+
+    // set up the planner's thread
+    bool runPlanner_;
+    boost::recursive_mutex planner_mutex_;
+    boost::condition_variable_any planner_cond_;
+    geometry_msgs::PoseStamped planner_goal_;
+    boost::thread *planner_thread_;
+
+    // boost::recursive_mutex configuration_mutex_;
+    // dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
+    // void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
+    move_base::MoveBaseConfig last_config_;
+    move_base::MoveBaseConfig default_config_;
+
+    bool setup_, p_freq_change_, c_freq_change_;
+    bool new_global_plan_;
+    bool cancel_ctr_;
+    bool pause_ctr_, paused_;
+    double min_approach_linear_velocity_{0.1};
+    geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
+    geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
+    double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
+
+    // defined planner name
+    std::string position_planner_name_{"MKTLocalPlanner"};
+    std::string docking_planner_name_{"NAVDockingLocalPlanner"};
+    std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
+    std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
+  };
+
+} // namespace move_base
+
+#endif // NAV_MOVE_BASE_ACTION_H_

src/Navigations/Packages/move_base/package.xml

@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>move_base</name>
+  <version>1.0.0</version>
+  <description>
+    The move_base package provides an implementation of an action (see the actionlib package)
+    that, given a goal in the world, will attempt to achieve it with a mobile base.
+    The move_base node links together a global and local planner to accomplish its global
+    navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner
+    interface and any local planner adhering to the nav_core::BaseLocalPlanner interface.
+  </description>
+  <author>Kevin Lee</author>
+  <maintainer email="kevin@example.com">Kevin Lee</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/move_base</url>
+
+  <!-- Build tool -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Build dependencies -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>yaml-cpp</build_depend>
+  <build_depend>boost</build_depend>
+
+  <!-- Internal dependencies (custom packages in workspace) -->
+  <build_depend>move_base_core</build_depend>
+  <build_depend>nav_core</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>xmlrpcpp</build_depend>
+  <build_depend>node_handle</build_depend>
+  <build_depend>tf3_sensor_msgs</build_depend>
+  <build_depend>tf3_geometry_msgs</build_depend>
+
+  <!-- Runtime dependencies -->
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>yaml-cpp</exec_depend>
+  <exec_depend>boost</exec_depend>
+
+  <!-- Internal runtime dependencies -->
+  <exec_depend>move_base_core</exec_depend>
+  <exec_depend>nav_core</exec_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>xmlrpcpp</exec_depend>
+  <exec_depend>node_handle</exec_depend>
+  <exec_depend>tf3_sensor_msgs</exec_depend>
+  <exec_depend>tf3_geometry_msgs</exec_depend>
+
+  <!-- Export -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
+

src/Navigations/Packages/move_base/src/move_base_main.cpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <move_base/move_base.h>
+#include <iostream>
+
+int main(int argc, char** argv)
+{
+  std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
+  move_base::MoveBase move_base;
+  move_base.initialize(tf);
+  std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl;
+  return(0);
+}