add delay after transmit

Author: honghyun79

c++/src/dynamixel_sdk/port_handler_mac.cpp

@@ -122,7 +122,18 @@ int PortHandlerMac::readPort(uint8_t *packet, int length)
 
 int PortHandlerMac::writePort(uint8_t *packet, int length)
 {
-  return write(socket_fd_, packet, length);
+  int result = write(socket_fd_, packet, length);
+
+  // Wait Tx Done
+  int unsent = 1;
+  while (unsent > 0) {
+    ioctl(socket_fd_, TIOCOUTQ, &unsent);
+  }
+  unsigned int tx_time = ((float)(length * 10) / baudrate_) * 1000000000;
+  struct timespec delay = {0, tx_time};
+  nanosleep(&delay, NULL);
+
+  return result;
 }
 
 void PortHandlerMac::setPacketTimeout(uint16_t packet_length)