[Spinal][Dynamixel] refactor code to support quick reading for Dynami…

…xel. The current freq. for servo is 100hz, and the current freq. for dshot is 125hz. Tested on real machine.
sugihara-16 · Oct 3, 2024 · 54ce3fc · 54ce3fc
1 parent 3c7857f
commit 54ce3fc
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 9 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.cpp
@@ -229,7 +229,7 @@ void DynamixelSerial::update()
     }
     if (instruction_last_.first == INST_GET_PRESENT_POS)
     {
-    	flag_new_servo_data_ = true;
+      setROSCommFlag(true);
     }
   }
   read_status_packet_flag_ = false;

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/Dynamixel/dynamixel_serial.h
@@ -345,6 +345,10 @@ class DynamixelSerial
   const std::array<ServoData, MAX_SERVO_NUM>& getServo() const {return servo_;}
   ServoData& getOneServo(uint8_t id);
   uint8_t getServoIndex(uint8_t id);
+
+  void setROSCommFlag(bool flag) {flag_send_ros_ = flag;}
+  bool getROSCommFlag() const {return flag_send_ros_;}
+
 private:
   UART_HandleTypeDef* huart_; // uart handlercmdReadPresentPosition
   osMutexId* mutex_; // for UART (RS485) I/O mutex
@@ -364,6 +368,9 @@ class DynamixelSerial
   // uint8_t rx_buf_[RX_BUFFER_SIZE];
   uint32_t rd_ptr_;
 
+  /* ros comm */
+  bool flag_send_ros_ = false;
+
   /* a new and quicker method to read servo data */
   bool read_status_packet_flag_ = false;
   std::pair<uint8_t, uint8_t> instruction_last_ = std::make_pair(255, 255);

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp
@@ -23,13 +23,15 @@ void DirectServo::init(UART_HandleTypeDef* huart,  ros::NodeHandle* nh, osMutexI
   nh_->advertiseService(board_info_srv_);
 
   //temp
-  servo_state_msg_.servos_length = MAX_SERVO_NUM;
-  servo_state_msg_.servos = new spinal::ServoState[MAX_SERVO_NUM];
-  servo_torque_state_msg_.torque_enable_length = MAX_SERVO_NUM;
-  servo_torque_state_msg_.torque_enable = new uint8_t[MAX_SERVO_NUM];
-
   servo_handler_.init(huart, mutex);
 
+  unsigned int actual_servo_num = servo_handler_.getServoNum();
+
+  servo_state_msg_.servos_length = actual_servo_num;
+  servo_state_msg_.servos = new spinal::ServoState[actual_servo_num];
+  servo_torque_state_msg_.torque_enable_length = actual_servo_num;
+  servo_torque_state_msg_.torque_enable = new uint8_t[actual_servo_num];
+
   servo_last_pub_time_ = 0;
   servo_torque_last_pub_time_ = 0;
 
@@ -42,15 +44,38 @@ void DirectServo::init(UART_HandleTypeDef* huart,  ros::NodeHandle* nh, osMutexI
 void DirectServo::update()
 {
   servo_handler_.update();
-  sendData();
-
+  sendData(true);
 }
 
-void DirectServo::sendData()
+void DirectServo::sendData(bool flag_send_asap)
 {
   uint32_t now_time = HAL_GetTick();
+
+  if (flag_send_asap && servo_handler_.getROSCommFlag() == true)  // This setting will ignore the setting of SERVO_PUB_INTERVAL and pub the information once the measurement is updated.
+  {
+	  servo_state_msg_.stamp = nh_->now();
+      for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
+        const ServoData& s = servo_handler_.getServo()[i];
+        if (s.send_data_flag_ != 0) {
+          spinal::ServoState servo;
+          servo.index = i;
+          servo.angle = s.present_position_;
+          servo.temp = s.present_temp_;
+          servo.load = s.present_current_;
+          servo.error = s.hardware_error_status_;
+          servo_state_msg_.servos[i] = servo;
+        }
+      }
+      servo_state_pub_.publish(&servo_state_msg_);
+      servo_last_pub_time_ = now_time;
+
+      servo_handler_.setROSCommFlag(false);
+  }
+  else
+  {
   if( now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
     {
+	  servo_state_msg_.stamp = nh_->now();
       for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {
         const ServoData& s = servo_handler_.getServo()[i];
         if (s.send_data_flag_ != 0) {
@@ -66,6 +91,8 @@ void DirectServo::sendData()
       servo_state_pub_.publish(&servo_state_msg_);
       servo_last_pub_time_ = now_time;
     }
+  }
+
   if( now_time - servo_torque_last_pub_time_ >= SERVO_TORQUE_PUB_INTERVAL)
     {
       for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) {