update

Controllers/Packages/amr_control/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
   loc_core
   amr_comunication
   vda5050_msgs
+  nav_msgs
 
   robot_nav_2d_utils
   move_base_core
@@ -74,6 +75,10 @@ add_library(${PROJECT_NAME}
   src/amr_make_plan_with_order.cpp
 )
 
+add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp)
+add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 ## 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
@@ -115,21 +120,9 @@ set_target_properties(${PROJECT_NAME}_node PROPERTIES
     BUILD_RPATH "${CATKIN_DEVEL_PREFIX}/lib"
 )
 
-# add_dependencies(vda_5050_api_test
-#   ${PROJECT_NAME}
-#   ${${PROJECT_NAME}_EXPORTED_TARGETS} 
-#   ${catkin_EXPORTED_TARGETS}
-# )
-# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
-# Libraries will be found via LD_LIBRARY_PATH or system paths
-# set_target_properties(vda_5050_api_test PROPERTIES
-#     SKIP_BUILD_RPATH TRUE
-#     BUILD_WITH_INSTALL_RPATH FALSE
-#     INSTALL_RPATH_USE_LINK_PATH FALSE
-# )
-
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}
+  ${PROJECT_NAME}_converter
   ${FreeOpcUa_LIBRARIES}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
@@ -137,16 +130,11 @@ target_link_libraries(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME}_node
   ${PROJECT_NAME}
+  ${PROJECT_NAME}_converter
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
 
-# target_link_libraries(vda_5050_api_test
-#   ${PROJECT_NAME}
-#   ${catkin_LIBRARIES}
-#   ${Boost_LIBRARIES}
-# )
-
 
 #############
 ## Install ##

Controllers/Packages/amr_control/include/amr_control/amr_control.h

@@ -10,6 +10,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 #include <std_msgs/Bool.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/LaserScan.h>
 
 #include "delta_modbus/delta_modbus_tcp.h"
 
@@ -24,10 +26,9 @@
 #include <amr_control/amr_monitor.h>
 #include <nova5_control/imr_nova_control.h>
 #include <amr_control/amr_safety.h>
+#include <amr_control/tf_converter.h>
+#include <amr_control/sensor_converter.h>
 
-// #include <robot/node_handle.h>
-// Plugin Loader
-// #include <robot/plugin_loader_helper.h>
 #include <boost/dll/import.hpp>
 
 namespace amr_control
@@ -35,10 +36,28 @@ namespace amr_control
   class AmrController
   {
   public:
+    /**
+     * @brief Constructor with NodeHandle and TF buffer
+     * @param nh ROS NodeHandle for communication
+     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
+     */
     AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
+    
+    /**
+     * @brief Default constructor
+     */
     AmrController();
+    
+    /**
+     * @brief Destructor - cleans up threads and resources
+     */
     virtual ~AmrController();
 
+    /**
+     * @brief Initialize the AMR controller with NodeHandle and TF buffer
+     * @param nh ROS NodeHandle for communication
+     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
+     */
     void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
 
     /**
@@ -47,37 +66,88 @@ namespace amr_control
     virtual void stop(void);
 
   protected:
+    /**
+     * @brief Initialize communication handles (OPC UA server and VDA5050 client)
+     * @param nh ROS NodeHandle for communication setup
+     */
     virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize move_base navigation system and subscribe to sensor topics
+     * @param nh ROS NodeHandle for move_base configuration
+     */
     virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize localization system (loc_base)
+     * @param nh ROS NodeHandle for localization configuration
+     */
     virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
+    
+    /**
+     * @brief Initialize monitoring system (AmrMonitor)
+     * @param nh ROS NodeHandle for monitor configuration
+     */
     virtual void initalizingMonitorHandle(ros::NodeHandle &nh);
 
   private:  
+
+    /**
+     * @brief Callback for arm control - starts arm thread for robot arm operations
+     */
     void ArmCallBack();
 
-    void ArmDotuff();
+    /**
-
+     * @brief Callback for load operation - starts receiving conveyor belt thread
-    void unLoadCallBack();
+     */
-
-    void conveyorBeltsShipping(amr_control::State &state);
-
     void loadCallBack();
 
-    void conveyorBeltsReceiving(amr_control::State &state);
+    /**
-
+     * @brief Callback for unload operation - starts shipping conveyor belt thread
-    void controllerDotuff();
+     */
+    void unLoadCallBack();
 
+    /**
+     * @brief Callback for marker detection status
+     * @param msg Boolean message indicating if marker is detected
+     */
     void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);
 
+    /**
+     * @brief Control shipping conveyor belt via PLC communication
+     * @param state Reference to state variable that tracks belt operation status
+     */
+    void conveyorBeltsShipping(amr_control::State &state);
+
+    /**
+     * @brief Control receiving conveyor belt via PLC communication
+     * @param state Reference to state variable that tracks belt operation status
+     */
+    void conveyorBeltsReceiving(amr_control::State &state);
+
+    /**
+     * @brief Main controller loop - handles PLC communication, safety checks, and velocity control
+     */
+    void controllerDotuff();
+
+    /**
+     * @brief Arm control thread function - manages robot arm operations (home, mode control)
+     */
+    void ArmDotuff();
+
+    /**
+     * @brief Main thread handler - coordinates initialization and main control loop
+     */
     void threadHandle();
 
     bool initalized_;
     ros::NodeHandle nh_;
     std::shared_ptr<tf2_ros::Buffer> tf_;
     robot::TFListenerPtr tf_core_ptr_;
+    std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
+    std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
 
     ros::Subscriber is_detected_maker_sub_;
-
     std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
     std::shared_ptr<amr_control::VDA5050ClientAPI> vda_5050_client_api_ptr_;
     std::shared_ptr<std::thread> server_thr_;

Controllers/Packages/amr_control/include/amr_control/sensor_converter.h

@@ -0,0 +1,64 @@
+#ifndef __SENSOR_CONVERTER_H_INCLUDED_
+#define __SENSOR_CONVERTER_H_INCLUDED_
+#include <ros/ros.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/GetMap.h>
+#include <sensor_msgs/LaserScan.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot/console.h>
+#include <robot/time.h>
+#include <move_base_core/navigation.h>
+#include <memory>
+
+namespace amr_control
+{
+  class SensorConverter
+  {
+  public:
+    SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
+    virtual ~SensorConverter();
+
+    /**
+     * @brief Request map from map server service or wait for message
+     * @param nh NodeHandle to use for service client and topic wait
+     */
+     void requestMapFromServer(ros::NodeHandle &nh);
+     
+  private:
+    /**
+     * @brief Callback for map server topic - receives static map and adds it to move_base
+     * @param msg OccupancyGrid message from map server
+     */
+    void mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
+
+    /**
+     * @brief Callback for front laser scan topic - receives and processes front laser data
+     * @param msg LaserScan message from front laser sensor
+     */
+    void laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
+
+    /**
+     * @brief Callback for back laser scan topic - receives and processes back laser data
+     * @param msg LaserScan message from back laser sensor
+     */
+    void laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
+
+    /**
+     * @brief Convert nav_msgs::OccupancyGrid to robot_nav_msgs::OccupancyGrid and add to move_base
+     * @param nav_map The nav_msgs map to convert
+     */
+    void convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map);
+
+    ros::NodeHandle nh_;
+    std::shared_ptr<robot::move_base_core::BaseNavigation> move_base_ptr_;
+    ros::Subscriber map_server_sub_;
+    ros::Subscriber laser_f_scan_sub_;
+    ros::Subscriber laser_b_scan_sub_;
+  };
+}
+#endif

Controllers/Packages/amr_control/include/amr_control/tf_converter.h

@@ -0,0 +1,46 @@
+#ifndef __TF_CONVERTER_H_INCLUDED_
+#define __TF_CONVERTER_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <robot/console.h>
+#include <robot/time.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf3/buffer_core.h>
+#include <tf3/exceptions.h>
+#include <move_base_core/common.h>
+
+namespace amr_control
+{
+  class TfConverter
+  {
+  public:
+    TfConverter(std::shared_ptr<tf2_ros::Buffer> tf);
+    ~TfConverter();
+
+    /**
+     * @brief Lấy buffer tf3 từ tf_core_ptr_
+     * @param tf_buffer Shared pointer to tf3::BufferCore
+     */
+    void TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer);
+
+    /**
+     * @brief Kiểm tra và in ra thông tin về tf_core_ptr_ để verify dữ liệu cập nhật
+     * @param check_frames Các frame pairs cần kiểm tra (ví dụ: {"map", "base_footprint"})
+     */
+    void checkTfCoreData(const std::vector<std::pair<std::string, std::string>> &check_frames, robot::TFListenerPtr &tf_core_ptr);
+
+  private:
+    /**
+     * @brief Thread worker for tf conversion
+     */
+    void tfWorker();
+
+    std::shared_ptr<tf2_ros::Buffer> tf_;
+    std::shared_ptr<tf3::BufferCore> tf3_buffer_;
+    std::thread tf_thread_;
+    std::atomic<bool> stop_tf_thread_{false};
+    std::mutex tf3_mutex_;
+  };
+}
+
+#endif

Controllers/Packages/amr_control/package.xml

@@ -63,6 +63,8 @@
   <build_depend>loc_core</build_depend>
   <build_depend>amr_comunication</build_depend>
   <build_depend>vda5050_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>nav_2d_utils</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
@@ -72,6 +74,8 @@
   <build_export_depend>loc_core</build_export_depend>
   <build_export_depend>amr_comunication</build_export_depend>
   <build_export_depend>vda5050_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav_2d_utils</exec_depend>
   <exec_depend>std_msgs</exec_depend>
@@ -81,6 +85,7 @@
   <exec_depend>loc_core</exec_depend>
   <exec_depend>amr_comunication</exec_depend>
   <exec_depend>vda5050_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
   
 <!-- PNKX core  -->
   <build_depend>robot_cpp</build_depend>

Controllers/Packages/amr_control/src/amr_control.cpp

@@ -1,9 +1,10 @@
 #include "amr_control/amr_control.h"
-#include <geometry_msgs/Vector3.h>
+
 #include <robot/plugin_loader_helper.h>
 #include <robot/console.h>  
 #include <robot/node_handle.h>
 #include <tf3/buffer_core.h>
+#include <geometry_msgs/Vector3.h>
 
 namespace amr_control
 {
@@ -33,6 +34,7 @@ namespace amr_control
         loc_base_ptr_(nullptr),
         move_base_ptr_(nullptr),
         amr_safety_ptr_(nullptr),
+        tf_converter_ptr_(nullptr),
         tf_core_ptr_(nullptr),
         loc_base_loader_("loc_core", "loc_core::BaseLocalization")
   {
@@ -82,8 +84,6 @@ namespace amr_control
     {
       nh_ = nh;
       tf_ = buffer;
-      // Create tf3::BufferCore instance for move_base
-      tf_core_ptr_ = std::make_shared<tf3::BufferCore>();
 
       monitor_thr_ = std::make_shared<std::thread>(
           [this]()
@@ -225,6 +225,9 @@ namespace amr_control
     std::string obj_name("move_base::MoveBase");
     if (tf_ == nullptr)
       throw std::runtime_error("tf2_ros::Buffer object is null");
+
+    tf_converter_ptr_ = std::make_shared<amr_control::TfConverter>(tf_);
+    tf_converter_ptr_->TfBuffer(tf_core_ptr_);
     try
     {
       robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
@@ -236,8 +239,13 @@ namespace amr_control
       
       robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
       move_base_ptr_ = move_base_loader_();
+      sensor_converter_ptr_ = std::make_shared<amr_control::SensorConverter>(nh, move_base_ptr_);
       move_base_ptr_->initialize(tf_core_ptr_);
       
+      // Request map từ service hoặc đợi message (vì map server chỉ publish một lần)
+      robot::log_info("[%s:%d]\n Requesting map from server...", __FILE__, __LINE__);
+      sensor_converter_ptr_->requestMapFromServer(nh);
+      
       ros::Rate r(3);
       do
       {

Controllers/Packages/amr_control/src/sensor_converter.cpp

@@ -0,0 +1,132 @@
+#include "amr_control/sensor_converter.h"
+
+amr_control::SensorConverter::SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
+  : nh_(nh), move_base_ptr_(move_base_ptr)
+{
+    map_server_sub_ = nh_.subscribe("/map", 1, &SensorConverter::mapServerCallback, this);
+    laser_f_scan_sub_ = nh_.subscribe("/f_scan", 1, &SensorConverter::laserFScanCallback, this);
+    laser_b_scan_sub_ = nh_.subscribe("/b_scan", 1, &SensorConverter::laserBScanCallback, this);
+}
+
+amr_control::SensorConverter::~SensorConverter()
+{
+    map_server_sub_.shutdown();
+    laser_f_scan_sub_.shutdown();
+    laser_b_scan_sub_.shutdown();
+}
+
+void amr_control::SensorConverter::requestMapFromServer(ros::NodeHandle &nh)
+{
+  // Thử request map từ service trước (nếu map server có service)
+  ros::ServiceClient map_client = nh.serviceClient<nav_msgs::GetMap>("/static_map");
+  
+  if (map_client.exists())
+  {
+    nav_msgs::GetMap srv;
+    if (map_client.call(srv))
+    {
+      robot::log_info("[%s:%d] Got map from service", __FILE__, __LINE__);
+      // Convert và add map
+      convertAndAddMap(srv.response.map);
+      return;
+    }
+    else
+    {
+      robot::log_warning("[%s:%d] Service call failed, trying to wait for message...", __FILE__, __LINE__);
+    }
+  }
+  else
+  {
+    robot::log_info("[%s:%d] Map service not available, waiting for message on /map topic...", __FILE__, __LINE__);
+  }
+  
+  // Nếu không có service hoặc service call failed, đợi message từ topic
+  nav_msgs::OccupancyGridConstPtr map_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map", nh, ros::Duration(5.0));
+  if (map_msg)
+  {
+    robot::log_info("[%s:%d] Got map from topic", __FILE__, __LINE__);
+    convertAndAddMap(*map_msg);
+  }
+  else
+  {
+    robot::log_warning("[%s:%d] No map received after waiting 5 seconds", __FILE__, __LINE__);
+  }
+}
+
+void amr_control::SensorConverter::convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map)
+{
+  if (move_base_ptr_ != nullptr)
+  {
+    robot_nav_msgs::OccupancyGrid map;
+    map.header.frame_id = nav_map.header.frame_id;
+    map.header.stamp = robot::Time::now();
+    map.header.seq = nav_map.header.seq;
+
+    map.info.map_load_time = robot::Time::now();
+    map.info.width = nav_map.info.width;
+    map.info.height = nav_map.info.height;
+    map.info.resolution = nav_map.info.resolution;
+    map.info.origin.position.x = nav_map.info.origin.position.x;
+    map.info.origin.position.y = nav_map.info.origin.position.y;
+    map.info.origin.position.z = nav_map.info.origin.position.z;
+    map.info.origin.orientation.x = nav_map.info.origin.orientation.x;
+    map.info.origin.orientation.y = nav_map.info.origin.orientation.y;
+    map.info.origin.orientation.z = nav_map.info.origin.orientation.z;
+    map.info.origin.orientation.w = nav_map.info.origin.orientation.w;
+    
+    map.data = nav_map.data;
+    move_base_ptr_->addStaticMap("navigation_map", map);
+  }
+}
+
+void amr_control::SensorConverter::mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
+{
+  robot::log_info("[%s:%d]\n Adding static map to move_base from callback... %p", __FILE__, __LINE__, move_base_ptr_.get());
+  convertAndAddMap(*msg);
+}
+
+void amr_control::SensorConverter::laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+  if (move_base_ptr_ != nullptr && msg)
+  {
+    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
+    robot_sensor_msgs::LaserScan robot_scan;
+    robot_scan.header.frame_id = msg->header.frame_id;
+    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
+    robot_scan.header.seq = msg->header.seq;
+    robot_scan.angle_min = msg->angle_min;
+    robot_scan.angle_max = msg->angle_max;
+    robot_scan.angle_increment = msg->angle_increment;
+    robot_scan.time_increment = msg->time_increment;
+    robot_scan.scan_time = msg->scan_time;
+    robot_scan.range_min = msg->range_min;
+    robot_scan.range_max = msg->range_max;
+    robot_scan.ranges = msg->ranges;
+    robot_scan.intensities = msg->intensities;
+    
+    move_base_ptr_->addLaserScan("f_scan", robot_scan);
+  }
+}
+
+void amr_control::SensorConverter::laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+  if (move_base_ptr_ != nullptr && msg)
+  {
+    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
+    robot_sensor_msgs::LaserScan robot_scan;
+    robot_scan.header.frame_id = msg->header.frame_id;
+    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
+    robot_scan.header.seq = msg->header.seq;
+    robot_scan.angle_min = msg->angle_min;
+    robot_scan.angle_max = msg->angle_max;
+    robot_scan.angle_increment = msg->angle_increment;
+    robot_scan.time_increment = msg->time_increment;
+    robot_scan.scan_time = msg->scan_time;
+    robot_scan.range_min = msg->range_min;
+    robot_scan.range_max = msg->range_max;
+    robot_scan.ranges = msg->ranges;
+    robot_scan.intensities = msg->intensities;
+    
+    move_base_ptr_->addLaserScan("b_scan", robot_scan);
+  }
+}

Controllers/Packages/amr_control/src/tf_converter.cpp

@@ -0,0 +1,258 @@
+#include "amr_control/tf_converter.h"
+
+amr_control::TfConverter::TfConverter(std::shared_ptr<tf2_ros::Buffer> tf)
+  : tf_(tf)
+{
+  tf3_buffer_ = std::make_shared<tf3::BufferCore>();
+  tf3_buffer_->setUsingDedicatedThread(true);
+  tf_thread_ = std::thread([this]() { this->tfWorker(); });
+}
+
+amr_control::TfConverter::~TfConverter()
+{
+  stop_tf_thread_ = true;
+  tf_thread_.join();
+}
+void amr_control::TfConverter::tfWorker()
+{
+  struct TFEdge {
+      std::string parent;
+      std::string child;
+  }; 
+  std::vector<TFEdge> edges;
+  tf2_ros::Buffer tfBuffer;
+  tf2_ros::TransformListener tfListener(tfBuffer);
+
+  std::string last_tree;
+  std::string tree;
+  std::string line;
+
+  ros::Rate rate(20);
+  while (ros::ok() && !stop_tf_thread_)
+  {
+      tree = tfBuffer.allFramesAsString();
+
+      if (!tree.empty() && tree == last_tree)
+      {
+          // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+          break;
+      }
+
+      last_tree = tree;
+      ros::spinOnce();
+      rate.sleep();
+  }
+
+  std::istringstream iss(tree);
+
+  while (ros::ok() && std::getline(iss, line) && !stop_tf_thread_)
+  {
+      // Frame X exists with parent Y
+      std::size_t f1 = line.find("Frame ");
+      std::size_t f2 = line.find(" exists with parent ");
+
+      if (f1 != std::string::npos && f2 != std::string::npos)
+      {
+          std::string child  = line.substr(f1 + 6, f2 - (f1 + 6));
+          std::string parent = line.substr(f2 + 20);
+
+          if (!parent.empty() && parent.back() == '.')
+          parent.pop_back();
+
+          edges.push_back({parent, child});
+          ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
+      }
+  }
+
+  while (ros::ok() && !stop_tf_thread_)
+  {
+    if (!tf_ || !tf3_buffer_)
+    {
+      rate.sleep();
+      continue;
+    }
+
+    try
+    {
+      for (const auto& e : edges)
+      {
+        try
+        {
+          if (tf_->canTransform(e.parent, e.child, ros::Time(0)))
+          { 
+            auto transform =
+            tf_->lookupTransform(e.parent, e.child, ros::Time(0));
+            
+            tf3::TransformStampedMsg t;
+            t.header.stamp = tf3::Time::now();
+            t.header.frame_id = transform.header.frame_id;
+            t.child_frame_id = transform.child_frame_id;
+            t.transform.translation.x = transform.transform.translation.x;
+            t.transform.translation.y = transform.transform.translation.y;
+            t.transform.translation.z = transform.transform.translation.z;
+            t.transform.rotation.x = transform.transform.rotation.x;
+            t.transform.rotation.y = transform.transform.rotation.y;
+            t.transform.rotation.z = transform.transform.rotation.z;
+            t.transform.rotation.w = transform.transform.rotation.w;
+
+            {
+              std::lock_guard<std::mutex> lock(tf3_mutex_);
+              tf3_buffer_->setTransform(t, "dynamic_tf");
+            }
+          }
+        }
+        catch (tf2::TransformException &ex)
+        {
+            ROS_WARN("TF %s -> %s failed: %s",
+                    e.parent.c_str(),e.child.c_str(),
+                    ex.what());
+        }
+      }
+      // ROS_INFO("TF worker thread running");
+    }
+    catch (const std::exception& e)
+    {
+      ROS_WARN("TF worker exception: %s", e.what());
+    }
+
+    rate.sleep();
+  }
+
+  ROS_INFO("TF worker thread cleanly exited");
+}
+
+void amr_control::TfConverter::TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer)
+{
+  tf_buffer = tf3_buffer_;
+}
+
+void amr_control::TfConverter::checkTfCoreData(const std::vector<std::pair<std::string, std::string>>& check_frames, robot::TFListenerPtr &tf_core_ptr)
+{
+  if (!tf_core_ptr)
+  {
+    robot::log_warning("[%s:%d] tf_core_ptr_ is nullptr", __FILE__, __LINE__);
+    return;
+  }
+
+  robot::log_info("[%s:%d] ========== Checking tf_core_ptr_ Data ==========", __FILE__, __LINE__);
+  
+  // In ra tất cả frames có trong buffer
+  try
+  {
+    std::string all_frames = tf_core_ptr->allFramesAsString();
+    if (!all_frames.empty())
+    {
+      robot::log_info("[%s:%d] All frames in tf_core_ptr_:\n%s", __FILE__, __LINE__, all_frames.c_str());
+    }
+    else
+    {
+      robot::log_warning("[%s:%d] No frames found in tf_core_ptr_", __FILE__, __LINE__);
+    }
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[%s:%d] Exception getting all frames: %s", __FILE__, __LINE__, ex.what());
+  }
+
+  // Kiểm tra các frame pairs được chỉ định
+  if (!check_frames.empty())
+  {
+    robot::log_info("[%s:%d] Checking specific frame pairs:", __FILE__, __LINE__);
+    for (const auto& frame_pair : check_frames)
+    {
+      const std::string& target_frame = frame_pair.first;
+      const std::string& source_frame = frame_pair.second;
+      
+      try
+      {
+        std::string error_msg;
+        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
+        
+        if (can_transform)
+        {
+          try
+          {
+            tf3::TransformStampedMsg transform = tf_core_ptr->lookupTransform(target_frame, source_frame, tf3::Time());
+            robot::log_info("[%s:%d] Transform [%s] -> [%s]:", __FILE__, __LINE__, 
+                           source_frame.c_str(), target_frame.c_str());
+            robot::log_info("  Translation: (%.3f, %.3f, %.3f)", 
+                           transform.transform.translation.x,
+                           transform.transform.translation.y,
+                           transform.transform.translation.z);
+            robot::log_info("  Rotation: (%.3f, %.3f, %.3f, %.3f)", 
+                           transform.transform.rotation.x,
+                           transform.transform.rotation.y,
+                           transform.transform.rotation.z,
+                           transform.transform.rotation.w);
+            robot::log_info("  Stamp: %.3f", transform.header.stamp.toSec());
+          }
+          catch (const tf3::LookupException& ex)
+          {
+            robot::log_error("[%s:%d] LookupException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+          catch (const tf3::ConnectivityException& ex)
+          {
+            robot::log_error("[%s:%d] ConnectivityException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+          catch (const tf3::ExtrapolationException& ex)
+          {
+            robot::log_error("[%s:%d] ExtrapolationException for [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+          }
+        }
+        else
+        {
+          robot::log_warning("[%s:%d] Cannot transform [%s] -> [%s]: %s", 
+                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
+        }
+      }
+      catch (const std::exception& ex)
+      {
+        robot::log_error("[%s:%d] Exception checking transform [%s] -> [%s]: %s", 
+                       __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+      }
+    }
+  }
+  else
+  {
+    // Nếu không có frame pairs được chỉ định, kiểm tra một số frame phổ biến
+    std::vector<std::pair<std::string, std::string>> common_frames = {
+      {"map", "base_footprint"},
+      {"odom", "base_footprint"},
+      {"base_footprint", "base_link"}
+    };
+    
+    robot::log_info("[%s:%d] Checking common frame pairs:", __FILE__, __LINE__);
+    for (const auto& frame_pair : common_frames)
+    {
+      const std::string& target_frame = frame_pair.first;
+      const std::string& source_frame = frame_pair.second;
+      
+      try
+      {
+        std::string error_msg;
+        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
+        
+        if (can_transform)
+        {
+          robot::log_info("[%s:%d] ✓ Transform available: [%s] -> [%s]", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str());
+        }
+        else
+        {
+          robot::log_info("[%s:%d] ✗ Transform NOT available: [%s] -> [%s] (%s)", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
+        }
+      }
+      catch (const std::exception& ex)
+      {
+        robot::log_warning("[%s:%d] Exception checking [%s] -> [%s]: %s", 
+                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
+      }
+    }
+  }
+  
+  robot::log_info("[%s:%d] ========== End Checking tf_core_ptr ==========", __FILE__, __LINE__);
+}

pnkx_nav_core

@@ -1 +1 @@
-Subproject commit a3e5168d8878630da4cc8b5c24c1a32ca16be9a7
+Subproject commit f5e7e1f1e06189a181a9c099652e2c95f6166e49