update

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:
+  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<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