diff --git a/motor_dev/CMakeLists.txt b/motor_dev/CMakeLists.txt
index 790395e..a78e553 100644
--- a/motor_dev/CMakeLists.txt
+++ b/motor_dev/CMakeLists.txt
@@ -10,8 +10,8 @@ find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
-# rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
+#rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
+rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
 include_directories(include ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp)
 add_executable(motor_dev_node src/main.cpp src/rs485_driver.cpp)
 add_dependencies(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
diff --git a/motor_dev/src/main.cpp b/motor_dev/src/main.cpp
index 86af296..c4552c0 100644
--- a/motor_dev/src/main.cpp
+++ b/motor_dev/src/main.cpp
@@ -1,6 +1,7 @@
 #include"rclcpp/rclcpp.hpp"
 #include"motor_dev/rs485_driver.hpp"
 #include"motor_dev/msg/motor_cmd.hpp"
+#include"motor_dev/msg/motor_pos.hpp"
 using namespace motor_dev;
 RS485Driver rs485_driver_;
 
@@ -12,27 +13,26 @@ int main(int argc,char* argv[]){
     if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
         std::cout << "Failed to open RS485 port." << std::endl;
     }
-    usleep(100000);
-    
     
     rs485_driver_.bm_set_mode(1,4);
-    //usleep(10000);
     rs485_driver_.bm_set_enable(1,2);
-    //usleep(10000);
     rs485_driver_.bm_set_mode(2,4);
-    //usleep(10000);
     rs485_driver_.bm_set_enable(2,2);
-    //usleep(10000);
     //rs485_driver_.mt_set_pos(1,-5);
     //init
     /////rs485_driver_.bm_set_mode(1,3);//位置模式
     auto rs485_sub_=node->create_subscription<motor_dev::msg::MotorCmd>("/motor_cmd",10,[](const motor_dev::msg::MotorCmd::SharedPtr msg){
-            size_t n = msg->motor_id.size();
+            ///
             if (msg->target == "rs485") {
                 if(msg->type=="bm"){
+                    size_t n = msg->motor_id.size();
+                    if(n<2){
+                        printf("bm msg need two angle\n");
+                        return;
+                    }
                     float angle1    =msg->motor_angle[0];
                     float angle2    =msg->motor_angle[1];
-                    std::cout<<"########:"<<msg->type<<","<<angle1<<","<<angle2<<std::endl;
+                    printf("##############:%s,%.1f,%.1f\n",msg->type.c_str(),angle1,angle2);
                     rs485_driver_.bm_set_pos(angle1,angle2);
                 }else if(msg->type=="mt"){
                     uint8_t id=msg->motor_id[0];
@@ -43,25 +43,44 @@ int main(int argc,char* argv[]){
                 //TODO
             }
         });
+    auto motor_pub=node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos",10);
     //读取电机位置
     rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() { 
-        //rs485_driver_.bm_set_pos(1);
-        float angle=rs485_driver_.bm_get_pos(2);
-        printf("bm cur angle:%.1f\n",angle);
+        static int pub_cnt=0;
+        motor_dev::msg::MotorPos motor_pos;
+        float angle1=rs485_driver_.bm_get_pos(1);
+        float angle2=rs485_driver_.bm_get_pos(2);
+        ///printf("bm cur angle:%.1f\n",angle);
         ///rs485_driver_.bm_query_id();
         //rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95 
-        //float angle=rs485_driver_.mt_get_pos(1);
-        //if(angle!=9999)
-        //    printf("angle:%.1f\n",angle);
-        // MotorPos motor_pos;
-        // input_msg->source = cfg.getSource();
-        // input_msg->type = cfg.getType();
-        // input_msg->position=cfg.getPosition();
-        // input_msg->code=key;
-        // input_msg->value=val;
-        // input_pub->publish(*input_msg);
-        // pub_cnt+=1;
-        // std::cout<<"pub msg["<<pub_cnt<<"]:"<<key<<std::endl;
+        motor_pos.motor_id.clear();
+        motor_pos.motor_angle.clear();
+        printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
+        if(angle1>-1&&angle2>-1)
+        {
+            motor_pos.source = "rs485";
+            motor_pos.type = "bm";
+            motor_pos.position="none";
+            motor_pos.motor_id.push_back(1);
+            motor_pos.motor_id.push_back(2);
+            motor_pos.motor_angle.push_back(angle1);
+            motor_pos.motor_angle.push_back(angle2);
+            motor_pub->publish(motor_pos);
+            
+        }
+        pub_cnt+=1;
+        #if 1
+        motor_pos.motor_id.clear();
+        motor_pos.motor_angle.clear();
+        float angle=rs485_driver_.mt_get_pos(1);
+        if(angle!=9999){
+            motor_pos.type = "mt";
+            motor_pos.motor_id.push_back(1);
+            motor_pos.motor_angle.push_back(angle);
+            motor_pub->publish(motor_pos);
+            std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle<<std::endl;
+        }
+        #endif
     });
     
     ////rs485_driver_.closePort();
diff --git a/motor_dev/src/rs485_driver.cpp b/motor_dev/src/rs485_driver.cpp
index fee028d..8efad2a 100644
--- a/motor_dev/src/rs485_driver.cpp
+++ b/motor_dev/src/rs485_driver.cpp
@@ -376,7 +376,7 @@ namespace motor_dev
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 20);
+            ret=receiveData(data, 20, 50);
             if(ret){
                 printf("<---:");
                 for(auto d:data)
@@ -388,6 +388,9 @@ namespace motor_dev
                     float angle=360.0*pos/32768.0;
                     return angle;
                     ///printf("pos:%d,angle:%.1f\n",pos,angle);
+                }
+                else{
+
                 }
                 //float angle = pos * 360.0 /255.0;
                 ///float angle=0;