diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt index e89ffa8..102eeca 100644 --- a/Controllers/Packages/amr_control/CMakeLists.txt +++ b/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 ## diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_control.h b/Controllers/Packages/amr_control/include/amr_control/amr_control.h index 1de21dc..5a11b9f 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_control.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include "delta_modbus/delta_modbus_tcp.h" @@ -24,10 +26,9 @@ #include #include #include +#include +#include -// #include -// Plugin Loader -// #include #include 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 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 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: + private: + + /** + * @brief Callback for arm control - starts arm thread for robot arm operations + */ void ArmCallBack(); - void ArmDotuff(); - - void unLoadCallBack(); - - void conveyorBeltsShipping(amr_control::State &state); - + /** + * @brief Callback for load operation - starts receiving conveyor belt thread + */ void loadCallBack(); - void conveyorBeltsReceiving(amr_control::State &state); - - void controllerDotuff(); + /** + * @brief Callback for unload operation - starts shipping conveyor belt thread + */ + 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 tf_; robot::TFListenerPtr tf_core_ptr_; + std::shared_ptr tf_converter_ptr_; + std::shared_ptr sensor_converter_ptr_; ros::Subscriber is_detected_maker_sub_; - std::shared_ptr opc_ua_server_api_ptr_; std::shared_ptr vda_5050_client_api_ptr_; std::shared_ptr server_thr_; diff --git a/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h b/Controllers/Packages/amr_control/include/amr_control/sensor_converter.h new file mode 100644 index 0000000..132acb9 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 move_base_ptr_; + ros::Subscriber map_server_sub_; + ros::Subscriber laser_f_scan_sub_; + ros::Subscriber laser_b_scan_sub_; + }; +} +#endif \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/tf_converter.h b/Controllers/Packages/amr_control/include/amr_control/tf_converter.h new file mode 100644 index 0000000..3a8dac1 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +namespace amr_control +{ + class TfConverter + { + public: + TfConverter(std::shared_ptr tf); + ~TfConverter(); + + /** + * @brief Lấy buffer tf3 từ tf_core_ptr_ + * @param tf_buffer Shared pointer to tf3::BufferCore + */ + void TfBuffer(std::shared_ptr &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> &check_frames, robot::TFListenerPtr &tf_core_ptr); + + private: + /** + * @brief Thread worker for tf conversion + */ + void tfWorker(); + + std::shared_ptr tf_; + std::shared_ptr tf3_buffer_; + std::thread tf_thread_; + std::atomic stop_tf_thread_{false}; + std::mutex tf3_mutex_; + }; +} + +#endif \ No newline at end of file diff --git a/Controllers/Packages/amr_control/package.xml b/Controllers/Packages/amr_control/package.xml index 9e8c63c..66f35a8 100644 --- a/Controllers/Packages/amr_control/package.xml +++ b/Controllers/Packages/amr_control/package.xml @@ -63,6 +63,8 @@ loc_core amr_comunication vda5050_msgs + nav_msgs + geometry_msgs nav_2d_utils std_msgs @@ -72,6 +74,8 @@ loc_core amr_comunication vda5050_msgs + nav_msgs + geometry_msgs nav_2d_utils std_msgs @@ -81,7 +85,8 @@ loc_core amr_comunication vda5050_msgs - + nav_msgs + robot_cpp robot_nav_2d_utils diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp index 2639df2..ae488f1 100644 --- a/Controllers/Packages/amr_control/src/amr_control.cpp +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -1,9 +1,10 @@ #include "amr_control/amr_control.h" -#include + #include #include #include #include +#include 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(); monitor_thr_ = std::make_shared( [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(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(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 { diff --git a/Controllers/Packages/amr_control/src/sensor_converter.cpp b/Controllers/Packages/amr_control/src/sensor_converter.cpp new file mode 100644 index 0000000..483500a --- /dev/null +++ b/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("/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("/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); + } +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/tf_converter.cpp b/Controllers/Packages/amr_control/src/tf_converter.cpp new file mode 100644 index 0000000..95da0a3 --- /dev/null +++ b/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 tf) + : tf_(tf) +{ + tf3_buffer_ = std::make_shared(); + 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 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 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 &tf_buffer) +{ + tf_buffer = tf3_buffer_; +} + +void amr_control::TfConverter::checkTfCoreData(const std::vector>& 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> 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__); +} \ No newline at end of file diff --git a/pnkx_nav_core b/pnkx_nav_core index a3e5168..f5e7e1f 160000 --- a/pnkx_nav_core +++ b/pnkx_nav_core @@ -1 +1 @@ -Subproject commit a3e5168d8878630da4cc8b5c24c1a32ca16be9a7 +Subproject commit f5e7e1f1e06189a181a9c099652e2c95f6166e49