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
index 1015135..989027e 100644
--- a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
+++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
@@ -39,8 +39,6 @@ private:
   std::string name_space_;
   std::string device_name_;
   
-  std::thread control_thread_;
-
   double x= 0.0;
   double y= 0.0;
   double theta= 0.0;
@@ -60,6 +58,7 @@ private:
   float battery_voltage = 0.0f;
 
   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;
diff --git a/agv_pro_base/package.xml b/agv_pro_base/package.xml
index 986e49f..c669a21 100644
--- a/agv_pro_base/package.xml
+++ b/agv_pro_base/package.xml
@@ -2,7 +2,7 @@
 <?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>
+  <version>1.0.1</version>
   <description>Control Nodes for AGV Pro</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>
diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp
index 70b93ad..f22e75b 100644
--- a/agv_pro_base/src/agv_pro_ros.cpp
+++ b/agv_pro_base/src/agv_pro_ros.cpp
@@ -243,18 +243,18 @@ void AGV_PRO::publisherOdom(double dt)
 
 void AGV_PRO::Control()
 {
-  lastTime = this->get_clock()->now();
-  while(rclcpp::ok())
+  if (true == readData())
   {
     currentTime = this->get_clock()->now();
-    double dt = (currentTime - lastTime).seconds();
-    if (true == readData()) 
-    {    
-      publisherOdom(dt);
-      //RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
-      publisherVoltage();
+    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();
   }
 }
 
@@ -284,6 +284,8 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
   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));
+
+  lastTime = this->get_clock()->now();
       
   drivers::serial_driver::SerialPortConfig config(
     1000000,
@@ -309,7 +311,11 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
     return;
   }
   
-  control_thread_ = std::thread(&AGV_PRO::Control, this);
+  control_timer_ = this->create_wall_timer(
+  std::chrono::milliseconds(20),
+  std::bind(&AGV_PRO::Control, this)
+  );
+
 }
 
 AGV_PRO::~AGV_PRO()