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
index e10d6b0bf..108ee103d 100644
--- 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
@@ -219,6 +219,21 @@ uint8_t DynamixelSerial::getServoIndex(uint8_t id)
 
 void DynamixelSerial::update()
 {
+  /* receive data process */
+  /* For one round, change from "send -> receive" to " receive -> send" */
+  /* This setting can accelerate the receiving process */
+  if(read_status_packet_flag_) {
+    for (unsigned int i = 0; i < servo_num_; i++) {
+      if(!servo_[i].send_data_flag_ && !servo_[i].first_get_pos_flag_) continue;
+      readStatusPacket(instruction_last_.first);
+    }
+    if (instruction_last_.first == INST_GET_PRESENT_POS)
+    {
+      setROSCommFlag(true);
+    }
+  }
+  read_status_packet_flag_ = false;
+
   uint32_t current_time = HAL_GetTick();
 
   /* send position command to servo */
@@ -312,7 +327,6 @@ void DynamixelSerial::update()
   std::pair<uint8_t, uint8_t> instruction;
   while(instruction_buffer_.pop(instruction))
     {
-      bool read_status_packet_flag = false;
       uint8_t servo_index = instruction.second;
 
       if (mutex_ != NULL)  osMutexWait(*mutex_, osWaitForever);
@@ -400,49 +414,46 @@ void DynamixelSerial::update()
         switch (instruction.first) {
         case INST_GET_PRESENT_POS: /* read servo position(angle) */
           cmdSyncReadPresentPosition(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_PRESENT_CURRENT: /* read servo load */
           cmdSyncReadPresentCurrent(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_PRESENT_TEMPERATURE: /* read servo temp */
           cmdSyncReadPresentTemperature(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_PRESENT_MOVING: /* read servo movement */
           cmdSyncReadMoving(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_HARDWARE_ERROR_STATUS:
           cmdSyncReadHardwareErrorStatus(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_HOMING_OFFSET:
           cmdSyncReadHomingOffset(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_POSITION_GAINS:
           cmdSyncReadPositionGains(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_PROFILE_VELOCITY:
           cmdSyncReadProfileVelocity(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         case INST_GET_CURRENT_LIMIT:
           cmdSyncReadCurrentLimit(false);
-          read_status_packet_flag = true;
+          read_status_packet_flag_ = true;
           break;
         default:
           break;
         }
-        /* receive data process */
-        if(read_status_packet_flag) {
-          for (unsigned int i = 0; i < servo_num_; i++) {
-            if(!servo_[i].send_data_flag_ && !servo_[i].first_get_pos_flag_) continue;
-            readStatusPacket(instruction.first);
-          }
+
+        if (read_status_packet_flag_) {
+          instruction_last_ = instruction;
         }
       }
 
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
index 533cf106e..88abc4ebe 100644
--- 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,14 @@ 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);
+
+
   void transmitInstructionPacket(uint8_t id, uint16_t len, uint8_t instruction, uint8_t* parameters);
   int8_t readStatusPacket(uint8_t status_packet_instruction);
 
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
index 4ea89f1f4..97e4bd4b0 100644
--- 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++) {
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
index a5aecde04..96dd08c71 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
@@ -48,7 +48,7 @@ class DirectServo
 
   void init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexId* mutex);
   void update();
-  void sendData();
+  void sendData(bool flag_send_asap);
   void torqueEnable(const std::map<uint8_t, float>& servo_map);
   void setGoalAngle(const std::map<uint8_t, float>& servo_map, uint8_t value_type = 0);
   DynamixelSerial& getServoHnadler() {return servo_handler_;}