From e76217001d93db2b7051ac1ec179c7564ec64210 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Fri, 9 Mar 2018 10:31:57 -0500 Subject: [PATCH] drake_lcm: Add `IsReceiveThreadRunning()` --- lcm/drake_lcm.cc | 5 ++++- lcm/drake_lcm.h | 4 ++++ lcm/drake_lcm_interface.h | 5 +++++ lcm/drake_lcm_log.h | 13 +++++++++++-- lcm/drake_mock_lcm.h | 4 ++++ lcm/test/drake_lcm_log_test.cc | 6 ++++++ lcm/test/drake_lcm_test.cc | 4 ++++ lcm/test/drake_mock_lcm_test.cc | 4 ++++ 8 files changed, 42 insertions(+), 3 deletions(-) diff --git a/lcm/drake_lcm.cc b/lcm/drake_lcm.cc index c9d2619da1e5..12e411f7da97 100644 --- a/lcm/drake_lcm.cc +++ b/lcm/drake_lcm.cc @@ -39,7 +39,10 @@ void DrakeLcm::StartReceiveThread() { } void DrakeLcm::StopReceiveThread() { - if (receive_thread_ != nullptr) receive_thread_->Stop(); + if (receive_thread_ != nullptr) { + receive_thread_->Stop(); + receive_thread_.reset(); + } } ::lcm::LCM* DrakeLcm::get_lcm_instance() { return &lcm_; } diff --git a/lcm/drake_lcm.h b/lcm/drake_lcm.h index af05e4fb93b3..b631d1edbb76 100644 --- a/lcm/drake_lcm.h +++ b/lcm/drake_lcm.h @@ -32,6 +32,10 @@ class DrakeLcm : public DrakeLcmInterface { void StopReceiveThread() override; + bool IsReceiveThreadRunning() const override { + return receive_thread_ != nullptr; + } + /** * An accessor to the real LCM instance encapsulated by this object. The * returned pointer is guaranteed to be valid for the duration of this diff --git a/lcm/drake_lcm_interface.h b/lcm/drake_lcm_interface.h index cb157130c4b6..68a57a1ab474 100644 --- a/lcm/drake_lcm_interface.h +++ b/lcm/drake_lcm_interface.h @@ -39,6 +39,11 @@ class DrakeLcmInterface { */ virtual void StopReceiveThread() = 0; + /** + * Indicates that the receiving thread is running. + */ + virtual bool IsReceiveThreadRunning() const = 0; + /** * Publishes an LCM message on channel @p channel. * diff --git a/lcm/drake_lcm_log.h b/lcm/drake_lcm_log.h index 29ee1021c5c3..6b6b7032f1e9 100644 --- a/lcm/drake_lcm_log.h +++ b/lcm/drake_lcm_log.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -116,8 +117,15 @@ class DrakeLcmLog : public DrakeLcmInterface { return static_cast(sec * 1e6); } - void StartReceiveThread() override {} - void StopReceiveThread() override {} + void StartReceiveThread() override { + receive_thread_started_ = true; + } + void StopReceiveThread() override { + receive_thread_started_ = false; + } + bool IsReceiveThreadRunning() const override { + return receive_thread_started_; + } private: const bool is_write_; @@ -129,6 +137,7 @@ class DrakeLcmLog : public DrakeLcmInterface { mutable std::mutex mutex_; std::unique_ptr<::lcm::LogFile> log_; const ::lcm::LogEvent* next_event_{nullptr}; + std::atomic receive_thread_started_{false}; }; } // namespace lcm diff --git a/lcm/drake_mock_lcm.h b/lcm/drake_mock_lcm.h index 4fcec525c3e0..314b46fca5ab 100644 --- a/lcm/drake_mock_lcm.h +++ b/lcm/drake_mock_lcm.h @@ -40,6 +40,10 @@ class DrakeMockLcm : public DrakeLcmInterface { void StopReceiveThread() override; + bool IsReceiveThreadRunning() const override { + return receive_thread_started_; + } + void Publish(const std::string& channel, const void* data, int data_size, double time_sec = 0) override; diff --git a/lcm/test/drake_lcm_log_test.cc b/lcm/test/drake_lcm_log_test.cc index 9a2dc652c7df..054ac12e043c 100644 --- a/lcm/test/drake_lcm_log_test.cc +++ b/lcm/test/drake_lcm_log_test.cc @@ -37,6 +37,12 @@ GTEST_TEST(LcmLogTest, LcmLogTestSaveAndRead) { auto w_log = std::make_unique("test.log", true); const std::string channel_name("test_channel"); + EXPECT_FALSE(w_log->IsReceiveThreadRunning()); + w_log->StartReceiveThread(); + EXPECT_TRUE(w_log->IsReceiveThreadRunning()); + w_log->StopReceiveThread(); + EXPECT_FALSE(w_log->IsReceiveThreadRunning()); + drake::lcmt_drake_signal msg; msg.dim = 1; msg.val.push_back(0.1); diff --git a/lcm/test/drake_lcm_test.cc b/lcm/test/drake_lcm_test.cc index 5e2efc16f88a..d6363c5ce6eb 100644 --- a/lcm/test/drake_lcm_test.cc +++ b/lcm/test/drake_lcm_test.cc @@ -102,7 +102,10 @@ TEST_F(DrakeLcmTest, PublishTest) { // order of construction, this ensures the LCM receive thread stops before any // resources it uses are destroyed. If the Lcm receive thread is stopped after // the resources it relies on are destroyed, a segmentation fault may occur. + + EXPECT_FALSE(dut.IsReceiveThreadRunning()); dut.StartReceiveThread(); + EXPECT_TRUE(dut.IsReceiveThreadRunning()); // Records whether the receiver received an LCM message published by the DUT. bool done = false; @@ -130,6 +133,7 @@ TEST_F(DrakeLcmTest, PublishTest) { dut.StopReceiveThread(); EXPECT_TRUE(done); + EXPECT_FALSE(dut.IsReceiveThreadRunning()); } // Handles received LCM messages. diff --git a/lcm/test/drake_mock_lcm_test.cc b/lcm/test/drake_mock_lcm_test.cc index e1ba7ca372ca..ca606f23d4b3 100644 --- a/lcm/test/drake_mock_lcm_test.cc +++ b/lcm/test/drake_mock_lcm_test.cc @@ -29,7 +29,9 @@ GTEST_TEST(DrakeMockLcmTest, PublishTest) { // Instantiates the Device Under Test (DUT). DrakeMockLcm dut; + EXPECT_FALSE(dut.IsReceiveThreadRunning()); dut.StartReceiveThread(); + EXPECT_TRUE(dut.IsReceiveThreadRunning()); dut.Publish(channel_name, &message_bytes[0], message_size); // Verifies that the message was "published". @@ -38,6 +40,8 @@ GTEST_TEST(DrakeMockLcmTest, PublishTest) { EXPECT_EQ(message_size, static_cast(published_message_bytes.size())); EXPECT_EQ(message_bytes, published_message_bytes); + dut.StopReceiveThread(); + EXPECT_FALSE(dut.IsReceiveThreadRunning()); } // Tests DrakeMockLcm::DecodeLastPublishedMessageAs() using an lcmt_drake_signal