diff --git a/lcm/drake_lcm.cc b/lcm/drake_lcm.cc index 3663a2dec4ea..aa3b3355802e 100644 --- a/lcm/drake_lcm.cc +++ b/lcm/drake_lcm.cc @@ -13,6 +13,7 @@ #include "drake/common/drake_copyable.h" #include "drake/common/drake_throw.h" #include "drake/common/scope_exit.h" +#include "drake/common/text_logging.h" namespace drake { namespace lcm { @@ -140,6 +141,9 @@ class DrakeSubscription final : public DrakeSubscriptionInterface { static std::shared_ptr CreateMultichannel( ::lcm::LCM* native_instance, MultichannelHandlerFunction multichannel_handler) { + // TODO(jwnimmer-tri) If a channel_suffix was given, we should use it here + // for efficiency (to drop unwanted packets as early as possible). Be sure + // to regex-escape it first. return Create(native_instance, ".*", std::move(multichannel_handler)); } @@ -284,6 +288,23 @@ std::shared_ptr DrakeLcm::SubscribeAllChannels( MultichannelHandlerFunction handler) { DRAKE_THROW_UNLESS(handler != nullptr); impl_->CleanUpOldSubscriptions(); + const std::string& suffix = impl_->channel_suffix_; + if (!suffix.empty()) { + handler = + [&suffix, handler](std::string_view channel, + const void* data, int length) { + // TODO(ggould-tri) Use string_view::ends_with() once we have C++20. + if (channel.length() >= suffix.length() && + channel.substr(channel.length() - suffix.length()) == suffix) { + channel.remove_suffix(suffix.length()); + handler(channel, data, length); + } else { + drake::log()->debug("DrakeLcm with suffix {} received message on" + " channel {}, which lacks the suffix.", + suffix, channel); + } + }; + } // Add the new subscriber. auto result = DrakeSubscription::CreateMultichannel( diff --git a/lcm/drake_lcm_params.h b/lcm/drake_lcm_params.h index 062b9e45cc2a..a31b22716907 100644 --- a/lcm/drake_lcm_params.h +++ b/lcm/drake_lcm_params.h @@ -23,9 +23,17 @@ struct DrakeLcmParams { When provided, calls to DrakeLcm::Publish() or DrakeLcm::Subscribe() will append this string to the `channel` name requested for publish or subscribe. - The callback of DrakeLcm::SubscribeAllChannels() will receive the "fully - qualified" channel name including this suffix. - */ + For example, with the channel_suffix set to "_ALT" a call to + `Publish(&drake_lcm, "FOO", message)` will transmit on the network using the + channel name "FOO_ALT", and a call to `Subscribe(&lcm, "BAR", handler)` will + only call the handler for messages received on the "BAR_ALT" channel name. + + Simiarly, DrakeLcm::SubscribeAllChannels() only subscribes to network messages + that end with the suffix. A network message on a non-matching channel name + (e.g., "QUUX") will silently discarded. + The DrakeLcmInterface::MultichannelHandlerFunction callback will be passed the + _unadaorned_ channel name as its first argument (e.g., "FOO" or "BAR"), not + "FOO_ALT", etc. */ std::string channel_suffix; /** (Advanced) Controls whether or not LCM's background receive thread will diff --git a/lcm/test/drake_lcm_test.cc b/lcm/test/drake_lcm_test.cc index 6c843621c2d1..5366e5eba3fa 100644 --- a/lcm/test/drake_lcm_test.cc +++ b/lcm/test/drake_lcm_test.cc @@ -384,20 +384,25 @@ TEST_F(DrakeLcmTest, Suffix) { // Tests the channel name suffix feature. TEST_F(DrakeLcmTest, SuffixInSubscribeAllChannels) { + // The device under test will listen for messages. DrakeLcmParams params; params.channel_suffix = "_SUFFIX"; + params.lcm_url = kUdpmUrl; dut_ = std::make_unique(params); - // SubscribeAll using Drake LCM, expecting to see the fully qualified - // channel name. + // Use a separate publisher, to have direct control over the channel name. + auto publisher = std::make_unique(kUdpmUrl); + + // Check SubscribeAll, expecting to see the unadorned channel name. lcmt_drake_signal received_drake{}; auto subscription = dut_->SubscribeAllChannels([&received_drake]( std::string_view channel_name, const void* data, int size) { - EXPECT_EQ(channel_name, "SuffixDrakeLcmTest_SUFFIX"); + EXPECT_EQ(channel_name, "SuffixDrakeLcmTest"); received_drake.decode(data, 0, size); }); LoopUntilDone(&received_drake, 20 /* retries */, [&]() { - Publish(dut_.get(), "SuffixDrakeLcmTest", message_); + Publish(publisher.get(), "SuffixDrakeLcmTest_ShouldBeDiscarded", message_); + Publish(publisher.get(), "SuffixDrakeLcmTest_SUFFIX", message_); dut_->HandleSubscriptions(50 /* millis */); }); }