diff --git a/lcm/lcm-cpp-impl.hpp b/lcm/lcm-cpp-impl.hpp index 4da2ac1f..377bb47c 100644 --- a/lcm/lcm-cpp-impl.hpp +++ b/lcm/lcm-cpp-impl.hpp @@ -215,6 +215,15 @@ inline int LCM::handleTimeout(int timeout_millis) return lcm_handle_timeout(this->lcm, timeout_millis); } +inline int LCM::handleTimeoutUs(int timeout_micros) +{ + if (!this->lcm) { + fprintf(stderr, "LCM instance not initialized. Ignoring call to handle()\n"); + return -1; + } + return lcm_handle_timeout_us(this->lcm, timeout_micros); +} + template Subscription *LCM::subscribe(const std::string &channel, void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, diff --git a/lcm/lcm-cpp.hpp b/lcm/lcm-cpp.hpp index 0b95c4ef..54aca6fb 100644 --- a/lcm/lcm-cpp.hpp +++ b/lcm/lcm-cpp.hpp @@ -142,6 +142,17 @@ class LCM { */ inline int handleTimeout(int timeout_millis); + /** + * @brief Waits for and dispatches messages, with a timeout. + * + * New in LCM 1.1.0. + * + * @return >0 if a message was handled, 0 if the function timed out, + * and <0 if an error occured. + * @sa lcm_handle_timeout_us() + */ + inline int handleTimeoutUs(int timeout_micros); + /** * @brief Subscribes a callback method of an object to a channel, with * automatic message decoding. diff --git a/lcm/lcm.c b/lcm/lcm.c index 1379a431..90d1c018 100644 --- a/lcm/lcm.c +++ b/lcm/lcm.c @@ -237,6 +237,32 @@ int lcm_handle_timeout(lcm_t *lcm, int timeout_milis) } } +int lcm_handle_timeout_us(lcm_t *lcm, int timeout_micros) +{ + fd_set fds; + FD_ZERO(&fds); + SOCKET lcm_fd = lcm_get_fileno(lcm); + FD_SET(lcm_fd, &fds); + + struct timeval timeout; + timeout.tv_sec = timeout_micros / 1000000; + timeout.tv_usec = timeout_micros % 1000000; + + if (timeout_micros < 0) { + return -1; + } + + int select_result = select(lcm_fd + 1, &fds, NULL, NULL, &timeout); + if (select_result > 0) { + int lcm_handle_result = lcm_handle(lcm); + return lcm_handle_result == 0 ? 1 : lcm_handle_result; + } else if (select_result == 0) { + return 0; + } else { + return select_result; + } +} + int lcm_get_fileno(lcm_t *lcm) { if (lcm->provider && lcm->vtable->get_fileno) diff --git a/lcm/lcm.h b/lcm/lcm.h index fbd592cb..1f8a7ed8 100644 --- a/lcm/lcm.h +++ b/lcm/lcm.h @@ -322,6 +322,30 @@ int lcm_handle(lcm_t *lcm); LCM_EXPORT int lcm_handle_timeout(lcm_t *lcm, int timeout_millis); +/** + * @brief Wait for and dispatch the next incoming message, up to a time limit. + * + * This function is equivalent to lcm_handle(), but if no messages are received + * and handled by the time @p timeout_micros microseconds elapses, then the + * function returns. + * + * This function largely exists for convenience, and its behavior can be + * replicated by using lcm_fileno() and lcm_handle() in conjunction with + * select() or poll(). + * + * New in LCM 1.1.0. + * + * @param lcm the %LCM object + * @param timeout_micros the maximum amount of time to wait for a message, in + * microseconds. If 0, then dispatches any available messages and then + * returns immediately. Values less than 0 are not allowed. + * + * @return >0 if a message was handled, 0 if the function timed out, and <0 if + * an error occured. + */ +LCM_EXPORT +int lcm_handle_timeout_us(lcm_t *lcm, int timeout_micros); + /** * @brief Adjusts the maximum number of received messages that can be queued up * for a subscription. diff --git a/test/cpp/memq_test.cpp b/test/cpp/memq_test.cpp index a1cfa1ad..3965f5ac 100644 --- a/test/cpp/memq_test.cpp +++ b/test/cpp/memq_test.cpp @@ -99,6 +99,15 @@ TEST(LCM_CPP, MemqTimeout) // Invalid timeout specification should result in an error. EXPECT_GT(0, lcm.handleTimeout(-1)); + // No messages available. Call should timeout immediately. + EXPECT_EQ(0, lcm.handleTimeoutUs(0)); + + // No messages available. Call should timeout in a few ms. + EXPECT_EQ(0, lcm.handleTimeoutUs(10)); + + // Invalid timeout specification should result in an error. + EXPECT_GT(0, lcm.handleTimeoutUs(-1)); + // Subscribe to and publish on a channel. Expect that the message gets // handled with an ample timeout. bool msg_handled = false;