update

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__);
+}