From 1da1846ceafdfa4e21cd688aef68e099d8bd5b7d Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 27 Nov 2023 13:59:08 +0000
Subject: [PATCH] joint_state_broadcaster: Add proper subscription to
 TestCustomInterfaceMappingUpdate (#859) (#864)

---
 .../test/test_joint_state_broadcaster.cpp     | 21 +++++++++----------
 .../test/test_joint_state_broadcaster.hpp     |  3 +++
 2 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
index 7f95e232e2..98dac68924 100644
--- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
+++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
@@ -531,19 +531,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
   state_broadcaster_->get_node()->set_parameter(
     {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_});
 
-  // configure ok
-  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
-
-  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-
-  ASSERT_EQ(
-    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+  sensor_msgs::msg::JointState joint_state_msg;
+  activate_and_get_joint_state_message("joint_states", joint_state_msg);
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
   // joint state initialized
-  const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
   ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
   ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
   ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_);
@@ -585,7 +578,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest)
     controller_interface::return_type::OK);
 }
 
-void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic)
+void JointStateBroadcasterTest::activate_and_get_joint_state_message(
+  const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg)
 {
   auto node_state = state_broadcaster_->get_node()->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -616,9 +610,14 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
                                             "controller/broadcaster update loop";
 
   // take message from subscription
-  sensor_msgs::msg::JointState joint_state_msg;
   rclcpp::MessageInfo msg_info;
   ASSERT_TRUE(subscription->take(joint_state_msg, msg_info));
+}
+
+void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic)
+{
+  sensor_msgs::msg::JointState joint_state_msg;
+  activate_and_get_joint_state_message(topic, joint_state_msg);
 
   const size_t NUM_JOINTS = joint_names_.size();
   ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS));
diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp
index 63cc7a5483..fcaa40e8d5 100644
--- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp
+++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp
@@ -71,6 +71,9 @@ class JointStateBroadcasterTest : public ::testing::Test
 
   void test_published_dynamic_joint_state_message(const std::string & topic);
 
+  void activate_and_get_joint_state_message(
+    const std::string & topic, sensor_msgs::msg::JointState & msg);
+
 protected:
   // dummy joint state values used for tests
   const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};