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 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& 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 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& servo_map); void setGoalAngle(const std::map& servo_map, uint8_t value_type = 0); DynamixelSerial& getServoHnadler() {return servo_handler_;}