#include <pnkx_local_planner/pnkx_docking_local_planner.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/footprint.h>
#include <angles/angles.h>
#include <robot_nav_msgs/Path.h>
#include <robot_std_msgs/Bool.h>
#include <robot_std_msgs/UInt16.h>
#include <robot_visualization_msgs/MarkerArray.h>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>

pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner()
  : PNKXLocalPlanner(),
    start_docking_(false),
    original_xy_goal_tolerance_(0.0),
    original_yaw_goal_tolerance_(0.0)
{
}

pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner()
{
  if (traj_generator_)
    traj_generator_.reset();

  if (nav_algorithm_)
    nav_algorithm_.reset();

  if (rotate_algorithm_)
    rotate_algorithm_.reset();

  if (goal_checker_)
    goal_checker_.reset();
}

void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
  if (!initialized_)
  {
    tf_ = tf;
    name_ = name;
    costmap_robot_ = costmap_robot;
    costmap_ = costmap_robot_->getCostmap();

    info_.width = costmap_->getSizeInCellsX();
    info_.height = costmap_->getSizeInCellsY();
    info_.resolution = costmap_->getResolution();
    info_.frame_id = costmap_robot_->getGlobalFrameID();
    info_.origin_x = costmap_->getOriginX();
    info_.origin_y = costmap_->getOriginY();

    if (!costmap_robot_->isCurrent())
      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");

    parent_ = parent;
    planner_nh_ = robot::NodeHandle(parent_, name);
    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
    this->getParams(planner_nh_);

    std::string traj_generator_name;
    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator"));
    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(traj_generator_name);
      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
      traj_generator_ = traj_gen_loader_();
      if (!traj_generator_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
      }
      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
      traj_generator_->initialize(nh_traj_gen);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
    }

    std::string algorithm_nav_name; 
    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
      nav_algorithm_ = nav_algorithm_loader_();
      if (!nav_algorithm_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
      }
      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
    }

    std::string algorithm_rotate_name;
    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
      rotate_algorithm_ = rotate_algorithm_loader_();
      if (!rotate_algorithm_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
      }
      rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
    }

    std::string goal_checker_name;
    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
      goal_checker_ = goal_checker_loader_();
      if (!goal_checker_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
      }
      goal_checker_->initialize(parent_);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
    }

    this->initializeOthers();
    this->getMaker();
    robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
    initialized_ = true;
  }
}

void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
{
  std::string maker_name, maker_sources;
  parent_.param("maker_name", maker_name, std::string(""));
  parent_.param("maker_sources", maker_sources, std::string(""));
  robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
  robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
  std::stringstream ss(maker_sources);
  std::string source;

  while (ss >> source)
  {
    if (maker_name == source)
    {
      robot::NodeHandle source_node(parent_, source);

      if (source_node.hasParam("plugins"))
      {
        robot_xmlrpcpp::XmlRpcValue plugins;
        source_node.getParam("plugins", plugins);

        if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
        {
          for (int i = 0; i < plugins.size(); ++i)
          {
            if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
            {
              std::stringstream name;
              name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
              std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
              std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
              // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
              DockingPlanner* dkpl = new DockingPlanner(name.str());
              dkpl->docking_planner_name_ = docking_planner_name;
              dkpl->docking_nav_name_ = docking_nav_name;
              dkpl_.push_back(dkpl);
            }
          }
        }
        else
        {
          robot::log_warning_at(__FILE__, __LINE__, "Expected 'plugins' to be an array under namespace [%s]", source.c_str());
        }
      }
      else
      {
        robot::log_warning_at(__FILE__, __LINE__, "No 'plugins' found under namespace [%s]", source.c_str());
      }
      break;
    }
  }
}

void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
{
  std::string algorithm_nav_name;
  planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
  if (!parent_.getParam(algorithm_nav_name, original_papams_))
    robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
  original_xy_goal_tolerance_ = xy_goal_tolerance_;
  original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
}

void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &nh)
{
  // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
  nh.param("update_costmap_before_planning", update_costmap_before_planning_, true);
  if (!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
  {
    xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
  }
  if (!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
  {
    yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
  }
}

void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
{
  robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received.");
  this->unlock();
  if(traj_generator_) traj_generator_->reset();
  if(goal_checker_) goal_checker_->reset();
  if(nav_algorithm_) nav_algorithm_->reset();
  if(rotate_algorithm_) rotate_algorithm_->reset();
  ret_nav_ = ret_angle_ = false;

  std::string algorithm_nav_name;
  planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));

  parent_.setParam(algorithm_nav_name, original_papams_);
  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
  nh_algorithm.setParam("allow_rotate", false);
}

void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{  
  this->getParams(planner_nh_);
  if (update_costmap_before_planning_)
  {
    info_.width = costmap_->getSizeInCellsX();
    info_.height = costmap_->getSizeInCellsY();
    info_.resolution = costmap_->getResolution();
    info_.frame_id = costmap_robot_->getGlobalFrameID();
    info_.origin_x = costmap_->getOriginX();
    info_.origin_y = costmap_->getOriginY();

    if (!costmap_robot_->isCurrent())
      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
  }

  // Update time stamp of goal pose
  // goal_pose_.header.stamp = pose.header.stamp;
  robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                             local_goal_pose = this->transformPoseToLocal(goal_pose_);
  if (start_docking_)
  {
    local_goal_pose = goal_pose_;
  }

  try
  {
    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
      robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
  }
  catch(const robot_nav_core2::LocalPlannerException& e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
  }

  double x_direction, y_direction, theta_direction;
  if (!ret_nav_)
  {
    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
    {
      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
    }
    // else
    //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);

    if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
    {
      this->lock();
      robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
      traj_generator_->setTwistLinear(linear);
      linear.x *= (-1);
      traj_generator_->setTwistLinear(linear);
      traj_generator_->setTwistAngular(dkpl_.front()->angular_);

      std::string algorithm_nav_name;
      planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
      nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
      nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
      nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
      nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
      nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);

      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);

      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
      {

        if (dkpl_.front()->following_)
        {
          robot_nav_2d_msgs::Pose2DStamped follow_pose;
          if (dkpl_.front()->geLocalGoal(local_goal_pose))
          {
            local_goal_pose = follow_pose;
          }
        }
        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
        {
          throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
          robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
        }
      }
    }
  }
  else
  {
    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
    {
      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
    }
    // else
    //   ROS_INFO("chieu %f %f %f", x_direction, y_direction, theta_direction);
  }

  int xytheta_direct[3];
  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
  traj_generator_->setDirect(xytheta_direct);
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
                                                                                                 const robot_nav_2d_msgs::Twist2D &velocity)
{
  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  robot::log_error("DEBUG 300");
  robot_nav_2d_msgs::Twist2DStamped cmd_vel;
  try
  {
    if (global_plan_.poses.empty())
      return cmd_vel;
    this->prepare(pose, velocity);
    cmd_vel = this->ScoreAlgorithm(pose, velocity);
    return cmd_vel;
  }
  catch (const robot_nav_core2::PlannerException &e)
  {
    robot::log_warning_at(__FILE__, __LINE__, "%s", e.what());
    throw robot_nav_core2::LocalPlannerException(e.what());
  }
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
  robot_nav_2d_msgs::Twist2D twist;
  mkt_msgs::Trajectory2D traj;
  robot_nav_2d_msgs::Twist2DStamped cmd_vel;
  cmd_vel.header.stamp = robot::Time::now();

  if (ret_nav_ && ret_angle_)
    return cmd_vel;
  else if (!ret_nav_)
  {
    traj = nav_algorithm_->calculator(pose, velocity);
    if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
    {
      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
      {
        traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
      }
    }
    local_plan_.header.stamp = robot::Time::now();
    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
    local_plan_ = robot_nav_2d_utils::pathToPath(path);
  }
  else
  {
    traj = rotate_algorithm_->calculator(pose, velocity);
    local_plan_.header.stamp = robot::Time::now();
    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
    local_plan_ = robot_nav_2d_utils::pathToPath(path);
  }

  cmd_vel.velocity = traj.velocity;
  return cmd_vel;
}

bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
  if (goal_pose_.header.frame_id == "")
  {
    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
    return false;
  }
  robot::log_error("DEBUG 400.1");
  bool dock_ok = dockingHanlde(pose, velocity);
  robot::log_error("DEBUG 400.2");
  // Update time stamp of goal pose
  // goal_pose_.header.stamp = pose.header.stamp;
  robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
  robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
  if (start_docking_)
  {
    local_goal = goal_pose_;
    if (!dkpl_.empty() && dkpl_.front()->initialized_)
    {
      if (dkpl_.front()->allow_rotate_)
        plan = robot_nav_2d_msgs::Path2D();
    }
  }

  if (!ret_nav_)
  {
    ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);

    if (ret_nav_)
    {
      robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
      robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
    }
  }

  if (ret_nav_ && !ret_angle_ && !dock_ok)
  {
    double delta_orient =
        fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
    ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_);
    if(ret_angle_)
      robot::log_warning_at(__FILE__, __LINE__, "xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_);
  }

  if (start_docking_)
  {
    double dx = local_goal.pose.x - local_pose.pose.x;
    double dy = local_goal.pose.y - local_pose.pose.y;
    double dtheta = local_goal.pose.theta - local_pose.pose.theta;
  }
  if (ret_nav_ && ret_angle_ && dock_ok)
  {
    // nh_.setParam("xy_goal_tolerance", original_xy_goal_tolerance_);
    // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
    std::string algorithm_nav_name;
    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
    robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
    parent_.setParam(algorithm_nav_name, original_papams_);
    nh_algorithm.setParam("allow_rotate", false);
  }
  return ret_nav_ && ret_angle_ && dock_ok;
}

bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
  if (!dkpl_.empty())
  {
    if (ret_nav_ && ret_angle_)
    {
      if (!dkpl_.front()->is_detected_)
      {
        if (!dkpl_.front()->initialized_)
        {
          dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
          dkpl_.front()->initialized_ = true;
        }
        else
        {
          if (dkpl_.front()->docking_planner_ && !dkpl_.front()->docking_nav_)
          {
            robot_nav_2d_msgs::Pose2DStamped local_goal;
            try
            {
              if (dkpl_.front()->geLocalGoal(local_goal))
              {
                robot::log_error("DEBUG 100");
                dkpl_.front()->is_detected_ = true;
                start_docking_ = true;
                robot_nav_msgs::Path path;
                robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
                dkpl_.front()->getLocalPath(local_pose, local_goal, path);
                this->setPlan(robot_nav_2d_utils::pathToPath(path));
                this->setGoalPose(local_goal);
              }
            }
            catch (const std::exception &e)
            {
              throw robot_nav_core2::LocalPlannerException(e.what());
            }
          }
          else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_)
          {
            robot_nav_2d_msgs::Pose2DStamped local_goal;
            try
            {
              if (dkpl_.front()->geLocalGoal(local_goal))
              {
                robot::log_error("DEBUG 200");
                dkpl_.front()->is_detected_ = true;
                start_docking_ = true;
                robot_nav_2d_msgs::Path2D path;
                robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
                path.header.stamp = robot::Time::now();
                path.header.frame_id = costmap_robot_->getGlobalFrameID();
                path.poses.push_back(local_pose);
                path.poses.push_back(local_goal);
                this->setPlan(path);
                this->setGoalPose(local_goal);
              }
            }
            catch (const std::exception &e)
            {
              throw robot_nav_core2::LocalPlannerException(e.what());
            }
          }
        }
      }
      else
      {
        if (dkpl_.front()->initialized_ && !dkpl_.front()->is_goal_reached_)
        {
          dkpl_.front()->is_goal_reached_ = true;
          robot::log_info_at(__FILE__, __LINE__, "%s is goal reached", dkpl_.front()->name_.c_str());
        }

        if (dkpl_.front()->initialized_ && dkpl_.front()->is_goal_reached_)
        {
          if(!dkpl_.empty()) 
          {
            // if(dkpl_.front()) delete(dkpl_.front());
            dkpl_.erase(dkpl_.begin());
          }
          start_docking_ = false;
        }
      }
    }
  }
  bool result = false;
  if (dkpl_.empty())
  {
    robot::log_info_at(__FILE__, __LINE__, "Goal reached!");
    result = true;
    this->getMaker();
  }
  return result;
}

pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
    : initialized_(false),
      is_detected_(false),
      is_goal_reached_(false),
      following_(false),
      allow_rotate_(false),
      xy_goal_tolerance_(0.05),
      yaw_goal_tolerance_(0.05),
      min_lookahead_dist_(0.4),
      max_lookahead_dist_(1.0),
      lookahead_time_(1.5),
      angle_threshold_(0.4),
      name_(name),
      docking_planner_name_(),
      docking_nav_name_(),
      docking_planner_(nullptr),
      docking_nav_(nullptr),
      linear_(),
      angular_(),
      nh_(),
      nh_priv_(),
      tf_(nullptr),
      costmap_robot_(nullptr),
      traj_generator_(),
      docking_planner_loader_(),
      docking_nav_loader_(),
      detected_timeout_wt_(),
      delayed_wt_(),
      delayed_(false),
      detected_timeout_(false),
      robot_base_frame_(),
      maker_goal_frame_()
{
}

pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
{
  detected_timeout_wt_.stop();
  delayed_wt_.stop();
  if (docking_planner_ != nullptr) {
    docking_planner_.reset();
  }
  if (docking_nav_ != nullptr) {
    docking_nav_.reset();
  }
}

void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                                                                             score_algorithm::TrajectoryGenerator::Ptr traj_generator)
{
  nh_ = nh;
  nh_priv_ = robot::NodeHandle(nh_, name_);
  tf_ = tf;
  costmap_robot_ = costmap_robot;
  traj_generator_ = traj_generator;

  if (docking_planner_name_ != std::string(""))
  {
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
        path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
      docking_planner_ = docking_planner_loader_();
      if (!docking_planner_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
      }
      docking_planner_->initialize(name_, costmap_robot_);
      robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
                docking_planner_name_.c_str(), ex.what());
    }
  }

  if (docking_nav_name_ != std::string(""))
  {
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
      docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
      docking_nav_ = docking_nav_loader_();
      if (!docking_nav_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
      }
      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
      docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
      robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
                docking_nav_name_.c_str(), ex.what());
    }
  }

  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
  if (!nh_priv_.param("maker_goal_frame", maker_goal_frame_, std::string("")))
  {
    std::stringstream re;
    re << "Can not get 'maker_goal_frame' in " << name_;
    robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
    throw robot_nav_core2::LocalPlannerException(re.str());
  }

  nh_priv_.param("vel_x", linear_.x, 0.5);
  nh_priv_.param("vel_theta", angular_.z, 0.5);
  nh_priv_.param("following", following_, false);
  nh_priv_.param("allow_rotate", allow_rotate_, false);

  double delay_time, time_out;
  nh_priv_.param("delay", delay_time, 1.0);
  nh_priv_.param("timeout", time_out, 1.0);

  nh_priv_.param("xy_goal_tolerance", xy_goal_tolerance_, 0.05);
  nh_priv_.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);

  nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.4);
  nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 1.0);
  nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
  nh_priv_.param("angle_threshold", angle_threshold_, 0.4);

  robot::log_info("delay: %f", delay_time);
  robot::log_info("timeout: %f", time_out);
  robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
  robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
  robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
  robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
  robot::log_info("lookahead_time: %f", lookahead_time_);
  robot::log_info("angle_threshold: %f", angle_threshold_);

  detected_timeout_wt_ = ::robot::WallTimer(
      ::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
  detected_timeout_wt_.start();
  robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());

  delayed_wt_ = ::robot::WallTimer(
      ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
  delayed_wt_.start();
  robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());

  initialized_ = true;
}

bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal)
{
  if (!delayed_)
    return false;

  bool get_pose_result = false;
  try
  {
    if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal, 2.0))
    {
      get_pose_result = true;
      detected_timeout_wt_.stop();
    }

    if (get_pose_result)
      return true;
  }
  catch (const std::exception &e)
  {
    if (detected_timeout_)
    {
      std::stringstream re;
      re << "No detected " << maker_goal_frame_;
      robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
      throw robot_nav_core2::LocalPlannerException(re.str());
    }
  }
  return false;
}

bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(robot_nav_2d_msgs::Pose2DStamped &local_goal)
{
  if (!delayed_)
    return false;
  bool get_pose_result = false;
  try
  {
    robot_nav_2d_msgs::Pose2DStamped maker_goal_on_robot;
    if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal_on_robot, 2.0))
    {
      if (robot_nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), maker_goal_on_robot, local_goal))
      {
        get_pose_result = true;
        detected_timeout_wt_.stop();
      }
    }

    if (get_pose_result)
      return true;
  }
  catch (const std::exception &e)
  {
    if (detected_timeout_)
    {
      std::stringstream re;
      re << "No detected " << maker_goal_frame_;
      robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
      throw robot_nav_core2::LocalPlannerException(re.str());
    }
  }
  return false;
}

bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
    const robot_nav_2d_msgs::Pose2DStamped &local_pose, const robot_nav_2d_msgs::Pose2DStamped &local_goal, robot_nav_msgs::Path &local_path)
{
  robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
  robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
  std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
  robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
  robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
  if (!docking_planner_->makePlan(start, goal, docking_plan))
  {
    throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");
    return false;
  }
  else
  {
    local_path = robot_nav_2d_utils::posesToPath(docking_plan);
    return true;
  }
}

void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event)
{
  detected_timeout_ = true;
  detected_timeout_wt_.stop();
  robot::log_warning_at(__FILE__, __LINE__, "%s time_out end %f", name_.c_str(), robot::Time::now().toSec());
}

void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb(const ::robot::WallTimerEvent &timer_event)
{
  delayed_ = true;
  delayed_wt_.stop();
  robot::log_warning_at(__FILE__, __LINE__, "%s delay end %f", name_.c_str(), robot::Time::now().toSec());
}

robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
{
  return std::make_shared<PNKXDockingLocalPlanner>();
}

//  register this planner as a LocalPlanner plugin
BOOST_DLL_ALIAS(pnkx_local_planner::PNKXDockingLocalPlanner::create, PNKXDockingLocalPlanner)
