git commit -m "first commit for v2"

2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
--- a/Localizations/Packages/loc_base/src/loc_base_node.cpp
+++ b/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);
+}