diff --git a/agv_pro_base/CMakeLists.txt b/agv_pro_base/CMakeLists.txt
new file mode 100644
index 0000000..537e256
--- /dev/null
+++ b/agv_pro_base/CMakeLists.txt
@@ -0,0 +1,48 @@
+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)
+# 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)
+
+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
+  std_msgs
+  geometry_msgs
+  serial_driver
+)
+
+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()
diff --git a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
new file mode 100644
index 0000000..ccacd43
--- /dev/null
+++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
@@ -0,0 +1,84 @@
+#ifndef AGV_PRO_DRIVER_H
+#define AGV_PRO_DRIVER_H
+
+#include <algorithm> 
+#include "serial_driver/serial_driver.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.h>
+
+#define RECEIVE_DATA_SIZE 13         //The length of the data sent by the esp32
+#define SEND_DATA_SIZE    13         //The length of data sent by ROS to the esp32
+#define RETURN_COMMAND 0x25
+
+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} };
+
+class AGV_PRO : public rclcpp::Node
+{
+public:
+  AGV_PRO(std::string node_name);
+  ~AGV_PRO();
+private:
+  void Control();
+  bool readData();
+  void publisherOdom(double dt);
+  void publisherVoltage();
+  void clearSerialBuffer();
+  void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+  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;
+
+  uint8_t motor_status = 0;
+  uint8_t motor_error = 0;
+  uint8_t enable_status = 0;
+  
+  float battery_voltage = 0.0f;
+
+  rclcpp::Time currentTime, lastTime;
+  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;
+
+  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
\ No newline at end of file
diff --git a/agv_pro_base/package.xml b/agv_pro_base/package.xml
new file mode 100644
index 0000000..cc71e2b
--- /dev/null
+++ b/agv_pro_base/package.xml
@@ -0,0 +1,23 @@
+<?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.0</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>serial_driver</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_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>
diff --git a/agv_pro_base/src/agv_pro_node.cpp b/agv_pro_base/src/agv_pro_node.cpp
new file mode 100644
index 0000000..acb2f8a
--- /dev/null
+++ b/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;
+}
\ No newline at end of file
diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp
new file mode 100644
index 0000000..b798d8c
--- /dev/null
+++ b/agv_pro_base/src/agv_pro_ros.cpp
@@ -0,0 +1,262 @@
+#include "agv_pro_base/agv_pro_driver.h"
+
+AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
+{
+  this->declare_parameter<std::string>("port_name","/dev/ttyS0");
+  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(""));
+  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));
+      
+  drivers::serial_driver::SerialPortConfig config(
+    115200,
+    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_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
+    serial_driver_->init_port(device_name_, config);
+    serial_driver_->port()->open();
+    
+    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(), "Baud_rate: %d", config.get_baud_rate());
+  }
+  catch (const std::exception &ex){
+    RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
+    return;
+  }
+  
+  AGV_PRO::Control();//Loop through data collection and publish the topic
+
+}
+
+AGV_PRO::~AGV_PRO()
+{
+
+}
+
+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]);
+    for (int j = 0; j < 8; ++j) {
+      if (crc & 0x0001)
+        crc = (crc >> 1) ^ 0xA001;
+      else
+        crc = crc >> 1;
+    }
+  }
+  return crc;
+}
+
+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 * 1000);
+  int16_t y_send = static_cast<int16_t>(linearY * 1000);
+  int16_t rot_send = static_cast<int16_t>(angularZ * 1000);
+
+  uint8_t buf[13] = { 0xfe,0xfe,0x21 };
+
+  buf[3] = x_send & 0xff;
+  buf[4] = (x_send >> 8) & 0xff;
+  buf[5] = y_send & 0xff;
+  buf[6] = (y_send >> 8) & 0xff;
+  buf[7] = rot_send & 0xff;
+  buf[8] = (rot_send >> 8) & 0xff;
+
+  uint16_t crc = crc16_ibm(buf, 9);
+  buf[9] = crc & 0xff;
+  buf[10] = (crc >> 8) & 0xff;
+
+  std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
+
+  auto port = serial_driver_->port();
+
+  try
+  {
+    size_t bytes_transmit_size = port->send(data_vec);
+
+    // std::stringstream ss;
+    // for (auto b : data_vec) {
+    //   ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
+    //      << static_cast<int>(b) << " ";
+    // }
+    // RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str());
+  }
+  catch(const std::exception &ex)
+  {
+    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
+  }
+}
+
+void AGV_PRO::clearSerialBuffer() 
+{
+  auto port = serial_driver_->port();
+  std::vector<uint8_t> temp_buf(64);
+  size_t total_discarded = 0;
+
+  while (true) {
+    size_t n = port->receive(temp_buf);
+    if (n == 0) {
+      break;
+    }
+    total_discarded += n;
+  }
+
+  RCLCPP_WARN(this->get_logger(), "Serial buffer flushed, %zu bytes discarded.", total_discarded);
+}
+
+bool AGV_PRO::readData()
+{
+  std::vector<uint8_t> buf_header(1);
+  std::vector<uint8_t> recv_buf(RECEIVE_DATA_SIZE);
+
+  auto port = serial_driver_->port();
+  
+  while (true)
+  {
+    size_t ret = port->receive(buf_header);
+    if (ret != 1 || buf_header[0] != 0xfe) {
+      continue;
+    }
+
+    ret = port->receive(buf_header);
+    if (ret == 1 && buf_header[0] == 0xfe) {
+      break; 
+    }
+  }
+
+  std::vector<uint8_t> remaining_buf(RECEIVE_DATA_SIZE - 2);
+  size_t ret = port->receive(remaining_buf);
+  if (ret != (RECEIVE_DATA_SIZE - 2)) {
+    RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%zu", ret);
+    clearSerialBuffer();
+    return false;
+  }
+
+  recv_buf[0] = 0xFE;
+  recv_buf[1] = 0xFE;
+  std::copy(remaining_buf.begin(), remaining_buf.end(), recv_buf.begin() + 2);
+
+  if (recv_buf[2] != 0x25) {
+    RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
+    clearSerialBuffer();
+    return false;
+  }
+
+  uint16_t received_crc = recv_buf[11] | (recv_buf[12] << 8);
+  uint16_t computed_crc = crc16_ibm(recv_buf.data(), 11);
+
+  if (received_crc != computed_crc) {
+    RCLCPP_ERROR(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
+    return false;
+  }
+
+  vx = (static_cast<double>(recv_buf[3]) - 128.0) * 0.01;
+  vy = (static_cast<double>(recv_buf[4]) - 128.0) * 0.01;
+  vtheta = (static_cast<double>(recv_buf[5]) - 128.0) * 0.01;
+
+  motor_status = recv_buf[6];
+  motor_error  = recv_buf[7];
+  battery_voltage = static_cast<float>(recv_buf[8]) / 10.0f;
+  enable_status = recv_buf[9];
+
+  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::publisherOdom(double dt)
+{   
+  geometry_msgs::msg::TransformStamped odom_trans;
+  odom_trans.header.stamp = this->get_clock()->now();
+  odom_trans.header.frame_id = "odom";
+  odom_trans.child_frame_id = "base_footprint";
+
+  tf2::Quaternion quat;
+  quat.setRPY(0.0, 0.0, theta);
+  geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
+
+  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;
+
+  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 = this->get_clock()->now();;
+  odom.header.frame_id = "odom";
+  odom.child_frame_id = "base_footprint";
+
+  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 = 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;
+
+  pub_odom->publish(odom);
+}
+
+void AGV_PRO::Control()
+{
+  lastTime = this->get_clock()->now();
+  while(rclcpp::ok())
+  {
+    currentTime = this->get_clock()->now();
+    double dt = (currentTime - lastTime).seconds();
+    if (true == readData()) 
+    {    
+      publisherOdom(dt);
+      //RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
+      publisherVoltage();
+    }
+    lastTime = currentTime;
+  }
+}
\ No newline at end of file