#include <XmlRpcException.h>
#include "loc_base/loc_base.h"
#include "loc_base/loc_base_util.h"
#include "map_server/map_generator.h"
#include <pluginlib/class_list_macros.h>
#include <signal.h>
#include <boost/version.hpp>

std::shared_ptr<amcl::Amcl> loc_base::LocBase::localization_althm_ = nullptr;
std::shared_ptr<slam_toolbox::AsynchronousSlamToolbox> loc_base::LocBase::mapping_althm_ = nullptr;
std::shared_ptr<tf2_ros::Buffer> loc_base::LocBase::tf_;
std::string loc_base::LocBase::base_frame_id_;
std::string loc_base::LocBase::global_frame_id_;
std::string* loc_base::LocBase::working_dir_ptr_ = NULL;

loc_base::LocBase::LocBase()
  : initialized_(false)
{
  ROS_INFO_STREAM("Boost version: " << BOOST_LIB_VERSION);
}

loc_base::LocBase::~LocBase() {
  cleanup();
}

void loc_base::LocBase::cleanup() {
  // Clean up map servers
  for (auto& map_pair : static_map_vt_) {
    map_pair.second.reset();
  }
  static_map_vt_.clear();

  // Clean up localization
  if (loc_base::LocBase::localization_althm_)
    loc_base::LocBase::localization_althm_.reset();
  // Clean up mapping
  if (loc_base::LocBase::mapping_althm_)
    loc_base::LocBase::mapping_althm_.reset();
}

void loc_base::LocBase::initialize(ros::NodeHandle nh, std::shared_ptr<tf2_ros::Buffer> tf)
{
  if (!initialized_)
  {
    nh_ = nh;
    tf_ = tf;
    private_nh_ = ros::NodeHandle("~");
    this->loadParameters();
    std::string activated_map_filename;
    if (this->loadActivateMapFileName(activated_map_filename))
    {
      loc_base::LocBase::activated_map_filename_ = activated_map_filename;
      ROS_INFO("Active map name %s", loc_base::LocBase::activated_map_filename_.c_str());
    }
    else ROS_WARN("Can not get active map name");

    ros::NodeHandle node;
    init_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);

    switch (slam_state_)
    {
    case loc_core::Ready:
      this->startLocalization();
      break;
    case loc_core::Localization:
      this->startLocalization();
      break;
    default:
      break;
    }
    initialized_ = true;
  }
}

bool loc_base::LocBase::loadActivateMapFileName(std::string& activated_map_filename)
{
  if (this->working_dir_.empty()) return false;
  std::string filename = std::string(this->working_dir_ + "/activated_map.txt");
  bool result = loc_base::LocBaseUtil::loadActivateMapFileName(filename, activated_map_filename);
  return result;
}

bool loc_base::LocBase::saveActivateMapFileName(const std::string& activated_map_filename)
{
  if (activated_map_filename.empty()) return false;
  if (this->working_dir_.empty()) return false;
  std::string filename = std::string(this->working_dir_ + "/activated_map.txt");

  bool result = this->createStaticMap(activated_map_filename);
  if (!result) return false;

  if (slam_state_ != loc_core::Mapping)
  {
    std::string static_map_folder = std::string(this->working_dir_ + "/" + activated_map_filename);
    if (!boost::filesystem::exists(static_map_folder))
    {
      ROS_ERROR("%s file is not existed", static_map_folder.c_str());
      return false;
    }
  }
  result = loc_base::LocBaseUtil::saveActivateMapFileName(filename, activated_map_filename);
  if (!result) return false;;
  activated_map_filename_ = activated_map_filename;
  return result;
}

bool loc_base::LocBase::createStaticMap(const std::string map_name)
{
  if (slam_state_ == loc_core::Mapping)
  {
    if (this->working_dir_.empty())
    {
      ROS_WARN("Working directory is not seting");
      return false;
    }
    std::string static_map_folder = std::string(this->working_dir_ + "/" + map_name);
    if (!boost::filesystem::exists(static_map_folder))
      boost::filesystem::create_directories(static_map_folder);
    else
    {
      ROS_WARN("Map file is existed");
      return false;
    }
    std::string static_map_name = std::string(static_map_folder + "/" + map_name);
    std::shared_ptr<map_server::MapGenerator> map_generator_ptr =
      std::make_shared<map_server::MapGenerator>(static_map_name, this->map_topic_);
    return map_generator_ptr->waitingGenerator(5.0);
  }
  return true;
}

bool loc_base::LocBase::changeStaticMap(const std::string filename)
{
  if (filename.empty()) return false;
  ROS_INFO("filename %s", filename.c_str());
  if (!loc_base::LocBase::activated_map_filename_.empty() && activated_map_filename_ == filename) return true;
  try
  {
    for (int32_t i = 0; i < plugins_.size(); ++i)
    {
      std::string pname = static_cast<std::string>(plugins_[i]["name"]);
      std::string type = static_cast<std::string>(plugins_[i]["type"]);
      if (type == std::string("costmap_2d::StaticLayer"))
      {
        if (!nh_.hasParam(pname))
          continue;
        else
        {
          std::string map_topic, ns;
          nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
          nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));

          if (ns == std::string("") && (map_topic == std::string("map") || map_topic == std::string("/map")))
          {
            if (std::map<std::string, std::shared_ptr<map_server::MapServer>>::iterator ms = static_map_vt_.find(pname); ms != static_map_vt_.end())
            {
              std::string path_to_yaml = std::string(this->working_dir_ + "/" + filename + "/" + filename + ".yaml");
              ROS_INFO("path_to_yaml %s", path_to_yaml.c_str());
              if (!boost::filesystem::exists(path_to_yaml))
              {
                ROS_WARN("%s is not exist", path_to_yaml.c_str());
                return false;
              }
              ROS_INFO("loadMapFromYaml %s", path_to_yaml.c_str());
              bool result = ms->second->loadMapFromYaml(path_to_yaml);
              if (result) this->saveActivateMapFileName(filename);
              return result;
            }
            else
            {
              ROS_WARN("Not found");
              return false;
            }
          }
        }
      }
    }
  }
  catch (XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("Change map is error: %s", e.getMessage().c_str());
    return false;
  }
  return true;
}

bool loc_base::LocBase::changeVirtualWallsMap(const std::string filename)
{
  if (filename.empty()) return false;
  try
  {
    for (int32_t i = 0; i < plugins_.size(); ++i)
    {
      std::string pname = static_cast<std::string>(plugins_[i]["name"]);
      std::string type = static_cast<std::string>(plugins_[i]["type"]);
      if (type == std::string("costmap_2d::StaticLayer"))
      {
        if (!nh_.hasParam(pname))
          continue;
        else
        {
          std::string map_topic, ns;
          nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
          nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));
        }
      }
    }
  }
  catch (XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("Change virtual map is error: %s", e.getMessage().c_str());
    return false;
  }
  return true;
}

void loc_base::LocBase::listMapFiles(std::stringstream& directories)
{
  directories = loc_base::LocBaseUtil::listMapFiles(*loc_base::LocBase::working_dir_ptr_);
}

bool loc_base::LocBase::startMapping()
{
  if (slam_state_ != loc_core::Mapping)
  {
    this->stopLocalization();
    this->init_mapping();
    slam_state_ = loc_core::Mapping;
  }
  return true;
}

bool loc_base::LocBase::stopMapping()
{
  if (slam_state_ == loc_core::Mapping)
  {
    this->close_mapping();
    slam_state_ = loc_core::Ready;
    return true;
  }
  else return false;
}

bool loc_base::LocBase::startLocalization()
{
  if (slam_state_ != loc_core::Localization)
  {
    this->stopMapping();
    bool resule_map_server = this->init_map_server();
    bool resule_localization = this->init_localization();

    if (!resule_map_server || !resule_localization)
    {
      this->stopLocalization();
      slam_state_ = loc_core::Error;
      return false;
    }
    slam_state_ = loc_core::Localization;
    return resule_map_server && resule_localization;
  }
  return true;
}

bool loc_base::LocBase::stopLocalization()
{
  if (slam_state_ == loc_core::Localization)
  {
    this->close_localization();
    this->close_map_server();
    slam_state_ = loc_core::Ready;
  }
  return true;
}

loc_core::slam_state_en loc_base::LocBase::getState()
{
  return slam_state_;
}

bool loc_base::LocBase::loadParameters()
{
  std::string package;
  std::string map_topic;
  std::string ns;
  plugins_.clear();
  if (!nh_.getParam("plugins", plugins_))
  {
    ROS_WARN("Can not get plugins");
    return false;
  }
  else
  {
    try
    {
      for (int32_t i = 0; i < plugins_.size(); ++i)
      {
        std::string pname = static_cast<std::string>(plugins_[i]["name"]);
        std::string type = static_cast<std::string>(plugins_[i]["type"]);
        if (pname != std::string("navigation_map")) continue;
        if (type == std::string("costmap_2d::StaticLayer"))
        {
          if (!nh_.hasParam(pname))
            continue;
          else
          {
            nh_.param(std::string(pname + "/map_pkg"), package, std::string(""));
            nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
            nh_.param(std::string(pname + "/map_topic"), map_topic, std::string(""));
            if (package.empty())
            {
              ROS_FATAL("'/map_pkg' is empty");
              exit(1);
            }
            if (map_topic.empty())
            {
              ROS_WARN("'/map_topic' is empty");
              exit(1);
            }
            break;
          }
        }
      }
    }
    catch (XmlRpc::XmlRpcException& e)
    {
      ROS_ERROR("Load map is error: %s", e.getMessage().c_str());
      return false;
    }
  }

  try
  {
    std::string working_dir = loc_base::LocBaseUtil::filepathFromPackage(package, "");
    std::string static_map_folder = std::string(working_dir + "/maps");
    if (!boost::filesystem::exists(static_map_folder))
      boost::filesystem::create_directories(static_map_folder);
    ROS_INFO("static_map_folder : %s", static_map_folder.c_str());
    loc_base::LocBase::working_dir_ptr_ = &this->working_dir_;
    this->working_dir_ = static_map_folder;
    this->map_topic_ = map_topic;
  }
  catch (std::exception& e)
  {
    ROS_ERROR("Error to check working directory: %s", e.what());
    return false;
  }
  return true;
}

bool loc_base::LocBase::init_map_server()
{
  ROS_INFO("Init Map Server");
  ROS_INFO("Getting %s/plugins .....", nh_.getNamespace().c_str());
  try
  {
    for (int32_t i = 0; i < plugins_.size(); ++i)
    {
      std::string pname = static_cast<std::string>(plugins_[i]["name"]);
      std::string type = static_cast<std::string>(plugins_[i]["type"]);
      if (type == std::string("costmap_2d::StaticLayer"))
      {
        if (!nh_.hasParam(pname))
          ROS_WARN("Can not get plugins/%s", pname.c_str());
        else
        {
          try
          {
            std::string map_pkg, map_file, ns;
            nh_.param(std::string(pname + "/map_pkg"), map_pkg, std::string(""));
            nh_.param(std::string(pname + "/map_file"), map_file, std::string(""));
            nh_.param(std::string(pname + "/namespace"), ns, std::string(""));
            std::string path_map_file;
            if (loc_base::LocBase::activated_map_filename_.empty())
            {
              path_map_file = loc_base::LocBaseUtil::filepathFromPackage(map_pkg, std::string("/maps/" + map_file + "/" + map_file + ".yaml"));
              if (pname == std::string("navigation_map"))
              {
                if (!boost::filesystem::exists(std::string(this->working_dir_ + "/activated_map.txt")))
                {
                  loc_base::LocBase::activated_map_filename_ = map_file;
                  this->saveActivateMapFileName(loc_base::LocBase::activated_map_filename_);
                }
              }
            }
            else
              path_map_file = std::string(this->working_dir_ + "/" + loc_base::LocBase::activated_map_filename_ + "/" + loc_base::LocBase::activated_map_filename_ + ".yaml");

            ROS_INFO(" Using plugin \"%s\" with type %s, map_file %s", pname.c_str(), type.c_str(), path_map_file.c_str());
            static_map_vt_[pname] = std::make_shared<map_server::MapServer>(nh_, ns, path_map_file, 0.0);
          }
          catch (std::runtime_error& e)
          {
            ROS_ERROR("map_server exception: %s", e.what());
            return false;
          }
        }
      }
    }
  }
  catch (XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("Init map is error: %s", e.getMessage().c_str());
    return false;
  }
  return true;

}

bool loc_base::LocBase::close_map_server()
{
  // plugins_.clear();
  // map_file_vt_.clear();
  ROS_INFO("Waiting MapServer be closed");
  static_map_vt_.clear();
  ROS_INFO("Closed MapServer");
  return true;
}

bool loc_base::LocBase::init_localization()
{
  ROS_INFO("Init localization");
  signal(SIGINT, &loc_base::LocBase::signalHandler);
  ros::NodeHandle nh = ros::NodeHandle(private_nh_, "Amcl");
  nh.param("base_frame_id", loc_base::LocBase::base_frame_id_, std::string("base_link"));
  nh.param("global_frame_id", loc_base::LocBase::global_frame_id_, std::string("map"));

  geometry_msgs::PoseWithCovariance init_pose;
  init_pose.pose.position.x = init_pose.pose.position.y = 0.0;
  init_pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
  init_pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
  init_pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0;

  std::string path_file = std::string(this->working_dir_ + "/initial_pose.txt");
  if (boost::filesystem::exists(path_file))
  {
    try
    {
      std::ifstream file(path_file, std::ios::in);
      if (!file.is_open())
      {
        ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n");
      }
      else
      {
        double x, y, theta;
        float init_cov[3];

        file >> x >> y >> theta >> init_cov[0] >> init_cov[1] >> init_cov[2];
        // Check if the read operation was successful
        if (file.fail())
        {
          ROS_ERROR("Failed to read data from the file: %s", path_file.c_str());
        }
        ROS_INFO_STREAM(x << " " << y << " " << theta << " " << init_cov[0] << " " << init_cov[1] << " " << init_cov[2]);

        init_pose.pose.position.x = std::isnan(x) || fabs(x) < 1e-5 ? 0.0 : x;
        init_pose.pose.position.y = std::isnan(y) || fabs(y) < 1e-5 ? 0.0 : y;
        theta = std::isnan(theta) || fabs(theta) < 1e-5 ? 0.0 : theta;
        tf::Quaternion quat;
        quat.setRPY(0.0, 0.0, theta);
        tf::quaternionTFToMsg(quat, init_pose.pose.orientation);

        init_pose.covariance[6 * 0 + 0] = std::isnan(init_cov[0]) || fabs(init_cov[0]) < 1e-4 ? 0.5 * 0.5 : init_cov[0];
        init_pose.covariance[6 * 1 + 1] = std::isnan(init_cov[1]) || fabs(init_cov[1]) < 1e-4 ? 0.5 * 0.5 : init_cov[1];
        init_pose.covariance[6 * 5 + 5] = std::isnan(init_cov[2]) || fabs(init_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : init_cov[2];
        file.close(); // Optionally close the file explicitly, though it's closed when it goes out of scope
      }
    }
    catch (const std::exception& e)
    {
      ROS_ERROR("Caught an exception: %s", e.what());
      // Handle the exception appropriately, maybe return or do additional logging
    }
  }
  else
  {
    ROS_INFO("Creating file: %s", path_file.c_str());
    double zero = 0.0;
    double initial_cov[3];
    initial_cov[0] = 0.5 * 0.5;
    initial_cov[1] = 0.5 * 0.5;
    initial_cov[2] = M_PI / 12.0 * M_PI / 12.0;
    std::ofstream file(path_file);
    file << zero << " " << zero << " " << zero << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[3];
    file.close();
  }

  loc_base::LocBase::localization_althm_ = std::make_shared<amcl::Amcl>(nh);
  while (loc_base::LocBase::localization_althm_ &&
    !loc_base::LocBase::localization_althm_->getInitialized())
  {
    ros::spinOnce();
  }

  this->initalPose(init_pose);
  localization_timer_ptr_ = std::make_shared<ros::WallTimer>(
    private_nh_.createWallTimer(ros::WallDuration(1.0), &loc_base::LocBase::savePoseCallback, this));
  return true;
}

bool loc_base::LocBase::close_localization()
{
  localization_timer_ptr_->stop();
  if (localization_timer_ptr_) localization_timer_ptr_.reset();

  if (loc_base::LocBase::localization_althm_)
    loc_base::LocBase::localization_althm_.reset();
  ROS_INFO("Waiting Localization be closed");
  while (ros::ok() && loc_base::LocBase::localization_althm_)
  {
    ros::spinOnce();
  }
  ROS_INFO("Closed Localization");
  return true;
}

bool loc_base::LocBase::init_mapping()
{
  int stack_size;
  ros::NodeHandle nh = ros::NodeHandle(private_nh_, "SlamToolBox");
  if (nh.getParam("stack_size_to_use", stack_size))
  {
    ROS_INFO("Node using stack size %i", (int)stack_size);
    const rlim_t max_stack_size = stack_size;
    struct rlimit stack_limit;
    getrlimit(RLIMIT_STACK, &stack_limit);
    if (stack_limit.rlim_cur < stack_size)
    {
      stack_limit.rlim_cur = stack_size;
    }
    setrlimit(RLIMIT_STACK, &stack_limit);
  }
  ROS_INFO("Waiting for init mapping");
  loc_base::LocBase::mapping_althm_ = std::make_shared<slam_toolbox::AsynchronousSlamToolbox>(nh_);
  while (ros::ok() && !loc_base::LocBase::mapping_althm_->getInitialized())
  {
    ros::Duration(0.1).sleep();
    ros::spinOnce();
  }

  ROS_INFO("Initialized Mapping");
  return true;
}

bool loc_base::LocBase::close_mapping()
{
  if (loc_base::LocBase::mapping_althm_)
    loc_base::LocBase::mapping_althm_.reset();

  ROS_INFO("Waiting for close mapping");
  while (ros::ok() && loc_base::LocBase::mapping_althm_)
  {
    ros::spinOnce();
  }
  ROS_INFO("Closed Mapping");
  return true;
}

void loc_base::LocBase::savePose()
{
  geometry_msgs::PoseStamped global_pose;
  geometry_msgs::Pose2D pose2d;
  float initial_cov[3];
  bool updating;
  if (loc_base::LocBase::localization_althm_)
  {
    if (loc_base::LocBase::localization_althm_->getInitialized())
    {
      geometry_msgs::PoseWithCovarianceStamped amcl_pose;
      localization_althm_->savePoseToServer(amcl_pose);
      pose2d.x = amcl_pose.pose.pose.position.x;
      pose2d.y = amcl_pose.pose.pose.position.y;
      pose2d.theta = tf2::getYaw(amcl_pose.pose.pose.orientation);
      initial_cov[0] = amcl_pose.pose.covariance[6 * 0 + 0];
      initial_cov[1] = amcl_pose.pose.covariance[6 * 1 + 1];
      initial_cov[2] = amcl_pose.pose.covariance[6 * 5 + 5];
      updating = true;
    }
  }
  else
  {
    if (loc_base::LocBase::getPose(loc_base::LocBase::base_frame_id_, loc_base::LocBase::global_frame_id_, global_pose))
    {
      pose2d.x = global_pose.pose.position.x;
      pose2d.y = global_pose.pose.position.y;
      pose2d.theta = tf2::getYaw(global_pose.pose.orientation);
      initial_cov[0] = 0.5 * 0.5;
      initial_cov[1] = 0.5 * 0.5;
      initial_cov[2] = M_PI / 12.0 * M_PI / 12.0;
      updating = true;
    }
  }
  std::string path_file;
  if (loc_base::LocBase::working_dir_ptr_)
    path_file = std::string((*loc_base::LocBase::working_dir_ptr_) + "/initial_pose.txt");
  else return;

  if (updating)
  {
    try
    {
      std::ofstream file(path_file, std::ios::out);
      //check to see that the file was opened correctly:
      if (!file.is_open()) {
        ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n");
      }
      else
      {
        // ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]);
        pose2d.x = std::isnan(pose2d.x) || fabs(pose2d.x) < 1e-4 ? 0.0 : pose2d.x;
        pose2d.y = std::isnan(pose2d.y) || fabs(pose2d.y) < 1e-4 ? 0.0 : pose2d.y;
        pose2d.theta = std::isnan(pose2d.theta) || fabs(pose2d.theta) < 1e-3 ? 0.0 : pose2d.theta;

        initial_cov[0] = std::isnan(initial_cov[0]) || fabs(initial_cov[0]) < 1e-4 ? 0.5 * 0.5 : initial_cov[0];
        initial_cov[1] = std::isnan(initial_cov[1]) || fabs(initial_cov[1]) < 1e-4 ? 0.5 * 0.5 : initial_cov[1];
        initial_cov[2] = std::isnan(initial_cov[2]) || fabs(initial_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : initial_cov[2];
        // ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]);
        file << pose2d.x << " " << pose2d.y << " " << pose2d.theta << " "
          << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2];
        // file << pose2d.x << " " << pose2d.y << " " << pose2d.theta;
        file.close();
      }

    }
    catch (const std::exception& e)
    {
      return;
    }
  }
}

void loc_base::LocBase::signalHandler(int sign)
{
  loc_base::LocBase::savePose();
  ros::shutdown();
}

void loc_base::LocBase::savePoseCallback(const ros::WallTimerEvent& event)
{
  loc_base::LocBase::savePose();
}

bool loc_base::LocBase::getPose(std::string base_frame_id, std::string map_frame,
  geometry_msgs::PoseStamped& global_pose, double transform_tolerance)
{
  geometry_msgs::PoseStamped global_pose_stamped;
  tf2::toMsg(tf2::Transform::getIdentity(), global_pose_stamped.pose);
  geometry_msgs::PoseStamped robot_pose;
  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
  robot_pose.header.frame_id = base_frame_id;
  robot_pose.header.stamp = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

  // get the global pose of the robot
  try
  {
    // use current time if possible (makes sure it's not in the future)
    if (loc_base::LocBase::tf_->canTransform(map_frame, base_frame_id, current_time))
    {
      geometry_msgs::TransformStamped transform = loc_base::LocBase::tf_->lookupTransform(map_frame, base_frame_id, current_time);
      tf2::doTransform(robot_pose, global_pose_stamped, transform);
    }
    // use the latest otherwise
    else
    {
      loc_base::LocBase::tf_->transform(robot_pose, global_pose_stamped, map_frame);
    }
    global_pose = global_pose_stamped;
  }
  catch (tf2::LookupException& ex)
  {
    return false;
  }
  catch (tf2::ConnectivityException& ex)
  {
    return false;
  }
  catch (tf2::ExtrapolationException& ex)
  {
    return false;
  }
  if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance)
  {
    return false;
  }
  return true;
}

void loc_base::LocBase::initalPose(geometry_msgs::PoseWithCovariance init_pose)
{
  geometry_msgs::PoseWithCovarianceStamped pose;
  pose.header.frame_id = loc_base::LocBase::global_frame_id_;
  pose.header.stamp = ros::Time::now();
  pose.pose = init_pose;
  this->init_pub_.publish(pose);
}

//  register this planner as a LocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(loc_base::LocBase, loc_core::BaseLocalization)