Support publishing loaned messages in LifecyclePublisher (#2159)

* Support loaned messages in LifecyclePublisher Signed-off-by: Michael Babenko <mbabenko@wayve.ai>
ros2 · Apr 14, 2023 · 6ffd54d · 6ffd54d
1 parent fd7a0dc
commit 6ffd54d
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp
@@ -91,6 +91,24 @@ class LifecyclePublisher : public SimpleManagedEntity,
     rclcpp::Publisher<MessageT, Alloc>::publish(msg);
   }
 
+  /// LifecyclePublisher publish function
+  /**
+   * The publish function checks whether the communication
+   * was enabled or disabled and forwards the message
+   * to the actual rclcpp Publisher base class
+   */
+  virtual void
+  publish(
+    rclcpp::LoanedMessage<typename rclcpp::Publisher<MessageT,
+    Alloc>::ROSMessageType, Alloc> && loaned_msg)
+  {
+    if (!this->is_activated()) {
+      log_publisher_not_enabled();
+      return;
+    }
+    rclcpp::Publisher<MessageT, Alloc>::publish(std::move(loaned_msg));
+  }
+
   void
   on_activate() override
   {

diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp
@@ -129,6 +129,10 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
+  {
+    auto loaned_msg = node_->publisher()->borrow_loaned_message();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+  }
   node_->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret);
   ASSERT_EQ(success, ret);
@@ -143,6 +147,10 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
+  {
+    auto loaned_msg = node_->publisher()->borrow_loaned_message();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+  }
 }
 
 TEST_P(TestLifecyclePublisher, publish) {
@@ -157,6 +165,10 @@ TEST_P(TestLifecyclePublisher, publish) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
+  {
+    auto loaned_msg = node_->publisher()->borrow_loaned_message();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+  }
   node_->publisher()->on_activate();
   EXPECT_TRUE(node_->publisher()->is_activated());
   {
@@ -167,6 +179,10 @@ TEST_P(TestLifecyclePublisher, publish) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
+  {
+    auto loaned_msg = node_->publisher()->borrow_loaned_message();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
+  }
 }
 
 INSTANTIATE_TEST_SUITE_P(