From 83e331abd17ca1beb9903ff0a866a5944dcc91bc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 6 Nov 2019 14:39:35 -0500 Subject: [PATCH 1/2] Use select to reduce CPU usage. Since the file descriptor to the COM port is non-blocking, use select to sleep for as long as we need to get data. This reduces CPU usage quite a lot. Signed-off-by: Chris Lalancette --- .../src/dynamixel_sdk/port_handler_linux.cpp | 36 ++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp index 57564a94..b065060b 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp +++ b/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) From 9795bc0b1140a9e3bb14ddd1afe323ed2d87738e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 11 Dec 2019 18:50:58 +0000 Subject: [PATCH 2/2] Fix two potential memory leaks. Signed-off-by: Chris Lalancette --- .../src/dynamixel_sdk/protocol1_packet_handler.cpp | 7 ++++++- .../src/dynamixel_sdk/protocol2_packet_handler.cpp | 4 +++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index 25259684..5468dc1a 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/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; diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index c9d90c6e..09d80039 100755 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/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;