Use select to reduce CPU usage.

dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp

@@ -129,7 +129,41 @@ int PortHandlerLinux::getBytesAvailable()
 
 int PortHandlerLinux::readPort(uint8_t *packet, int length)
 {
-  return read(socket_fd_, packet, length);
+  if (packet_timeout_ != 0)
+  {
+    fd_set readfds;
+    FD_ZERO(&readfds);
+    FD_SET(socket_fd_, &readfds);
+    struct timeval timeout;
+    timeout.tv_sec = 0;
+    // packet_timeout_ is in milliseconds
+    timeout.tv_usec = packet_timeout_ * 1000;
+    int select_ret = ::select(socket_fd_ + 1, &readfds, nullptr, nullptr, &timeout);
+    if (select_ret < 0)
+    {
+      // Some kind of error happened; just return 0
+      return 0;
+    }
+    else if (select_ret == 0)
+    {
+      // timeout happened; return 0
+      return 0;
+    }
+    else
+    {
+      if (!FD_ISSET(socket_fd_, &readfds))
+      {
+        // We didn't see the file descriptor we expected; just return 0
+        return 0;
+      }
+
+      return read(socket_fd_, packet, length);
+    }
+  }
+  else
+  {
+    return read(socket_fd_, packet, length);
+  }
 }
 
 int PortHandlerLinux::writePort(uint8_t *packet, int length)

dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp

@@ -441,8 +441,13 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add
   uint8_t txpacket[8]         = {0};
   uint8_t *rxpacket           = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6);
 
-  if (id >= BROADCAST_ID)
+  if (rxpacket == NULL)
+    return result;
+
+  if (id >= BROADCAST_ID) {
+    free(rxpacket);
     return COMM_NOT_AVAILABLE;
+  }
 
   txpacket[PKT_ID]            = id;
   txpacket[PKT_LENGTH]        = 4;

dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp

@@ -712,8 +712,10 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add
   if (rxpacket == NULL)
     return result;
 
-  if (id >= BROADCAST_ID)
+  if (id >= BROADCAST_ID) {
+    free(rxpacket);
     return COMM_NOT_AVAILABLE;
+  }
 
   txpacket[PKT_ID]            = id;
   txpacket[PKT_LENGTH_L]      = 7;