/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Author: Kevin Lee
 *********************************************************************/
#ifdef BUILD_WITH_ROS
#include <ros/ros.h>
#endif
#include <move_base/move_base.h>
#include <move_base/convert_data.h>
#include <cmath>
#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/robot.h>

#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_nav_2d_utils/conversions.h>
#ifndef BUILD_WITH_ROS
  #include <laser_filter/laser_filter.h>
#endif

move_base::MoveBase::MoveBase()
  : initialized_(false),
    tf_(),
    as_(NULL),
    planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    recovery_index_(0),
    recovery_behavior_enabled_(false),
    planner_frequency_(0.0), controller_frequency_(0.0),
    inscribed_radius_(0.0), circumscribed_radius_(0.0),
    planner_patience_(0.0), controller_patience_(0.0),
    max_planning_retries_(0), planning_retries_(0),
    conservative_reset_dist_(0.0), clearing_radius_(0.0),
    shutdown_costmaps_(false), clearing_rotation_allowed_(false),
    make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
    oscillation_timeout_(0.0), oscillation_distance_(0.0),
    state_(PLANNING), recovery_trigger_(PLANNING_R),
    runPlanner_(false), planner_thread_(NULL),
    setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
    pause_ctr_(false), paused_(false), cancel_ctr_(false),
    original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
}

move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
  : initialized_(false),
    tf_(),
    as_(NULL),
    planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    recovery_index_(0),
    recovery_behavior_enabled_(false),
    planner_frequency_(0.0), controller_frequency_(0.0),
    inscribed_radius_(0.0), circumscribed_radius_(0.0),
    planner_patience_(0.0), controller_patience_(0.0),
    max_planning_retries_(0), planning_retries_(0),
    conservative_reset_dist_(0.0), clearing_radius_(0.0),
    shutdown_costmaps_(false), clearing_rotation_allowed_(false),
    make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
    oscillation_timeout_(0.0), oscillation_distance_(0.0),
    state_(PLANNING), recovery_trigger_(PLANNING_R),
    runPlanner_(false), planner_thread_(NULL),
    setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
    pause_ctr_(false), paused_(false), cancel_ctr_(false),
    original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
  initialize(tf);
}

move_base::MoveBase::~MoveBase()
{
  robot::log_warning("Destroying MoveBase instance...");
  if (as_ != NULL)
  { 
    as_->stop();
    delete as_;
    as_ = NULL;
  }

  recovery_behaviors_.clear();

  // Cleanup static maps (shared_ptr tự động cleanup)
  static_maps_.clear();

  // Cleanup laser scans (shared_ptr tự động cleanup)
  laser_scans_.clear();

  // Cleanup point clouds (shared_ptr tự động cleanup)
  point_clouds_.clear();

  // Cleanup point cloud2s (shared_ptr tự động cleanup)
  point_cloud2s_.clear();

  if (planner_costmap_robot_ != NULL)
  {
    delete planner_costmap_robot_;
    planner_costmap_robot_ = NULL;
  }

  if (controller_costmap_robot_ != NULL)
  {
    delete controller_costmap_robot_;
    controller_costmap_robot_ = NULL;
  }

  if (planner_thread_ != NULL)
  {
    planner_thread_->interrupt();
    planner_thread_->join();
    delete planner_thread_;
    planner_thread_ = NULL;
  }

  if (planner_plan_ != NULL)
  {
    delete planner_plan_;
    planner_plan_ = NULL;
  }

  if (latest_plan_ != NULL)
  {
    delete latest_plan_;
    latest_plan_ = NULL;
  }

  if (controller_plan_ != NULL)
  {
    delete controller_plan_;
    controller_plan_ = NULL;
  }

  planner_.reset();
  tc_.reset();
  initialized_ = false;
  tf_.reset();
}

void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
  printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
  if (!initialized_)
  {
    if(tf == nullptr)
    {
      robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
      throw std::runtime_error("Failed to create the tf");
    }
    else
    {
      robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
    }
    tf_ = tf;
    if(tf_)
    {
      std::string all_frames_string = tf_->allFramesAsString();
      robot::log_info("[%s:%d]\n INFO: tf_ allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
    }
    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;

    // get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh_.getParam("base_global_planner", global_planner, std::string(""));
    robot::log_info("[%s %d] global_planner: %s", __FILE__, __LINE__, global_planner.c_str());
    private_nh_.getParam("base_local_planner", local_planner, std::string(""));
    robot::log_info("[%s %d] local_planner: %s", __FILE__, __LINE__, local_planner.c_str());

    // Handle nested YAML nodes for costmap config
    std::string robot_base_frame;
    private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
    std::string global_frame;
    private_nh_.getParam("global_frame", global_frame, std::string("map"));
    robot_base_frame_ = robot_base_frame;
    global_frame_ = global_frame;
    double planner_frequency;
    private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
    planner_frequency_ = planner_frequency;
    double controller_frequency;
    private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
    controller_frequency_ = controller_frequency;
    double planner_patience;
    private_nh_.getParam("planner_patience", planner_patience, 5.0);
    planner_patience_ = planner_patience;
    private_nh_.getParam("controller_patience", controller_patience_, 15.0);
    double max_planning_retries;
    private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
    max_planning_retries_ = max_planning_retries;
    double oscillation_timeout;
    private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
    oscillation_timeout_ = oscillation_timeout;
    double oscillation_distance;
    private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
    oscillation_distance_ = oscillation_distance;

    double original_xy_goal_tolerance;
    private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
    original_xy_goal_tolerance_ = original_xy_goal_tolerance;
    double original_yaw_goal_tolerance;
    private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
    original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;

    // defined local planner name
    private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
    private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
    private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
    private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));

    // parameters of make_plan service
    private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
    private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
    private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
    min_approach_linear_velocity_ *= 1.2;
    // set up plan triple buffer
    planner_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();

    // set up the planner's thread
    planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
    // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    //  From config param
    double inscribed_radius;
    private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
    double circumscribed_radius;
    private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
    inscribed_radius_ = inscribed_radius;
    circumscribed_radius_ = circumscribed_radius;
    private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
    double conservative_reset_dist;
    private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
    conservative_reset_dist_ = conservative_reset_dist;
    private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);

    // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    try
    {
      planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
      planner_costmap_robot_->pause();
      robot_costmap_2d::LayeredCostmap *layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the planner_costmap_robot_");
    }

    try
    {
      controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
      if(controller_costmap_robot_ == nullptr)
      {
        robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__);
        throw std::runtime_error("Failed to create the controller_costmap_robot_");
      }
      controller_costmap_robot_->pause();
      robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the controller_costmap_robot_");
    }

    addStaticMap(map_name_save_, map_save_);

    // initialize the global planner
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(global_planner);
      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
      planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
          path_file_so, global_planner, boost::dll::load_mode::append_decorations);

      planner_ = planner_loader_();
      if (!planner_)
      {
        robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
        throw std::runtime_error("Failed to load global planner " + global_planner);
      }
      last_config_.base_global_planner = global_planner;
      default_config_.base_global_planner = global_planner;
      if (planner_->initialize(global_planner, planner_costmap_robot_))
        robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
      else
        robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the " + global_planner + " planner");
    }

    // create a local planner
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(local_planner);
      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
      controller_loader_ =
          boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
              path_file_so, local_planner, boost::dll::load_mode::append_decorations);
      tc_ = controller_loader_();
      if (!tc_)
      {
        robot::log_error("[%s:%d]\n ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
        throw std::runtime_error("Failed to load local planner " + local_planner);
      }
      tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
      tc_->setOdom(&odometry_);
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the " + local_planner + " planner");
    }

    // Start actively updating costmaps based on sensor data
    planner_costmap_robot_->start();
    controller_costmap_robot_->start();

    try
    {
      old_linear_fwd_ = tc_->getTwistLinear(true);
      old_linear_bwd_ = tc_->getTwistLinear(false);

      old_angular_fwd_ = tc_->getTwistAngular(true);
      old_angular_rev_ = tc_->getTwistAngular(false);
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << '\n';
    }

    // if we shutdown our costmaps when we're deactivated... we'll do that now
    if (shutdown_costmaps_)
    {
      planner_costmap_robot_->stop();
      controller_costmap_robot_->stop();
    }

    // load any user specified recovery behaviors, and if that fails load the defaults
    if (!loadRecoveryBehaviors(private_nh_))
    {
      robot::log_warning("[%s:%d]\n Loading default recovery behaviors", __FILE__, __LINE__);
      loadDefaultRecoveryBehaviors();
    }

    // initially, we'll need to make a plan
    state_ = move_base::PLANNING;

    // we'll start executing recovery behaviors at the beginning of our list
    recovery_index_ = 0;
    nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
      nav_feedback_->is_ready = true;
    }
    else
    {
      robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
    }
    initialized_ = true;
    setup_ = true;
    robot::log_info("========== End: initialize() - SUCCESS ==========");
  }
  else
  {
    robot::log_warning("[%s:%d]\n Already initialized, skipping", __FILE__, __LINE__);
  }
}

void move_base::MoveBase::swapPlanner(std::string base_global_planner)
{
  if (base_global_planner != last_config_.base_global_planner)
  {
    robot_nav_core::BaseGlobalPlanner::Ptr old_planner = planner_;
    auto old_loader = planner_loader_;
    // initialize the global planner
    try
    {
      // wait for the current planner to finish planning
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(base_global_planner);
      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
      auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
          path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
      auto new_planner = new_loader();
      if (!new_planner)
      {
        robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr", __FILE__, __LINE__);
        throw std::runtime_error("Failed to load global planner " + base_global_planner);
      }

      if (planner_plan_)
        planner_plan_->clear();
      if (latest_plan_)
        latest_plan_->clear();
      if (controller_plan_)
        controller_plan_->clear();
      resetState();

      new_planner->initialize(base_global_planner, planner_costmap_robot_);

      planner_ = new_planner;
      planner_loader_ = new_loader;
      last_config_.base_global_planner = base_global_planner;

      // Release old planner while old loader is still alive.
      old_planner.reset();
      old_loader = boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()>();
      lock.unlock();
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n Failed to create the %s planner, are you sure it is properly registered and that the \
                  containing library is built? Exception: %s",__FILE__, __LINE__, base_global_planner.c_str(), ex.what());
      planner_ = old_planner;
      planner_loader_ = old_loader;
    }
  }
}

void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
{
  if (planner_costmap_robot_)
    planner_costmap_robot_->setUnpaddedRobotFootprint(fprt);

  if (controller_costmap_robot_)
    controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
}

std::vector<robot_geometry_msgs::Point> move_base::MoveBase::getRobotFootprint()
{
  if (planner_costmap_robot_)
    return planner_costmap_robot_->getUnpaddedRobotFootprint();
  return std::vector<robot_geometry_msgs::Point>();
}

void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map)
{
  // Kiểm tra xem map_name đã tồn tại trong static_maps_ chưa
  auto it = static_maps_.find(map_name);
  if (it == static_maps_.end())
  {
    static_maps_[map_name] = map;
  }
  else
  {
    it->second = map;
  }
  // robot::log_info("map header: %s", map.header.frame_id.c_str());
  // robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec);
  // robot::log_info("map header: %d", map.header.seq);
  // robot::log_info("map info resolution: %f", map.info.resolution);
  // robot::log_info("map info width: %d", map.info.width);
  // robot::log_info("map info height: %d", map.info.height);
  // robot::log_info("map info origin position x: %f", map.info.origin.position.x);
  // robot::log_info("map info origin position y: %f", map.info.origin.position.y);
  // robot::log_info("map info origin position z: %f", map.info.origin.position.z);
  // robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x);
  // robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y);
  // robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z);
  // robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w);
  // robot::log_info("map data size: %zu", map.data.size());
  // for(size_t i = 0; i < map.data.size(); i++) {
  //   robot::log_info("map data[%zu]: %d", i, map.data[i]);
  // }
  robot::log_info("--------------------------------");
  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);
}

robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::string &map_name)
{
  auto it = static_maps_.find(map_name);
  if (it != static_maps_.end())
  {
    return it->second;
  }
  return robot_nav_msgs::OccupancyGrid();
}

void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
{
  auto it = laser_scans_.find(laser_scan_name);
#ifndef BUILD_WITH_ROS
  laser_filter::LaserScanSOR sor;
  sor.setMeanK(10);              // xét 10 điểm láng giềng gần nhất
  sor.setStddevMulThresh(1.0);   // ngưỡng = mean + 1.0 * stddev
  robot_sensor_msgs::LaserScan laser_scan_filter = sor.filter(laser_scan);
  if (it == laser_scans_.end())
  {
    laser_scans_[laser_scan_name] = laser_scan_filter;
  }
  else
  {
    it->second = laser_scan_filter;
  }
#else
  if (it == laser_scans_.end())
  {
    laser_scans_[laser_scan_name] = laser_scan;
  }
  else
  {
    it->second = laser_scan;
  }
#endif
  // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
  // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
  // robot::log_info("angle_min: %f", laser_scan.angle_min);
  // robot::log_info("angle_max: %f", laser_scan.angle_max);
  // robot::log_info("angle_increment: %f", laser_scan.angle_increment);
  // robot::log_info("time_increment: %f", laser_scan.time_increment);
  // robot::log_info("scan_time: %f", laser_scan.scan_time);
  // robot::log_info("range_min: %f", laser_scan.range_min);
  // robot::log_info("range_max: %f", laser_scan.range_max);
  // robot::log_info("ranges_size: %zu", laser_scan.ranges.size());
  // robot::log_info("intensities_size: %zu", laser_scan.intensities.size());
  // std::stringstream ranges_str;
  // for (size_t i = 0; i < laser_scan.ranges.size(); i++) {
  //   ranges_str << laser_scan.ranges[i] << " ";
  // }
  // robot::log_info("ranges: %s", ranges_str.str().c_str());
  // std::stringstream intensities_str;  
  // for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
  //   intensities_str << laser_scan.intensities[i] << " ";
  // }
  // robot::log_error("intensities: %s", intensities_str.str().c_str());

#ifndef BUILD_WITH_ROS
  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
#else
  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
#endif
}

robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
{
  auto it = laser_scans_.find(laser_scan_name);
  if (it != laser_scans_.end())
  {
    return it->second;
  }
  return robot_sensor_msgs::LaserScan();
}

void move_base::MoveBase::addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud)
{
  auto it = point_clouds_.find(point_cloud_name);
  if (it == point_clouds_.end())
  {
    point_clouds_[point_cloud_name] = point_cloud;
  }
  else
  {
    it->second = point_cloud;
  }
  updateLocalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud_name);
  updateGlobalCostmap<robot_sensor_msgs::PointCloud>(point_cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud_name);
}

robot_sensor_msgs::PointCloud move_base::MoveBase::getPointCloud(const std::string &point_cloud_name)
{
  auto it = point_clouds_.find(point_cloud_name);
  if (it != point_clouds_.end())
  {
    return it->second;
  }
  return robot_sensor_msgs::PointCloud();
}

void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2)
{
  auto it = point_cloud2s_.find(point_cloud2_name);
  if (it == point_cloud2s_.end())
  {
    point_cloud2s_[point_cloud2_name] = point_cloud2;
  }
  else
  {
    it->second = point_cloud2;
  }
  updateLocalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud2_name);
  updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud2_name);
}

void move_base::MoveBase::addDepthCameraData(const std::string &topic,
                                             robot_sensor_msgs::DepthCameraData::ConstPtr data)
{
  if (!data)
    return;

  // robot::log_error("DEBUG");

  updateLocalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
      data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
  updateGlobalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
      data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
}

robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name)
{
  auto it = point_cloud2s_.find(point_cloud2_name);
  if (it != point_cloud2s_.end())
  {
    return it->second;
  }
  return robot_sensor_msgs::PointCloud2();
}

std::map<std::string, robot_nav_msgs::OccupancyGrid> move_base::MoveBase::getAllStaticMaps()
{
  return static_maps_;
}

std::map<std::string, robot_sensor_msgs::LaserScan> move_base::MoveBase::getAllLaserScans()
{
  return laser_scans_;
}

std::map<std::string, robot_sensor_msgs::PointCloud> move_base::MoveBase::getAllPointClouds()
{
  return point_clouds_;
}

std::map<std::string, robot_sensor_msgs::PointCloud2> move_base::MoveBase::getAllPointCloud2s()
{
  return point_cloud2s_;
}

bool move_base::MoveBase::removeStaticMap(const std::string &map_name)
{
  auto it = static_maps_.find(map_name);
  if (it != static_maps_.end())
  {
    static_maps_.erase(it);
    return true;
  }
  return false;
}

bool move_base::MoveBase::removeLaserScan(const std::string &laser_scan_name)
{
  auto it = laser_scans_.find(laser_scan_name);
  if (it != laser_scans_.end())
  {
    laser_scans_.erase(it);
    return true;
  }
  return false;
}

bool move_base::MoveBase::removePointCloud(const std::string &point_cloud_name)
{
  auto it = point_clouds_.find(point_cloud_name);
  if (it != point_clouds_.end())
  {
    point_clouds_.erase(it);
    return true;
  }
  return false;
}

bool move_base::MoveBase::removePointCloud2(const std::string &point_cloud2_name)
{
  auto it = point_cloud2s_.find(point_cloud2_name);
  if (it != point_cloud2s_.end())
  {
    point_cloud2s_.erase(it);
    return true;
  }
  return false;
}

bool move_base::MoveBase::removeAllStaticMaps()
{
  static_maps_.clear();
  return true;
}

bool move_base::MoveBase::removeAllLaserScans()
{
  laser_scans_.clear();
  return true;
}

bool move_base::MoveBase::removeAllPointClouds()
{
  point_clouds_.clear();
  return true;
}

bool move_base::MoveBase::removeAllPointCloud2s()
{
  point_cloud2s_.clear();
  return true;
}

bool move_base::MoveBase::removeAllData()
{
  static_maps_.clear();
  laser_scans_.clear();
  point_clouds_.clear();
  point_cloud2s_.clear();
  return true;
}

template<typename T>
void move_base::MoveBase::updateGlobalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name)
{
  if (!planner_costmap_robot_)
    return;

  // Tìm layer phù hợp và update với data đã cho
  try
  {
    for (const auto &layer : *planner_costmap_robot_->getLayeredCostmap()->getPlugins())
    {
      // Kiểm tra layer có đúng type và name không
      // Nếu layer type khớp HOẶC name khớp thì update
      // if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER)
      //   robot::log_warning ("[%s:%d] Update GlobalCostmap: layer type: %d, name: %s, layer->getType(): %d, layer->getName(): %s", __FILE__, __LINE__, layer_type, name.c_str(), layer->getType(), layer->getName().c_str());
      if (layer->getType() == layer_type || layer->getName() == name)
      {
        if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER)
        {
          robot::log_warning("[%s:%d] Update GlobalCostmap: layer type is STATIC_LAYER, name: %s", __FILE__, __LINE__, name.c_str());
        }

        // Update costmap với data
        layer->dataCallBack<T>(value, name);
      }
    }
  }
  catch (const std::exception &ex)
  {
    robot::log_error("[%s:%d] EXCEPTION in Update GlobalCostmap: %s", __FILE__, __LINE__, ex.what());
    return;
  }
}

template<typename T>
void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::LayerType layer_type, const std::string &name)
{
  if (!controller_costmap_robot_)
    return;

  // Tìm layer phù hợp và update với data đã cho
  try
  {
    for (const auto &layer : *controller_costmap_robot_->getLayeredCostmap()->getPlugins())
    {
      // Kiểm tra layer có đúng type và name không
      // Nếu layer type khớp HOẶC name khớp thì update
      if (layer->getType() == layer_type || layer->getName() == name)
      {
        // Update costmap với data
        layer->dataCallBack<T>(value, name);
      }
    }
  }
  catch (const std::exception &ex)
  {
    robot::log_error("[%s:%d] EXCEPTION in Update LocalCostmap: %s", __FILE__, __LINE__, ex.what());
    return;
  }
}

void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
  // robot::log_info("addOdometry: %s", odometry_name.c_str());
  // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
  // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
  // if(tf_)
  // {
  //   try
  //   {
  //     robot_geometry_msgs::PoseStamped global_pose_stamped;
  //     tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
  //     robot_geometry_msgs::PoseStamped robot_pose;
  //     tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
  //     robot_pose.header.frame_id = robot_base_frame_;
  //     robot_pose.header.stamp = robot::Time();
  //     robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
        
  //     // Convert robot::Time to tf3::Time
  //     tf3::Time tf3_current_time = data_convert::convertTime(current_time);
  //     tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
        
  //     std::string error_msg;
  //     if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
  //     {
  //       // Transform is available at current time
  //       tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time);
  //       tf3::doTransform(robot_pose, global_pose_stamped, transform);
  //       robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation));
  //     }
  //   }
  //   catch (const tf3::TransformException &ex)
  //   {
  //     robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what());
  //   }
  //   // robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
  // }
  // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation));
  // std::stringstream pose_covariance_str;
  // for(int i = 0; i < 36; i++) {
  //   pose_covariance_str << odometry.pose.covariance[i] << " ";
  // }
  // robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str());
  // robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x);
  // robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y);
  // robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z);
  // robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x);
  // robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y);
  // robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z);
  // std::stringstream twist_covariance_str;
  // for(int i = 0; i < 36; i++) {
  //   twist_covariance_str << odometry.twist.covariance[i] << " ";
  // }
  // robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str());
  // robot::log_info("--------------------------------");

  odometry_ = odometry;
}

bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal, 
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
{
  if (setup_)
  {
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      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;
  }

  // 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_)
  {
    robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  if (!controller_costmap_robot_)
  {
    robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));

  // Swap planner
  try
  {
    if (!tc_->swapPlanner(position_planner_name_))
    {
      robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  // 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);
    
    // Clear Order message since this is a non-Order moveTo call
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_.reset();
    }
    
    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_)
  {
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      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
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
  robot::Duration(0.01).sleep();

  if (!tc_)
  {
    robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  if (!controller_costmap_robot_)
  {
    robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));

  try
  {
    if (!tc_->swapPlanner(position_planner_name_))
    {
      robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  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;
  }

  if (cancel_ctr_)
    cancel_ctr_ = false;

  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;

    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());

    // Store Order message for use in planThread
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
      robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
    }
    if (!planner_order_)
    {
      robot::log_error("[MoveBase::moveTo] Failed to store Order message for planning");
      lock.unlock();
      return false;
    }

    as_->processGoal(action_goal);
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
    lock.unlock();
    return false;
  }

  lock.unlock();
  return true;
}

bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance, double yaw_goal_tolerance)
{
  std::string maker_sources;
  private_nh_.param("maker_sources", maker_sources, std::string(""));
  std::stringstream ss(maker_sources);
  std::string source;
  bool has_maker = false;
  while (ss >> source)
  {
    if (marker == source)
    {
      private_nh_.setParam("maker_name", marker);
      has_maker = true;
    }
  }
  if (!has_maker)
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      std::stringstream fb_ss;
      fb_ss << "The system has not been '" << marker << "'";
      nav_feedback_->feed_back_str = fb_ss.str();
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (setup_)
  {
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      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
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  if (!tc_)
  {
    robot::log_error("[MoveBase::dockTo] tc_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  if (!controller_costmap_robot_)
  {
    robot::log_error("[MoveBase::dockTo] controller_costmap_robot_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  try
  {
    if (!tc_->swapPlanner(docking_planner_name_))
    {
      robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  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::dockTo] nav_feedback_ pointer is null!");
    lock.unlock();
    return false;
  }
  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);
    
    // Clear Order message since this is a non-Order moveTo call
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_.reset();
    }
    
    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::dockTo(const robot_protocol_msgs::Order &msg,
                                 const std::string &marker,
                                 const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance, double yaw_goal_tolerance)
{
  std::string maker_sources;
  private_nh_.param("maker_sources", maker_sources, std::string(""));
  std::stringstream ss(maker_sources);
  std::string source;
  bool has_maker = false;
  while (ss >> source)
  {
    if (marker == source)
    {
      private_nh_.setParam("maker_name", marker);
      has_maker = true;
    }
  }
  if (!has_maker)
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      std::stringstream fb_ss;
      fb_ss << "The system has not been '" << marker << "'";
      nav_feedback_->feed_back_str = fb_ss.str();
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (setup_)
  {
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      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
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  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()));
  try
  {
    if(!tc_->swapPlanner(docking_planner_name_))
    {
      robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  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;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;

  if (!as_)
  {
    robot::log_error("[MoveBase::dockTo] 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;

    robot::Time now = robot::Time::now();
    action_goal->goal_id.stamp = now;
    std::ostringstream goal_id_stream;
    goal_id_stream << "move_base_dock_goal_" << now.sec << "_" << now.nsec;
    action_goal->goal_id.id = goal_id_stream.str();

    robot::log_info("[MoveBase::dockTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());

    // Store Order message for use in planThread (same as moveTo with Order)
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
      robot::log_info("[MoveBase::dockTo] Stored Order message for planning");
    }
    if (!planner_order_)
    {
      robot::log_error("[MoveBase::dockTo] Failed to store Order message for planning");
      lock.unlock();
      return false;
    }
    as_->processGoal(action_goal);
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::dockTo] Exception during processGoal: %s", e.what());
    lock.unlock();
    return false;
  }

  lock.unlock();
  return true;
}

bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
  robot::log_info("[MoveBase::moveStraightTo] Entry");
  if (setup_)
  {
    std::string global_planner = "TwoPointsPlanner";
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, go_straight_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      swapPlanner(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
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (!tc_)
  {
    robot::log_error("[MoveBase::moveStraightTo] tc_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  if (!controller_costmap_robot_)
  {
    robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));

  try
  {
    robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner");
    if (!tc_->swapPlanner(go_straight_planner_name_))
    {
      robot::log_error("[MoveBase::moveStraightTo] Failed to swapPlanner");
      lock.unlock();
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::moveStraightTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  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::moveStraightTo] nav_feedback_ pointer is null!");
    lock.unlock();
    return false;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;

  if (!as_)
  {
    robot::log_error("[MoveBase::moveStraightTo] 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;

    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::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
    robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
                    action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);

    // Clear Order message since this is a non-Order moveStraightTo call
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_.reset();
    }

    robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
    as_->processGoal(action_goal);
    robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
  }
  catch (const std::exception &e)
  {
    lock.unlock();
    robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
    return false;
  }

  lock.unlock();
  return true;
}

bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
{
  if (setup_)
  {
    std::string global_planner = "TwoPointsPlanner";
    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, rotate_planner_name_);
    if(nh_planner.hasParam("base_global_planner"))
    {
      std::string base_global_planner;
      nh_planner.getParam("base_global_planner", base_global_planner);
      swapPlanner(base_global_planner);
    }
    else
    {
      swapPlanner(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;
  }

  this->setXyGoalTolerance(std::numeric_limits<double>::infinity());

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  if (!tc_)
  {
    robot::log_error("[MoveBase::rotateTo] tc_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  if (!controller_costmap_robot_)
  {
    robot::log_error("[MoveBase::rotateTo] controller_costmap_robot_ pointer is null");
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));

  try
  {
    if (!tc_->swapPlanner(rotate_planner_name_))
    {
      robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner");
      lock.unlock();
      return false;
    }
  }
  catch (const std::exception &e)
  {
    robot::log_error("[MoveBase::rotateTo] Exception in swapPlanner: %s", e.what());
    lock.unlock();
    return false;
  }

  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::rotateTo] nav_feedback_ pointer is null!");
    lock.unlock();
    return false;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;

  if (!as_)
  {
    robot::log_error("[MoveBase::rotateTo] as_ pointer is null!");
    lock.unlock();
    return false;
  }

  try
  {
    robot_geometry_msgs::Pose2D pose;
    if (!this->getRobotPose(pose))
    {
      if (nav_feedback_)
      {
        nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
        nav_feedback_->feed_back_str = std::string("Couldn't get robot pose");
        nav_feedback_->goal_checked = false;
      }
      lock.unlock();
      return 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;
    double distance = planner_costmap_robot_ ? planner_costmap_robot_->getCostmap()->getResolution() : 0.05;
    action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
    action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);

    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::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
    robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
                    action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);

    // Clear Order message since this is a non-Order rotateTo call
    {
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_.reset();
    }

    robot::log_info("[MoveBase::rotateTo] Processing goal through action server...");
    as_->processGoal(action_goal);
    robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
  }
  catch (const std::exception &e)
  {
    lock.unlock();
    robot::log_error("[MoveBase::rotateTo] Exception during processGoal: %s", e.what());
    return false;
  }

  lock.unlock();
  return true;
}

void move_base::MoveBase::pause()
{
  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()));
  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }

  try
  {
    old_linear_fwd_ = tc_->getTwistLinear(true);
    old_linear_bwd_ = tc_->getTwistLinear(false);

    old_angular_fwd_ = tc_->getTwistAngular(true);
    old_angular_rev_ = tc_->getTwistAngular(false);
  }
  catch (const std::exception &e)
  {
    std::cerr << e.what() << '\n';
  }
  pause_ctr_ = true;
  paused_ = false;
}

void move_base::MoveBase::resume()
{
  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()));
  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  tc_->setTwistLinear(old_linear_fwd_);
  tc_->setTwistLinear(old_linear_bwd_);

  tc_->setTwistAngular(old_angular_fwd_);
  tc_->setTwistAngular(old_angular_rev_);

  pause_ctr_ = false;
  paused_ = false;
}

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");
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
{
  if (!initialized_)
    return false;
  this->getRobotPose(pose, planner_costmap_robot_);
  return true;
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::Pose2D &pose)
{
  if (!initialized_)
    return false;
  robot_geometry_msgs::PoseStamped global_pose;
  this->getRobotPose(global_pose, planner_costmap_robot_);
  pose.x = global_pose.pose.position.x;
  pose.y = global_pose.pose.position.y;

  tf3::Quaternion quaternion = tf3::Quaternion(
      global_pose.pose.orientation.x,
      global_pose.pose.orientation.y,
      global_pose.pose.orientation.z,
      global_pose.pose.orientation.w);

  pose.theta = tf3::getYaw(quaternion);
  return true;
}

bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &linear)
{
  try
  {
    if (tc_ && !cancel_ctr_)
    {
      if (tc_->islock())
        return true;
      return tc_->setTwistLinear(linear);
    }
    else if (tc_ && cancel_ctr_)
    {
      tc_->unlock();
      return false;
    }
    else
    {
      throw std::runtime_error("Null 'tc_' pointer encountered");
    }
  }
  catch (const std::exception &e)
  {
    return false;
  }
}

bool move_base::MoveBase::setTwistAngular(const robot_geometry_msgs::Vector3 &angular)
{
  if (tc_ && !cancel_ctr_)
  {
    if (tc_->islock())
      return true;
    return tc_->setTwistAngular(angular);
  }
  else if (tc_ && cancel_ctr_)
  {
    return false;
  }
  else
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  return false;
}

void move_base::MoveBase::setYawGoalTolerance(double yaw_goal_tolerance)
{
  double yaw = 0.0;
  if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0))
  {
    private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance);
    robot::log_info("yaw_goal_tolerance %s is %f", private_nh_.getNamespace().c_str(), yaw_goal_tolerance);
  }
  else
    robot::log_error("yaw_goal_tolerance couldn't get namespace is %s", private_nh_.getNamespace().c_str());
}

void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance)
{
  double xy = 0.0;
  if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0))
  {
    private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance);
    robot::log_info("xy_goal_tolerance %s is %f", private_nh_.getNamespace().c_str(), xy_goal_tolerance);
  }
  else
    robot::log_error("xy_goal_tolerance couldn't get namespace is %s", private_nh_.getNamespace().c_str());
}

bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
  // make sure to set the plan to be empty initially
  plan.clear();

  // since this gets called on handle activate
  if (!planner_costmap_robot_)
  {
    robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
    return false;
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));

  // get the starting pose of the robot
  robot_geometry_msgs::PoseStamped global_pose;
  if (!getRobotPose(global_pose, planner_costmap_robot_))
  {
    robot::log_error("Unable to get starting pose of robot, unable to create global plan");
    return false;
  }

  const robot_geometry_msgs::PoseStamped &start = global_pose;

  // if the planner fails or returns a zero length plan, planning failed
  if (!planner_->makePlan(start, goal, plan) || plan.empty())
  {
    robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
    return false;
  }
  else
  {
    global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
  }
  lock.unlock();
  return true;
}

bool move_base::MoveBase::makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
  // make sure to set the plan to be empty initially
  plan.clear();

  // since this gets called on handle activate
  if (!planner_costmap_robot_)
  {
    robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
    return false;
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));

  // get the starting pose of the robot
  robot_geometry_msgs::PoseStamped global_pose;
  if (!getRobotPose(global_pose, planner_costmap_robot_))
  {
    robot::log_error("Unable to get starting pose of robot, unable to create global plan");
    return false;
  }

  const robot_geometry_msgs::PoseStamped &start = global_pose;

  // if the planner fails or returns a zero length plan, planning failed
  if (!planner_->makePlan(msg, start, goal, plan) || plan.empty())
  {
    robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
    return false;
  }
  else
  {
    global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
  }
  lock.unlock();
  return true;
}
bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
{
  YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
  if (behavior_list && behavior_list.IsSequence())
  {
    // Validate all behaviors first
    for (size_t i = 0; i < behavior_list.size(); ++i)
    {
      YAML::Node behavior = behavior_list[i].as<YAML::Node>();
      if (!behavior.IsMap())
      {
        robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead.");
        return false;
      }

      if (!behavior["name"] || !behavior["type"])
      {
        robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead.");
        return false;
      }

      // check for recovery behaviors with the same name
      std::string name_i = behavior["name"].as<std::string>();
      for (size_t j = i + 1; j < behavior_list.size(); ++j)
      {
        YAML::Node behavior_j = behavior_list[j].as<YAML::Node>();
        if (behavior_j.IsMap() && behavior_j["name"])
        {
          std::string name_j = behavior_j["name"].as<std::string>();
          if (name_i == name_j)
          {
            robot::log_warning("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", name_i.c_str());
            return false;
          }
        }
      }
    }

    // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
    for (size_t i = 0; i < behavior_list.size(); ++i)
    {
      try
      {
        YAML::Node behavior = behavior_list[i].as<YAML::Node>();
        // Load recovery behavior using boost::dll::import_alias
        // 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>();
        robot::PluginLoaderHelper loader;
        std::string path_file_so = loader.findLibraryPath(behavior_type);
        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
        // Load the factory function from the shared library
        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_loaders_[behavior_name]());

        // shouldn't be possible, but it won't hurt to check
        if (behavior_ptr == nullptr)
        {
          std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl;
          return false;
        }
        // initialize the recovery behavior with its name
        behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
        recovery_behavior_names_.push_back(behavior_name);
        recovery_behaviors_.push_back(behavior_ptr);
      }
      catch (const std::exception &ex)
      {
        std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>()
                  << ". Error: " << ex.what() << std::endl;
        return false;
      }
    }
  }
  else if (behavior_list)
  {
    std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl;
    return false;
  }
  else
  {
    // if no recovery_behaviors are specified, we'll just load the defaults
    return false;
  }

  // if we've made it here... we've constructed a recovery behavior list successfully
  return true;
}

void move_base::MoveBase::loadDefaultRecoveryBehaviors()
{
  recovery_behaviors_.clear();
  recovery_behavior_names_.clear();

  try
  {
    // Note: Parameters should be set via YAML config file, not programmatically
    // The recovery behaviors will read their parameters from the YAML node during initialization
    // first, we'll load a recovery behavior to clear the costmap (conservative reset)
    try
    {
      std::string path_file_so;
      auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
          path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
      std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
      cons_clear->initialize("conservative_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
      recovery_behavior_names_.push_back("conservative_reset");
      recovery_behaviors_.push_back(cons_clear);
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d] Failed to load clear_costmap_recovery plugin: %s", __FILE__, __LINE__, ex.what());
    }

    // next, we'll load a recovery behavior to rotate in place
    std::shared_ptr<robot_nav_core::RecoveryBehavior> rotate;
    if (clearing_rotation_allowed_)
    {
      try
      {
        std::string path_file_so;
        auto rotate_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
            path_file_so, "rotate_recovery", boost::dll::load_mode::append_decorations);
        rotate = std::shared_ptr<robot_nav_core::RecoveryBehavior>(rotate_loader());
        rotate->initialize("rotate_recovery", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
        recovery_behavior_names_.push_back("rotate_recovery");
        recovery_behaviors_.push_back(rotate);
      }
      catch (const std::exception &ex)
      {
        robot::log_error("[%s:%d] Failed to load rotate_recovery plugin: %s", __FILE__, __LINE__, ex.what());
      }
    }

    // next, we'll load a recovery behavior that will do an aggressive reset of the costmap
    try
    {
      std::string path_file_so;
      auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
          path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
      std::shared_ptr<robot_nav_core::RecoveryBehavior> ags_clear(clear_costmap_loader());
      ags_clear->initialize("aggressive_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
      recovery_behavior_names_.push_back("aggressive_reset");
      recovery_behaviors_.push_back(ags_clear);
    }
    catch (const std::exception &ex)
    {
      std::cerr << "Failed to load clear_costmap_recovery plugin for aggressive reset: " << ex.what() << std::endl;
    }

    // we'll rotate in-place one more time
    if (clearing_rotation_allowed_ && rotate)
    {
      recovery_behaviors_.push_back(rotate);
      recovery_behavior_names_.push_back("rotate_recovery");
    }
  }
  catch (const std::exception &ex)
  {
    std::cerr << "Failed to load default recovery behaviors. Error: " << ex.what() << std::endl;
  }

  return;
}

void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
{
  robot_geometry_msgs::PoseStamped global_pose;

  // clear the planner's costmap
  getRobotPose(global_pose, planner_costmap_robot_);

  std::vector<robot_geometry_msgs::Point> clear_poly;
  double x = global_pose.pose.position.x;
  double y = global_pose.pose.position.y;
  robot_geometry_msgs::Point pt;

  pt.x = x - size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x - size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);

  // clear the controller's costmap
  getRobotPose(global_pose, controller_costmap_robot_);

  clear_poly.clear();
  x = global_pose.pose.position.x;
  y = global_pose.pose.position.y;

  pt.x = x - size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x - size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
}

void move_base::MoveBase::publishZeroVelocity()
{
  twist_.velocity.x = 0.0;
  twist_.velocity.y = 0.0;
  twist_.velocity.theta = 0.0;
  twist_.header.stamp = robot::Time::now();
  twist_.header.frame_id = robot_base_frame_;
}

void move_base::MoveBase::resetState()
{
  // Disable the planner thread
  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  runPlanner_ = false;
  lock.unlock();

  // Reset statemachine
  state_ = move_base::PLANNING;

  recovery_index_ = 0;
  recovery_trigger_ = PLANNING_R;
  paused_ = false;
  publishZeroVelocity();

  // if we shutdown our costmaps when we're deactivated... we'll do that now
  if (shutdown_costmaps_)
  {
    robot::log_info("Stopping costmaps");
    planner_costmap_robot_->stop();
    controller_costmap_robot_->stop();
  }
}

void move_base::MoveBase::wakePlanner(const robot::TimerEvent &event)
{

  // Nếu timer bị delay quá nhiều, có thể bỏ qua
  robot::Duration drift = event.current_real - event.current_expected;
  
  if (drift.toSec() > 0.5)  // Delay quá nhiều
  {
    // Có thể skip một số công việc không quan trọng
    return;
  }
  // we have slept long enough for rate
  planner_cond_.notify_one();
}

void move_base::MoveBase::planThread()
{
  robot::Timer timer;
  bool wait_for_wake = false;
  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  while (true)
  {
    // check if we should run the planner (the mutex is locked)
    while (wait_for_wake || !runPlanner_)
    {
      // if we should not be running the planner then suspend this thread
      robot::log_info("Planner thread is suspending");
      planner_cond_.wait(lock);
      wait_for_wake = false;
    }
    robot::Time start_time = robot::Time::now();

    // time to plan! get a copy of the goal and Order message (if available) and unlock the mutex
    robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
    boost::shared_ptr<robot_protocol_msgs::Order> temp_order = planner_order_;
    lock.unlock();
    robot::log_info("Planning...");
    // run planner
    planner_plan_->clear();
    // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
    
    // Choose makePlan based on whether Order message is available
    bool gotPlan = false;
    if (temp_order)
    {
      // Use Order-based makePlan
      robot::log_info("[MoveBase::planThread] Using Order-based planning");
      gotPlan = makePlan(*temp_order, temp_goal, *planner_plan_);
    }
    else
    {
      // Use standard makePlan without Order
      robot::log_info("[MoveBase::planThread] Using standard planning (no Order)");
      gotPlan = makePlan(temp_goal, *planner_plan_);
    }

    if (gotPlan)
    {
      robot::log_info("Got Plan with %d points!", planner_plan_->size());
      // pointer swap the plans under mutex (the controller will pull from latest_plan_)
      std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;

      lock.lock();
      planner_plan_ = latest_plan_;
      latest_plan_ = temp_plan;
      last_valid_plan_ = robot::Time::now();
      planning_retries_ = 0;
      new_global_plan_ = true;

      robot::log_info("Generated a plan from the base_global_planner");

      // make sure we only start the controller if we still haven't reached the goal
      if (runPlanner_)
      {
        state_ = move_base::CONTROLLING;
        if (nav_feedback_)
          nav_feedback_->navigation_state = robot::move_base_core::State::CONTROLLING;
      }
      if (planner_frequency_ <= 0)
        runPlanner_ = false;
      lock.unlock();
    }
    // if we didn't get a plan and we are in the planning state (the robot isn't moving)
    else if (state_ == move_base::PLANNING)
    {
      robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);

      // check if we've tried to make a plan for over our time limit or our maximum number of retries
      // issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
      // is negative (the default), it is just ignored and we have the same behavior as ever
      lock.lock();
      planning_retries_++;
      if (runPlanner_ &&
          (robot::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)))
      {
        // we'll move into our obstacle clearing mode
        state_ = move_base::CLEARING;
        if (nav_feedback_)
          nav_feedback_->navigation_state = robot::move_base_core::State::CLEARING;

        runPlanner_ = false; // proper solution for issue #523
        publishZeroVelocity();
        recovery_trigger_ = PLANNING_R;
      }

      lock.unlock();
    }

    // take the mutex for the next iteration
    lock.lock();

    // setup sleep interface if needed
    if (planner_frequency_ > 0)
    {
      robot::Duration sleep_time = (start_time + robot::Duration(1.0 / planner_frequency_)) - robot::Time::now();
      if (sleep_time > robot::Duration(0.0))
      {
        wait_for_wake = true;
        timer = robot::Timer(sleep_time, &move_base::MoveBase::wakePlanner, this);
      }
    }
  }
}

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::log_warning("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);
  robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
  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;
        // Clear Order message for new goal (will be set if moveTo with Order is called)
        planner_order_.reset();
        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;
      // Clear Order message for new goal (will be set if moveTo with Order is called)
      planner_order_.reset();
      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());
      // Clear Order message for new goal (will be set if moveTo with Order is called)
      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
      planner_order_.reset();
    }
  );

  // 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)
{
  // first we need to check if the quaternion has nan's or infs
  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
  {
    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
    return false;
  }
  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);

  // next, we need to check if the length of the quaternion is close to zero
  if (tf_q.length2() < 1e-6)
  {
    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
    return false;
  }

  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
  tf_q.normalize();

  tf3::Vector3 up(0, 0, 1);

  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

  if (fabs(dot - 1) > 1e-3)
  {
    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
    return false;
  }

  return true;
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
{
  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
  robot_geometry_msgs::PoseStamped robot_pose;
  tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
  robot_pose.header.frame_id = robot_base_frame_;
  robot_pose.header.stamp = robot::Time();       // latest available
  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later

  // get robot pose on the given costmap frame
  try
  {
    tf3::TransformStampedMsg transform = tf_->lookupTransform(costmap->getGlobalFrameID(), robot_base_frame_, tf3::Time());
    tf3::doTransform(robot_pose, global_pose, transform);
  }
  catch (tf3::LookupException &ex)
  {
    robot::log_error("Move Base No Transform available Error looking up robot pose: %s", ex.what());
    return false;
  }
  catch (tf3::ConnectivityException &ex)
  {
    robot::log_error("Move Base Connectivity Error looking up robot pose: %s", ex.what());
    return false;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    robot::log_error("Move Base Extrapolation Error looking up robot pose: %s", ex.what());
    return false;
  }

  // check if global_pose time stamp is within costmap transform tolerance
  if (!global_pose.header.stamp.isZero() &&
      current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
  {
    robot::log_error("Transform timeout for %s. Current time: %f, pose stamp: %f, tolerance: %f", costmap->getName().c_str(), current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
    return false;
  }

  return true;
}

double move_base::MoveBase::distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2)
{
  return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}

bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
  boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
  // we need to be able to publish velocity commands
  robot_geometry_msgs::Twist cmd_vel;

  // update feedback to correspond to our curent position
  robot_geometry_msgs::PoseStamped global_pose;
  getRobotPose(global_pose, planner_costmap_robot_);
  robot_geometry_msgs::Pose2D pose2d;
  pose2d.x = global_pose.pose.position.x;
  pose2d.y = global_pose.pose.position.y;
  // Extract yaw from quaternion
  tf3::Quaternion q(global_pose.pose.orientation.x, global_pose.pose.orientation.y,
                     global_pose.pose.orientation.z, global_pose.pose.orientation.w);
  pose2d.theta = tf3::getYaw(q);
  
  if (nav_feedback_)
  {
    nav_feedback_->current_pose = pose2d;
  }
  const robot_geometry_msgs::PoseStamped &current_position = global_pose;

  // // push the feedback out
  // robot_move_base_msgs::MoveBaseFeedback feedback;
  // feedback.base_position = current_position;
  // as_->publishFeedback(feedback);

  // check to see if we've moved far enough to reset our oscillation timeout
  if (distance(current_position, oscillation_pose_) >= oscillation_distance_)
  {
    last_oscillation_reset_ = robot::Time::now();
    oscillation_pose_ = current_position;

    // if our last recovery was caused by oscillation, we want to reset the recovery index
    if (recovery_trigger_ == OSCILLATION_R)
      recovery_index_ = 0;
  }

  // check that the observation buffers for the costmap are current, we don't want to drive blind
  if (!controller_costmap_robot_->isCurrent())
  {
    robot::log_warning("Sensor data is out of date, we're not going to allow commanding of the base for safety");
    publishZeroVelocity();
    return false;
  }

  // if we have a new plan then grab it and give it to the controller
  if (new_global_plan_)
  {
    // make sure to set the new plan flag to false
    new_global_plan_ = false;

    robot::log_debug("Got a new plan...swap pointers");

    // do a pointer swap under mutex
    std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = controller_plan_;

    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    controller_plan_ = latest_plan_;
    latest_plan_ = temp_plan;
    lock.unlock();
    robot::log_debug("pointers swapped!");

    if (!tc_->setPlan(*controller_plan_))
    {
      // ABORT and SHUTDOWN COSTMAPS
      robot::log_error("Failed to pass global plan to the controller, aborting.");
      resetState();

      // disable the planner thread
      lock.lock();
      runPlanner_ = false;
      lock.unlock();

      if (nav_feedback_)
      {
        nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
        nav_feedback_->feed_back_str = std::string("Failed to pass global plan to the controller.");
      }
      // as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
      return true;
    }

    // make sure to reset recovery_index_ since we were able to find a valid plan
    if (recovery_trigger_ == PLANNING_R)
      recovery_index_ = 0;
  }


  // Cancle executeCycle
  if (cancel_ctr_ && tc_)
  {
    robot_geometry_msgs::Vector3 linear;
    // ROS_INFO_THROTTLE(1.0,"MoveTo is Canling ....");
    tc_->setTwistLinear(linear);
    try
    {
      double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
      if (actual_linear_velocity <= min_approach_linear_velocity_)
      {
        robot::log_info("MoveTo is canceled.");
        resetState();
        cancel_ctr_ = false;
        // if(default_config_.base_global_planner != last_config_.base_global_planner)
        robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
        if(nh_planner.hasParam("base_global_planner"))
        {
          std::string base_global_planner;
          nh_planner.getParam("base_global_planner", base_global_planner);
          swapPlanner(base_global_planner);
        }
        else
        {
          swapPlanner(default_config_.base_global_planner);
        }
        cancel_ctr_ = false;
        // disable the planner thread
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
        lock.unlock();
        if (nav_feedback_)
        {
          nav_feedback_->feed_back_str = std::string("Canled.");
          nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
        }
        // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");

        if (as_)
        {
          robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
          msg->id = "";  // Empty string cancels current goal
          msg->stamp = robot::Time::now();
          // 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");
        }
        return true;
      }
    }
    catch (const std::exception &e)
    {
      robot::log_info("MoveTo is canceled.");
      resetState();
      // if(default_config_.base_global_planner != last_config_.base_global_planner)
      robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
      if(nh_planner.hasParam("base_global_planner"))
      {
        std::string base_global_planner;
        nh_planner.getParam("base_global_planner", base_global_planner);
        swapPlanner(base_global_planner);
      }
      else
      {
        swapPlanner(default_config_.base_global_planner);
      }

      // disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();
      if (nav_feedback_)
      {
        nav_feedback_->feed_back_str = std::string("Canled.");
        nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
      }
      // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
      if (as_)
      {
        robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
        msg->id = "";  // Empty string cancels current goal
        msg->stamp = robot::Time::now();
        // 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");
      }
      cancel_ctr_ = false;
      return true;
    }
  }
  // the move_base state machine, handles the control logic for navigation
  switch (state_)
  {
    // if we are in a planning state, then we'll attempt to make a plan
  case move_base::PLANNING:
  {
    boost::recursive_mutex::scoped_lock lock(planner_mutex_);
    runPlanner_ = true;
    cancel_ctr_ = false;
    planner_cond_.notify_one();
  }
    robot::log_debug("Waiting for plan, in the planning state.");
    break;

  // if we're controlling, we'll attempt to find valid velocity commands
  case move_base::CONTROLLING:

    // check to see if we've reached our goal
    try
    {
      if (tc_->isGoalReached())
      {
        robot::log_debug("[MoveBase] Goal reached!");
        resetState();
        // swapPlanner(default_config_.base_global_planner);
        // disable the planner thread
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
        lock.unlock();

        if (nav_feedback_)
        {
          nav_feedback_->feed_back_str = std::string("Goal reached.");
          nav_feedback_->goal_checked = true;
          nav_feedback_->navigation_state = robot::move_base_core::State::SUCCEEDED;
        }
        publishZeroVelocity();
        // as_->setSucceeded(robot_move_base_msgs::MoveBaseResult(), "Goal reached.");
        return true;
      }
    }
    catch (const std::exception &e)
    {
      // disable the planner thread
      resetState();
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();
      // as_->setAborted(robot_move_base_msgs::MoveBaseResult(), e.what());
      if (nav_feedback_)
      {
        nav_feedback_->feed_back_str = e.what();
        nav_feedback_->goal_checked = false;
        nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
        robot::log_error("[Move_base] %s", e.what());
      }
      return true;
    }

    // check for an oscillation condition
    if (oscillation_timeout_ > 0.0 &&
        last_oscillation_reset_ + robot::Duration(oscillation_timeout_) < robot::Time::now())
    {
      publishZeroVelocity();
      state_ = move_base::CLEARING;
      robot::log_warning("Oscillation detected.  Clearing costmap. %f seconds without progress.", (robot::Time::now() - last_oscillation_reset_).toSec());
      if (nav_feedback_)
        nav_feedback_->navigation_state = robot::move_base_core::State::CLEARING;
      recovery_trigger_ = OSCILLATION_R;
    }

    {
      boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
      if (!paused_)
      {
        if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
        {
          robot_nav_msgs::Path path;
          tc_->getPlan(path.poses);
          if (!path.poses.empty())
          {
            path.header.stamp = path.poses[0].header.stamp;
            path.header.frame_id = path.poses[0].header.frame_id;
          }
          else
          {
            path.header.stamp = robot::Time::now();
            path.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
          }
          local_data_.plan = robot_nav_2d_utils::pathToPath(path);

          last_valid_control_ = robot::Time::now();
          // make sure that we send the velocity command to the base

          if (nav_feedback_)
          {
            nav_feedback_->navigation_state = robot::move_base_core::State::CONTROLLING;
            nav_feedback_->feed_back_str = std::string("");
            nav_feedback_->goal_checked = false;
          }
          if (recovery_trigger_ == CONTROLLING_R)
            recovery_index_ = 0;
        }
        else
        {
          robot::log_warning_at(__FILE__, __LINE__, "The local planner could not find a valid plan.");
          robot::Time attempt_end = last_valid_control_ + robot::Duration(controller_patience_);

          // check if we've tried to find a valid control for longer than our time limit
          if (robot::Time::now() > attempt_end)
          {
            // we'll move into our obstacle clearing mode
            publishZeroVelocity();
            state_ = move_base::CLEARING;
  
            if (nav_feedback_)
            {
              nav_feedback_->navigation_state = robot::move_base_core::State::CLEARING;
              nav_feedback_->feed_back_str = std::string("The local planner could not find a valid plan.");
            }
            recovery_trigger_ = CONTROLLING_R;
          }
          else
          {
            // otherwise, if we can't find a valid control, we'll go back to planning
            last_valid_plan_ = robot::Time::now();
            planning_retries_ = 0;
            state_ = move_base::PLANNING;
            if (nav_feedback_)
              nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
            publishZeroVelocity();

            // enable the planner thread in case it isn't running on a clock
            boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
            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();
                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 false;
                }
                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);
                planner_goal_ = goal;
              }
            }
            runPlanner_ = true;
            cancel_ctr_ = false;
            planner_cond_.notify_one();
            lock.unlock();
          }
        }
        twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel);
        twist_.header.stamp = robot::Time::now();
        twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
      }
      else
      {
        if (nav_feedback_)
        {
          nav_feedback_->navigation_state = robot::move_base_core::State::PAUSED;
          nav_feedback_->feed_back_str = std::string("Paused");
          nav_feedback_->goal_checked = false;
        }
      }

      if (!paused_ && pause_ctr_)
      {
        // ROS_WARN("pause_ctr_");
        robot_geometry_msgs::Vector3 linear;
        tc_->setTwistLinear(linear);
        try
        {
          double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
          if (actual_linear_velocity <= min_approach_linear_velocity_)
          {
            paused_ = true;
          }
        }
        catch (const std::exception &e)
        {
          paused_ = true;
        }
      }
    }
    break;

  // we'll try to clear out space with any user-provided recovery behaviors
  case move_base::CLEARING:
    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("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size());
      recovery_behaviors_[recovery_index_]->runBehavior();

      // we at least want to give the robot some time to stop oscillating after executing the behavior
      last_oscillation_reset_ = robot::Time::now();

      // we'll check if the recovery behavior actually worked
      robot::log_debug("Going back to planning state");
      last_valid_plan_ = robot::Time::now();
      planning_retries_ = 0;
      state_ = move_base::PLANNING;
      if (nav_feedback_)
        nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;

      // update the index of the next recovery behavior that we'll try
      recovery_index_++;
    }
    else
    {
      robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
      resetState();
      // swapPlanner(default_config_.base_global_planner);

      // disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();

      robot::log_debug("Something should abort after this.");

      if (recovery_trigger_ == CONTROLLING_R)
      {
        robot::log_error("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
        if (nav_feedback_)
        {
          nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
          nav_feedback_->feed_back_str = std::string("Failed to find a valid control. Even after executing recovery behaviors.");
        }
        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
      }
      else if (recovery_trigger_ == PLANNING_R)
      {
        robot::log_error("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
        if (nav_feedback_)
        {
          nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
          nav_feedback_->feed_back_str = "Failed to find a valid plan. Even after executing recovery behaviors.";
        }
        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
      }
      else if (recovery_trigger_ == OSCILLATION_R)
      {
        robot::log_error("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
        if (nav_feedback_)
        {
          nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
          nav_feedback_->feed_back_str = std::string("Robot is oscillating. Even after executing recovery behaviors.");
        }
        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
      }
      return true;
    }
    break;
  default:
    robot::log_error("This case should never be reached, something is wrong, aborting");
    resetState();
    // disable the planner thread
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    runPlanner_ = false;
    lock.unlock();
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;  
      nav_feedback_->feed_back_str = std::string("Reached a case that should not be hit in move_base. This is a bug, please report it.");
    }
    as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
    return true;
  }

  // we aren't done yet
  return false;
}

robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
{

  if (!tf_)
  {
    return goal_pose_msg;
  }

  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
  robot_geometry_msgs::PoseStamped global_pose;
  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
  robot_geometry_msgs::PoseStamped goal_pose;
  tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
  goal_pose = goal_pose_msg;
  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
  tf3::Time tf3_current_time = data_convert::convertTime(current_time);
  tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
  
  try
  {
    // use current time if possible (makes sure it's not in the future)
    std::string error_msg;
    if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
    {
      // Transform is available at current time
      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
      tf3::doTransform(goal_pose, global_pose, transform);
    }
    // use the latest otherwise (tf3::Time() means latest available)
    else
    {
      // Try to get latest transform
      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
      tf3::doTransform(goal_pose, global_pose, transform);
    }
  }
  catch (tf3::LookupException &ex)
  {
    robot::log_error("Move Base No Transform available Error looking up robot pose: %s", ex.what());
    return goal_pose_msg;
  }
  catch (tf3::ConnectivityException &ex)
  {
    robot::log_error("Move Base Connectivity Error looking up robot pose: %s", ex.what());
    return goal_pose_msg;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    robot::log_error("Move Base Extrapolation Error looking up robot pose: %s", ex.what());
    return goal_pose_msg;
  }
  return global_pose;
}

robot_nav_2d_msgs::Twist2DStamped move_base::MoveBase::getTwist()
{
  return twist_;
}

robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
{
  if (!nav_feedback_)
  {
    return nullptr;
  }
  return nav_feedback_.get();
}

robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
{
  if (tc_)
  {
    robot_nav_msgs::Path path;
    tc_->getGlobalPlan(path.poses);
    robot_nav_msgs::Path global_path;
    global_path.header.stamp = robot::Time::now();
    global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
    for(auto &p : path.poses)
    {
      robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
      pose.header.stamp = robot::Time::now();
      global_path.poses.push_back(goalToGlobalFrame(pose));
    }
    global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
  }
  ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
  convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);
  convert_data.updateFootprint(global_data_.footprint);
  return global_data_;
}

robot::move_base_core::PlannerDataOutput move_base::MoveBase::getLocalData()
{
  ConvertData convert_data(controller_costmap_robot_, controller_costmap_robot_->getGlobalFrameID(), true);
  convert_data.updateCostmap(local_data_.costmap, local_data_.costmap_update, local_data_.is_costmap_updated);
  convert_data.updateFootprint(local_data_.footprint);
  return local_data_;
}

robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
{
  return std::make_shared<move_base::MoveBase>();
}

BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase)
