refactor(agv_pro_base): Change serial_driver to boost/asio serial port library

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -2,7 +2,8 @@
 #define AGV_PRO_DRIVER_H
 
 #include <algorithm> 
-#include "serial_driver/serial_driver.hpp"
+#include <iostream>
+#include <boost/asio.hpp>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -13,7 +14,9 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-#define RECEIVE_DATA_SIZE 14         //The length of the data sent by the esp32
+#define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32
+#define RECEIVE_FRAME_SIZE 32                           // Total bytes in a frame from ESP32
+#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3)   // Payload length (excluding header)
 
 extern std::array<double, 36> odom_pose_covariance;
 extern std::array<double, 36> odom_twist_covariance;
@@ -21,20 +24,131 @@ extern std::array<double, 36> odom_twist_covariance;
 class AGV_PRO : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   */
   AGV_PRO(std::string node_name);
+
+  /**
+   * @brief Destructor
+   */
   ~AGV_PRO();
+
 private:
+  /**
+   * @brief Main control loop for the AGV.
+   */
   void Control();
-  void print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Print a vector of bytes in hexadecimal format to the ROS logger.
+   *
+   * @param[in] label A label to prepend to the printed data.
+   * @param[in] data The byte vector to print.
+   * @param[in] override_size Optional size to display instead of the full data length.
+   */
+  void print_hex(const std::string& label,
+    const std::vector<uint8_t>& data,
+    std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Send a serial frame to the AGV and optionally print it in hex.
+   *
+   * @param[in] frame The byte vector representing the serial frame to send.
+   * @param[in] debug If true, prints the transmitted frame using print_hex().
+   */
   void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
-  void is_power_on();
+
-  void set_auto_report();
+  /**
+   * @brief Query and print the current power status of the AGV.
+   * @return true if AGV is successfully power on; false otherwise.
+   */
+  bool is_power_on();
+
+  /**
+   * @brief Enable or disable AGV auto-reporting
+   * @param[in] enable 0 = disable, 1 = enable
+   */
+  void set_auto_report(bool enable);
+
+  /**
+   * @brief Clear the serial port input and output buffers
+   * @param[in] fd File descriptor of the serial port
+   */
+  void clearSerialBuffer(int fd);
+
+  /**
+   * @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port
+   * @param[in] fd File descriptor of the serial port
+   */
+  void disableDTR_RTS(int fd);
+
+  /**
+   * @brief Read sensor and motor data from the AGV via serial port.
+   * @return true if data is successfully read and verified; false otherwise.
+   */
   bool readData();
+
+  /**
+   * @brief Odometry publisher
+   * @param[in] dt Time difference (in seconds) since the last odometry update.
+   */
   void publisherOdom(double dt);
+
+  /**
+   * @brief Voltage publisher
+   */
   void publisherVoltage();
+
+  /**
+   * @brief Callback for velocity command updates
+   * @param[in] msg The Twist message containing desired linear and angular velocities
+   */
   void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  
+  /**
+   * @brief Build a standard AGV serial frame with header, payload, and CRC.
+   *
+   * The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B,
+   * includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC.
+   *
+   * @param[in] cmd_id The command ID for the serial frame.
+   * @param[in] payload The payload bytes to include (up to 8 bytes).
+   * @return A vector containing the complete serial frame ready to transmit.
+   */
   std::vector<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
-  std::vector<uint8_t> read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec);
+
+  /**
+   * @brief Read a serial response from the AGV device, waiting for a specific header.
+   *
+   * This function reads bytes from the serial port until the expected header
+   * sequence is detected or the timeout expires. After detecting the header,
+   * it reads the remaining payload bytes along with a 2-byte CRC.
+   *
+   * @param[in] expected_header The byte sequence to identify the start of a valid frame.
+   * @param[in] payload_size The expected number of payload bytes following the header.
+   * @param[in] timeout_sec Maximum time (in seconds) to wait for the header.
+   * @return A vector containing the complete frame (header + payload + CRC).
+   *         Returns an empty vector if a timeout occurs or the full payload is not received.
+   */
+  std::vector<uint8_t> read_serial_response(
+    const std::vector<uint8_t>& expected_header,
+    size_t payload_size,
+    double timeout_sec);
+
+  /**
+   * @brief Compute the CRC-16-IBM checksum for a byte array.
+   *
+   * This function calculates the CRC using the standard IBM polynomial 0xA001.
+   *
+   * @param[in] data Pointer to the byte array.
+   * @param[in] length Number of bytes to include in the CRC calculation.
+   * @return The computed 16-bit CRC value.
+   */
+  uint16_t crc16_ibm(const uint8_t* data, size_t length);
+
+  boost::asio::io_service io_;
+  std::unique_ptr<boost::asio::serial_port> serial_port_;
 
   std::string frame_id_of_odometry_;
   std::string child_frame_id_of_odometry_;
@@ -63,6 +177,22 @@ private:
   
   float battery_voltage = 0.0f;
 
+  std::array<double, 36> odom_pose_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
+  std::array<double, 36> odom_twist_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
   rclcpp::Time currentTime, lastTime;
   rclcpp::TimerBase::SharedPtr control_timer_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
@@ -71,9 +201,6 @@ private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
 
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
-  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
-  std::shared_ptr<drivers::common::IoContext> io_context_;
-
 };
 
 #endif

agv_pro_base/src/agv_pro_ros.cpp

@@ -1,22 +1,6 @@
 #include "agv_pro_base/agv_pro_driver.h"
 
-std::array<double, 36> odom_pose_covariance = {
+uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-std::array<double, 36> odom_twist_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-uint16_t crc16_ibm(const uint8_t* data, size_t length) {
   uint16_t crc = 0xFFFF;
   for (size_t i = 0; i < length; ++i) {
     crc ^= static_cast<uint16_t>(data[i]);
@@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {
 
 std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
 {
-  std::vector<uint8_t> frame(RECEIVE_DATA_SIZE, 0x00);
+  std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
   frame[0] = 0xFE;
   frame[1] = 0xFE;
   frame[2] = 0x0B;
@@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& da
 void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
 {
   try {
-    auto port = serial_driver_->port();
+    size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
-    size_t bytes_transmit_size = port->send(frame);
     if (debug) {
       print_hex("Sent", frame, bytes_transmit_size);
     }
@@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
   }
 }
 
-std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec)
+std::vector<uint8_t> AGV_PRO::read_serial_response(
+  const std::vector<uint8_t>& expected_header,
+  size_t payload_size,
+  double timeout_sec)
 {
-  auto port = serial_driver_->port();
   std::vector<uint8_t> sliding_buf;
   uint8_t byte = 0;
 
@@ -82,15 +67,18 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
 
   while ((this->now() - start_time) < timeout) {
-    std::vector<uint8_t> temp_buf(1);
+    boost::asio::mutable_buffers_1 buf(&byte, 1);
-    if (port->receive(temp_buf) == 1) {
+    boost::system::error_code ec;
-      byte = temp_buf[0];
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    if (n == 1) {
       sliding_buf.push_back(byte);
-
       if (sliding_buf.size() > expected_header.size()) {
           sliding_buf.erase(sliding_buf.begin());
       }
-
       if (sliding_buf == expected_header) {
           break;
       }
@@ -104,7 +92,20 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
 
   size_t remain_len = payload_size + 2;
   std::vector<uint8_t> remain_buf(remain_len);
-  if (port->receive(remain_buf) != remain_len) {
+  size_t total_read = 0;
+
+  while (total_read < remain_len && (this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    total_read += n;
+  }
+
+  if (total_read != remain_len) {
     RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
     return {};
   }
@@ -115,22 +116,23 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   return full_buf;
 }
 
-void AGV_PRO::is_power_on(){
+bool AGV_PRO::is_power_on(){
   auto power_query_frame = build_serial_frame(0x12, {});
   send_serial_frame(power_query_frame,true);
 
   const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
-  auto power_query_response = read_serial_response(expected_header, 8, 1.0);
+  RCLCPP_INFO(this->get_logger(),"1111111");
+  auto power_query_response = read_serial_response(expected_header, 8, 12.0);
 
   print_hex("recv_buf", power_query_response);
   
-  if (power_query_response.size() != 14) return;
+  if (power_query_response.size() != 14) return false;
 
   uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
   uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-    return;
+    return false;
   }
 
   int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
@@ -140,19 +142,19 @@ void AGV_PRO::is_power_on(){
     auto status_query_frame = build_serial_frame(0x10, {});
     send_serial_frame(status_query_frame,true);
 
-    rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
+    //rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
 
     const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x10};
     auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
     print_hex("recv_buf", status_query_response);
   
-    if (status_query_response.size() != 14) return;
+    if (status_query_response.size() != 14) return false;
 
     uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
     uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
     if (received_crc != computed_crc) {
       RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-      return;
+      return false;
     }
 
     int poweron_status = static_cast<int8_t>(status_query_response[4]);
@@ -162,37 +164,61 @@ void AGV_PRO::is_power_on(){
       case 1:
         status_msg = "Motor is operating normally.";
         RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return true;
       case 2:
         status_msg = "Emergency stop button is not released.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 3:
         status_msg = "Battery voltage is below 19.5V.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 4:
         status_msg = "CAN initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 5:
         status_msg = "Motor initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       default:
         RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
-        break;
+        return false;
     }
   }
-  else
+  else{
     RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+    return true;
+  }
 }
 
-void AGV_PRO::set_auto_report(){
+void AGV_PRO::set_auto_report(bool enable){
-  auto frame = build_serial_frame(0x23, {0x01});
+  auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
   send_serial_frame(frame,true);
 }
 
+void AGV_PRO::clearSerialBuffer(int fd) {
+  if (::tcflush(fd, TCIOFLUSH) != 0) {
+    RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
+  }
+}
+
+void AGV_PRO::disableDTR_RTS(int fd) {
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
+    } else {
+      RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
+    }
+  } else {
+    RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
+  }
+}
+
 void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
   linearX = std::clamp(msg->linear.x, -1.5, 1.5);
@@ -220,11 +246,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
   std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
 
-  auto port = serial_driver_->port();
-
   try
   {
-    port->send(data_vec);
+    boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
     // print_hex("Sent", data_vec);//debug
   }
   catch(const std::exception &ex)
@@ -235,34 +259,46 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
 bool AGV_PRO::readData()
 {
-  std::vector<uint8_t> buf_header(1);
   std::vector<uint8_t> buf_length(1);
-  std::vector<uint8_t> data_buf(RECEIVE_DATA_SIZE-3);
+  std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
 
-  auto port = serial_driver_->port();
+  uint8_t byte = 0;
+  boost::system::error_code ec;
   
   while (true)
   {
-    size_t ret = port->receive(buf_header);
+    size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
-    if (ret != 1 || buf_header[0] != 0xfe) {
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret != 1 || byte != 0xfe) {
       continue;
     }
 
-    ret = port->receive(buf_header);
+    ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
-    if (ret == 1 && buf_header[0] == 0xfe) {
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret == 1 && byte == 0xfe) {
       break; 
     }
   }
 
-  size_t ret = port->receive(buf_length);
+  size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
+  if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+  }
 
-  if (buf_length[0] != 0x0b) {
+  if (buf_length[0] != RECEIVE_FRAME_SIZE-6) {
     RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
     return false;
   }
 
-  ret = port->receive(data_buf);
+  ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
-  if (ret != data_buf.size())
+  if (ec || ret != data_buf.size())
   {
     RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
     return false;
@@ -271,7 +307,7 @@ bool AGV_PRO::readData()
   std::vector<uint8_t> recv_buf;
   recv_buf.push_back(0xFE);
   recv_buf.push_back(0xFE);
-  recv_buf.push_back(0x0B);
+  recv_buf.push_back(RECEIVE_FRAME_SIZE-6);
   recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
   
   // print_hex("recv_buf", recv_buf); //debug
@@ -281,8 +317,8 @@ bool AGV_PRO::readData()
     return false;
   }
 
-  uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8);
+  uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
-  uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12);
+  uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
 
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
@@ -345,12 +381,12 @@ void AGV_PRO::publisherOdom(double dt)
   odom.pose.pose.position.y = y;
   odom.pose.pose.position.z = 0.0;
   odom.pose.pose.orientation = odom_quat;
-  odom.pose.covariance = odom_pose_covariance;
+  odom.pose.covariance = this->odom_pose_covariance;
 
   odom.twist.twist.linear.x = vx;
   odom.twist.twist.linear.y = vy;
   odom.twist.twist.angular.z = vtheta;
-  odom.twist.covariance = odom_twist_covariance;
+  odom.twist.covariance = this->odom_twist_covariance;
 
   pub_odom->publish(odom);
 }
@@ -401,64 +437,52 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
 
   lastTime = this->get_clock()->now();
       
-  drivers::serial_driver::SerialPortConfig config(
-    1000000,
-    drivers::serial_driver::FlowControl::NONE,
-    drivers::serial_driver::Parity::NONE,
-    drivers::serial_driver::StopBits::ONE
-  );
-
   try{
-    io_context_ = std::make_shared<drivers::common::IoContext>(1);
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
-    serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
+
-    serial_driver_->init_port(device_name_, config);
+    serial_port_->open(device_name_);
-    serial_driver_->port()->open();
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    this->clearSerialBuffer(fd);
+    this->disableDTR_RTS(fd);
 
     RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
-    RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
-    RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
 
-    AGV_PRO::is_power_on();
+    boost::asio::serial_port_base::baud_rate baud_option;
-    AGV_PRO::set_auto_report();
+    serial_port_->get_option(baud_option);
+    unsigned int current_baud = baud_option.value();
+    RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
   }
   catch (const std::exception &ex){
     RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
     return;
   }
 
+  if (this->is_power_on()) {
+    this->set_auto_report(1);
+
     control_timer_ = this->create_wall_timer(
     std::chrono::milliseconds(20),
     std::bind(&AGV_PRO::Control, this)
     );
     RCLCPP_INFO(this->get_logger(), "Control timer started");
-
+  }
+  else {
+    RCLCPP_WARN(this->get_logger(), "Control timer not started.");
+  }
 }
 
 AGV_PRO::~AGV_PRO()
 {
-  std::array<uint8_t, 14> buf = {
+  if (serial_port_ && serial_port_->is_open()) {
-    0xFE, 0xFE, 0x0b, 0x22, 
+    this->set_auto_report(0);
-    0x01, 0x00, 0x00, 0x00,
+    serial_port_->cancel();
-    0x00, 0x00, 0x00, 0x00
+    serial_port_->close();
-  };
-
-  uint16_t crc = crc16_ibm(buf.data(), 12);
-  buf[12] = (crc >> 8) & 0xff;
-  buf[13] = crc & 0xff;
-
-  std::vector<uint8_t> data_vec(buf.begin(), buf.end());
-
-  auto port = serial_driver_->port();
-
-  try
-  {
-    port->send(data_vec);
   } 
-  catch(const std::exception &ex)
-  {
-    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
-  }
-
-  serial_driver_->port()->close();
-  RCLCPP_INFO(this->get_logger(),"Shutting down");
 }