Skip to content

Commit ab7cf87

Browse files
Properly test get_service_names_and_types_by_node in rclcpp_lifecycle (#2599)
The current `TestLifecycleServiceClient.get_service_names_and_types_by_node` test was using `LifecycleServiceClient`, which is just a normal `rclcpp::Node` with some `rclcpp::Client`s, to test `NodeGraph::get_service_names_and_types_by_node()`. However, `NodeGraph::get_service_names_and_types_by_node()` is for service servers, not clients. The test just ended up checking the built-in services of an `rclcpp::Node`, since it wasn't actually checking the names of the services, and not checking the clients of the `LifecycleServiceClient` or the built-in services of a `rclcpp_lifecycle::LifecycleNode`. I believe this was probably not the intention, since this is an `rclcpp_lifecycle` test. Instead, use an actual `rclcpp_lifecycle::LifecycleNode` and check its service servers by calling `NodeGraph::get_service_names_and_types_by_node()` through `LifecycleNode::get_service_names_and_types_by_node()`. Signed-off-by: Christophe Bedard <christophe.bedard@apex.ai>
1 parent 496c455 commit ab7cf87

File tree

1 file changed

+25
-14
lines changed

1 file changed

+25
-14
lines changed

rclcpp_lifecycle/test/test_lifecycle_service_client.cpp

Lines changed: 25 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -18,11 +18,14 @@
1818
*/
1919

2020
#include <gtest/gtest.h>
21+
#include <algorithm>
22+
#include <array>
2123
#include <chrono>
2224
#include <memory>
2325
#include <mutex>
2426
#include <string>
2527
#include <thread>
28+
#include <utility>
2629
#include <vector>
2730

2831
#include "lifecycle_msgs/msg/state.hpp"
@@ -368,26 +371,34 @@ TEST_F(TestLifecycleServiceClient, lifecycle_transitions) {
368371

369372
TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node)
370373
{
371-
auto node1 = std::make_shared<LifecycleServiceClient>("client1");
372-
auto node2 = std::make_shared<LifecycleServiceClient>("client2");
373-
374-
auto node_graph = node1->get_node_graph_interface();
375-
ASSERT_NE(nullptr, node_graph);
376-
377374
EXPECT_THROW(
378-
node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
375+
lifecycle_node()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
379376
std::runtime_error);
380-
auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/");
381-
auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/");
377+
auto service_names_and_types =
378+
lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/");
382379
auto start = std::chrono::steady_clock::now();
383-
while (0 == service_names_and_types1.size() ||
384-
service_names_and_types1.size() != service_names_and_types2.size() ||
380+
while (0 == service_names_and_types.size() ||
385381
(std::chrono::steady_clock::now() - start) < std::chrono::seconds(1))
386382
{
387-
service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/");
388-
service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/");
383+
service_names_and_types =
384+
lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/");
385+
}
386+
const std::array services = {
387+
std::make_pair(node_get_state_topic, "lifecycle_msgs/srv/GetState"),
388+
std::make_pair(node_change_state_topic, "lifecycle_msgs/srv/ChangeState"),
389+
std::make_pair(node_get_available_states_topic, "lifecycle_msgs/srv/GetAvailableStates"),
390+
std::make_pair(
391+
node_get_available_transitions_topic, "lifecycle_msgs/srv/GetAvailableTransitions"),
392+
std::make_pair(node_get_transition_graph_topic, "lifecycle_msgs/srv/GetAvailableTransitions"),
393+
};
394+
for (const auto & [service_name, service_type] : services) {
395+
ASSERT_TRUE(service_names_and_types.find(service_name) != service_names_and_types.end())
396+
<< service_name;
397+
const auto service_types = service_names_and_types.at(service_name);
398+
EXPECT_TRUE(
399+
std::find(service_types.cbegin(), service_types.cend(), service_type) != service_types.cend())
400+
<< service_name;
389401
}
390-
EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size());
391402
}
392403

393404
TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values)

0 commit comments

Comments
 (0)