update

CMakeLists.txt

@@ -138,6 +138,17 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_actionlib_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
+endif()
+
+if (NOT TARGET robot_move_base_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_move_base_msgs)
+endif()
+
+if (NOT TARGET robot_clear_costmap_recovery)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery)
+endif()
 
 # 2. Main packages (phụ thuộc vào cores)
 # message(STATUS "[move_base] Shared library configured")

config/costmap_common_params.yaml

@@ -76,8 +76,8 @@ global_costmap:
     lethal_cost_threshold: 100
 
   obstacles:
-    observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    ffffff_scan_marking:
+    f_scan_marking:
       topic: /f_scan
       data_type: LaserScan
       clearing: false
@@ -131,8 +131,8 @@ local_costmap:
     lethal_cost_threshold: 100
 
   obstacles:
-    observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    ffffff_scan_marking:
+    f_scan_marking:
       topic: /f_scan
       data_type: LaserScan
       clearing: false

config/move_base_common_params.yaml

@@ -10,8 +10,8 @@ oscillation_distance: 0.4
 ### recovery behaviors
 recovery_behavior_enabled: true
 recovery_behaviors: [       
-                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
+                      {name: aggressive_reset, type: ClearCostmapRecovery},
-                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
+                      {name: conservative_reset, type: ClearCostmapRecovery},                      
                     ]
 
 conservative_reset:
@@ -21,5 +21,8 @@ conservative_reset:
 aggressive_reset:
   reset_distance: 3.0
 
+ClearCostmapRecovery:
+  library_path: librobot_clear_costmap_recovery
+
 MoveBase:
   library_path: libmove_base

src/APIs/c_api/src/nav_c_api.cpp

@@ -12,7 +12,7 @@
 #include <cstring>
 #include <cstdlib>
 #include <boost/dll/import.hpp>
-#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
 
 
 // ============================================================================

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

@@ -1,8 +1,7 @@
 #ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__
 #define _SCORE_GOAL_CHECKER_H_INCLUDED__
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
-#include <robot/console.h>
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>

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

@@ -1,7 +1,7 @@
 #ifndef _SCORE_ALGORITHM_H_INCLUDED__
 #define _SCORE_ALGORITHM_H_INCLUDED__
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <iostream>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <robot_nav_2d_msgs/Path2D.h>

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

@@ -1,7 +1,7 @@
 #ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
 #define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <mkt_msgs/Trajectory2D.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Twist2D.h>

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

@@ -1,7 +1,7 @@
 #ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
 #define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <score_algorithm/score_algorithm.h>
 
 #include <boost/dll/alias.hpp>

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

@@ -1,6 +1,6 @@
 #include <mkt_algorithm/diff/diff_go_straight.h>
 #include <boost/dll/alias.hpp>
-#include <robot/console.h>
+#include <robot/robot.h>
 
 void mkt_algorithm::diff::GoStraight::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)

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

@@ -1,6 +1,6 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
 #include <boost/dll/import.hpp>
-#include <robot/console.h>
+#include <robot/robot.h>
 
 #define LIMIT_VEL_THETA 0.33
 

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

@@ -1,4 +1,4 @@
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <mkt_algorithm/diff/diff_rotate_to_goal.h>
 #include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/alias.hpp>

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

@@ -4,7 +4,7 @@
 #ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H
 #define MKT_MSGS_MESSAGE_TRAJECTORY2D_H
 
-#include <robot/duration.h>
+#include <robot/robot.h>
 #include <string>
 #include <vector>
 #include <memory>

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h

@@ -1,6 +1,6 @@
 #ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
 #define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_geometry_msgs/Pose2D.h>
 #include <iostream>
 

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h

@@ -1,7 +1,7 @@
 #ifndef MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
 #define MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <memory>
 
 namespace mkt_plugins

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h

@@ -3,7 +3,7 @@
 
 #include <algorithm>
 #include <cmath>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 
 namespace mkt_plugins
 {

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h

@@ -1,7 +1,7 @@
 #ifndef _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__
 #define _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <score_algorithm/goal_checker.h>
 
 namespace mkt_plugins

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h

@@ -1,7 +1,7 @@
 #ifndef MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_
 #define MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <score_algorithm/trajectory_generator.h>
 #include <mkt_plugins/velocity_iterator.h>
 #include <mkt_plugins/kinematic_parameters.h>

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h

@@ -1,7 +1,7 @@
 #ifndef MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
 #define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <mkt_plugins/kinematic_parameters.h>
 

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -1,4 +1,4 @@
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <mkt_plugins/goal_checker.h>
 #include <boost/dll/alias.hpp>
 #include <robot_nav_2d_utils/conversions.h>

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -1,4 +1,4 @@
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <mkt_plugins/limited_accel_generator.h>
 #include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/alias.hpp>

src/Algorithms/Packages/global_planners/dock_planner

@@ -1 +1 @@
-Subproject commit c4ae3961ab21c5a2fc48bdcfa9007369fa2efe4c
+Subproject commit 8f0cd33ec717d4c65cc71bb6a9bf1a756fca3a2e

src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h

@@ -7,7 +7,7 @@
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_core/base_global_planner.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 
 namespace two_points_planner
 {

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -1,4 +1,4 @@
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <two_points_planner/two_points_planner.h>
 #include <robot_costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
@@ -6,8 +6,6 @@
 #include <tf3/LinearMath/Quaternion.h>
 #include <tf3/utils.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
-#include <robot/time.h>
-#include <robot/console.h>
 #include <cmath>
 #include <cstdio>
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h

@@ -1,8 +1,7 @@
 #ifndef _PNKX_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_LOCAL_PLANNER_H_INCLUDED_
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
-#include <robot/console.h>
 #include <robot_nav_core2/local_planner.h>
 #include <score_algorithm/trajectory_generator.h>
 #include <score_algorithm/score_algorithm.h>

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h

@@ -1,7 +1,7 @@
 #ifndef _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
 #define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_utils/conversions.h>
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -14,7 +14,7 @@
 #include <mkt_msgs/Trajectory2D.h>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
-#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
 
 pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
   : initialized_(false)

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp

@@ -1,7 +1,7 @@
 #include <pnkx_local_planner/transforms.h>
 #include <robot_nav_2d_utils/path_ops.h>
 #include <robot_nav_2d_utils/tf_help.h>
-#include <robot/console.h>
+#include <robot/robot.h>
 #include <data_convert/data_convert.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 

src/Libraries/common_msgs

@@ -1 +1 @@
-Subproject commit b8b0528f1e649b93ff07b3458aa01f251b286405
+Subproject commit 98543e6c540f48075f0a6976c83a884524956264

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit 81e78742744c0a5b8a8ef298fa254035c41dc1da
+Subproject commit 9026c03e1e3bbecdce224958c04147f9e9ae7fd2

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

@@ -14,7 +14,7 @@
 #include <map>
 #include <yaml-cpp/yaml.h>
 #include <filesystem>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 
 namespace robot
 {

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

@@ -0,0 +1,12 @@
+#ifndef ROBOT_ROBOT_H
+#define ROBOT_ROBOT_H
+
+#include <robot/init.h>
+#include <robot/time.h>
+#include <robot/timer.h>
+#include <robot/rate.h>
+#include <robot/console.h>
+#include <robot/node_handle.h>
+#include <robot/plugin_loader_helper.h>
+
+#endif

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -1982,6 +1982,11 @@ namespace robot
   template float NodeHandle::param<float>(const std::string &param_name, const float &default_val) const;
   template bool NodeHandle::param<float>(const std::string &param_name, float &param_val, const float &default_val) const;
   
+  // Vector<std::string> instantiations
+  template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string &param_name) const;
+  template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string &param_name, const std::vector<std::string> &default_val) const;
+  template bool NodeHandle::param<std::vector<std::string>>(const std::string &param_name, std::vector<std::string> &param_val, const std::vector<std::string> &default_val) const;
+
   // Static member initialization
   std::string NodeHandle::config_directory_;
   YAML::Node NodeHandle::root_;

src/Libraries/robot_time

@@ -1 +1 @@
-Subproject commit 35b77e9fa25fe4473c9fb0215f902ea587347859
+Subproject commit 750dc94c61be406937ad49e0f215a4fdce538fc5

src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt

@@ -0,0 +1,156 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_actionlib_msgs VERSION 1.0.0 LANGUAGES CXX)
+
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_actionlib_msgs with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_actionlib_msgs with Standalone CMake")
+endif()
+
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Find dependencies
+find_package(Boost REQUIRED)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
+    robot_std_msgs
+    utils
+    robot_time
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    utils
+    robot_time
+  )
+
+  find_package(Boost REQUIRED)
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME} 
+    CATKIN_DEPENDS robot_std_msgs utils robot_time
+    DEPENDS Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
+endif()
+
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_actionlib_msgs/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+      Boost::headers
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+      Boost::headers
+  )
+
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  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: robot_std_msgs, utils, robot_time, Boost")
+  message(STATUS "=================================")
+endif()
+

src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalID.h

@@ -0,0 +1,65 @@
+// Generated by gencpp from file actionlib_msgs/GoalID.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H
+#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot/time.h>
+
+namespace robot_actionlib_msgs
+{
+  template <class ContainerAllocator>
+  struct GoalID_
+  {
+    typedef GoalID_<ContainerAllocator> Type;
+
+    GoalID_()
+        : stamp(), id()
+    {
+    }
+    GoalID_(const ContainerAllocator &_alloc)
+        : stamp(), id(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef robot::Time _stamp_type;
+    _stamp_type stamp;
+
+    typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _id_type;
+    _id_type id;
+
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct GoalID_
+
+  typedef ::robot_actionlib_msgs::GoalID_<std::allocator<void>> GoalID;
+
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID> GoalIDPtr;
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID const> GoalIDConstPtr;
+
+  // constants requiring out of line definition
+
+
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_actionlib_msgs::GoalID_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalID_<ContainerAllocator2> &rhs)
+  {
+    return lhs.stamp == rhs.stamp &&
+           lhs.id == rhs.id;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_actionlib_msgs::GoalID_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalID_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_actionlib_msgs
+
+#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H

src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatus.h

@@ -0,0 +1,112 @@
+// Generated by gencpp from file actionlib_msgs/GoalStatus.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H
+#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <robot_actionlib_msgs/GoalID.h>
+
+namespace robot_actionlib_msgs
+{
+  template <class ContainerAllocator>
+  struct GoalStatus_
+  {
+    typedef GoalStatus_<ContainerAllocator> Type;
+
+    GoalStatus_()
+        : goal_id(), status(0), text()
+    {
+    }
+    GoalStatus_(const ContainerAllocator &_alloc)
+        : goal_id(_alloc), status(0), text(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_actionlib_msgs::GoalID_<ContainerAllocator> _goal_id_type;
+    _goal_id_type goal_id;
+
+    typedef uint8_t _status_type;
+    _status_type status;
+
+    typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _text_type;
+    _text_type text;
+
+// reducing the odds to have name collisions with Windows.h
+#if defined(_WIN32) && defined(PENDING)
+#undef PENDING
+#endif
+#if defined(_WIN32) && defined(ACTIVE)
+#undef ACTIVE
+#endif
+#if defined(_WIN32) && defined(PREEMPTED)
+#undef PREEMPTED
+#endif
+#if defined(_WIN32) && defined(SUCCEEDED)
+#undef SUCCEEDED
+#endif
+#if defined(_WIN32) && defined(ABORTED)
+#undef ABORTED
+#endif
+#if defined(_WIN32) && defined(REJECTED)
+#undef REJECTED
+#endif
+#if defined(_WIN32) && defined(PREEMPTING)
+#undef PREEMPTING
+#endif
+#if defined(_WIN32) && defined(RECALLING)
+#undef RECALLING
+#endif
+#if defined(_WIN32) && defined(RECALLED)
+#undef RECALLED
+#endif
+#if defined(_WIN32) && defined(LOST)
+#undef LOST
+#endif
+
+    enum
+    {
+      PENDING = 0u,
+      ACTIVE = 1u,
+      PREEMPTED = 2u,
+      SUCCEEDED = 3u,
+      ABORTED = 4u,
+      REJECTED = 5u,
+      PREEMPTING = 6u,
+      RECALLING = 7u,
+      RECALLED = 8u,
+      LOST = 9u,
+    };
+
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct GoalStatus_
+
+  typedef ::robot_actionlib_msgs::GoalStatus_<std::allocator<void>> GoalStatus;
+
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus> GoalStatusPtr;
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus const> GoalStatusConstPtr;
+
+  // constants requiring out of line definition
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator2> &rhs)
+  {
+    return lhs.goal_id == rhs.goal_id &&
+           lhs.status == rhs.status &&
+           lhs.text == rhs.text;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_actionlib_msgs
+
+#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H

src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatusArray.h

@@ -0,0 +1,64 @@
+// Generated by gencpp from file actionlib_msgs/GoalStatusArray.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H
+#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_std_msgs/Header.h>
+#include <robot_actionlib_msgs/GoalStatus.h>
+
+namespace robot_actionlib_msgs
+{
+  template <class ContainerAllocator>
+  struct GoalStatusArray_
+  {
+    typedef GoalStatusArray_<ContainerAllocator> Type;
+
+    GoalStatusArray_()
+        : header(), status_list()
+    {
+    }
+    GoalStatusArray_(const ContainerAllocator &_alloc)
+        : header(_alloc), status_list(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_std_msgs::Header_<ContainerAllocator> _header_type;
+    _header_type header;
+
+    typedef std::vector<::robot_actionlib_msgs::GoalStatus_<ContainerAllocator>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<::robot_actionlib_msgs::GoalStatus_<ContainerAllocator>>> _status_list_type;
+    _status_list_type status_list;
+
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct GoalStatusArray_
+
+  typedef ::robot_actionlib_msgs::GoalStatusArray_<std::allocator<void>> GoalStatusArray;
+
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray> GoalStatusArrayPtr;
+  typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray const> GoalStatusArrayConstPtr;
+
+  // constants requiring out of line definition
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator2> &rhs)
+  {
+    return lhs.header == rhs.header &&
+           lhs.status_list == rhs.status_list;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator1> &lhs, const ::robot_actionlib_msgs::GoalStatusArray_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_actionlib_msgs
+
+#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H

src/Navigations/Libraries/robot_actionlib_msgs/package.xml

@@ -0,0 +1,30 @@
+<package>
+  <name>robot_actionlib_msgs</name>
+  <version>0.7.10</version>
+  <description>
+    robot_actionlib_msgs is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. robot_actionlib_msgs
+    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/robot_actionlib_msgs</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>utils</build_depend>
+  <run_depend>utils</run_depend>
+
+  <build_depend>robot_time</build_depend>
+  <run_depend>robot_time</run_depend>
+</package>

src/Navigations/Libraries/robot_clear_costmap_recovery/CMakeLists.txt

@@ -0,0 +1,142 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_clear_costmap_recovery VERSION 1.0.0 LANGUAGES CXX)
+
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_clear_costmap_recovery with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_clear_costmap_recovery with Standalone CMake")
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Find dependencies
+find_package(console_bridge REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+# Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
+    robot_nav_core
+    robot_costmap_2d
+    robot_cpp
+    tf3
+    console_bridge
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_nav_core
+    robot_costmap_2d
+    robot_cpp
+    tf3
+  )
+
+# Find dependencies
+  find_package(console_bridge REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread)
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_cpp robot_nav_core robot_costmap_2d tf3
+    DEPENDS console_bridge Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
+endif()
+
+# Libraries
+add_library(${PROJECT_NAME} src/clear_costmap_recovery.cpp)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+else()
+
+  target_link_libraries(${PROJECT_NAME}
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+  )
+
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+      EXPORT ${PROJECT_NAME}-targets
+      LIBRARY DESTINATION lib
+      ARCHIVE DESTINATION lib
+      RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+      FILE ${PROJECT_NAME}-targets.cmake
+      DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # 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: ${PROJECT_NAME}")
+  message(STATUS "Dependencies: robot_nav_core, robot_costmap_2d, robot_cpp, tf3, console_bridge, Boost")
+  message(STATUS "=================================")
+endif()

src/Navigations/Libraries/robot_clear_costmap_recovery/include/robot_clear_costmap_recovery/clear_costmap_recovery.h

@@ -0,0 +1,68 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*
+* Author: Kevin Lee
+*********************************************************************/
+#ifndef _ROBOT_CLEAR_COSTMAP_RECOVERY_H_
+#define _ROBOT_CLEAR_COSTMAP_RECOVERY_H_
+#include <robot/robot.h>
+#include <robot_nav_core/recovery_behavior.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+
+#include <robot_costmap_2d/costmap_layer.h>
+
+namespace robot_clear_costmap_recovery
+{
+  /**
+   * @class ClearCostmapRecovery
+   * @brief A recovery behavior that reverts the navigation stack's costmaps to the static map outside of a user-specified region.
+   */
+  class ClearCostmapRecovery : public robot_nav_core::RecoveryBehavior {
+    public:
+      /**
+       * @brief  Constructor, make sure to call initialize in addition to actually initialize the object
+       * @param
+       * @return
+       */
+      ClearCostmapRecovery();
+
+      /**
+       * @brief  Initialization function for the ClearCostmapRecovery recovery behavior
+       * @param tf A pointer to a transform listener
+       * @param global_costmap A pointer to the global_costmap used by the navigation stack
+       * @param local_costmap A pointer to the local_costmap used by the navigation stack
+       */
+      void initialize(std::string name, tf3::BufferCore* tf,
+          robot_costmap_2d::Costmap2DROBOT* global_costmap, robot_costmap_2d::Costmap2DROBOT* local_costmap);
+
+      /** 
+       * @brief  Run the ClearCostmapRecovery recovery behavior. Reverts the
+       * costmap to the static map outside of a user-specified window and
+       * clears unknown space around the robot.
+       */
+      void runBehavior();
+
+      /**
+       * @brief  Export factory function for the ClearCostmapRecovery recovery behavior
+       * @return A pointer to the ClearCostmapRecovery recovery behavior
+       */
+      static std::shared_ptr<ClearCostmapRecovery> create();
+
+    private:
+      void clear(robot_costmap_2d::Costmap2DROBOT* costmap);
+      void clearMap(boost::shared_ptr<robot_costmap_2d::CostmapLayer> costmap, double pose_x, double pose_y);
+      robot_costmap_2d::Costmap2DROBOT* global_costmap_, *local_costmap_;
+      std::string name_;
+      tf3::BufferCore* tf_;
+      bool initialized_;
+      bool force_updating_; ///< force costmap update after clearing, so we don't need to wait for update thread
+      double reset_distance_;
+      bool invert_area_to_clear_;
+      std::string affected_maps_; ///< clear only local, global or both costmaps
+      std::set<std::string> clearable_layers_; ///< Layer names which will be cleared.
+  };
+} // namespace robot_clear_costmap_recovery
+
+#endif // _ROBOT_CLEAR_COSTMAP_RECOVERY_H_

src/Navigations/Libraries/robot_clear_costmap_recovery/package.xml

@@ -0,0 +1,27 @@
+<package>
+  <name>robot_clear_costmap_recovery</name>
+  <version>0.7.10</version>
+  <description>
+    robot_clear_costmap_recovery is a recovery behavior that reverts the navigation stack's 
+    costmaps to the static map outside of a user-specified region.
+  </description>
+  <author>Eitan Marder-Eppstein</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/robot_clear_costmap_recovery</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_nav_core</build_depend>
+  <run_depend>robot_nav_core</run_depend>
+
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>tf3</build_depend>
+  <run_depend>tf3</run_depend>
+</package>

src/Navigations/Libraries/robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp

@@ -0,0 +1,337 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, 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 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: Eitan Marder-Eppstein
+ *********************************************************************/
+#include <robot_clear_costmap_recovery/clear_costmap_recovery.h>
+#include <boost/dll/alias.hpp>
+#include <boost/pointer_cast.hpp>
+#include <vector>
+
+using robot_costmap_2d::NO_INFORMATION;
+
+namespace robot_clear_costmap_recovery
+{
+  ClearCostmapRecovery::ClearCostmapRecovery() : global_costmap_(NULL), local_costmap_(NULL),
+                                                 tf_(NULL), initialized_(false) {}
+
+  void ClearCostmapRecovery::initialize(std::string name, tf3::BufferCore *tf,
+                                        robot_costmap_2d::Costmap2DROBOT *global_costmap, robot_costmap_2d::Costmap2DROBOT *local_costmap)
+  {
+    if (!initialized_)
+    {
+      name_ = name;
+      tf_ = tf;
+      global_costmap_ = global_costmap;
+      local_costmap_ = local_costmap;
+
+      // get some parameters from the parameter server
+      robot::NodeHandle private_nh("~/" + name_);
+
+      private_nh.param("reset_distance", reset_distance_, 3.0);
+      private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
+      private_nh.param("force_updating", force_updating_, false);
+      private_nh.param("affected_maps", affected_maps_, std::string("both"));
+      if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
+      {
+        robot::log_warning("Wrong value for affected_maps parameter: '%s'; valid values are 'local', 'global' or 'both'; "
+                 "defaulting to 'both'",
+                 affected_maps_.c_str());
+        affected_maps_ = "both";
+      }
+
+      std::vector<std::string> clearable_layers_default, clearable_layers;
+      clearable_layers_default.push_back(std::string("obstacles"));
+      private_nh.param("layer_names", clearable_layers, clearable_layers_default);
+
+      for (unsigned i = 0; i < clearable_layers.size(); i++)
+      {
+        robot::log_info("Recovery behavior will clear layer '%s'", clearable_layers[i].c_str());
+        clearable_layers_.insert(clearable_layers[i]);
+      }
+      initialized_ = true;
+    }
+    else
+    {
+      robot::log_error("You should not call initialize twice on this object, doing nothing ");
+    }
+  }
+
+  void ClearCostmapRecovery::runBehavior()
+  {
+    robot::log_info("[ClearCostmapRecovery::runBehavior] ===== ENTRY =====");
+    robot::log_info("[ClearCostmapRecovery::runBehavior] Name: %s", name_.c_str());
+    
+    // Check initialization
+    if (!initialized_)
+    {
+      robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Not initialized!");
+      robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (not initialized) =====");
+      return;
+    }
+    robot::log_debug("[ClearCostmapRecovery::runBehavior] Initialization check: OK");
+
+    // Check costmaps
+    if (global_costmap_ == NULL || local_costmap_ == NULL)
+    {
+      robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Costmaps are NULL!");
+      robot::log_error("[ClearCostmapRecovery::runBehavior] global_costmap_: %s", global_costmap_ ? "valid" : "NULL");
+      robot::log_error("[ClearCostmapRecovery::runBehavior] local_costmap_: %s", local_costmap_ ? "valid" : "NULL");
+      robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (NULL costmaps) =====");
+      return;
+    }
+    robot::log_debug("[ClearCostmapRecovery::runBehavior] Costmaps check: OK");
+
+    // Log parameters
+    robot::log_info("[ClearCostmapRecovery::runBehavior] Parameters:");
+    robot::log_info("[ClearCostmapRecovery::runBehavior]   - reset_distance: %.2f m", reset_distance_);
+    robot::log_info("[ClearCostmapRecovery::runBehavior]   - invert_area_to_clear: %s", invert_area_to_clear_ ? "true" : "false");
+    robot::log_info("[ClearCostmapRecovery::runBehavior]   - force_updating: %s", force_updating_ ? "true" : "false");
+    robot::log_info("[ClearCostmapRecovery::runBehavior]   - affected_maps: %s", affected_maps_.c_str());
+    robot::log_info("[ClearCostmapRecovery::runBehavior]   - clearable_layers count: %zu", clearable_layers_.size());
+    for (const auto& layer : clearable_layers_)
+    {
+      robot::log_info("[ClearCostmapRecovery::runBehavior]     - layer: '%s'", layer.c_str());
+    }
+
+    // Log clearing mode
+    if (!invert_area_to_clear_)
+    {
+      robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s outside a square (%.2fm) large centered on the robot.", 
+                         name_.c_str(), affected_maps_.c_str(),
+                         affected_maps_ == "both" ? "s" : "", reset_distance_);
+    }
+    else
+    {
+      robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s inside a square (%.2fm) large centered on the robot.", 
+                         name_.c_str(), affected_maps_.c_str(),
+                         affected_maps_ == "both" ? "s" : "", reset_distance_);
+    }
+
+    // Process global costmap
+    robot::WallTime t0 = robot::WallTime::now();
+    if (affected_maps_ == "global" || affected_maps_ == "both")
+    {
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Processing GLOBAL costmap...");
+      robot::WallTime clear_start = robot::WallTime::now();
+      
+      clear(global_costmap_);
+      
+      robot::WallDuration clear_duration = robot::WallTime::now() - clear_start;
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap clear() completed in %.6f s", clear_duration.toSec());
+
+      if (force_updating_)
+      {
+        robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating global costmap...");
+        robot::WallTime update_start = robot::WallTime::now();
+        global_costmap_->updateMap();
+        robot::WallDuration update_duration = robot::WallTime::now() - update_start;
+        robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap updateMap() completed in %.6f s", update_duration.toSec());
+      }
+
+      robot::WallDuration total_duration = robot::WallTime::now() - t0;
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap total processing time: %.6f s", total_duration.toSec());
+      robot::log_debug("[ClearCostmapRecovery::runBehavior] Global costmap cleared in %fs", total_duration.toSec());
+    }
+    else
+    {
+      robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping global costmap (affected_maps='%s')", affected_maps_.c_str());
+    }
+
+    // Process local costmap
+    t0 = robot::WallTime::now();
+    if (affected_maps_ == "local" || affected_maps_ == "both")
+    {
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Processing LOCAL costmap...");
+      robot::WallTime clear_start = robot::WallTime::now();
+      
+      clear(local_costmap_);
+      
+      robot::WallDuration clear_duration = robot::WallTime::now() - clear_start;
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap clear() completed in %.6f s", clear_duration.toSec());
+
+      if (force_updating_)
+      {
+        robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating local costmap...");
+        robot::WallTime update_start = robot::WallTime::now();
+        local_costmap_->updateMap();
+        robot::WallDuration update_duration = robot::WallTime::now() - update_start;
+        robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap updateMap() completed in %.6f s", update_duration.toSec());
+      }
+
+      robot::WallDuration total_duration = robot::WallTime::now() - t0;
+      robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap total processing time: %.6f s", total_duration.toSec());
+      robot::log_debug("[ClearCostmapRecovery::runBehavior] Local costmap cleared in %fs", total_duration.toSec());
+    }
+    else
+    {
+      robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping local costmap (affected_maps='%s')", affected_maps_.c_str());
+    }
+
+    robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (success) =====");
+  }
+
+  void ClearCostmapRecovery::clear(robot_costmap_2d::Costmap2DROBOT *costmap)
+  {
+    robot::log_debug("[ClearCostmapRecovery::clear] ===== ENTRY =====");
+    
+    std::vector<boost::shared_ptr<robot_costmap_2d::Layer>> *plugins = costmap->getLayeredCostmap()->getPlugins();
+    robot::log_debug("[ClearCostmapRecovery::clear] Found %zu plugins", plugins ? plugins->size() : 0);
+
+    robot_geometry_msgs::PoseStamped pose;
+
+    if (!costmap->getRobotPose(pose))
+    {
+      robot::log_error("[ClearCostmapRecovery::clear] ERROR: Cannot clear map because pose cannot be retrieved");
+      robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (no pose) =====");
+      return;
+    }
+
+    double x = pose.pose.position.x;
+    double y = pose.pose.position.y;
+    robot::log_debug("[ClearCostmapRecovery::clear] Robot pose: x=%.3f, y=%.3f", x, y);
+
+    if (!plugins)
+    {
+      robot::log_error("[ClearCostmapRecovery::clear] ERROR: plugins pointer is NULL");
+      robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (NULL plugins) =====");
+      return;
+    }
+
+    int processed_layers = 0;
+    int skipped_layers = 0;
+    
+    for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
+    {
+      boost::shared_ptr<robot_costmap_2d::Layer> plugin = *pluginp;
+      if (!plugin)
+      {
+        robot::log_warning("[ClearCostmapRecovery::clear] WARNING: Found NULL plugin, skipping");
+        skipped_layers++;
+        continue;
+      }
+      
+      std::string name = plugin->getName();
+      std::string original_name = name;
+      int slash = name.rfind('/');
+      if (slash != std::string::npos)
+      {
+        name = name.substr(slash + 1);
+      }
+      
+      robot::log_debug("[ClearCostmapRecovery::clear] Processing layer: '%s' (original: '%s')", name.c_str(), original_name.c_str());
+
+      if (clearable_layers_.count(name) != 0)
+      {
+        robot::log_info("[ClearCostmapRecovery::clear] Layer '%s' is in clearable_layers, processing...", name.c_str());
+        
+        // check if the value is convertable
+        if (!dynamic_cast<robot_costmap_2d::CostmapLayer *>(plugin.get()))
+        {
+          robot::log_error("[ClearCostmapRecovery::clear] ERROR: Layer '%s' is not derived from costmap_2d::CostmapLayer", name.c_str());
+          skipped_layers++;
+          continue;
+        }
+
+        boost::shared_ptr<robot_costmap_2d::CostmapLayer> costmap_layer;
+        costmap_layer = boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin);
+        
+        robot::log_debug("[ClearCostmapRecovery::clear] Calling clearMap for layer '%s' at (%.3f, %.3f)", name.c_str(), x, y);
+        clearMap(costmap_layer, x, y);
+        processed_layers++;
+        robot::log_info("[ClearCostmapRecovery::clear] Successfully cleared layer '%s'", name.c_str());
+      }
+      else
+      {
+        robot::log_debug("[ClearCostmapRecovery::clear] Layer '%s' is NOT in clearable_layers, skipping", name.c_str());
+        skipped_layers++;
+      }
+    }
+    
+    robot::log_info("[ClearCostmapRecovery::clear] Summary: processed=%d, skipped=%d, total=%zu", 
+                    processed_layers, skipped_layers, plugins->size());
+    robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT =====");
+  }
+
+  void ClearCostmapRecovery::clearMap(boost::shared_ptr<robot_costmap_2d::CostmapLayer> costmap,
+                                      double pose_x, double pose_y)
+  {
+    robot::log_debug("[ClearCostmapRecovery::clearMap] ===== ENTRY =====");
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Robot pose: (%.3f, %.3f)", pose_x, pose_y);
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Reset distance: %.3f m", reset_distance_);
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Invert area: %s", invert_area_to_clear_ ? "true" : "false");
+    
+    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Mutex acquired");
+
+    double start_point_x = pose_x - reset_distance_ / 2;
+    double start_point_y = pose_y - reset_distance_ / 2;
+    double end_point_x = start_point_x + reset_distance_;
+    double end_point_y = start_point_y + reset_distance_;
+
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (world coords):");
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   start: (%.3f, %.3f)", start_point_x, start_point_y);
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   end: (%.3f, %.3f)", end_point_x, end_point_y);
+
+    int start_x, start_y, end_x, end_y;
+    costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+    costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
+    
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (map coords):");
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   start: (%d, %d)", start_x, start_y);
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   end: (%d, %d)", end_x, end_y);
+    
+    costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
+    robot::log_debug("[ClearCostmapRecovery::clearMap] clearArea() called");
+
+    double ox = costmap->getOriginX(), oy = costmap->getOriginY();
+    double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
+    robot::log_debug("[ClearCostmapRecovery::clearMap] Costmap bounds:");
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   origin: (%.3f, %.3f)", ox, oy);
+    robot::log_debug("[ClearCostmapRecovery::clearMap]   size: %.3f x %.3f m", width, height);
+    
+    costmap->addExtraBounds(ox, oy, ox + width, oy + height);
+    robot::log_debug("[ClearCostmapRecovery::clearMap] addExtraBounds() called");
+    robot::log_debug("[ClearCostmapRecovery::clearMap] ===== EXIT =====");
+    return;
+  }
+
+  std::shared_ptr<ClearCostmapRecovery> ClearCostmapRecovery::create()
+  {
+    return std::make_shared<ClearCostmapRecovery>();
+  }
+
+};
+
+BOOST_DLL_ALIAS(robot_clear_costmap_recovery::ClearCostmapRecovery::create, ClearCostmapRecovery)

src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tester.cpp

@@ -0,0 +1,95 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <robot_clear_costmap_recovery/robot_clear_costmap_recovery.h>
+
+#include <costmap_2d/testing_helper.h>
+#include <tf2_ros/transform_listener.h>
+
+tf2_ros::Buffer* transformer;
+tf2_ros::TransformListener* tfl;
+
+using costmap_2d::LETHAL_OBSTACLE;
+
+void testClearBehavior(std::string name, 
+                       double distance, 
+                       bool obstacles, 
+                       bool static_map,
+                       costmap_2d::Costmap2DROS* global_costmap, 
+                       costmap_2d::Costmap2DROS* local_costmap){
+    robot_clear_costmap_recovery::ClearCostmapRecovery clear = robot_clear_costmap_recovery::ClearCostmapRecovery();
+    
+    ros::NodeHandle clr("~/" + name);
+    clr.setParam("reset_distance", distance);
+    
+    std::vector<std::string> clearable_layers;
+    if(obstacles)
+        clearable_layers.push_back( std::string("obstacles") );
+    if(static_map)
+        clearable_layers.push_back( std::string("static_map") );
+        
+    clr.setParam("layer_names", clearable_layers);
+    
+    clear.initialize(name, transformer, global_costmap, local_costmap);
+    
+    clear.runBehavior();
+}
+
+void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
+{
+    costmap_2d::Costmap2DROS global(name + "/global", *transformer);
+    costmap_2d::Costmap2DROS  local(name + "/local" , *transformer);
+    boost::shared_ptr<costmap_2d::ObstacleLayer> olayer;
+        
+    std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
+    for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
+        boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
+        if(plugin->getName().find("obstacles")!=std::string::npos){
+            olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
+            addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
+            addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
+        }
+    }
+    
+    global.updateMap();
+    local.updateMap();
+    olayer->clearStaticObservations(true, true);
+
+    testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
+    
+    global.updateMap();
+    local.updateMap();
+    
+    printMap(*global.getCostmap());
+    ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
+    ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE),  local_lethal);
+    
+}
+
+TEST(ClearTester, basicTest){
+  testCountLethal("base", 3.0, true, false, 20);
+}
+
+TEST(ClearTester, bigRadiusTest){
+  testCountLethal("base", 20.0, true, false, 22);
+}
+
+TEST(ClearTester, clearNoLayersTest){
+  testCountLethal("base", 20.0, false, false, 22);
+}
+
+TEST(ClearTester, clearBothTest){
+  testCountLethal("base", 3.0, true, true, 0);
+}
+
+TEST(ClearTester, clearBothTest2){
+  testCountLethal("base", 12.0, true, true, 6);
+}
+
+
+int main(int argc, char** argv){
+  ros::init(argc, argv, "clear_tests");
+  testing::InitGoogleTest(&argc, argv);
+  transformer = new tf2_ros::Buffer(ros::Duration(10));
+  tfl = new tf2_ros::TransformListener(*transformer);
+  return RUN_ALL_TESTS();
+}

src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tests.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
+  <node name="static_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /base_link 100" />
+  <test time-limit="300" test-name="clear_tests" pkg="robot_clear_costmap_recovery" type="clear_tester" />
+  <rosparam command="load" file="$(find robot_clear_costmap_recovery)/test/params.yaml" ns="clear_tests"/>
+</launch>

src/Navigations/Libraries/robot_clear_costmap_recovery/test/params.yaml

@@ -0,0 +1,13 @@
+base:
+    global:
+        plugins:
+        - {name: static_map,       type: "costmap_2d::StaticLayer"}
+        - {name: obstacles,        type: "costmap_2d::ObstacleLayer"}
+        - {name: inflation,        type: "costmap_2d::InflationLayer"}
+        obstacles:
+            track_unknown_space: true
+    local:
+        plugins: []
+    robot_radius: .5
+clear:
+    clearing_distance: 7.0    

src/Navigations/Libraries/robot_move_base_msgs/CMakeLists.txt

@@ -0,0 +1,156 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_move_base_msgs VERSION 1.0.0 LANGUAGES CXX)
+
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_move_base_msgs with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_move_base_msgs with Standalone CMake")
+endif()
+
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Find dependencies
+find_package(Boost REQUIRED)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
+    robot_std_msgs
+    robot_actionlib_msgs
+    robot_geometry_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    robot_actionlib_msgs
+    robot_geometry_msgs
+  )
+
+  find_package(Boost REQUIRED)
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME} 
+    CATKIN_DEPENDS robot_std_msgs robot_actionlib_msgs robot_geometry_msgs
+    DEPENDS Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
+endif()
+
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_move_base_msgs/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+      Boost::headers
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+      Boost::headers
+  )
+
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  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: robot_std_msgs, robot_actionlib_msgs, robot_geometry_msgs, Boost")
+  message(STATUS "=================================")
+endif()
+

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseAction.h

@@ -0,0 +1,69 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseAction.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_move_base_msgs/MoveBaseActionGoal.h>
+#include <robot_move_base_msgs/MoveBaseActionResult.h>
+#include <robot_move_base_msgs/MoveBaseActionFeedback.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseAction_
+  {
+    typedef MoveBaseAction_<ContainerAllocator> Type;
+
+    MoveBaseAction_()
+        : action_goal(), action_result(), action_feedback()
+    {
+    }
+    MoveBaseAction_(const ContainerAllocator &_alloc)
+        : action_goal(_alloc), action_result(_alloc), action_feedback(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator> _action_goal_type;
+    _action_goal_type action_goal;
+
+    typedef ::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator> _action_result_type;
+    _action_result_type action_result;
+
+    typedef ::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator> _action_feedback_type;
+    _action_feedback_type action_feedback;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseAction_
+
+  typedef ::robot_move_base_msgs::MoveBaseAction_<std::allocator<void>> MoveBaseAction;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction> MoveBaseActionPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction const> MoveBaseActionConstPtr;
+
+  // constants requiring out of line definition
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator2> &rhs)
+  {
+    return lhs.action_goal == rhs.action_goal &&
+           lhs.action_result == rhs.action_result &&
+           lhs.action_feedback == rhs.action_feedback;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseAction_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionFeedback.h

@@ -0,0 +1,68 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionFeedback.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_std_msgs/Header.h>
+#include <robot_actionlib_msgs/GoalStatus.h>
+#include <robot_move_base_msgs/MoveBaseFeedback.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseActionFeedback_
+  {
+    typedef MoveBaseActionFeedback_<ContainerAllocator> Type;
+
+    MoveBaseActionFeedback_()
+        : header(), status(), feedback()
+    {
+    }
+    MoveBaseActionFeedback_(const ContainerAllocator &_alloc)
+        : header(_alloc), status(_alloc), feedback(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_std_msgs::Header_<ContainerAllocator> _header_type;
+    _header_type header;
+
+    typedef ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator> _status_type;
+    _status_type status;
+
+    typedef ::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator> _feedback_type;
+    _feedback_type feedback;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseActionFeedback_
+
+  typedef ::robot_move_base_msgs::MoveBaseActionFeedback_<std::allocator<void>> MoveBaseActionFeedback;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback> MoveBaseActionFeedbackPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback const> MoveBaseActionFeedbackConstPtr;
+
+  // constants requiring out of line definition
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator2> &rhs)
+  {
+    return lhs.header == rhs.header &&
+           lhs.status == rhs.status &&
+           lhs.feedback == rhs.feedback;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionFeedback_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionGoal.h

@@ -0,0 +1,69 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionGoal.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_std_msgs/Header.h>
+#include <robot_actionlib_msgs/GoalID.h>
+#include <robot_move_base_msgs/MoveBaseGoal.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseActionGoal_
+  {
+    typedef MoveBaseActionGoal_<ContainerAllocator> Type;
+
+    MoveBaseActionGoal_()
+        : header(), goal_id(), goal()
+    {
+    }
+    MoveBaseActionGoal_(const ContainerAllocator &_alloc)
+        : header(_alloc), goal_id(_alloc), goal(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_std_msgs::Header_<ContainerAllocator> _header_type;
+    _header_type header;
+
+    typedef ::robot_actionlib_msgs::GoalID_<ContainerAllocator> _goal_id_type;
+    _goal_id_type goal_id;
+
+    typedef ::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator> _goal_type;
+    _goal_type goal;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseActionGoal_
+
+  typedef ::robot_move_base_msgs::MoveBaseActionGoal_<std::allocator<void>> MoveBaseActionGoal;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal> MoveBaseActionGoalPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal const> MoveBaseActionGoalConstPtr;
+
+  // constants requiring out of line definition
+  
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator2> &rhs)
+  {
+    return lhs.header == rhs.header &&
+           lhs.goal_id == rhs.goal_id &&
+           lhs.goal == rhs.goal;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionGoal_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionResult.h

@@ -0,0 +1,68 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionResult.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_std_msgs/Header.h>
+#include <robot_actionlib_msgs/GoalStatus.h>
+#include <robot_move_base_msgs/MoveBaseResult.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseActionResult_
+  {
+    typedef MoveBaseActionResult_<ContainerAllocator> Type;
+
+    MoveBaseActionResult_()
+        : header(), status(), result()
+    {
+    }
+    MoveBaseActionResult_(const ContainerAllocator &_alloc)
+        : header(_alloc), status(_alloc), result(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_std_msgs::Header_<ContainerAllocator> _header_type;
+    _header_type header;
+
+    typedef ::robot_actionlib_msgs::GoalStatus_<ContainerAllocator> _status_type;
+    _status_type status;
+
+    typedef ::robot_move_base_msgs::MoveBaseResult_<ContainerAllocator> _result_type;
+    _result_type result;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseActionResult_
+
+  typedef ::robot_move_base_msgs::MoveBaseActionResult_<std::allocator<void>> MoveBaseActionResult;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult> MoveBaseActionResultPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult const> MoveBaseActionResultConstPtr;
+
+  // constants requiring out of line definition
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator2> &rhs)
+  {
+    return lhs.header == rhs.header &&
+           lhs.status == rhs.status &&
+           lhs.result == rhs.result;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseActionResult_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseFeedback.h

@@ -0,0 +1,58 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseFeedback.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_geometry_msgs/PoseStamped.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseFeedback_
+  {
+    typedef MoveBaseFeedback_<ContainerAllocator> Type;
+
+    MoveBaseFeedback_()
+        : base_position()
+    {
+    }
+    MoveBaseFeedback_(const ContainerAllocator &_alloc)
+        : base_position(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_geometry_msgs::PoseStamped_<ContainerAllocator> _base_position_type;
+    _base_position_type base_position;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseFeedback_
+
+  typedef ::robot_move_base_msgs::MoveBaseFeedback_<std::allocator<void>> MoveBaseFeedback;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback> MoveBaseFeedbackPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback const> MoveBaseFeedbackConstPtr;
+
+  // constants requiring out of line definition
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator2> &rhs)
+  {
+    return lhs.base_position == rhs.base_position;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseFeedback_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseGoal.h

@@ -0,0 +1,57 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseGoal.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <robot_geometry_msgs/PoseStamped.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseGoal_
+  {
+    typedef MoveBaseGoal_<ContainerAllocator> Type;
+
+    MoveBaseGoal_()
+        : target_pose()
+    {
+    }
+    MoveBaseGoal_(const ContainerAllocator &_alloc)
+        : target_pose(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_geometry_msgs::PoseStamped_<ContainerAllocator> _target_pose_type;
+    _target_pose_type target_pose;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseGoal_
+
+  typedef ::robot_move_base_msgs::MoveBaseGoal_<std::allocator<void>> MoveBaseGoal;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal> MoveBaseGoalPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal const> MoveBaseGoalConstPtr;
+
+  // constants requiring out of line definition
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator2> &rhs)
+  {
+    return lhs.target_pose == rhs.target_pose;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::MoveBaseGoal_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseResult.h

@@ -0,0 +1,39 @@
+// Generated by gencpp from file robot_move_base_msgs/MoveBaseResult.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct MoveBaseResult_
+  {
+    typedef MoveBaseResult_<ContainerAllocator> Type;
+
+    MoveBaseResult_()
+    {
+    }
+    MoveBaseResult_(const ContainerAllocator &_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct MoveBaseResult_
+
+  typedef ::robot_move_base_msgs::MoveBaseResult_<std::allocator<void>> MoveBaseResult;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult> MoveBaseResultPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult const> MoveBaseResultConstPtr;
+
+  // constants requiring out of line definition
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H

src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/RecoveryStatus.h

@@ -0,0 +1,71 @@
+// Generated by gencpp from file robot_move_base_msgs/RecoveryStatus.msg
+// DO NOT EDIT!
+
+#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H
+#define ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <robot_geometry_msgs/PoseStamped.h>
+
+namespace robot_move_base_msgs
+{
+  template <class ContainerAllocator>
+  struct RecoveryStatus_
+  {
+    typedef RecoveryStatus_<ContainerAllocator> Type;
+
+    RecoveryStatus_()
+        : pose_stamped(), current_recovery_number(0), total_number_of_recoveries(0), recovery_behavior_name()
+    {
+    }
+    RecoveryStatus_(const ContainerAllocator &_alloc)
+        : pose_stamped(_alloc), current_recovery_number(0), total_number_of_recoveries(0), recovery_behavior_name(_alloc)
+    {
+      (void)_alloc;
+    }
+
+    typedef ::robot_geometry_msgs::PoseStamped_<ContainerAllocator> _pose_stamped_type;
+    _pose_stamped_type pose_stamped;
+
+    typedef uint16_t _current_recovery_number_type;
+    _current_recovery_number_type current_recovery_number;
+
+    typedef uint16_t _total_number_of_recoveries_type;
+    _total_number_of_recoveries_type total_number_of_recoveries;
+
+    typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _recovery_behavior_name_type;
+    _recovery_behavior_name_type recovery_behavior_name;
+
+    typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator>> Ptr;
+    typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator> const> ConstPtr;
+
+  }; // struct RecoveryStatus_
+
+  typedef ::robot_move_base_msgs::RecoveryStatus_<std::allocator<void>> RecoveryStatus;
+
+  typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus> RecoveryStatusPtr;
+  typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus const> RecoveryStatusConstPtr;
+
+  // constants requiring out of line definition
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator==(const ::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator2> &rhs)
+  {
+    return lhs.pose_stamped == rhs.pose_stamped &&
+           lhs.current_recovery_number == rhs.current_recovery_number &&
+           lhs.total_number_of_recoveries == rhs.total_number_of_recoveries &&
+           lhs.recovery_behavior_name == rhs.recovery_behavior_name;
+  }
+
+  template <typename ContainerAllocator1, typename ContainerAllocator2>
+  bool operator!=(const ::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator1> &lhs, const ::robot_move_base_msgs::RecoveryStatus_<ContainerAllocator2> &rhs)
+  {
+    return !(lhs == rhs);
+  }
+
+} // namespace robot_move_base_msgs
+
+#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H

src/Navigations/Libraries/robot_move_base_msgs/package.xml

@@ -0,0 +1,30 @@
+<package>
+  <name>robot_move_base_msgs</name>
+  <version>0.7.10</version>
+  <description>
+    robot_move_base_msgs is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. robot_move_base_msgs
+    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/robot_move_base_msgs</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>robot_actionlib_msgs</build_depend>
+  <run_depend>robot_actionlib_msgs</run_depend>
+
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+</package>

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -42,6 +42,7 @@ if (NOT BUILDING_WITH_CATKIN)
     data_convert
     robot_nav_2d_utils
     robot_cpp
+    robot_move_base_msgs
   )
 
 else()
@@ -60,12 +61,13 @@ else()
     data_convert
     robot_nav_2d_utils
     robot_cpp
+    robot_move_base_msgs
   )
 
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp
+    CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp robot_move_base_msgs
     DEPENDS Boost yaml-cpp
   )
 
@@ -82,6 +84,7 @@ endif()
 add_library(${PROJECT_NAME} SHARED
   src/move_base.cpp
   src/convert_data.cpp
+  src/simple_action_server.cpp
 )
 
 if(BUILDING_WITH_CATKIN)
@@ -201,6 +204,6 @@ else()
   message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
   message(STATUS "Libraries: ${PROJECT_NAME}")
   message(STATUS "Executables: ${PROJECT_NAME}_main")
-  message(STATUS "Dependencies: geometry_msgs, robot_std_msgs, move_base_core, robot_nav_core, robot_costmap_2d, robot_tf3_sensor_msgs, robot_tf3_geometry_msgs, data_convert, robot_nav_2d_utils, robot_cpp, Boost, yaml-cpp")
+    message(STATUS "Dependencies: geometry_msgs, robot_std_msgs, move_base_core, robot_nav_core, robot_costmap_2d, robot_tf3_sensor_msgs, robot_tf3_geometry_msgs, data_convert, robot_nav_2d_utils, robot_cpp, robot_move_base_msgs, Boost, yaml-cpp")
   message(STATUS "=================================")
 endif()

src/Navigations/Packages/move_base/README_ACTION_SERVER.md

@@ -1,161 +0,0 @@
-# MoveBaseActionServer - Hướng dẫn sử dụng
-
-## Tổng quan
-
-`MoveBaseActionServer` cung cấp một interface tương tự ROS Action Server, cho phép bạn:
-- Gửi goals và nhận feedback
-- Theo dõi tiến trình navigation
-- Hủy goals (cancel/preempt)
-- Nhận kết quả khi hoàn thành
-
-## So sánh với ROS Action Server
-
-| ROS Action Server | MoveBaseActionServer |
-|-------------------|---------------------|
-| `actionlib::ActionServer` | `move_base::MoveBaseActionServer` |
-| `as_->acceptNewGoal()` | `action_server.sendGoal()` |
-| `as_->setSucceeded()` | `action_server.setSucceeded()` |
-| `as_->setAborted()` | `action_server.setAborted()` |
-| `as_->setPreempted()` | `action_server.setPreempted()` |
-| `as_->publishFeedback()` | Tự động qua callback |
-| `as_->isNewGoalAvailable()` | `action_server.isNewGoalAvailable()` |
-| `as_->isPreemptRequested()` | `action_server.isPreemptRequested()` |
-
-## Cách sử dụng cơ bản
-
-### 1. Khởi tạo
-
-```cpp
-#include <move_base/move_base_action_server.h>
-
-// Tạo MoveBase instance (như bình thường)
-robot::move_base_core::BaseNavigation::Ptr move_base_ptr = ...;
-move_base_ptr->initialize(tf);
-
-// Tạo Action Server
-move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz
-action_server.start();
-```
-
-### 2. Thiết lập Callbacks
-
-```cpp
-// Callback khi nhận goal mới
-action_server.setGoalCallback([](const move_base::MoveBaseActionGoal& goal) {
-    std::cout << "New goal: (" << goal.target_pose.pose.position.x 
-              << ", " << goal.target_pose.pose.position.y << ")" << std::endl;
-});
-
-// Callback cho feedback (gọi định kỳ)
-action_server.setFeedbackCallback([](const move_base::MoveBaseActionFeedback& feedback) {
-    std::cout << "State: " << robot::move_base_core::toString(
-        feedback.nav_feedback.navigation_state) << std::endl;
-    std::cout << "Distance to goal: " << feedback.distance_to_goal << " m" << std::endl;
-});
-
-// Callback khi goal hoàn thành
-action_server.setResultCallback([](const move_base::MoveBaseActionResult& result) {
-    if (result.success) {
-        std::cout << "Goal succeeded!" << std::endl;
-    } else {
-        std::cout << "Goal failed: " << result.message << std::endl;
-    }
-});
-
-// Callback khi goal bị preempt
-action_server.setPreemptCallback([]() {
-    std::cout << "Goal was preempted" << std::endl;
-});
-```
-
-### 3. Gửi Goal
-
-```cpp
-move_base::MoveBaseActionGoal goal;
-goal.target_pose.header.frame_id = "map";
-goal.target_pose.header.stamp = robot::Time::now();
-goal.target_pose.pose.position.x = 5.0;
-goal.target_pose.pose.position.y = 3.0;
-goal.target_pose.pose.position.z = 0.0;
-
-// Set orientation
-tf3::Quaternion q;
-q.setRPY(0, 0, 0.5); // yaw = 0.5 radians
-goal.target_pose.pose.orientation = tf3::toMsg(q);
-
-goal.xy_goal_tolerance = 0.2;
-goal.yaw_goal_tolerance = 0.1;
-goal.goal_id = "goal_1";
-
-if (action_server.sendGoal(goal)) {
-    std::cout << "Goal accepted!" << std::endl;
-} else {
-    std::cout << "Goal rejected!" << std::endl;
-}
-```
-
-### 4. Hủy Goal
-
-```cpp
-if (action_server.cancelGoal()) {
-    std::cout << "Goal cancelled" << std::endl;
-}
-```
-
-### 5. Kiểm tra trạng thái
-
-```cpp
-// Kiểm tra có goal mới không (cho preemption)
-if (action_server.isNewGoalAvailable()) {
-    move_base::MoveBaseActionGoal new_goal = action_server.acceptNewGoal();
-    // Xử lý goal mới
-}
-
-// Kiểm tra có yêu cầu preempt không
-if (action_server.isPreemptRequested()) {
-    // Dừng và cleanup
-}
-```
-
-## Ví dụ đầy đủ
-
-Xem file `examples/action_server_example.cpp` để có ví dụ hoàn chỉnh.
-
-## Thread Safety
-
-- `MoveBaseActionServer` sử dụng mutex để đảm bảo thread-safe
-- Control loop chạy trong thread riêng
-- Tất cả các method đều thread-safe
-
-## Lưu ý
-
-1. **Control Loop**: Action Server tự động chạy control loop ở tần số đã chỉ định (mặc định 20 Hz)
-2. **Goal Management**: Chỉ có thể có 1 goal active tại một thời điểm. Goal mới sẽ preempt goal cũ.
-3. **Feedback**: Feedback được gửi tự động qua callback, không cần gọi `publishFeedback()` như ROS
-4. **Result**: Result được set tự động dựa trên state của MoveBase, hoặc có thể set thủ công
-
-## Migration từ ROS Action Server
-
-Nếu bạn đang dùng ROS Action Server, migration rất đơn giản:
-
-**Trước (ROS):**
-```cpp
-MoveBaseActionServer *as_;
-as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", 
-    [this](auto &goal) { executeCb(goal); }, false);
-as_->start();
-```
-
-**Sau (MoveBaseActionServer):**
-```cpp
-move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0);
-action_server.setGoalCallback([this](const auto& goal) { 
-    executeCb(goal); 
-});
-action_server.start();
-```
-
-## API Reference
-
-Xem file `include/move_base/move_base_action_server.h` để có đầy đủ API documentation.
-

src/Navigations/Packages/move_base/examples/IMPLEMENTATION_GUIDE.md

@@ -0,0 +1,265 @@
+# Hướng dẫn triển khai SimpleActionServer trong MoveBase
+
+## Tổng quan
+
+Tài liệu này hướng dẫn cách tích hợp `SimpleActionServer` vào class `MoveBase` hiện có.
+
+## Bước 1: Thêm SimpleActionServer vào MoveBase class
+
+### Trong `move_base.h`:
+
+```cpp
+#include <move_base/simple_action_server.h>
+
+namespace move_base
+{
+  class MoveBase : public robot::move_base_core::BaseNavigation
+  {
+  private:
+    // Thêm member variable
+    std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> as_;
+    
+    // Thêm method declaration
+    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal);
+  };
+}
+```
+
+## Bước 2: Khởi tạo trong constructor hoặc initialize()
+
+### Trong `move_base.cpp` constructor hoặc `initialize()`:
+
+```cpp
+void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
+{
+  // ... existing initialization code ...
+  
+  // Khởi tạo action server
+  as_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
+    "move_base",
+    std::bind(&MoveBase::executeCb, this, std::placeholders::_1),
+    true  // auto_start
+  );
+  
+  // Setup callbacks (optional)
+  setupActionServerCallbacks();
+  
+  robot::log_info("[MoveBase] Action server initialized");
+}
+```
+
+## Bước 3: Setup callbacks (nếu cần publish messages)
+
+```cpp
+void move_base::MoveBase::setupActionServerCallbacks()
+{
+  if (!as_)
+    return;
+  
+  // Set callback để publish result
+  // Customize để gửi qua hệ thống communication của bạn
+  as_->setResultCallback([](const auto& result) {
+    // TODO: Gửi result qua hệ thống communication của bạn
+    // your_communication_system.send(result);
+    robot::log_info("Result: status=%d", result->status.status);
+  });
+  
+  // Set callback để publish feedback
+  as_->setFeedbackCallback([](const auto& feedback) {
+    // TODO: Gửi feedback qua hệ thống communication của bạn
+    // your_communication_system.send(feedback);
+  });
+  
+  // Set callback để publish status
+  as_->setStatusCallback([](const auto& status) {
+    // TODO: Gửi status qua hệ thống communication của bạn
+    // your_communication_system.send(status);
+  });
+}
+```
+
+## Bước 4: Triển khai executeCb()
+
+### Trong `move_base.cpp`:
+
+```cpp
+void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
+{
+  robot::log_info("[MoveBase] Execute callback: x=%.2f, y=%.2f",
+                 goal->target_pose.pose.position.x,
+                 goal->target_pose.pose.position.y);
+  
+  // Validate quaternion
+  if (!isQuaternionValid(goal->target_pose.pose.orientation))
+  {
+    robot::log_error("[MoveBase] Invalid quaternion in goal");
+    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                   "Invalid quaternion in goal");
+    return;
+  }
+  
+  // Convert goal to global frame
+  robot_geometry_msgs::PoseStamped target_pose = goalToGlobalFrame(goal->target_pose);
+  
+  // Send goal to move_base
+  bool success = moveTo(target_pose, 0.2, 0.2);
+  
+  if (!success)
+  {
+    robot::log_error("[MoveBase] Failed to send goal");
+    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                   "Failed to send goal to move_base");
+    return;
+  }
+  
+  // Monitor execution
+  robot::Rate rate(10.0);
+  robot::Time last_feedback_time = robot::Time::now();
+  
+  while (as_->isActive() && !as_->isPreemptRequested())
+  {
+    // Get feedback
+    robot::move_base_core::NavFeedback* feedback = getFeedback();
+    
+    if (feedback)
+    {
+      // Check if goal reached
+      if (feedback->goal_checked && 
+          feedback->navigation_state == robot::move_base_core::State::SUCCEEDED)
+      {
+        as_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
+                         "Goal reached");
+        return;
+      }
+      
+      // Check for errors
+      if (feedback->navigation_state == robot::move_base_core::State::ABORTED)
+      {
+        as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                       feedback->feed_back_str);
+        return;
+      }
+      
+      // Publish feedback periodically
+      robot::Time now = robot::Time::now();
+      if ((now - last_feedback_time).toSec() >= 0.5)
+      {
+        robot_move_base_msgs::MoveBaseFeedback fb;
+        fb.base_position.x = feedback->current_pose.x;
+        fb.base_position.y = feedback->current_pose.y;
+        fb.base_position.theta = feedback->current_pose.theta;
+        fb.base_position.header.stamp = robot::Time::now();
+        fb.base_position.header.frame_id = global_frame_;
+        
+        as_->publishFeedback(fb);
+        last_feedback_time = now;
+      }
+    }
+    
+    // Check for preemption
+    if (as_->isPreemptRequested())
+    {
+      cancel();
+      as_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
+                       "Preempted");
+      return;
+    }
+    
+    rate.sleep();
+  }
+  
+  // Handle unexpected exit
+  if (as_->isPreemptRequested())
+  {
+    cancel();
+    as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Preempted");
+  }
+  else
+  {
+    as_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                   "Execution ended unexpectedly");
+  }
+}
+```
+
+## Bước 5: Xử lý goal và cancel messages từ bên ngoài
+
+Nhận goal và cancel messages từ hệ thống communication của bạn:
+
+```cpp
+// Trong callback nhận goal từ hệ thống communication của bạn
+void move_base::MoveBase::goalCallback(
+    const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
+{
+  if (as_)
+  {
+    as_->processGoal(msg);
+  }
+}
+
+// Trong callback nhận cancel từ hệ thống communication của bạn
+void move_base::MoveBase::cancelCallback(
+    const robot_actionlib_msgs::GoalIDConstPtr& msg)
+{
+  if (as_)
+  {
+    as_->processCancel(msg);
+  }
+}
+
+// Ví dụ: Đăng ký với hệ thống communication của bạn
+// your_communication_system.onGoal(
+//     std::bind(&MoveBase::goalCallback, this, std::placeholders::_1));
+// your_communication_system.onCancel(
+//     std::bind(&MoveBase::cancelCallback, this, std::placeholders::_1));
+```
+
+## Bước 6: Cleanup trong destructor
+
+```cpp
+move_base::MoveBase::~MoveBase()
+{
+  // ... existing cleanup ...
+  
+  // Stop action server
+  if (as_)
+  {
+    as_->stop();
+    as_.reset();
+  }
+}
+```
+
+## Ví dụ hoàn chỉnh
+
+Xem các file trong thư mục `examples/`:
+- `simple_usage_example.cpp` - Ví dụ đơn giản nhất
+- `simple_action_server_example.cpp` - Ví dụ đầy đủ
+- `move_base_with_action_server_example.cpp` - Ví dụ tích hợp vào MoveBase
+
+## Lưu ý
+
+1. **Thread Safety**: `executeCb` chạy trong thread riêng, đảm bảo thread-safe khi truy cập các member variables
+2. **State Management**: Action server tự động quản lý state, không cần can thiệp thủ công
+3. **Feedback Rate**: Điều chỉnh rate publish feedback phù hợp (khuyến nghị 0.5-1.0 giây)
+4. **Error Handling**: Luôn kiểm tra và xử lý các trường hợp lỗi
+
+## Testing
+
+Để test action server:
+
+```cpp
+// Tạo goal message
+robot_move_base_msgs::MoveBaseActionGoalPtr goal = 
+    boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+goal->goal_id.id = "test_goal_1";
+goal->goal.target_pose.pose.position.x = 5.0;
+goal->goal.target_pose.pose.position.y = 3.0;
+
+// Gửi goal
+move_base_ptr->getActionServer()->processGoal(goal);
+
+// Kiểm tra status
+auto status = move_base_ptr->getActionServer()->getCurrentStatus();
+```
+

src/Navigations/Packages/move_base/examples/README_SIMPLE_ACTION_SERVER.md

@@ -0,0 +1,220 @@
+# SimpleActionServer - Hướng dẫn sử dụng
+
+## Tổng quan
+
+`SimpleActionServer` là một thư viện tương tự ROS actionlib::SimpleActionServer nhưng không phụ thuộc vào ROS. Nó cung cấp interface đơn giản để triển khai action server với các tính năng:
+
+- Quản lý goal requests
+- Hỗ trợ preemption (hủy goal)
+- Publish feedback và result thông qua callbacks
+- Thread-safe và thread-safe state management
+- Tương thích với framework robot hiện có
+
+## Cấu trúc Files
+
+- `simple_action_server.h` - Header file với class declaration
+- `simple_action_server.cpp` - Implementation file với template code
+- `simple_action_server_example.cpp` - Ví dụ triển khai đầy đủ
+
+## Ví dụ sử dụng cơ bản
+
+### 1. Khởi tạo SimpleActionServer
+
+```cpp
+#include <move_base/simple_action_server.h>
+
+// Tạo action server với execute callback
+auto action_server = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
+    "move_base",
+    [this](const auto& goal) { this->executeCb(goal); },
+    true  // auto_start
+);
+```
+
+### 2. Setup callbacks để publish messages
+
+```cpp
+// Set callback để publish result
+action_server->setResultCallback([](const auto& result) {
+    // Publish result message qua ROS hoặc hệ thống khác
+    result_pub_.publish(*result);
+});
+
+// Set callback để publish feedback
+action_server->setFeedbackCallback([](const auto& feedback) {
+    // Publish feedback message
+    feedback_pub_.publish(*feedback);
+});
+
+// Set callback để publish status
+action_server->setStatusCallback([](const auto& status) {
+    // Publish status message
+    status_pub_.publish(*status);
+});
+```
+
+### 3. Xử lý goal messages từ bên ngoài
+
+```cpp
+// Trong ROS subscriber callback hoặc nơi nhận goal
+void goalCallback(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
+{
+    action_server->processGoal(msg);
+}
+
+// Trong cancel callback
+void cancelCallback(const robot_actionlib_msgs::GoalIDConstPtr& msg)
+{
+    action_server->processCancel(msg);
+}
+```
+
+### 4. Triển khai execute callback
+
+```cpp
+void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
+{
+    robot::log_info("Received goal: x=%.2f, y=%.2f", 
+                   goal->target_pose.pose.position.x,
+                   goal->target_pose.pose.position.y);
+    
+    // Gửi goal đến move_base
+    bool success = move_base_ptr_->moveTo(goal->target_pose, 0.2, 0.2);
+    
+    if (!success)
+    {
+        action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                                   "Failed to send goal");
+        return;
+    }
+    
+    // Monitor execution
+    robot::Rate rate(10.0);
+    while (action_server_->isActive() && !action_server_->isPreemptRequested())
+    {
+        // Kiểm tra trạng thái
+        auto* feedback = move_base_ptr_->getFeedback();
+        if (feedback->goal_checked && 
+            feedback->navigation_state == robot::move_base_core::State::SUCCEEDED)
+        {
+            action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
+                                        "Goal reached");
+            return;
+        }
+        
+        // Publish feedback
+        robot_move_base_msgs::MoveBaseFeedback fb;
+        fb.base_position.x = feedback->current_pose.x;
+        fb.base_position.y = feedback->current_pose.y;
+        action_server_->publishFeedback(fb);
+        
+        rate.sleep();
+    }
+    
+    // Xử lý preemption
+    if (action_server_->isPreemptRequested())
+    {
+        move_base_ptr_->cancel();
+        action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
+                                    "Preempted");
+    }
+}
+```
+
+## Tích hợp vào MoveBase
+
+### Cách 1: Thêm vào MoveBase class
+
+```cpp
+// Trong move_base.h
+#include <move_base/simple_action_server.h>
+
+class MoveBase : public robot::move_base_core::BaseNavigation
+{
+private:
+    std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> as_;
+    
+    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal);
+};
+```
+
+```cpp
+// Trong move_base.cpp constructor
+MoveBase::MoveBase(robot::TFListenerPtr tf)
+{
+    // ... existing initialization ...
+    
+    // Create action server
+    as_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
+        "move_base",
+        std::bind(&MoveBase::executeCb, this, std::placeholders::_1),
+        true
+    );
+    
+    // Setup callbacks
+    as_->setResultCallback([this](const auto& result) {
+        // Publish result if needed
+    });
+}
+```
+
+### Cách 2: Sử dụng trong wrapper class
+
+Xem file `simple_action_server_example.cpp` để xem ví dụ đầy đủ.
+
+## API Reference
+
+### Constructor
+```cpp
+SimpleActionServer(const std::string& name,
+                   ExecuteCallback execute_callback,
+                   bool auto_start = true)
+```
+
+### Main Methods
+- `void processGoal(const ActionGoalConstPtr& goal_msg)` - Xử lý goal message
+- `void processCancel(const GoalIDConstPtr& cancel_msg)` - Xử lý cancel message
+- `GoalConstPtr acceptNewGoal()` - Chấp nhận goal mới
+- `bool isNewGoalAvailable()` - Kiểm tra có goal mới
+- `bool isPreemptRequested()` - Kiểm tra preemption
+- `void setSucceeded(const Result&, const std::string&)` - Đặt thành công
+- `void setAborted(const Result&, const std::string&)` - Đặt lỗi
+- `void setPreempted(const Result&, const std::string&)` - Đặt preempted
+- `void publishFeedback(const Feedback&)` - Publish feedback
+
+### Callback Setup
+- `void setGoalCallback(GoalCallback)` - Callback khi nhận goal
+- `void setCancelCallback(CancelCallback)` - Callback khi nhận cancel
+- `void setFeedbackCallback(FeedbackCallback)` - Callback để publish feedback
+- `void setResultCallback(ResultCallback)` - Callback để publish result
+- `void setStatusCallback(StatusCallback)` - Callback để publish status
+
+## Lưu ý
+
+1. **Thread Safety**: Tất cả các phương thức đều thread-safe
+2. **Callbacks**: Các callbacks được gọi trong context của thread tương ứng
+3. **State Management**: Action server tự động quản lý state machine
+4. **No ROS Dependency**: Không cần ROS, chỉ cần setup callbacks để publish messages
+
+## Tích hợp với hệ thống communication khác
+
+SimpleActionServer không phụ thuộc vào ROS. Bạn có thể tích hợp với bất kỳ hệ thống communication nào bằng cách setup callbacks:
+
+```cpp
+// Setup callbacks để publish messages qua hệ thống của bạn
+action_server->setResultCallback([](const auto& result) {
+    // Gửi result qua hệ thống communication của bạn
+    // your_communication_system.send(result);
+});
+
+action_server->setFeedbackCallback([](const auto& feedback) {
+    // Gửi feedback qua hệ thống communication của bạn
+    // your_communication_system.send(feedback);
+});
+
+// Nhận goal messages từ hệ thống của bạn
+// your_communication_system.onGoal([&action_server](const auto& msg) {
+//     action_server->processGoal(msg);
+// });
+```
+

src/Navigations/Packages/move_base/examples/action_server_example.cpp

@@ -1,192 +0,0 @@
-/*********************************************************************
- *
- * Example: Using MoveBaseActionServer
- * 
- * This example demonstrates how to use MoveBaseActionServer
- * which provides a ROS Action Server-like interface
- *********************************************************************/
-
-#include <move_base/move_base_action_server.h>
-#include <move_base/move_base.h>
-#include <robot/node_handle.h>
-#include <robot/console.h>
-#include <robot/time.h>
-#include <tf3/buffer_core.h>
-#include <robot/plugin_loader_helper.h>
-#include <boost/dll/import.hpp>
-#include <iostream>
-#include <thread>
-#include <chrono>
-
-using namespace move_base;
-
-void goalCallback(const MoveBaseActionGoal& goal)
-{
-  robot::log_info("[Example] New goal received:");
-  robot::log_info("  Position: (%.2f, %.2f)", 
-                  goal.target_pose.pose.position.x,
-                  goal.target_pose.pose.position.y);
-  robot::log_info("  Frame: %s", goal.target_pose.header.frame_id.c_str());
-  robot::log_info("  XY Tolerance: %.2f", goal.xy_goal_tolerance);
-  robot::log_info("  Yaw Tolerance: %.2f", goal.yaw_goal_tolerance);
-}
-
-void feedbackCallback(const MoveBaseActionFeedback& feedback)
-{
-  const auto& nav_fb = feedback.nav_feedback;
-  robot::log_info("[Example] Feedback:");
-  robot::log_info("  State: %s", 
-                  robot::move_base_core::toString(nav_fb.navigation_state).c_str());
-  robot::log_info("  Current Pose: (%.2f, %.2f, %.2f)",
-                  nav_fb.current_pose.x,
-                  nav_fb.current_pose.y,
-                  nav_fb.current_pose.theta);
-  robot::log_info("  Distance to Goal: %.2f m", feedback.distance_to_goal);
-  if (!nav_fb.feed_back_str.empty())
-  {
-    robot::log_info("  Message: %s", nav_fb.feed_back_str.c_str());
-  }
-}
-
-void resultCallback(const MoveBaseActionResult& result)
-{
-  robot::log_info("[Example] Result received:");
-  robot::log_info("  Success: %s", result.success ? "Yes" : "No");
-  robot::log_info("  Final State: %s",
-                  robot::move_base_core::toString(result.final_state).c_str());
-  robot::log_info("  Message: %s", result.message.c_str());
-}
-
-void preemptCallback()
-{
-  robot::log_warning("[Example] Goal was preempted!");
-}
-
-int main(int argc, char** argv)
-{
-  try
-  {
-    robot::log_info("[Example] Starting MoveBaseActionServer example...");
-
-    // 1. Create TF buffer
-    std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
-    
-    // Set up static transforms (example)
-    // In real usage, these would come from your localization system
-    // tf->setTransform(...);
-
-    // 2. Load and initialize MoveBase
-    robot::log_info("[Example] Loading MoveBase plugin...");
-    robot::PluginLoaderHelper loader;
-    std::string path_file_so = loader.findLibraryPath("MoveBase");
-    auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
-        path_file_so,
-        "MoveBase", 
-        boost::dll::load_mode::append_decorations);
-    
-    robot::move_base_core::BaseNavigation::Ptr move_base_ptr = create_plugin();
-    if (!move_base_ptr)
-    {
-      robot::log_error("[Example] Failed to create MoveBase instance");
-      return 1;
-    }
-
-    robot::log_info("[Example] Initializing MoveBase...");
-    move_base_ptr->initialize(tf);
-    
-    // Wait for initialization
-    robot::Rate init_rate(10);
-    while (!move_base_ptr->getFeedback()->is_ready)
-    {
-      init_rate.sleep();
-    }
-    robot::log_info("[Example] MoveBase is ready!");
-
-    // 3. Create Action Server
-    robot::log_info("[Example] Creating MoveBaseActionServer...");
-    MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz control loop
-
-    // 4. Set up callbacks
-    action_server.setGoalCallback(goalCallback);
-    action_server.setFeedbackCallback(feedbackCallback);
-    action_server.setResultCallback(resultCallback);
-    action_server.setPreemptCallback(preemptCallback);
-
-    // 5. Start the action server
-    action_server.start();
-    robot::log_info("[Example] Action server started");
-
-    // 6. Example: Send a goal
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-    
-    MoveBaseActionGoal goal;
-    goal.target_pose.header.frame_id = "map";
-    goal.target_pose.header.stamp = robot::Time::now();
-    goal.target_pose.pose.position.x = 5.0;
-    goal.target_pose.pose.position.y = 3.0;
-    goal.target_pose.pose.position.z = 0.0;
-    
-    // Set orientation (yaw = 0.5 radians)
-    tf3::Quaternion q;
-    q.setRPY(0, 0, 0.5);
-    goal.target_pose.pose.orientation = tf3::toMsg(q);
-    
-    goal.xy_goal_tolerance = 0.2;
-    goal.yaw_goal_tolerance = 0.1;
-    goal.goal_id = "example_goal_1";
-
-    robot::log_info("[Example] Sending goal...");
-    if (action_server.sendGoal(goal))
-    {
-      robot::log_info("[Example] Goal accepted!");
-    }
-    else
-    {
-      robot::log_error("[Example] Goal rejected!");
-      return 1;
-    }
-
-    // 7. Monitor progress (in real application, this would be in your main loop)
-    robot::log_info("[Example] Monitoring navigation progress...");
-    robot::Rate monitor_rate(1.0); // Check every second
-    
-    int timeout_seconds = 60;
-    int elapsed_seconds = 0;
-    
-    while (action_server.isRunning() && elapsed_seconds < timeout_seconds)
-    {
-      // Check if goal is still active
-      if (!action_server.isNewGoalAvailable() && !action_server.isPreemptRequested())
-      {
-        // Goal is being processed
-        robot::log_info("[Example] Navigation in progress... (elapsed: %d s)", elapsed_seconds);
-      }
-      
-      monitor_rate.sleep();
-      elapsed_seconds++;
-    }
-
-    // 8. Example: Cancel goal (optional)
-    // Uncomment to test cancellation:
-    // robot::log_info("[Example] Cancelling goal...");
-    // action_server.cancelGoal();
-
-    // 9. Cleanup
-    robot::log_info("[Example] Stopping action server...");
-    action_server.stop();
-    
-    robot::log_info("[Example] Example completed successfully");
-    return 0;
-  }
-  catch (const std::exception& e)
-  {
-    robot::log_error("[Example] Exception: %s", e.what());
-    return 1;
-  }
-  catch (...)
-  {
-    robot::log_error("[Example] Unknown exception occurred");
-    return 1;
-  }
-}
-

src/Navigations/Packages/move_base/examples/simple_action_server_example.cpp

@@ -0,0 +1,343 @@
+/**
+ * @file simple_action_server_example.cpp
+ * @brief Example implementation of SimpleActionServer for MoveBaseAction
+ * 
+ * This example demonstrates how to use SimpleActionServer to implement
+ * an action server without ROS dependency. It shows:
+ * - How to initialize the action server
+ * - How to set up callbacks for publishing messages
+ * - How to handle goal execution
+ * - How to publish feedback and results
+ */
+
+#include <move_base/simple_action_server.h>
+#include <move_base/move_base.h>
+
+// Message types
+#include <robot_move_base_msgs/MoveBaseActionGoal.h>
+#include <robot_move_base_msgs/MoveBaseActionResult.h>
+#include <robot_move_base_msgs/MoveBaseActionFeedback.h>
+#include <robot_actionlib_msgs/GoalID.h>
+#include <robot_actionlib_msgs/GoalStatusArray.h>
+
+#include <robot/robot.h>
+#include <memory>
+#include <thread>
+#include <chrono>
+
+namespace move_base
+{
+
+/**
+ * @class MoveBaseActionServerExample
+ * @brief Example class demonstrating SimpleActionServer usage
+ */
+class MoveBaseActionServerExample
+{
+public:
+  MoveBaseActionServerExample()
+    : move_base_ptr_(nullptr)
+  {
+    // Initialize move_base (your existing navigation system)
+    // move_base_ptr_ = std::make_shared<MoveBase>(tf_);
+    
+    // Create SimpleActionServer with execute callback
+    action_server_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
+      "move_base",
+      std::bind(&MoveBaseActionServerExample::executeCb, this, std::placeholders::_1),
+      true  // auto_start
+    );
+    
+    // Set up callbacks for publishing messages
+    setupCallbacks();
+    
+    robot::log_info("[MoveBaseActionServerExample] Initialized");
+  }
+  
+  ~MoveBaseActionServerExample()
+  {
+    if (action_server_)
+    {
+      action_server_->stop();
+    }
+  }
+  
+  /**
+   * @brief Setup callbacks for SimpleActionServer
+   * You can customize these callbacks to publish messages to your communication system
+   */
+  void setupCallbacks()
+  {
+    if (!action_server_)
+      return;
+    
+    // Set callback for result publishing
+    // Customize this to send result to your communication system
+    action_server_->setResultCallback(
+      [this](const robot_move_base_msgs::MoveBaseActionResultConstPtr& result)
+      {
+        // TODO: Publish result to your communication system (e.g., custom message bus, network, etc.)
+        robot::log_info("[MoveBaseActionServerExample] Result: status=%d, text=%s",
+                       result->status.status, result->status.text.c_str());
+      }
+    );
+    
+    // Set callback for feedback publishing
+    // Customize this to send feedback to your communication system
+    action_server_->setFeedbackCallback(
+      [this](const robot_move_base_msgs::MoveBaseActionFeedbackConstPtr& feedback)
+      {
+        // TODO: Publish feedback to your communication system
+        robot::log_debug("[MoveBaseActionServerExample] Published feedback");
+      }
+    );
+    
+    // Set callback for status publishing
+    // Customize this to send status to your communication system
+    action_server_->setStatusCallback(
+      [this](const robot_actionlib_msgs::GoalStatusArrayConstPtr& status)
+      {
+        // TODO: Publish status to your communication system
+      }
+    );
+    
+    // Set callback for goal messages (optional - for logging)
+    action_server_->setGoalCallback(
+      [this](const robot_move_base_msgs::MoveBaseActionGoalConstPtr& goal)
+      {
+        robot::log_info("[MoveBaseActionServerExample] Goal callback: x=%.2f, y=%.2f",
+                       goal->goal.target_pose.pose.position.x,
+                       goal->goal.target_pose.pose.position.y);
+      }
+    );
+    
+    // Set callback for cancel messages (optional - for logging)
+    action_server_->setCancelCallback(
+      [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel)
+      {
+        robot::log_info("[MoveBaseActionServerExample] Cancel callback: id=%s", cancel->id.c_str());
+      }
+    );
+    
+    // Set status publish rate (default is 10 Hz)
+    action_server_->setStatusPublishRate(10.0);
+    
+    robot::log_info("[MoveBaseActionServerExample] Callbacks setup complete");
+  }
+  
+  /**
+   * @brief Process goal message from external source
+   * Call this when you receive a goal message from your communication system
+   */
+  void processGoalMessage(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
+  {
+    if (action_server_)
+    {
+      action_server_->processGoal(msg);
+    }
+  }
+  
+  /**
+   * @brief Process cancel message from external source
+   * Call this when you receive a cancel message from your communication system
+   */
+  void processCancelMessage(const robot_actionlib_msgs::GoalIDConstPtr& msg)
+  {
+    if (action_server_)
+    {
+      action_server_->processCancel(msg);
+    }
+  }
+  
+  /**
+   * @brief Execute callback - called when a new goal is received
+   * This is the main function that processes the goal
+   */
+  void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
+  {
+    robot::log_info("[MoveBaseActionServerExample] Execute callback started");
+    robot::log_info("  Goal: x=%.2f, y=%.2f, frame_id=%s",
+                   goal->target_pose.pose.position.x,
+                   goal->target_pose.pose.position.y,
+                   goal->target_pose.header.frame_id.c_str());
+    
+    // Check if we have a valid move_base instance
+    if (!move_base_ptr_)
+    {
+      robot::log_error("[MoveBaseActionServerExample] move_base_ptr_ is null, aborting goal");
+      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), 
+                                 "MoveBase not initialized");
+      return;
+    }
+    
+    // Convert goal to PoseStamped
+    robot_geometry_msgs::PoseStamped target_pose = goal->target_pose;
+    
+    // Send goal to move_base
+    bool success = move_base_ptr_->moveTo(target_pose, 0.2, 0.2);
+    
+    if (!success)
+    {
+      robot::log_error("[MoveBaseActionServerExample] Failed to send goal to move_base");
+      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                                 "Failed to send goal to move_base");
+      return;
+    }
+    
+    // Monitor execution and publish feedback
+    robot::Rate rate(10.0); // 10 Hz
+    robot::Time start_time = robot::Time::now();
+    robot::Time last_feedback_time = start_time;
+    
+    while (action_server_->isActive() && !action_server_->isPreemptRequested())
+    {
+      // Check if goal is reached
+      if (move_base_ptr_->getFeedback())
+      {
+        auto* feedback_ptr = move_base_ptr_->getFeedback();
+        
+        if (feedback_ptr->goal_checked && 
+            feedback_ptr->navigation_state == robot::move_base_core::State::SUCCEEDED)
+        {
+          robot::log_info("[MoveBaseActionServerExample] Goal reached!");
+          action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
+                                      "Goal reached successfully");
+          return;
+        }
+        
+        // Check for errors
+        if (feedback_ptr->navigation_state == robot::move_base_core::State::ABORTED)
+        {
+          robot::log_error("[MoveBaseActionServerExample] Navigation aborted: %s",
+                           feedback_ptr->feed_back_str.c_str());
+          action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                                    feedback_ptr->feed_back_str);
+          return;
+        }
+        
+        // Publish feedback periodically (every 0.5 seconds)
+        robot::Time now = robot::Time::now();
+        if ((now - last_feedback_time).toSec() >= 0.5)
+        {
+          robot_move_base_msgs::MoveBaseFeedback feedback;
+          feedback.base_position.x = feedback_ptr->current_pose.x;
+          feedback.base_position.y = feedback_ptr->current_pose.y;
+          feedback.base_position.theta = feedback_ptr->current_pose.theta;
+          feedback.base_position.header.stamp = robot::Time::now();
+          feedback.base_position.header.frame_id = "map";
+          
+          action_server_->publishFeedback(feedback);
+          last_feedback_time = now;
+        }
+      }
+      
+      // Check for preemption
+      if (action_server_->isPreemptRequested())
+      {
+        robot::log_info("[MoveBaseActionServerExample] Goal preempted");
+        move_base_ptr_->cancel();
+        action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
+                                    "Goal preempted by user");
+        return;
+      }
+      
+      // Sleep
+      rate.sleep();
+    }
+    
+    // If we exit the loop without success, check why
+    if (action_server_->isPreemptRequested())
+    {
+      robot::log_info("[MoveBaseActionServerExample] Goal was preempted");
+      move_base_ptr_->cancel();
+      action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
+                                  "Goal preempted");
+    }
+    else
+    {
+      robot::log_warning("[MoveBaseActionServerExample] Goal execution ended unexpectedly");
+      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
+                                "Goal execution ended unexpectedly");
+    }
+  }
+  
+  /**
+   * @brief Get the action server instance
+   */
+  std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> getActionServer()
+  {
+    return action_server_;
+  }
+  
+  /**
+   * @brief Set move_base instance
+   */
+  void setMoveBase(robot::move_base_core::BaseNavigation::Ptr move_base)
+  {
+    move_base_ptr_ = move_base;
+  }
+
+private:
+  // Action server
+  std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> action_server_;
+  
+  // Move base instance
+  robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
+};
+
+} // namespace move_base
+
+/**
+ * @brief Example main function showing how to use MoveBaseActionServerExample
+ */
+int main()
+{
+  try
+  {
+    // Create example instance
+    move_base::MoveBaseActionServerExample example;
+    
+    // If you have a move_base instance, set it
+    // example.setMoveBase(move_base_ptr);
+    
+    robot::log_info("[Example] Action server is running. Waiting for goals...");
+    
+    // Example: Create and send a goal message
+    // In real usage, you would receive this from your communication system
+    robot_move_base_msgs::MoveBaseActionGoalPtr goal_msg = 
+        boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+    goal_msg->header.stamp = robot::Time::now();
+    goal_msg->header.frame_id = "map";
+    goal_msg->goal_id.stamp = robot::Time::now();
+    goal_msg->goal_id.id = "example_goal_1";
+    goal_msg->goal.target_pose.header.stamp = robot::Time::now();
+    goal_msg->goal.target_pose.header.frame_id = "map";
+    goal_msg->goal.target_pose.pose.position.x = 5.0;
+    goal_msg->goal.target_pose.pose.position.y = 3.0;
+    goal_msg->goal.target_pose.pose.position.z = 0.0;
+    goal_msg->goal.target_pose.pose.orientation.w = 1.0;
+    
+    // Process the goal
+    example.processGoalMessage(goal_msg);
+    
+    // Your event loop here
+    // while (your_system_running())
+    // {
+    //   // Receive messages from your communication system
+    //   // Process goals and cancels
+    //   // example.processGoalMessage(goal_msg);
+    //   // example.processCancelMessage(cancel_msg);
+    // }
+    
+    // Keep running for a while
+    robot::Time::sleep(robot::Duration(10.0));
+  }
+  catch (const std::exception& e)
+  {
+    robot::log_error("[Example] Exception: %s", e.what());
+    return 1;
+  }
+  
+  return 0;
+}
+

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

@@ -7,11 +7,11 @@
 #ifndef NAV_MOVE_BASE_ACTION_H_
 #define NAV_MOVE_BASE_ACTION_H_
 
+#include <robot/robot.h>
 #include <vector>
 #include <string>
 #include <tf3/buffer_core.h>
-#include <yaml-cpp/yaml.h>
+#include <move_base/simple_action_server.h>
-
 
 // interfaces headers
 #include <move_base_core/navigation.h>
@@ -28,13 +28,15 @@
 #include <robot_costmap_2d/costmap_2d.h>
 #include <robot_nav_msgs/GetPlan.h>
 #include <robot_protocol_msgs/Order.h>
+#include <robot_move_base_msgs/MoveBaseAction.h>
+#include <memory>
+#include <functional>
 
-#include <robot/node_handle.h>
-#include <robot/console.h>
-#include <robot/timer.h>
 
 namespace move_base
 {
+  // typedefs to help us out with the action server so that we don't hace to type so much
+  typedef move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction> MoveBaseActionServer;
 
   enum MoveBaseState
   {
@@ -472,7 +474,12 @@ namespace move_base
 
     void planThread();
 
-    // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
+    void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
+
+    /**
+     * @brief  Setup callbacks for the action server
+     */
+    void setupActionServerCallbacks();
 
     bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
 
@@ -492,9 +499,11 @@ namespace move_base
     robot::TFListenerPtr tf_;
     robot::NodeHandle private_nh_;
 
+    MoveBaseActionServer *as_;
+
     boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
     boost::function<robot_nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
-    boost::function<robot_nav_core::RecoveryBehavior::Ptr()> recovery_loader_;
+    std::map<std::string, boost::function<robot_nav_core::RecoveryBehavior::Ptr()>> recovery_loaders_;
 
     robot_nav_core::BaseLocalPlanner::Ptr tc_;
     robot_nav_core::BaseGlobalPlanner::Ptr planner_;

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

@@ -0,0 +1,259 @@
+#ifndef MOVE_BASE_SIMPLE_ACTION_SERVER_H_
+#define MOVE_BASE_SIMPLE_ACTION_SERVER_H_
+
+#include <string>
+#include <memory>
+#include <functional>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition_variable.hpp>
+
+// Action message types
+#include <robot_move_base_msgs/MoveBaseAction.h>
+#include <robot_move_base_msgs/MoveBaseActionGoal.h>
+#include <robot_move_base_msgs/MoveBaseActionResult.h>
+#include <robot_move_base_msgs/MoveBaseActionFeedback.h>
+#include <robot_move_base_msgs/MoveBaseGoal.h>
+#include <robot_move_base_msgs/MoveBaseResult.h>
+#include <robot_move_base_msgs/MoveBaseFeedback.h>
+
+// Actionlib message types
+#include <robot_actionlib_msgs/GoalID.h>
+#include <robot_actionlib_msgs/GoalStatus.h>
+#include <robot_actionlib_msgs/GoalStatusArray.h>
+
+// Robot framework
+#include <robot/robot.h>  
+
+namespace move_base
+{
+
+/**
+ * @class SimpleActionServer
+ * @brief A simplified action server implementation similar to ROS actionlib::SimpleActionServer
+ * 
+ * This class provides a simple interface for implementing action servers without ROS dependency.
+ * It handles goal requests, preemption, feedback publishing, and result publishing using callback functions.
+ * 
+ * @tparam ActionSpec The action message type (e.g., robot_move_base_msgs::MoveBaseAction)
+ */
+template<class ActionSpec>
+class SimpleActionServer
+{
+public:
+  // Typedefs for action message types
+  typedef typename ActionSpec::_action_goal_type ActionGoal;
+  typedef typename ActionSpec::_action_result_type ActionResult;
+  typedef typename ActionSpec::_action_feedback_type ActionFeedback;
+  
+  typedef typename ActionGoal::_goal_type Goal;
+  typedef typename ActionResult::_result_type Result;
+  typedef typename ActionFeedback::_feedback_type Feedback;
+  
+  typedef boost::shared_ptr<const Goal> GoalConstPtr;
+  typedef boost::shared_ptr<const Result> ResultConstPtr;
+  typedef boost::shared_ptr<const Feedback> FeedbackConstPtr;
+  typedef typename ActionGoal::ConstPtr ActionGoalConstPtr;
+  typedef typename ActionResult::ConstPtr ActionResultConstPtr;
+  typedef typename ActionFeedback::ConstPtr ActionFeedbackConstPtr;
+  typedef robot_actionlib_msgs::GoalStatusArrayConstPtr GoalStatusArrayConstPtr;
+
+  // Callback function types
+  typedef std::function<void(const ActionGoalConstPtr&)> GoalCallback;
+  typedef std::function<void(const robot_actionlib_msgs::GoalIDConstPtr&)> CancelCallback;
+  typedef std::function<void(const ActionFeedbackConstPtr&)> FeedbackCallback;
+  typedef std::function<void(const ActionResultConstPtr&)> ResultCallback;
+  typedef std::function<void(const GoalStatusArrayConstPtr&)> StatusCallback;
+  typedef std::function<void(const GoalConstPtr&)> ExecuteCallback;
+  
+  // Typedefs for non-const pointers
+  typedef typename ActionResult::Ptr ActionResultPtr;
+  typedef typename ActionFeedback::Ptr ActionFeedbackPtr;
+
+  /**
+   * @brief Constructor
+   * @param name Action name (e.g., "move_base")
+   * @param execute_callback Callback function to execute when a goal is received
+   * @param auto_start If true, automatically start the server
+   */
+  SimpleActionServer(const std::string& name,
+                     ExecuteCallback execute_callback,
+                     bool auto_start = true);
+
+  /**
+   * @brief Destructor
+   */
+  ~SimpleActionServer();
+
+  /**
+   * @brief Start the action server
+   */
+  void start();
+
+  /**
+   * @brief Stop the action server
+   */
+  void stop();
+
+  /**
+   * @brief Set callback for goal messages
+   * @param callback Function to call when a goal is received
+   */
+  void setGoalCallback(GoalCallback callback);
+
+  /**
+   * @brief Set callback for cancel messages
+   * @param callback Function to call when a cancel is received
+   */
+  void setCancelCallback(CancelCallback callback);
+
+  /**
+   * @brief Set callback for feedback publishing
+   * @param callback Function to call when feedback needs to be published
+   */
+  void setFeedbackCallback(FeedbackCallback callback);
+
+  /**
+   * @brief Set callback for result publishing
+   * @param callback Function to call when result needs to be published
+   */
+  void setResultCallback(ResultCallback callback);
+
+  /**
+   * @brief Set callback for status publishing
+   * @param callback Function to call when status needs to be published
+   */
+  void setStatusCallback(StatusCallback callback);
+
+  /**
+   * @brief Set status publish rate
+   * @param rate Publishing rate in Hz
+   */
+  void setStatusPublishRate(double rate);
+
+  /**
+   * @brief Process an incoming goal message
+   * @param goal_msg The goal message to process
+   */
+  void processGoal(const ActionGoalConstPtr& goal_msg);
+
+  /**
+   * @brief Process an incoming cancel message
+   * @param cancel_msg The cancel message to process
+   */
+  void processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg);
+
+  /**
+   * @brief Check if a new goal is available
+   * @return True if a new goal is available
+   */
+  bool isNewGoalAvailable();
+
+  /**
+   * @brief Check if preemption has been requested
+   * @return True if preemption has been requested
+   */
+  bool isPreemptRequested();
+
+  /**
+   * @brief Accept a new goal
+   * @return The new goal, or nullptr if no new goal is available
+   */
+  GoalConstPtr acceptNewGoal();
+
+  /**
+   * @brief Set the action as succeeded
+   * @param result The result to send
+   * @param text Success message
+   */
+  void setSucceeded(const Result& result = Result(), const std::string& text = "");
+
+  /**
+   * @brief Set the action as aborted
+   * @param result The result to send
+   * @param text Error message
+   */
+  void setAborted(const Result& result = Result(), const std::string& text = "");
+
+  /**
+   * @brief Set the action as preempted
+   * @param result The result to send
+   * @param text Message
+   */
+  void setPreempted(const Result& result = Result(), const std::string& text = "");
+
+  /**
+   * @brief Publish feedback
+   * @param feedback The feedback to publish
+   */
+  void publishFeedback(const Feedback& feedback);
+
+  /**
+   * @brief Get the current goal
+   * @return The current goal, or nullptr if no goal is active
+   */
+  GoalConstPtr getCurrentGoal();
+
+  /**
+   * @brief Get the current status
+   * @return The current goal status
+   */
+  robot_actionlib_msgs::GoalStatus getCurrentStatus();
+
+  /**
+   * @brief Check if the server is active
+   * @return True if the server is active
+   */
+  bool isActive();
+
+  /**
+   * @brief Get the action name
+   * @return The action name
+   */
+  std::string getName() const;
+
+private:
+  /**
+   * @brief Execute the goal in a separate thread
+   */
+  void executeGoal();
+
+  /**
+   * @brief Thread function to publish status periodically
+   */
+  void statusPublishThread();
+
+  // Action name
+  std::string name_;
+  
+  // Execute callback
+  ExecuteCallback execute_callback_;
+  
+  // Callbacks for external communication
+  GoalCallback goal_callback_;
+  CancelCallback cancel_callback_;
+  FeedbackCallback feedback_callback_;
+  ResultCallback result_callback_;
+  StatusCallback status_callback_;
+  
+  // State
+  boost::mutex state_mutex_;
+  boost::condition_variable goal_condition_;
+  bool active_;
+  bool preempt_requested_;
+  bool new_goal_available_;
+  GoalConstPtr goal_;
+  robot_actionlib_msgs::GoalID current_goal_id_;
+  robot_actionlib_msgs::GoalStatus current_status_;
+  double status_publish_rate_;
+  
+  // Threads
+  boost::thread status_thread_;
+  boost::thread execute_thread_;
+};
+
+} // namespace move_base
+#endif // MOVE_BASE_SIMPLE_ACTION_SERVER_H_
+

src/Navigations/Packages/move_base/package.xml

@@ -49,4 +49,7 @@
   <build_depend>robot_cpp</build_depend>
   <run_depend>robot_cpp</run_depend>
 
+  <build_depend>robot_move_base_msgs</build_depend>
+  <run_depend>robot_move_base_msgs</run_depend>
+  
 </package>

src/Navigations/Packages/move_base/src/convert_data.cpp

@@ -1,6 +1,5 @@
 #include <move_base/convert_data.h>
-#include <robot/time.h>
+#include <robot/robot.h>
-#include <robot/console.h>
 #include <robot_costmap_2d/cost_values.h>
 
 char* move_base::ConvertData::cost_translation_table_ = NULL;

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

@@ -10,13 +10,15 @@
 #include <stdexcept>
 #include <cstdlib>
 #include <memory>
+#include <string>
+#include <sstream>
 #include <data_convert/data_convert.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <boost/algorithm/string.hpp>
 #include <boost/thread.hpp>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
-#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
 
 #include <robot_costmap_2d/static_layer.h>
 #include <robot_costmap_2d/voxel_layer.h>
@@ -49,6 +51,13 @@ move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
 
 move_base::MoveBase::~MoveBase()
 {
+  if (as_ != NULL)
+  { 
+    as_->stop();
+    delete as_;
+    as_ = NULL;
+  }
+
   recovery_behaviors_.clear();
 
   // Cleanup static maps (shared_ptr tự động cleanup)
@@ -113,6 +122,10 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
   if (!initialized_)
   {
     tf_ = tf;
+
+    as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true);
+    setupActionServerCallbacks();
+
     // NodeHandle("~") will automatically load YAML files from config directory
     private_nh_ = robot::NodeHandle("~");
     recovery_trigger_ = PLANNING_R;
@@ -309,6 +322,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
       robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
     }
     initialized_ = true;
+    setup_ = true;
     robot::log_info("========== End: initialize() - SUCCESS ==========");
   }
   else
@@ -319,6 +333,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
 
 void move_base::MoveBase::swapPlanner(std::string base_global_planner)
 {
+
 }
 
 void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
@@ -349,7 +364,6 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms
   {
     it->second = map;
   }
-  robot::log_error("[%s:%d] Add StaticMap: %s %x", __FILE__, __LINE__, map_name.c_str(), controller_costmap_robot_ == nullptr);
   updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
   updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
 }
@@ -614,6 +628,132 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
     return false;
   }
 
+  // Set XY goal tolerance
+  double final_xy_tolerance;
+  if (fabs(xy_goal_tolerance) > 0.001)
+  {
+    final_xy_tolerance = fabs(xy_goal_tolerance);
+    this->setXyGoalTolerance(final_xy_tolerance);
+  }
+  else
+  {
+    final_xy_tolerance = fabs(original_xy_goal_tolerance_);
+    this->setXyGoalTolerance(final_xy_tolerance);
+  }
+
+  // Set Yaw goal tolerance
+  double final_yaw_tolerance;
+  if (fabs(yaw_goal_tolerance) > 0.001)
+  {
+    final_yaw_tolerance = fabs(yaw_goal_tolerance);
+    this->setYawGoalTolerance(final_yaw_tolerance);
+  }
+  else
+  {
+    final_yaw_tolerance = fabs(original_yaw_goal_tolerance_);
+    this->setYawGoalTolerance(final_yaw_tolerance);
+  }
+
+  // Check pointers
+  if (!tc_)
+  {
+    throw std::runtime_error("Null 'tc_' pointer encountered");
+  }
+  
+  if (!controller_costmap_robot_)
+  {
+    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
+  }
+
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+
+  // Swap planner
+  try
+  {
+    tc_->swapPlanner(position_planner_name_);
+  }
+  catch (const std::exception &e)
+  {
+    std::cerr << e.what() << "\n";
+    throw std::exception(e);
+  }
+
+  // Update navigation feedback
+  if (nav_feedback_)
+  {
+    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
+    nav_feedback_->feed_back_str = std::string("Planning");
+    nav_feedback_->goal_checked = false;
+  }
+  else
+  {
+    robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!");
+    lock.unlock();
+    return false;
+  }
+
+  // Reset cancel flag
+  if (cancel_ctr_)
+  {
+    cancel_ctr_ = false;
+  }
+  // Check if action server exists
+  if (!as_)
+  {
+    robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
+    lock.unlock();
+    return false;
+  }
+
+  try
+  {
+    robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+    action_goal->header.stamp = robot::Time::now();
+    action_goal->goal.target_pose = goal;
+    
+    // Generate unique goal ID using timestamp
+    robot::Time now = robot::Time::now();
+    action_goal->goal_id.stamp = now;
+    std::ostringstream goal_id_stream;
+    goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
+    action_goal->goal_id.id = goal_id_stream.str();
+    
+    robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+    robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", 
+                    action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
+    robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
+    as_->processGoal(action_goal);
+    robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
+  }
+  catch (const std::exception &e)
+  {
+    lock.unlock();
+    robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
+    return false;
+  }
+  lock.unlock();
+  return true;
+}
+
+bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
+                                 const robot_geometry_msgs::PoseStamped &goal,
+                                 double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  if (setup_)
+  {
+    swapPlanner(default_config_.base_global_planner);
+  }
+  else
+  {
+    if (nav_feedback_)
+    {
+      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
+      nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
+      nav_feedback_->goal_checked = false;
+    }
+    return false;
+  }
+
   if (fabs(xy_goal_tolerance) > 0.001)
     this->setXyGoalTolerance(fabs(xy_goal_tolerance));
   else
@@ -623,7 +763,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
     this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
   else
     this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
+  robot::Duration(0.01).sleep();
 
+  robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
   if (!tc_)
   {
     throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -640,7 +782,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
   }
   catch (const std::exception &e)
   {
-    std::cerr << e.what() << "\n";
+    robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what());
     throw std::exception(e);
   }
 
@@ -653,17 +795,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
 
   if (cancel_ctr_)
     cancel_ctr_ = false;
+
+  robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+  action_goal->header.stamp = robot::Time::now();
+  action_goal->goal.target_pose = goal;
+  
+  // Generate unique goal ID using timestamp
+  robot::Time now = robot::Time::now();
+  action_goal->goal_id.stamp = now;
+  std::ostringstream goal_id_stream;
+  goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
+  action_goal->goal_id.id = goal_id_stream.str();
+  
+  robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+  as_->processGoal(action_goal);
+
   lock.unlock();
   return true;
 }
 
-bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
-                                 const robot_geometry_msgs::PoseStamped &goal,
-                                 double xy_goal_tolerance, double yaw_goal_tolerance)
-{
-  return false;
-}
-
 bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
@@ -915,12 +1065,43 @@ void move_base::MoveBase::resume()
 
 void move_base::MoveBase::cancel()
 {
+  robot::log_info("[MoveBase::cancel] ===== ENTRY =====");
+  
   if (!controller_costmap_robot_)
   {
+    robot::log_error("[MoveBase::cancel] controller_costmap_robot_ is null!");
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
+  
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   cancel_ctr_ = true;
+  robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
+  
+  if (as_)
+  {
+    // Get current goal ID from action server to cancel specific goal
+    // If we want to cancel all goals, use empty string ""
+    robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+    
+    // Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
+    msg->id = "";  // Empty string cancels current goal
+    msg->stamp = robot::Time::now();
+    
+    robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
+    robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", 
+                    msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
+    
+    // Convert to ConstPtr for processCancel
+    robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+    as_->processCancel(cancel_msg);
+    robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+  }
+  else
+  {
+    robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+  }
+  
+  robot::log_info("[MoveBase::cancel] ===== EXIT =====");
 }
 
 bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -1102,13 +1283,14 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         // behavior["type"] should be the factory function name (e.g., "create_plugin")
         std::string behavior_type = behavior["type"].as<std::string>();
         std::string behavior_name = behavior["name"].as<std::string>();
-        std::string path_file_so;
+        robot::PluginLoaderHelper loader;
+        std::string path_file_so = loader.findLibraryPath(behavior_type);
         // Load the factory function from the shared library
-        auto recovery_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
+        recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
 
         // Create instance using the factory function
-        std::shared_ptr<robot_nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
+        std::shared_ptr<robot_nav_core::RecoveryBehavior> behavior_ptr(recovery_loaders_[behavior_name]());
 
         // shouldn't be possible, but it won't hurt to check
         if (behavior_ptr == nullptr)
@@ -1420,7 +1602,234 @@ void move_base::MoveBase::planThread()
   }
 }
 
-// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
+
+
+void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal)
+{
+  if (!isQuaternionValid(move_base_goal->target_pose.pose.orientation))
+  {
+    if (nav_feedback_)
+    {
+      nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
+      nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
+    }
+    as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
+    return;
+  }
+  else
+  {
+    if (nav_feedback_)
+    {
+      nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE;
+      nav_feedback_->feed_back_str = std::string("The goal is received");
+      robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
+    }
+  }
+
+  robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
+  publishZeroVelocity();
+  // we have a goal so start the planner
+  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
+  planner_goal_ = goal;
+  runPlanner_ = true;
+  cancel_ctr_ = false;
+  planner_cond_.notify_one();
+  lock.unlock();
+  robot::Rate r(controller_frequency_);
+  if (shutdown_costmaps_)
+  {
+    robot::log_debug("[MoveBase] Starting up costmaps that were shut down previously");
+    planner_costmap_robot_->start();
+    controller_costmap_robot_->start();
+  }
+  // we want to make sure that we reset the last time we had a valid plan and control
+  last_valid_control_ = robot::Time::now();
+  last_valid_plan_ = robot::Time::now();
+  last_oscillation_reset_ = robot::Time::now();
+  planning_retries_ = 0;
+
+  while(robot::ok())
+  {
+    if (c_freq_change_)
+    {
+      robot::log_info("[MoveBase] Setting controller frequency to %.2f", controller_frequency_);
+      r = robot::Rate(controller_frequency_);
+      c_freq_change_ = false;
+    }
+    if (as_->isPreemptRequested())
+    {
+      if (as_->isNewGoalAvailable())
+      {
+        // if we're active and a new goal is available, we'll accept it, but we won't shut anything down
+        robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
+        robot::log_info("[MoveBase] new goal %f %f", new_goal.target_pose.pose.position.x, new_goal.target_pose.pose.position.y);
+        if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
+        {
+          if (nav_feedback_)
+          {
+            nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
+            nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
+          }
+          as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
+          return;
+        }
+        else
+        {
+          if (nav_feedback_)
+          {
+            nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE;
+            nav_feedback_->feed_back_str = std::string("The new goal is received");
+          }
+        }
+
+        goal = goalToGlobalFrame(new_goal.target_pose);
+        // we'll make sure that we reset our state for the next execution cycle
+        recovery_index_ = 0;
+        state_ = move_base::PLANNING;
+        if (nav_feedback_)
+          nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
+        // we have a new goal so make sure the planner is awake
+        lock.lock();
+        planner_goal_ = goal;
+        runPlanner_ = true;
+        cancel_ctr_ = false;
+        planner_cond_.notify_one();
+        lock.unlock();
+
+        // publish the goal point to the visualizer
+        robot::log_debug("[MoveBase] move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
+        // make sure to reset our timeouts and counters
+        last_valid_control_ = robot::Time::now();
+        last_valid_plan_ = robot::Time::now();
+        last_oscillation_reset_ = robot::Time::now();
+        planning_retries_ = 0;
+      }
+      else
+      {
+        // if we've been preempted explicitly we need to shut things down
+        resetState();
+
+        // notify the ActionServer that we've successfully preempted
+        robot::log_debug("[MoveBase] Move base preempting the current goal");
+        as_->setPreempted();
+        if (nav_feedback_)
+        {
+          nav_feedback_->feed_back_str = std::string("Move base preempting the current goal.");
+          nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTING;
+        }
+        // we'll actually return from execute after preempting
+        return;
+      }
+    }
+
+    // we also want to check if we've changed global frames because we need to transform our goal pose
+    if (goal.header.frame_id != planner_costmap_robot_->getGlobalFrameID())
+    {
+      goal = goalToGlobalFrame(goal);
+
+      // we want to go back to the planning state for the next execution cycle
+      recovery_index_ = 0;
+      state_ = move_base::PLANNING;
+      if (nav_feedback_)
+        nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
+
+      // we have a new goal so make sure the planner is awake
+      lock.lock();
+      planner_goal_ = goal;
+      runPlanner_ = true;
+      cancel_ctr_ = false;
+      planner_cond_.notify_one();
+      lock.unlock();
+
+      // publish the goal point to the visualizer
+      robot::log_debug("[MoveBase] The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
+
+      // make sure to reset our timeouts and counters
+      last_valid_control_ = robot::Time::now();
+      last_valid_plan_ = robot::Time::now();
+      last_oscillation_reset_ = robot::Time::now();
+      planning_retries_ = 0;
+    }
+
+    // // for timing that gives real time even in simulation
+    // ros::WallTime start = ros::WallTime::now();
+
+    // the real work on pursuing a goal is done here
+    bool done = executeCycle(goal);
+
+    // if we're done, then we'll return from execute
+    if (done)
+      return;
+
+    // check if execution of the goal has completed in some way
+    // ros::WallDuration t_diff = ros::WallTime::now() - start;
+    // ROS_DEBUG_NAMED("move_base", "Full control cycle time: %.9f\n", t_diff.toSec());
+
+    r.sleep();
+  }
+}
+
+void move_base::MoveBase::setupActionServerCallbacks()
+{
+  if (!as_)
+  {
+    robot::log_error("[MoveBase] Action server is not initialized");
+    return;
+  }
+
+  // Set callback for result publishing
+  // Customize this to send result to your communication system
+  as_->setResultCallback(
+    [this](const robot_move_base_msgs::MoveBaseActionResultConstPtr& result)
+    {
+      // TODO: Publish result to your communication system (e.g., custom message bus, network, etc.)
+      robot::log_info("[MoveBase] Result: status=%d, text=%s",
+                    result->status.status, result->status.text.c_str());
+    }
+  );
+
+  // Set callback for feedback publishing
+  // Customize this to send feedback to your communication system
+  as_->setFeedbackCallback(
+    [this](const robot_move_base_msgs::MoveBaseActionFeedbackConstPtr& feedback)
+    {
+      // TODO: Publish feedback to your communication system
+      robot::log_debug("[MoveBase] Published feedback");
+    }
+  );
+
+  // Set callback for status publishing
+  // Customize this to send status to your communication system
+  as_->setStatusCallback(
+    [this](const robot_actionlib_msgs::GoalStatusArrayConstPtr& status)
+    {
+      // TODO: Publish status to your communication system (e.g., custom message bus, network, etc.)
+    }
+  );
+
+  // Set callback for goal messages (optional - for logging)
+  as_->setGoalCallback(
+    [this](const robot_move_base_msgs::MoveBaseActionGoalConstPtr& goal)
+    {
+      robot::log_info("[MoveBase] Goal callback: x=%.2f, y=%.2f",
+                    goal->goal.target_pose.pose.position.x,
+                    goal->goal.target_pose.pose.position.y);
+    }
+  );
+
+  // Set callback for cancel messages (optional - for logging)
+  as_->setCancelCallback(
+    [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel)
+    {
+      robot::log_info("[MoveBase] Cancel callback: id=%s", cancel->id.c_str());
+    }
+  );
+
+  // Set status publish rate (default is 10 Hz)
+  as_->setStatusPublishRate(10.0);
+
+  robot::log_info("[MoveBase] Callbacks setup complete");
+}
 
 bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternion &q)
 {
@@ -1837,11 +2246,11 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
     // we'll try to clear out space with any user-provided recovery behaviors
   case move_base::CLEARING:
-    robot::log_debug("In clearing/recovery state");
+    robot::log_debug("[%s:%d] In clearing/recovery state", __FILE__, __LINE__);
     // we'll invoke whatever recovery behavior we're currently on if they're enabled
     if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
     {
-      robot::log_debug("Executing behavior %u of %zu", recovery_index_ + 1, recovery_behaviors_.size());
+      robot::log_debug("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size());
 
       // move_base_msgs::RecoveryStatus msg;
       // msg.pose_stamped = current_position;

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

@@ -29,10 +29,9 @@
 
 #include <move_base/move_base.h>
 #include <iostream>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/import.hpp>
-#include <robot/console.h>
 #include <tf3/buffer_core.h>
 #include <tf3/transform_datatypes.h>
 

src/Navigations/Packages/move_base/src/simple_action_server.cpp

@@ -0,0 +1,632 @@
+#include <move_base/simple_action_server.h>
+
+namespace move_base
+{
+
+// Constructor
+template<class ActionSpec>
+SimpleActionServer<ActionSpec>::SimpleActionServer(const std::string& name,
+                                                   ExecuteCallback execute_callback,
+                                                   bool auto_start)
+  : name_(name),
+    execute_callback_(execute_callback),
+    active_(false),
+    preempt_requested_(false),
+    new_goal_available_(false),
+    goal_(nullptr),
+    status_publish_rate_(10.0)
+{
+  // Initialize status
+  current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING;
+  current_status_.goal_id.stamp = robot::Time::now();
+  current_status_.goal_id.id = "";
+  current_status_.text = "";
+  
+  if (auto_start)
+  {
+    start();
+  }
+  
+  robot::log_info("[SimpleActionServer] Initialized action server: %s", name_.c_str());
+}
+
+// Destructor
+template<class ActionSpec>
+SimpleActionServer<ActionSpec>::~SimpleActionServer()
+{
+  stop();
+}
+
+// Start the action server
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::start()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  if (!active_)
+  {
+    active_ = true;
+    // Start status publishing thread
+    status_thread_ = boost::thread(boost::bind(&SimpleActionServer::statusPublishThread, this));
+    robot::log_info("[SimpleActionServer] Started action server: %s", name_.c_str());
+  }
+}
+
+// Stop the action server
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::stop()
+{
+  {
+    boost::lock_guard<boost::mutex> lock(state_mutex_);
+    if (active_)
+    {
+      active_ = false;
+    }
+  }
+  
+  // Notify all waiting threads
+  goal_condition_.notify_all();
+  
+  if (status_thread_.joinable())
+  {
+    status_thread_.join();
+  }
+  
+  if (execute_thread_.joinable())
+  {
+    execute_thread_.join();
+  }
+  
+  robot::log_info("[SimpleActionServer] Stopped action server: %s", name_.c_str());
+}
+
+// Set callback for goal messages
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setGoalCallback(GoalCallback callback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  goal_callback_ = callback;
+}
+
+// Set callback for cancel messages
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setCancelCallback(CancelCallback callback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  cancel_callback_ = callback;
+}
+
+// Set callback for feedback publishing
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setFeedbackCallback(FeedbackCallback callback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  feedback_callback_ = callback;
+}
+
+// Set callback for result publishing
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setResultCallback(ResultCallback callback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  result_callback_ = callback;
+}
+
+// Set callback for status publishing
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setStatusCallback(StatusCallback callback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  status_callback_ = callback;
+}
+
+// Set status publish rate
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setStatusPublishRate(double rate)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  status_publish_rate_ = rate;
+}
+
+// Process an incoming goal message
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::processGoal(const ActionGoalConstPtr& goal_msg)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  if (!active_)
+  {
+    robot::log_warning("[SimpleActionServer] Received goal but server is not active");
+    return;
+  }
+  
+  // Check if we should accept this goal
+  bool accept_goal = false;
+  
+  if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING ||
+      current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
+      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
+  {
+    // Check if this is a new goal (different ID)
+    if (goal_msg->goal_id.id != current_goal_id_.id || 
+        goal_msg->goal_id.stamp != current_goal_id_.stamp)
+    {
+      // If we have an active goal, preempt it
+      if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
+      {
+        preempt_requested_ = true;
+        current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING;
+        robot::log_info("[SimpleActionServer] Preempting current goal for new goal");
+      }
+      
+      accept_goal = true;
+    }
+  }
+  else
+  {
+    // No active goal, accept this one
+    accept_goal = true;
+  }
+  
+  if (accept_goal)
+  {
+    // Store the new goal
+    goal_ = boost::make_shared<Goal>(goal_msg->goal);
+    current_goal_id_ = goal_msg->goal_id;
+    new_goal_available_ = true;
+    current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING;
+    current_status_.goal_id = current_goal_id_;
+    current_status_.text = "";
+    
+    robot::log_info("[SimpleActionServer] Received new goal, ID: %s", current_goal_id_.id.c_str());
+    
+    // Notify waiting threads
+    goal_condition_.notify_one();
+    
+    // Start execute thread if not already running
+    if (execute_callback_)
+    {
+      if (!execute_thread_.joinable())
+      {
+        execute_thread_ = boost::thread(boost::bind(&SimpleActionServer::executeGoal, this));
+      }
+    }
+    
+    // Call goal callback if set
+    if (goal_callback_)
+    {
+      goal_callback_(goal_msg);
+    }
+  }
+}
+
+// Process an incoming cancel message
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg)
+{
+  robot::log_info("[SimpleActionServer::processCancel] ===== ENTRY =====");
+  robot::log_info("[SimpleActionServer::processCancel] Cancel message ID: '%s'", cancel_msg->id.c_str());
+  robot::log_info("[SimpleActionServer::processCancel] Cancel message stamp: %ld.%09ld", 
+                  cancel_msg->stamp.sec, cancel_msg->stamp.nsec);
+  
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  robot::log_info("[SimpleActionServer::processCancel] Current goal ID: '%s'", current_goal_id_.id.c_str());
+  robot::log_info("[SimpleActionServer::processCancel] Current goal stamp: %ld.%09ld", 
+                  current_goal_id_.stamp.sec, current_goal_id_.stamp.nsec);
+  robot::log_info("[SimpleActionServer::processCancel] Current status: %d", current_status_.status);
+  
+  // Check if this cancel request is for the current goal
+  bool should_cancel = false;
+  bool is_empty_id = (cancel_msg->id == "");
+  bool is_matching_id = (cancel_msg->id == current_goal_id_.id);
+  bool is_matching_stamp = (cancel_msg->stamp == current_goal_id_.stamp);
+  
+  robot::log_debug("[SimpleActionServer::processCancel] Cancel check: empty_id=%s, matching_id=%s, matching_stamp=%s",
+                   is_empty_id ? "true" : "false",
+                   is_matching_id ? "true" : "false",
+                   is_matching_stamp ? "true" : "false");
+  
+  if (is_empty_id || (is_matching_id && is_matching_stamp))
+  {
+    should_cancel = true;
+    robot::log_info("[SimpleActionServer::processCancel] Cancel request matches current goal - will cancel");
+    
+    if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
+    {
+      robot::log_info("[SimpleActionServer::processCancel] Goal is ACTIVE - setting preemption");
+      preempt_requested_ = true;
+      current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING;
+      robot::log_info("[SimpleActionServer::processCancel] Cancel requested for goal ID: %s", current_goal_id_.id.c_str());
+      robot::log_info("[SimpleActionServer::processCancel] Status changed to PREEMPTING");
+    }
+    else if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING)
+    {
+      robot::log_info("[SimpleActionServer::processCancel] Goal is PENDING - rejecting it");
+      // Goal hasn't started yet, just reject it
+      current_status_.status = robot_actionlib_msgs::GoalStatus::REJECTED;
+      current_status_.text = "Goal cancelled before execution";
+      goal_.reset();
+      new_goal_available_ = false;
+      robot::log_info("[SimpleActionServer::processCancel] Goal cancelled before execution");
+      robot::log_info("[SimpleActionServer::processCancel] Status changed to REJECTED");
+    }
+    else
+    {
+      robot::log_info("[SimpleActionServer::processCancel] Goal status is %d - no action needed", current_status_.status);
+    }
+  }
+  else
+  {
+    robot::log_info("[SimpleActionServer::processCancel] Cancel request does NOT match current goal - ignoring");
+    robot::log_info("[SimpleActionServer::processCancel] Cancel ID '%s' != Current ID '%s'", 
+                    cancel_msg->id.c_str(), current_goal_id_.id.c_str());
+  }
+  
+  // Call cancel callback if set
+  robot::log_debug("[SimpleActionServer::processCancel] Checking cancel_callback_: %s", 
+                   cancel_callback_ ? "set" : "null");
+  if (cancel_callback_)
+  {
+    robot::log_debug("[SimpleActionServer::processCancel] Calling cancel_callback_...");
+    try
+    {
+      cancel_callback_(cancel_msg);
+      robot::log_debug("[SimpleActionServer::processCancel] cancel_callback_ executed successfully");
+    }
+    catch (const std::exception& e)
+    {
+      robot::log_error("[SimpleActionServer::processCancel] EXCEPTION in cancel_callback_: %s", e.what());
+    }
+  }
+  
+  robot::log_info("[SimpleActionServer::processCancel] ===== EXIT =====");
+}
+
+// Check if a new goal is available
+template<class ActionSpec>
+bool SimpleActionServer<ActionSpec>::isNewGoalAvailable()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  return new_goal_available_;
+}
+
+// Check if preemption has been requested
+template<class ActionSpec>
+bool SimpleActionServer<ActionSpec>::isPreemptRequested()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  return preempt_requested_;
+}
+
+// Accept a new goal
+template<class ActionSpec>
+typename SimpleActionServer<ActionSpec>::GoalConstPtr SimpleActionServer<ActionSpec>::acceptNewGoal()
+{
+  robot::log_info("[SimpleActionServer::acceptNewGoal] ===== ENTRY =====");
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  robot::log_debug("[SimpleActionServer::acceptNewGoal] new_goal_available_: %s", 
+                   new_goal_available_ ? "true" : "false");
+  robot::log_debug("[SimpleActionServer::acceptNewGoal] goal_ is %s", 
+                   goal_ ? "valid" : "null");
+  
+  if (!new_goal_available_)
+  {
+    robot::log_warning("[SimpleActionServer::acceptNewGoal] No new goal available - returning null");
+    robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (null) =====");
+    return GoalConstPtr();
+  }
+  
+  // Accept the new goal
+  GoalConstPtr new_goal = goal_;
+  goal_.reset();
+  new_goal_available_ = false;
+  preempt_requested_ = false;
+  
+  // Update status
+  current_status_.status = robot_actionlib_msgs::GoalStatus::ACTIVE;
+  current_status_.goal_id = current_goal_id_;
+  current_status_.text = "";
+  
+  robot::log_info("[SimpleActionServer::acceptNewGoal] Goal accepted successfully");
+  robot::log_info("[SimpleActionServer::acceptNewGoal] Goal ID: %s", current_goal_id_.id.c_str());
+  robot::log_info("[SimpleActionServer::acceptNewGoal] Status changed to ACTIVE");
+  if (new_goal)
+  {
+    robot::log_info("[SimpleActionServer::acceptNewGoal] Goal position: x=%.3f, y=%.3f", 
+                    new_goal->target_pose.pose.position.x,
+                    new_goal->target_pose.pose.position.y);
+  }
+  robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (success) =====");
+  return new_goal;
+}
+
+// Set the action as succeeded
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setSucceeded(const Result& result, const std::string& text)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
+      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
+  {
+    current_status_.status = robot_actionlib_msgs::GoalStatus::SUCCEEDED;
+    current_status_.text = text;
+    
+    // Create and publish result
+    ActionResultPtr action_result = boost::make_shared<ActionResult>();
+    action_result->header.stamp = robot::Time::now();
+    action_result->status = current_status_;
+    action_result->result = result;
+    
+    // Call result callback if set
+    if (result_callback_)
+    {
+      result_callback_(action_result);
+    }
+    
+    // Clear goal
+    goal_.reset();
+    new_goal_available_ = false;
+    preempt_requested_ = false;
+    
+    robot::log_info("[SimpleActionServer] Goal succeeded: %s", text.c_str());
+  }
+}
+
+// Set the action as aborted
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setAborted(const Result& result, const std::string& text)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
+      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
+  {
+    current_status_.status = robot_actionlib_msgs::GoalStatus::ABORTED;
+    current_status_.text = text;
+    
+    // Create and publish result
+    ActionResultPtr action_result = boost::make_shared<ActionResult>();
+    action_result->header.stamp = robot::Time::now();
+    action_result->status = current_status_;
+    action_result->result = result;
+    
+    // Call result callback if set
+    if (result_callback_)
+    {
+      result_callback_(action_result);
+    }
+    
+    // Clear goal
+    goal_.reset();
+    new_goal_available_ = false;
+    preempt_requested_ = false;
+    
+    robot::log_warning("[SimpleActionServer] Goal aborted: %s", text.c_str());
+  }
+}
+
+// Set the action as preempted
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::setPreempted(const Result& result, const std::string& text)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
+      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
+  {
+    current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTED;
+    current_status_.text = text;
+    
+    // Create and publish result
+    ActionResultPtr action_result = boost::make_shared<ActionResult>();
+    action_result->header.stamp = robot::Time::now();
+    action_result->status = current_status_;
+    action_result->result = result;
+    
+    // Call result callback if set
+    if (result_callback_)
+    {
+      result_callback_(action_result);
+    }
+    
+    // Clear goal
+    goal_.reset();
+    new_goal_available_ = false;
+    preempt_requested_ = false;
+    
+    robot::log_info("[SimpleActionServer] Goal preempted: %s", text.c_str());
+  }
+}
+
+// Publish feedback
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback& feedback)
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  
+  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
+  {
+    ActionFeedbackPtr action_feedback = boost::make_shared<ActionFeedback>();
+    action_feedback->header.stamp = robot::Time::now();
+    action_feedback->status = current_status_;
+    action_feedback->feedback = feedback;
+    
+    // Call feedback callback if set
+    if (feedback_callback_)
+    {
+      feedback_callback_(action_feedback);
+    }
+  }
+}
+
+// Get the current goal
+template<class ActionSpec>
+typename SimpleActionServer<ActionSpec>::GoalConstPtr SimpleActionServer<ActionSpec>::getCurrentGoal()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  return goal_;
+}
+
+// Get the current status
+template<class ActionSpec>
+robot_actionlib_msgs::GoalStatus SimpleActionServer<ActionSpec>::getCurrentStatus()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  return current_status_;
+}
+
+// Check if the server is active
+template<class ActionSpec>
+bool SimpleActionServer<ActionSpec>::isActive()
+{
+  boost::lock_guard<boost::mutex> lock(state_mutex_);
+  return active_;
+}
+
+// Get the action name
+template<class ActionSpec>
+std::string SimpleActionServer<ActionSpec>::getName() const
+{
+  return name_;
+}
+
+// Execute the goal in a separate thread
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::executeGoal()
+{
+  robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD STARTED =====");
+  robot::log_info("[SimpleActionServer::executeGoal] Action server name: %s", name_.c_str());
+  robot::log_debug("[SimpleActionServer::executeGoal] execute_callback_ is %s", 
+                   execute_callback_ ? "set" : "null");
+  
+  // Continuously process goals
+  while (active_)
+  {
+    robot::log_debug("[SimpleActionServer::executeGoal] Waiting for new goal...");
+    GoalConstPtr goal_to_execute;
+    
+    // Wait for a new goal to be available
+    {
+      boost::unique_lock<boost::mutex> lock(state_mutex_);
+      robot::log_debug("[SimpleActionServer::executeGoal] Mutex lock acquired in executeGoal");
+      robot::log_debug("[SimpleActionServer::executeGoal] active_=%s, new_goal_available_=%s", 
+                       active_ ? "true" : "false", 
+                       new_goal_available_ ? "true" : "false");
+      
+      // Wait until a new goal is available or server is stopped
+      while (active_ && !new_goal_available_)
+      {
+        robot::log_debug("[SimpleActionServer::executeGoal] Waiting on condition variable...");
+        goal_condition_.wait(lock);
+        robot::log_debug("[SimpleActionServer::executeGoal] Woke up from condition wait");
+        robot::log_debug("[SimpleActionServer::executeGoal] After wake: active_=%s, new_goal_available_=%s", 
+                         active_ ? "true" : "false", 
+                         new_goal_available_ ? "true" : "false");
+      }
+      
+      if (!active_)
+      {
+        robot::log_info("[SimpleActionServer::executeGoal] Server is not active - exiting thread");
+        break;
+      }
+      
+      // Accept the new goal
+      if (new_goal_available_)
+      {
+        robot::log_info("[SimpleActionServer::executeGoal] New goal available - accepting it...");
+        
+        // Release lock before calling acceptNewGoal() to avoid deadlock
+        // (acceptNewGoal() will acquire its own lock)
+        lock.unlock();
+        
+        goal_to_execute = acceptNewGoal();
+        
+        robot::log_info("[SimpleActionServer::executeGoal] Goal accepted: %s", 
+                        goal_to_execute ? "valid" : "null");
+      }
+      else
+      {
+        robot::log_warning("[SimpleActionServer::executeGoal] Woke up but new_goal_available_ is false!");
+        lock.unlock();
+      }
+    }
+    
+    // Execute the goal callback
+    if (goal_to_execute)
+    {
+      robot::log_info("[SimpleActionServer::executeGoal] Executing goal callback...");
+      robot::log_info("[SimpleActionServer::executeGoal] Goal position: x=%.3f, y=%.3f", 
+                      goal_to_execute->target_pose.pose.position.x,
+                      goal_to_execute->target_pose.pose.position.y);
+      
+      if (execute_callback_)
+      {
+        try
+        {
+          robot::log_info("[SimpleActionServer::executeGoal] Calling execute_callback_...");
+          execute_callback_(goal_to_execute);
+          robot::log_info("[SimpleActionServer::executeGoal] execute_callback_ completed");
+        }
+        catch (const std::exception& e)
+        {
+          robot::log_error("[SimpleActionServer::executeGoal] EXCEPTION in execute_callback_: %s", e.what());
+        }
+      }
+      else
+      {
+        robot::log_error("[SimpleActionServer::executeGoal] execute_callback_ is null - cannot execute goal!");
+      }
+    }
+    else
+    {
+      robot::log_warning("[SimpleActionServer::executeGoal] goal_to_execute is null - skipping execution");
+    }
+  }
+  
+  robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD EXITING =====");
+}
+
+// Thread function to publish status periodically
+template<class ActionSpec>
+void SimpleActionServer<ActionSpec>::statusPublishThread()
+{
+  double rate = status_publish_rate_;
+  robot::Duration sleep_duration(1.0 / rate);
+  
+  while (active_)
+  {
+    {
+      boost::lock_guard<boost::mutex> lock(state_mutex_);
+      
+      // Create status array
+      robot_actionlib_msgs::GoalStatusArrayPtr status_array = 
+          boost::make_shared<robot_actionlib_msgs::GoalStatusArray>();
+      status_array->header.stamp = robot::Time::now();
+      status_array->status_list.clear();
+      
+      if (current_status_.status != robot_actionlib_msgs::GoalStatus::PENDING ||
+          !current_goal_id_.id.empty())
+      {
+        status_array->status_list.push_back(current_status_);
+      }
+      
+      // Call status callback if set
+      if (status_callback_)
+      {
+        status_callback_(status_array);
+      }
+    }
+    
+    // Sleep
+    robot::Time::sleepUntil(robot::Time::now() + sleep_duration);
+  }
+}
+
+} // namespace move_base
+
+// Explicit template instantiation for MoveBaseAction
+// This is required because template method definitions are in .cpp file
+template class move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction>;
+