Start of refactor - remove upstream code and just contain deltas

src/elephant_robotics/agv_pro_base/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.8)
+project(agv_pro_base)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(serial_driver REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(agv_pro_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(agv_pro_node 
+  src/agv_pro_node.cpp
+  src/agv_pro_ros.cpp
+)
+
+target_include_directories(agv_pro_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+target_compile_features(agv_pro_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(agv_pro_node
+  rclcpp
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  serial_driver
+  nav_msgs
+  agv_pro_msgs
+)
+
+install(TARGETS agv_pro_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/elephant_robotics/agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -0,0 +1,284 @@
+#ifndef AGV_PRO_DRIVER_H
+#define AGV_PRO_DRIVER_H
+
+#include <algorithm> 
+#include <iostream>
+#include <boost/asio.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <agv_pro_msgs/srv/set_digital_output.hpp>
+#include <agv_pro_msgs/srv/get_digital_input.hpp>
+#include <agv_pro_msgs/srv/set_led_color.hpp>
+#include <agv_pro_msgs/srv/set_led_mode.hpp>
+
+#define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
+#define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
+#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3)   // Payload length (excluding header)
+
+#define POWER_ON 0x10
+#define GET_POWER_STATE 0x12
+#define SET_AUTO_REPORT_STATE 0x23
+#define SET_LED_COLOR 0x34
+#define SET_LED_MODE 0x3A
+#define SET_OUTPUT_IO 0x40
+#define GET_INPUT_IO 0x41
+
+extern std::array<double, 36> odom_pose_covariance;
+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();
+
+  /**
+   * @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);
+
+  /**
+   * @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 ImuSensor publisher
+   */
+  void publisherImuSensor();
+
+  /**
+   * @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);
+
+  /**
+   * @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);
+
+  /**
+   * @brief Handle the SetDigitalOutput service request.
+   *
+   * This service sets the state (HIGH/LOW) of a specific digital output pin on the AGV device.
+   * The request contains the pin number and desired state, which are sent to the hardware
+   * via the serial interface. The response reports whether the operation succeeded.
+   *
+   * @param[in] request The service request, containing:
+   *   - pin: The digital output pin number.
+   *   - state: Desired output state (true = HIGH, false = LOW).
+   * @param[out] response The service response, containing:
+   *   - success: True if the operation succeeded.
+   *   - message: Optional status or error description.
+   */
+  void handleSetDigitalOutput(
+    const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response);
+
+  /**
+   * @brief Handle the GetDigitalInput service request.
+   *
+   * This service reads the state (HIGH/LOW) of a specific digital input pin on the AGV device.
+   * The request specifies the pin number, and the node queries the hardware via the serial
+   * interface to retrieve its current state.
+   *
+   * @param[in] request The service request, containing:
+   *   - pin: The digital input pin number to read.
+   * @param[out] response The service response, containing:
+   *   - state: Current pin state (true = HIGH, false = LOW).
+   *   - success: True if the read operation succeeded.
+   *   - message: Optional status or error description.
+   */
+  void handleGetDigitalInput(
+    const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
+
+  void handleSetLedColor(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
+
+  void handleSetLedMode(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
+
+  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_;
+  std::string frame_id_of_imu_;
+  std::string name_space_;
+  std::string device_name_;
+  
+  double x= 0.0;
+  double y= 0.0;
+  double theta= 0.0;
+
+  double vx= 0.0;
+  double vy= 0.0;
+  double vtheta= 0.0;
+
+  double linearX = 0.0;
+  double linearY = 0.0;
+  double angularZ = 0.0;
+
+  double ax= 0.0;
+  double ay= 0.0;
+  double az= 0.0;
+
+  double wx= 0.0;
+  double wy= 0.0;
+  double wz= 0.0;
+
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
+  int is_poweron_status = 0;
+  int poweron_status = 0;
+
+  uint8_t motor_status = 0;
+  uint8_t motor_error = 0;
+  uint8_t enable_status = 0;
+  
+  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;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
+  rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
+  rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr set_led_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr set_led_mode_service;
+
+  sensor_msgs::msg::Imu imu_data;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
+};
+
+#endif

src/elephant_robotics/agv_pro_base/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_base</name>
+  <version>1.0.8</version>
+  <description>Control Nodes for AGV Pro</description>
+  <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
+  <license>BSD-3-Clause license</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>serial_driver</depend>
+  <depend>asio_cmake_module</depend>
+  <depend>io_context</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>agv_pro_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/elephant_robotics/agv_pro_base/src/agv_pro_node.cpp

@@ -0,0 +1,12 @@
+#include "agv_pro_base/agv_pro_driver.h"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<AGV_PRO>("agv_pro_base_node");
+
+  rclcpp::spin(node);
+  
+  rclcpp::shutdown();
+  return 0;
+}

src/elephant_robotics/agv_pro_base/src/agv_pro_ros.cpp

@@ -0,0 +1,717 @@
+#include "agv_pro_base/agv_pro_driver.h"
+
+uint16_t AGV_PRO::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]);
+    for (int j = 0; j < 8; ++j) {
+      if (crc & 0x0001)
+        crc = (crc >> 1) ^ 0xA001;
+      else
+        crc = crc >> 1;
+    }
+  }
+  return crc;
+}
+
+std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
+{
+  std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
+  frame[0] = 0xFE;
+  frame[1] = 0xFE;
+  frame[2] = 0x0B;
+  frame[3] = cmd_id;
+
+  for (size_t i = 0; i < payload.size() && i < 8; ++i) {
+    frame[4 + i] = payload[i];
+  }
+
+  uint16_t crc = crc16_ibm(frame.data(), 12);
+  frame[12] = (crc >> 8) & 0xff;
+  frame[13] = crc & 0xff;
+
+  return frame;
+}
+
+void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size) {
+  std::stringstream ss;
+  for (auto b : data) {
+    ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
+       << static_cast<int>(b) << " ";
+  }
+  size_t len = override_size.value_or(data.size());
+  RCLCPP_INFO(this->get_logger(), "%s (%zu bytes): [%s]", label.c_str(), len, ss.str().c_str());
+}
+
+void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
+{
+  try {
+    size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
+    if (debug) {
+      print_hex("Sent", frame, bytes_transmit_size);
+    }
+  } catch (const std::exception &ex) {
+    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port: %s", ex.what());
+  }
+}
+
+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> sliding_buf;
+  uint8_t byte = 0;
+
+  rclcpp::Time start_time = this->now();
+  rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
+
+  while ((this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&byte, 1);
+    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 {};
+    }
+    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;
+      }
+    }
+  }
+  
+  if (sliding_buf != expected_header) {
+    RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
+    return {};
+  }
+
+  size_t remain_len = payload_size + 2;
+  std::vector<uint8_t> 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 {};
+  }
+
+  std::vector<uint8_t> full_buf = expected_header;
+  full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end());
+
+  return full_buf;
+}
+
+bool AGV_PRO::is_power_on(){
+  auto power_query_frame = build_serial_frame(GET_POWER_STATE, {});
+  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, 12.0);
+
+  print_hex("recv_buf", power_query_response);
+  
+  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 false;
+  }
+
+  int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
+  RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status);
+
+  if (is_poweron_status == 0){
+    auto status_query_frame = build_serial_frame(POWER_ON, {});
+    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
+
+    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 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 false;
+    }
+
+    int poweron_status = static_cast<int8_t>(status_query_response[4]);
+    std::string status_msg;
+
+    switch (poweron_status) {
+      case 1:
+        status_msg = "Motor is operating normally.";
+        RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        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());
+        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());
+        return false;
+      case 4:
+        status_msg = "CAN initialization error.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      case 5:
+        status_msg = "Motor initialization error.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      default:
+        RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
+        return false;
+    }
+  }
+  else{
+    RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+    return true;
+  }
+}
+
+void AGV_PRO::set_auto_report(bool enable){
+  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);
+  linearY = std::clamp(msg->linear.y, -1.0, 1.0);
+  angularZ = std::clamp(msg->angular.z, -1.0, 1.0);
+
+  int16_t x_send = static_cast<int16_t>(linearX * 100);
+  int16_t y_send = static_cast<int16_t>(linearY * 100);
+  int16_t rot_send = static_cast<int16_t>(angularZ * 100);
+
+  uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 };
+
+  buf[4] = (x_send >> 8) & 0xff;
+  buf[5] = x_send & 0xff;
+  buf[6] = (y_send >> 8) & 0xff;
+  buf[7] = y_send & 0xff;
+  buf[8] = (rot_send >> 8) & 0xff;
+  buf[9] = rot_send & 0xff;
+  buf[10] = 0x00;
+  buf[11] = 0x00;
+
+  uint16_t crc = crc16_ibm(buf, 12);
+  buf[12] = (crc >> 8) & 0xff;
+  buf[13] = crc & 0xff;
+
+  std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
+
+  try
+  {
+    boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
+    // print_hex("Sent", data_vec);//debug
+  }
+  catch(const std::exception &ex)
+  {
+    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
+  }
+}
+
+void AGV_PRO::handleSetDigitalOutput(
+  const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response)
+{
+  uint8_t output_number = request->pin;
+  uint8_t output_state = request->state;
+
+  if (output_number < 1 || output_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number);
+    response->success = false;
+    response->message = "Invalid output pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetDigitalOutput succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleGetDigitalInput(
+  const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response)
+{
+  uint8_t input_number = request->pin;
+
+  if (input_number < 1 || input_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number);
+    response->success = false;
+    response->message = "Invalid input pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(GET_INPUT_IO, {input_number});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[5];
+  if (status == 0xff) {
+    RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
+    response->success = false;
+  } else {
+    RCLCPP_DEBUG(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    response->state = static_cast<int32_t>(status);
+    response->success = true;
+    response->message = "Success";
+  }
+}
+
+void AGV_PRO::handleSetLedColor(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response)
+{
+  if (request->position < 0 || request->position > 1) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid LED position: %d", request->position);
+    response->success = false;
+    response->message = "Invalid LED position";
+    return;
+  }
+
+  if (request->brightness < 0 || request->brightness > 255) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid brightness: %d", request->brightness);
+    response->success = false;
+    response->message = "Invalid brightness";
+    return;
+  }
+
+  if (request->r < 0 || request->r > 255 ||
+      request->g < 0 || request->g > 255 ||
+      request->b < 0 || request->b > 255) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Invalid RGB value: r=%d g=%d b=%d",
+      request->r, request->g, request->b
+    );
+    response->success = false;
+    response->message = "Invalid RGB value";
+    return;
+  }
+
+  uint8_t position   = static_cast<uint8_t>(request->position);
+  uint8_t brightness = static_cast<uint8_t>(request->brightness);
+  uint8_t r          = static_cast<uint8_t>(request->r);
+  uint8_t g          = static_cast<uint8_t>(request->g);
+  uint8_t b          = static_cast<uint8_t>(request->b);
+
+  auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_COLOR};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedColor succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedColor failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleSetLedMode(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response)
+{
+  uint8_t mode = request->mode ? 0x01 : 0x00;
+
+  auto frame = build_serial_frame(SET_LED_MODE, {mode});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_MODE};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedMode succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedMode failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+bool AGV_PRO::readData()
+{
+  std::vector<uint8_t> buf_length(1);
+  std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
+
+  uint8_t byte = 0;
+  boost::system::error_code ec;
+  
+  while (true)
+  {
+    size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret != 1 || byte != 0xfe) {
+      continue;
+    }
+
+    ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    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 = 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] != RECEIVE_PAYLOAD_SIZE) {
+    //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
+    return false;
+  }
+
+  ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
+  if (ec || ret != data_buf.size())
+  {
+    RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
+    return false;
+  }
+
+  std::vector<uint8_t> recv_buf;
+  recv_buf.push_back(0xFE);
+  recv_buf.push_back(0xFE);
+  recv_buf.push_back(0x1C);
+  recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
+  
+  // print_hex("recv_buf", recv_buf); //debug
+
+  if (recv_buf[3] != 0x25) {
+    //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
+    return false;
+  }
+
+  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(), 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);
+    return false;
+  }
+
+  vx = static_cast<double>(static_cast<int8_t>(recv_buf[4])) * 0.01;
+  vy = static_cast<double>(static_cast<int8_t>(recv_buf[5])) * 0.01;
+  vtheta = static_cast<double>(static_cast<int8_t>(recv_buf[6])) * 0.01;
+
+  motor_status = recv_buf[7];
+  motor_error  = recv_buf[8];
+  battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
+  enable_status = recv_buf[10];
+
+  imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
+
+  imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
+
+  roll  = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
+  pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
+  yaw   = static_cast<double>(static_cast<int16_t>((recv_buf[27] << 8) | recv_buf[28])) * 0.01;
+
+  // RCLCPP_INFO(this->get_logger(),
+  // "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], "
+  // "Gyro[x: %.2f, y: %.2f, z: %.2f], "
+  // "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]",
+  // imu_data.linear_acceleration.x,
+  // imu_data.linear_acceleration.y,
+  // imu_data.linear_acceleration.z,
+  // imu_data.angular_velocity.x,
+  // imu_data.angular_velocity.y,
+  // imu_data.angular_velocity.z,
+  // roll, pitch, yaw);
+
+  return true;
+}
+
+void AGV_PRO::publisherVoltage()
+{
+  std_msgs::msg::Float32 voltage_msg,voltage_backup_msg;
+  voltage_msg.data = battery_voltage;
+  pub_voltage->publish(voltage_msg);
+}
+
+void AGV_PRO::publisherImuSensor()
+{
+  sensor_msgs::msg::Imu ImuSensor;
+
+  ImuSensor.header.stamp = this->get_clock()->now();
+  ImuSensor.header.frame_id = "imu_link";
+
+  tf2::Quaternion qua;
+  qua.setRPY(0, 0, yaw * M_PI / 180.0);
+
+  ImuSensor.orientation.x = qua[0];
+  ImuSensor.orientation.y = qua[1];
+  ImuSensor.orientation.z = qua[2];
+  ImuSensor.orientation.w = qua[3];
+
+  ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
+  ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
+  ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
+
+  ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
+  ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
+  ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
+
+  ImuSensor.orientation_covariance[0] = 1e6;
+  ImuSensor.orientation_covariance[4] = 1e6;
+  ImuSensor.orientation_covariance[8] = 1e-6;
+
+  ImuSensor.angular_velocity_covariance[0] = 1e6;
+  ImuSensor.angular_velocity_covariance[4] = 1e6;
+  ImuSensor.angular_velocity_covariance[8] = 1e-6;
+
+  pub_imu->publish(ImuSensor);
+}
+
+void AGV_PRO::publisherOdom(double dt)
+{
+  currentTime = this->get_clock()->now();
+
+  double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
+  double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
+  double delta_th = vtheta * dt;
+
+  x += delta_x;
+  y += delta_y;
+  theta += delta_th;
+
+  geometry_msgs::msg::TransformStamped odom_trans;
+  odom_trans.header.stamp = currentTime;
+  odom_trans.header.frame_id = frame_id_of_odometry_;
+  odom_trans.child_frame_id = child_frame_id_of_odometry_;
+
+  tf2::Quaternion quat;
+  quat.setRPY(0.0, 0.0, theta);
+  geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
+
+  odom_trans.transform.translation.x = x; 
+  odom_trans.transform.translation.y = y; 
+  odom_trans.transform.translation.z = 0.0;
+  odom_trans.transform.rotation = odom_quat;
+
+  odomBroadcaster->sendTransform(odom_trans);
+
+  nav_msgs::msg::Odometry odom;
+  odom.header.stamp = currentTime;
+  odom.header.frame_id = frame_id_of_odometry_;
+  odom.child_frame_id = child_frame_id_of_odometry_;
+
+  odom.pose.pose.position.x = x;
+  odom.pose.pose.position.y = y;
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = odom_quat;
+  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 = this->odom_twist_covariance;
+
+  pub_odom->publish(odom);
+}
+
+void AGV_PRO::Control()
+{
+  if (true == readData())
+  {
+    currentTime = this->get_clock()->now();
+    double dt = 0.0;
+    if (lastTime.nanoseconds() != 0) {
+      dt = (currentTime - lastTime).seconds();
+    }
+
+    lastTime = currentTime;
+    publisherOdom(dt);
+    // RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
+    publisherVoltage();
+    publisherImuSensor();
+  }
+}
+
+AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
+{
+  this->declare_parameter<std::string>("port_name","/dev/agvpro_controller");
+  this->declare_parameter<std::string>("odometry.frame_id", "odom");
+  this->declare_parameter<std::string>("odometry.child_frame_id", "base_footprint");
+  this->declare_parameter<std::string>("imu.frame_id", "imu_link");
+  this->declare_parameter<std::string>("namespace", "");
+
+  this->get_parameter_or<std::string>("port_name",device_name_,std::string("/dev/agvpro_controller"));
+  this->get_parameter_or<std::string>("odometry.frame_id",frame_id_of_odometry_,std::string("odom"));
+  this->get_parameter_or<std::string>("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint"));
+  this->get_parameter_or<std::string>("imu.frame_id",frame_id_of_imu_,std::string("imu_link"));        
+  this->get_parameter_or<std::string>("namespace",name_space_,std::string(""));
+
+  if (name_space_ != "") {
+    frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_;
+    child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_;
+    frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_;
+  }
+
+  odomBroadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+  pub_imu =  this->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
+  pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", 50);
+  pub_voltage = create_publisher<std_msgs::msg::Float32>("voltage", 10);
+  cmd_sub = this->create_subscription<geometry_msgs::msg::Twist>(
+    "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
+
+  set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
+    "set_digital_output",
+    std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
+    "get_digital_input",
+    std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  set_led_service = this->create_service<agv_pro_msgs::srv::SetLedColor>(
+    "set_led_color",
+    std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  set_led_mode_service = this->create_service<agv_pro_msgs::srv::SetLedMode>(
+    "set_led_mode",
+    std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  lastTime = this->get_clock()->now();
+      
+  try{
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
+
+    serial_port_->open(device_name_);
+    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::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
+
+    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
+
+    boost::asio::serial_port_base::baud_rate baud_option;
+    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()
+{
+  if (serial_port_ && serial_port_->is_open()) {
+    this->set_auto_report(0);
+    serial_port_->cancel();
+    serial_port_->close();
+  } 
+}