diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 29fd7a9841..b44152a453 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -531,37 +531,8 @@ 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); - - rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; - auto subscription = - test_node.create_subscription("joint_states", 10, subs_callback); - - // call update to publish the test value - // since update doesn't guarantee a published message, republish until received - int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); - while (max_sub_check_loop_count--) - { - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { - break; - } - } - ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - "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)); + activate_and_get_joint_state_message("joint_states", joint_state_msg); const size_t NUM_JOINTS = JOINT_NAMES.size(); @@ -607,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); @@ -638,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 joint_names_ = {"joint1", "joint2", "joint3"};