git commit -m "first commit for v2"

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