From bfe060e01a2115bc03cb03b11925d13c174fec83 Mon Sep 17 00:00:00 2001
From: NuoDaJia02 <NuoDaJia02@hivecore.cn>
Date: Fri, 24 Oct 2025 10:40:42 +0800
Subject: [PATCH] add dev_test

---
 drv_test/CMakeLists.txt        | 30 ++++++++++++++++
 drv_test/package.xml           | 23 +++++++++++++
 drv_test/src/main.cpp          | 63 ++++++++++++++++++++++++++++++++++
 motor_dev/src/main.cpp         |  2 +-
 motor_dev/src/rs485_driver.cpp |  8 ++---
 5 files changed, 121 insertions(+), 5 deletions(-)
 create mode 100644 drv_test/CMakeLists.txt
 create mode 100644 drv_test/package.xml
 create mode 100644 drv_test/src/main.cpp

diff --git a/drv_test/CMakeLists.txt b/drv_test/CMakeLists.txt
new file mode 100644
index 0000000..31469eb
--- /dev/null
+++ b/drv_test/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(drv_test)
+
+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(sensor_msgs REQUIRED)
+find_package(motor_dev REQUIRED)
+find_package(interfaces REQUIRED)
+
+add_executable(motor_test src/main.cpp)
+ament_target_dependencies(motor_test rclcpp motor_dev interfaces)
+install(TARGETS motor_test 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/drv_test/package.xml b/drv_test/package.xml
new file mode 100644
index 0000000..55d6c36
--- /dev/null
+++ b/drv_test/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>drv_test</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="NuoDaJia02@hivecore.cn">demo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msg</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>motor_dev</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/drv_test/src/main.cpp b/drv_test/src/main.cpp
new file mode 100644
index 0000000..997f55b
--- /dev/null
+++ b/drv_test/src/main.cpp
@@ -0,0 +1,63 @@
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+
+using namespace std::chrono_literals;
+
+using MotorCmd = interfaces::msg::MotorCmd;
+
+class MotorCmdPublisher : public rclcpp::Node
+{
+public:
+  MotorCmdPublisher()
+  : Node("motor_test_node")
+  {
+    publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
+    
+    timer_ = this->create_wall_timer(100ms, std::bind(&MotorCmdPublisher::timer_callback, this));
+  }
+
+private:
+  int angle1=9999;
+  int angle2=6666;
+  void timer_callback()
+  {
+    auto message = MotorCmd();
+    
+    message.target = "rs485";
+    message.type = "bm";
+    message.position = "";
+    
+    message.motor_id.push_back(1);
+    message.motor_id.push_back(2);
+    idx+=1;
+    //angle1=angle1+20;
+    //angle2=angle2-20;
+    /*
+    if(idx%2==0){
+    }else{
+	angle1=-300;
+	angle2=300;
+    }
+    */
+    message.motor_angle.push_back((float)angle1);
+    message.motor_angle.push_back((float)angle2);
+    publisher_->publish(message);
+    
+    RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2);
+    
+    timer_->cancel();
+  }
+  int idx=0; 
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MotorCmdPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/motor_dev/src/main.cpp b/motor_dev/src/main.cpp
index da9e265..f839293 100644
--- a/motor_dev/src/main.cpp
+++ b/motor_dev/src/main.cpp
@@ -64,7 +64,7 @@ int main(int argc,char* argv[]){
         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)
+        //if(angle1>-1&&angle2>-1)
         {
             motor_pos.source = "rs485";
             motor_pos.type = "bm";
diff --git a/motor_dev/src/rs485_driver.cpp b/motor_dev/src/rs485_driver.cpp
index 1eec4d6..97214cb 100644
--- a/motor_dev/src/rs485_driver.cpp
+++ b/motor_dev/src/rs485_driver.cpp
@@ -415,10 +415,10 @@ namespace motor_dev
                 printf("\n");
                 uint8_t nid=0x70+motor_id;
                 if(data[0]==0&&data[1]==nid){
-                    uint16_t pos = (data[2]<<8)+(data[3]&0xff);
-                    float angle=360.0*pos/32768.0;
-                    pos=(data[6]<<8)+(data[7]&0xff);
-                    angle=3.60f*pos;
+                    //uint16_t pos = (data[2]<<8)+(data[3]&0xff);
+                    //float angle=360.0*pos/32768.0;
+                    int16_t pos=(data[6]<<8)+(data[7]&0xff);
+                    float angle=3.60f*pos;
                     //printf("id:%d,pos:%d\n",motor_id,pos);
                     return angle;
                     ///printf("pos:%d,angle:%.1f\n",pos,angle);