git commit -m "first commit for v2"

Localizations/Packages/loc_base/CMakeLists.txt

@@ -0,0 +1,235 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(loc_base)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  geometry_msgs
+  loc_core
+  pluginlib
+  roscpp
+  map_server
+  amcl 
+  slam_toolbox
+  slam_toolbox_msgs
+  slam_toolbox_rviz
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(yaml-cpp REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES loc_base
+  CATKIN_DEPENDS 
+    dynamic_reconfigure 
+    geometry_msgs 
+    loc_core 
+    amcl 
+    map_server 
+    slam_toolbox 
+    slam_toolbox_msgs  
+    slam_toolbox_rviz
+    pluginlib 
+    roscpp 
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${YAML_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+ add_library(${PROJECT_NAME}
+  src/loc_base.cpp
+  src/loc_base_util.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/loc_base_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+   PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_loc_base.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Localizations/Packages/loc_base/config/localization.yaml

@@ -0,0 +1,34 @@
+Amcl:
+  use_map_topic: true
+  odom_model_type: "diff-corrected"
+  gui_publish_rate: 10.0
+  save_pose_rate: 0.5
+  laser_max_beams: 200
+  laser_min_range: -1.0
+  laser_max_range: -1.0
+  min_particles: 500
+  max_particles: 5000
+  kld_err: 0.09
+  kld_z: 0.99
+  odom_alpha1: 0.02
+  odom_alpha2: 0.01
+  odom_alpha3: 0.01
+  odom_alpha4: 0.02
+  laser_z_hit: 0.5
+  laser_z_short:       0.05
+  laser_z_max: 0.05
+  laser_z_rand: 0.5
+  laser_sigma_hit:     0.2
+  laser_lambda_short:  0.1
+  laser_model_type:    "likelihood_field"
+  laser_likelihood_max_dist: 2.0
+  update_min_d: 0.2
+  update_min_a: 0.2
+  odom_frame_id: odom
+  base_frame_id: base_footprint
+  global_frame_id: map
+  resample_interval: 1
+  transform_tolerance: 0.2
+  recovery_alpha_slow: 0.0
+  recovery_alpha_fast: 0.0
+

Localizations/Packages/loc_base/config/map_managers.yaml

@@ -0,0 +1,15 @@
+plugins:
+  - {name: navigation_map, type: "costmap_2d::StaticLayer" }
+  - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
+
+navigation_map:
+  map_topic: map
+  map_pkg: managerments
+  map_file: maps/maze/maze.yaml
+  
+virtual_walls_map:
+  map_topic: /virtual_walls/map
+  namespace: /virtual_walls
+  map_pkg: managerments
+  map_file: maps/maze/maze.yaml
+

Localizations/Packages/loc_base/include/loc_base/loc_base.h

@@ -0,0 +1,168 @@
+#ifndef __LOC_BASE_H_INCLUDED_
+#define __LOC_BASE_H_INCLUDED_
+#include <ros/ros.h>
+#include <thread> 
+#include <memory> 
+#include "loc_core/localization.h"
+#include "amcl/amcl.h"
+#include "map_server/map_server.h"
+// #include "hector_mapping/HectorMappingRos.h"
+#include "slam_toolbox/slam_toolbox_async.hpp"
+
+namespace loc_base
+{
+	class LocBase : public loc_core::BaseLocalization
+	{
+	public:
+		/**
+		 * @brief Constructor
+		 */
+		LocBase();
+
+		/**
+		 * @brief Destructor
+		 */
+		virtual ~LocBase();
+
+		/**
+		 * @brief Initialization function for loc base
+		 * @param nh  A Node handle
+		 * @param tf  A poiter tranform listen
+		*/
+		void initialize(ros::NodeHandle nh, TFListenerPtr tf) override;
+
+		/**
+		 * @brief Loading a Activate Map File name 
+		 * @param activated_map_filename The Map File name 
+		 * @return True if activated_map_filename is exsits, else Fasle
+		 */
+		bool loadActivateMapFileName(std::string& activated_map_filename) override;
+
+		/**
+		 * @brief Saving a Activate Map File name 
+		 * @param activated_map_filename The Map File name 
+		 * @return True if activated_map_filename is exsits, else Fasle
+		 */
+		bool saveActivateMapFileName(const std::string& activated_map_filename) override;
+
+		/**
+		 * @brief Create any files to a folder of the path
+		 * @param path The path where is be saved any files (png, yaml ...)
+		 * @param map_name The name folder and files
+		 */
+		bool createStaticMap(const std::string map_name) override;
+
+		/**
+		* @brief Load a map given a path to a yaml file
+		* @param filename The file name
+		* @return True/False If load map succesed or not done
+		*/
+		bool changeStaticMap(const std::string filename) override;
+
+		/**
+		* @brief Load a virtual walls map given a path to a yaml file
+		* @param filename The file name
+		* @return True/False If load map succesed or not done
+		*/
+		bool changeVirtualWallsMap(const std::string filename) override;
+
+		/**
+		* @brief Get the list of map folder in the workingdirection
+		* @param directories The list map name
+		*/
+		void listMapFiles(std::stringstream &directories) override;
+
+		/**
+		 * @brief Start the mapping process.
+		 * 				When the state variable 'SlamState' is 'Ready', this function can be called.
+		 * 				Upon successful invocation, the state variable 'SlamState' will change to 'Mapping'.
+		 * @return True/False Call fucntion is successed/failed.
+		 */
+		bool startMapping() override;
+
+		/**
+		 * @brief Turn off the Localization mode.
+		 * 				At this point, the robot's coordinate information will not be updated.
+		 * 				If called successfully, the 'SlamState' variable will change to 'Ready'.
+		 * @return True/False Call fucntion is successed/failed
+		 */
+		bool stopMapping() override;
+
+		/**
+		 * @brief Set the initial position of the robot on the map.
+		 *        The condition to call the function is that the 'SlamState' is 'Localization'.
+		 *        When the function is called successfully, the 'SlamState' will change to 'Calibrations'.
+		 *        When the 'Calibrations' process ends, the 'SlamState' will change back to 'Localization'.
+		 * @return True/False Call fucntion is successed/failed
+		 */
+		bool startLocalization() override;
+
+		/**
+		 * @brief Turn off the Localization mode.
+		 * 				At this point, the robot's coordinate information will not be updated.
+		 * 				If called successfully, the 'SlamState' variable will change to 'Ready'.
+		 * @return True/False Call fucntion is successed/failed
+		 */
+		bool stopLocalization() override;
+
+		/**
+		 * @brief Get State of Loc Base
+		 */
+		loc_core::slam_state_en getState() override;
+
+		// static std::string activated_map_filename_;
+		// std::string map_topic_;
+
+	protected:
+
+		virtual bool loadParameters();
+
+		virtual void initalPose(geometry_msgs::PoseWithCovariance init_pose);
+
+		static bool getPose(std::string base_frame_id, std::string map_frame,
+			geometry_msgs::PoseStamped& global_pose, double transform_tolerance = 2.0);
+
+		static void savePose();
+
+		virtual bool init_map_server();
+
+		virtual bool close_map_server();
+
+		virtual bool init_localization();
+
+		virtual bool close_localization();
+
+		virtual bool init_mapping();
+
+		virtual bool close_mapping();
+
+		static void signalHandler(int sign);
+
+		virtual void savePoseCallback(const ros::WallTimerEvent& event);
+
+		virtual void cleanup();
+
+
+		bool initialized_;
+		ros::NodeHandle nh_;
+		ros::NodeHandle private_nh_;
+		ros::Publisher init_pub_;
+		static TFListenerPtr tf_;
+
+		XmlRpc::XmlRpcValue plugins_;
+		// std::map<std::string, std::string> map_file_vt_;
+		std::map<std::string, std::shared_ptr<map_server::MapServer>> static_map_vt_;
+
+		static std::string base_frame_id_, global_frame_id_;
+		static std::shared_ptr<amcl::Amcl> localization_althm_;
+		std::mutex localization_mutex_;
+		std::shared_ptr<ros::WallTimer> localization_timer_ptr_;
+		static std::shared_ptr<slam_toolbox::AsynchronousSlamToolbox> mapping_althm_;
+
+		loc_core::slam_state_en slam_state_{ loc_core::Ready };
+		static std::string *working_dir_ptr_;
+	};
+
+} // namespace loc_base
+
+#endif // __LOC_BASE_H_INCLUDED_

Localizations/Packages/loc_base/include/loc_base/loc_base_util.h

@@ -0,0 +1,39 @@
+#ifndef __LocBaseUtil_H_INCLUDED_
+#define __LocBaseUtil_H_INCLUDED_
+#include <ros/ros.h>
+
+namespace loc_base
+{
+  class LocBaseUtil
+  {
+  public:
+    /*
+     * @brief returns a full qualified filepath from package name and file name.
+     *
+     * @param package package name
+     *
+     * @return full qualified filepath (or filename if package is empty or couldn't be found).
+     */
+    static std::string filepathFromPackage(const std::string & package, const std::string & filename);
+
+    static bool loadActivateMapFileName(const std::string& filename, std::string &activated_map_filename);
+
+    static bool saveActivateMapFileName(const std::string& filename, const std::string &activated_map_filename);
+
+    static bool convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName);
+    
+    static bool writeGridMap(const std::string& filename);
+
+    static bool checkIfMapFilesExist(const std::string& filename);
+
+    static bool replaceMapDirectory(const std::string& mapDirectory);
+
+    /**
+		* @brief Get the list of map folder in the workingdirection
+		* @param directories The list map name
+		*/
+		static std::stringstream listMapFiles(const std::string &path);
+  };
+}
+
+#endif

Localizations/Packages/loc_base/launch/loc_base.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="loc_base" type="loc_base_node" respawn="false" name="loc_base" output="screen"> 
+    <rosparam file="$(find loc_base)/config/map_managers.yaml" command="load" />
+    <rosparam file="$(find loc_base)/config/localization.yaml" command="load" />
+  </node>
+</launch>

Localizations/Packages/loc_base/package.xml

@@ -0,0 +1,98 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>loc_base</name>
+  <version>0.0.0</version>
+  <description>The loc_base package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/loc_base</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>loc_core</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>loc_core</build_depend>
+  <build_depend>amcl</build_depend>
+  <build_depend>map_server</build_depend>
+  <build_depend>slam_toolbox</build_depend>
+  <build_depend>slam_toolbox_msgs</build_depend>
+  <build_depend>slam_toolbox_rviz</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>yaml-cpp</build_depend>
+
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>loc_core</build_export_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>loc_core</build_export_depend>
+  <build_export_depend>amcl</build_export_depend>
+  <build_export_depend>map_server</build_export_depend>
+  <build_export_depend>slam_toolbox</build_export_depend>
+  <build_export_depend>slam_toolbox_msgs</build_export_depend>
+  <build_export_depend>slam_toolbox_rviz</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>yaml-cpp</build_export_depend>
+
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>loc_core</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>loc_core</exec_depend>
+  <exec_depend>amcl</exec_depend>
+  <exec_depend>map_server</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+  <exec_depend>slam_toolbox_msgs</exec_depend>
+  <exec_depend>slam_toolbox_rviz</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>yaml-cpp</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <loc_core plugin="${prefix}/plugins.xml"/>
+  </export>
+</package>

Localizations/Packages/loc_base/plugins.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+    <library path="lib/libloc_base">
+        <class type="loc_base::LocBase" base_class_type="loc_core::BaseLocalization">
+            <description></description>
+        </class>
+    </library>
+</class_libraries>

Localizations/Packages/loc_base/src/loc_base.cpp

@@ -0,0 +1,699 @@
+#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;
+TFListenerPtr 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, TFListenerPtr 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)

Localizations/Packages/loc_base/src/loc_base_node.cpp

@@ -0,0 +1,95 @@
+#include <loc_base/loc_base.h>
+#include <loc_core/localization.h>
+#include <tf2_ros/transform_listener.h>
+#include <std_msgs/UInt16.h>
+
+// std::shared_ptr<loc_base::LocBase> loc_base_ptr = nullptr;
+std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr = nullptr;
+static loc_core::BaseLocalization* loc_base_;
+
+void callBack(const std_msgs::UInt16& msg)
+{
+  std::stringstream directories;
+  if (loc_base_)
+  {
+    switch (msg.data)
+    {
+    case 1:
+      loc_base_->startMapping();
+      /* code */
+      break;
+    case 2:
+      loc_base_->stopMapping();
+      /* code */
+      break;
+    case 3:
+      loc_base_->listMapFiles(directories);
+      ROS_INFO_STREAM(directories.str());
+      break;   
+    default:
+      break;
+    }
+  }
+}
+
+void createMapCallBack(const std_msgs::String& msg)
+{
+  if (loc_base_)
+    loc_base_->saveActivateMapFileName(msg.data);
+}
+
+void changeMapCallBack(const std_msgs::String& msg)
+{
+  if (loc_base_)
+    loc_base_->changeStaticMap(msg.data);
+}
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "loc_base_node");
+  ros::start();
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  std::shared_ptr<tf2_ros::Buffer> tf = std::make_shared<tf2_ros::Buffer>();
+  tf2_ros::TransformListener tf2(*tf);
+  // loc_base_ptr = std::make_shared<loc_base::LocBase>();
+  pluginlib::ClassLoader<loc_core::BaseLocalization>  loc_base_loader_("loc_core", "loc_core::BaseLocalization");
+
+  std::string obj_name("loc_base::LocBase");
+  if (tf == nullptr)
+    throw std::runtime_error("tf2_ros::Buffer object is null");
+  try
+  {
+    loc_base_ptr = loc_base_loader_.createUniqueInstance(obj_name);
+    loc_base_ptr->initialize(nh, tf);
+    loc_base_ = loc_base_ptr.get();
+  }
+  catch (pluginlib::PluginlibException& ex)
+  {
+    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
+    exit(1);
+  }
+  catch (std::exception& e)
+  {
+    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
+    exit(1);
+  }
+
+  ros::Subscriber sub = nh.subscribe("mode", 1000, callBack);
+  ros::Subscriber save_map_sub = nh.subscribe("save_map", 1000, createMapCallBack);
+  ros::Subscriber change_map_sub = nh.subscribe("change_map", 1000, changeMapCallBack);
+  ros::Publisher state = nh.advertise<std_msgs::UInt16>("state", 1);
+  ros::Rate rate(3);
+  
+  while (ros::ok())
+  {
+    std_msgs::UInt16 state_msg;
+    state_msg.data = loc_base_->getState();
+    state.publish(state_msg);
+    rate.sleep();
+    ros::spinOnce();
+  }
+
+  ros::spin();
+  loc_base_ptr.reset();
+  return(0);
+}

Localizations/Packages/loc_base/src/loc_base_util.cpp

@@ -0,0 +1,169 @@
+#include <boost/filesystem.hpp>
+#include <boost/exception/exception.hpp>
+#include <boost/system/error_code.hpp>
+#include <ros/package.h>
+#include <fstream>
+#include "loc_base/loc_base_util.h"
+
+std::string loc_base::LocBaseUtil::filepathFromPackage(const std::string& package, const std::string& filename)
+{
+  std::string filepath = filename;
+
+  if (!package.empty())
+  {
+    // Get the package path using ROS package utility
+    std::string pkg_path = ros::package::getPath(package);
+
+    // Log package path (info level, could be changed to ROS_DEBUG for less verbosity)
+    // ROS_INFO_STREAM("Package path: " << pkg_path);
+
+    if (!pkg_path.empty())
+    {
+      // Construct the full file path using Boost Filesystem (or std::filesystem)
+      filepath = (boost::filesystem::path(pkg_path) / filename).make_preferred().native();
+      // For C++17 and later, you can use the following line instead:
+      // filepath = (std::filesystem::path(pkg_path) / filename).string();
+
+      // Check if the file exists
+      if (!boost::filesystem::exists(filepath))  // Or std::filesystem::exists(filepath) for C++17+
+      {
+        // If file does not exist, throw an exception
+        std::string error_message = "File does not exist at: " + filepath;
+        // ROS_ERROR_STREAM(error_message);
+        throw std::runtime_error(error_message);
+      }
+    }
+    else
+    {
+      // If package path is empty, throw an exception
+      std::string error_message = "Failed to get path for package '" + package + "'.";
+      // ROS_ERROR_STREAM(error_message);
+      throw std::runtime_error(error_message);
+    }
+  }
+
+  return filepath;
+}
+
+
+
+bool loc_base::LocBaseUtil::loadActivateMapFileName(const std::string& filename, std::string& activated_map_filename)
+{
+  // Check if saved path file is existed
+  std::ifstream inputFile;
+  inputFile.open(filename);
+  std::stringstream buffer;
+  if (inputFile)
+  {
+    buffer << inputFile.rdbuf();
+    inputFile.close();
+    if (buffer.str().empty())
+      return false;
+  }
+  else return false;
+  activated_map_filename = buffer.str();
+
+  return true;
+}
+
+bool loc_base::LocBaseUtil::saveActivateMapFileName(const std::string& filename, const std::string& activated_map_filename)
+{
+  bool result = false;
+  // Check if saved path file is existed
+  std::ofstream outputFile(filename, std::ios::out);
+  if (outputFile.is_open()) {
+    outputFile << activated_map_filename;
+    outputFile.close();
+    result = true;
+  }
+  else
+    result = false;
+  return result;
+}
+
+bool loc_base::LocBaseUtil::convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName)
+{
+  std::string cmd;
+  int retVal;
+  cmd = "convert " + pgmFileName + " " + pngFileName;
+  retVal = system(cmd.c_str());
+  if (retVal != 0)
+    return false;
+
+  return true;
+}
+
+bool loc_base::LocBaseUtil::writeGridMap(const std::string& filename)
+{
+  std::string pgmFileName = filename + ".pgm";
+  std::string pngFileName = filename + ".png";
+  std::string yamlFileName = filename + ".yaml";
+  // bool is_SaveStateAsFiles = SaveStateAsFiles(filename); // cái này tham khảo https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
+  // if(is_SaveStateAsFiles)
+  // {
+  //   if(convertPgmToPng(pgmFileName, pngFileName)) 
+  //     return true;
+  // }
+  return false;
+}
+
+bool loc_base::LocBaseUtil::checkIfMapFilesExist(const std::string& filename)
+{
+
+  std::string SavedGridMapFileName = filename + ".png";
+  std::string SavedMapInfoFileName = filename + ".yaml";
+
+  std::ifstream SavedGridMapFile(SavedGridMapFileName);
+  std::ifstream SavedMapInfoFile(SavedMapInfoFileName);
+  if (SavedGridMapFile.good() && SavedMapInfoFile.good())
+  {
+    ROS_INFO("map file: %s is found", filename.c_str());
+    return true;
+  }
+  else ROS_WARN("map file: %s is not found", filename.c_str());
+  return false;
+}
+
+bool loc_base::LocBaseUtil::replaceMapDirectory(const std::string& mapDirectory)
+{
+  int retVal_1 = -1;
+  int retVal_2 = -1;
+  std::string removeCmd = "rm -rf " + mapDirectory;
+  retVal_1 = system(removeCmd.c_str());
+  if (retVal_1 == 0)
+  {
+    std::string createCmd = "mkdir " + mapDirectory;
+    retVal_2 = system(createCmd.c_str());
+  }
+  if (retVal_2 == 0)
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+  return false;
+}
+
+std::stringstream  loc_base::LocBaseUtil::listMapFiles(const std::string& path)
+{
+  std::stringstream directories;
+  try
+  {
+    for (const auto& entry : boost::filesystem::directory_iterator(path)) {
+      if(entry.path().extension() == ".txt")
+        continue;
+      directories << entry.path().filename() << std::endl;
+    }
+  }
+  catch (const boost::system::system_error& e) {
+    // Catching boost::system::system_error (which inherits from boost::exception)
+    ROS_ERROR_STREAM("Error reading directory: " << e.what());
+  }
+  catch (const boost::exception& e) {
+    // Catching general boost exceptions
+    ROS_ERROR_STREAM("Boost exception: List Files failed");
+  }
+  return directories;
+}