diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 58d0abce..57d07420 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -752,6 +753,50 @@ _demangle_if_ros_type(const std::string & dds_type_string) return type_namespace + type_name; } +bool +_type_hashes_equal(const rosidl_type_hash_t & lhs, const rosidl_type_hash_t & rhs) +{ + if (lhs.version != rhs.version) { + return false; + } + return 0 == std::memcmp(lhs.value, rhs.value, ROSIDL_TYPE_HASH_SIZE); +} + +rosidl_type_hash_t +_resolve_type_hash_for_type( + const GraphNode::TopicQoSMap & qos_map, + const std::string & topic_name, + const std::string & type_name) +{ + rosidl_type_hash_t selected_hash = rosidl_get_zero_initialized_type_hash(); + bool has_valid_hash = false; + + for (const auto & [_, topic_data] : qos_map) { + rosidl_type_hash_t parsed_hash = rosidl_get_zero_initialized_type_hash(); + rcutils_ret_t rc_ret = rosidl_parse_type_hash_string( + topic_data->info_.type_hash_.c_str(), + &parsed_hash); + if (RCUTILS_RET_OK != rc_ret) { + continue; + } + if (!has_valid_hash) { + selected_hash = parsed_hash; + has_valid_hash = true; + continue; + } + if (!_type_hashes_equal(selected_hash, parsed_hash)) { + RMW_ZENOH_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Conflicting type hashes for topic '%s', type '%s'; storing zero hash", + topic_name.c_str(), + type_name.c_str()); + return rosidl_get_zero_initialized_type_hash(); + } + } + + return has_valid_hash ? selected_hash : rosidl_get_zero_initialized_type_hash(); +} + rmw_ret_t fill_names_and_types( const GraphNode::TopicMap & entity_map, rcutils_allocator_t * allocator, @@ -786,6 +831,19 @@ rmw_ret_t fill_names_and_types( return RMW_RET_BAD_ALLOC; } + if (item.second.size() > 0) { + names_and_types->type_hashes[index] = static_cast( + allocator->allocate( + item.second.size() * sizeof(rosidl_type_hash_t), + allocator->state)); + if (!names_and_types->type_hashes[index]) { + RMW_SET_ERROR_MSG("failed to allocate memory for type hashes"); + return RMW_RET_BAD_ALLOC; + } + } else { + names_and_types->type_hashes[index] = nullptr; + } + size_t type_index = 0; for (const std::pair & type : item.second) { char * type_name = rcutils_strdup(_demangle_if_ros_type(type.first).c_str(), *allocator); @@ -794,6 +852,10 @@ rmw_ret_t fill_names_and_types( return RMW_RET_BAD_ALLOC; } names_and_types->types[index].data[type_index] = type_name; + names_and_types->type_hashes[index][type_index] = _resolve_type_hash_for_type( + type.second, + item.first, + type.first); ++type_index; } ++index; diff --git a/test_rmw_zenoh_cpp/CMakeLists.txt b/test_rmw_zenoh_cpp/CMakeLists.txt index fe8e5699..84b4bf89 100644 --- a/test_rmw_zenoh_cpp/CMakeLists.txt +++ b/test_rmw_zenoh_cpp/CMakeLists.txt @@ -19,7 +19,10 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_lint_common REQUIRED) find_package(rclcpp REQUIRED) + find_package(rmw REQUIRED) find_package(rmw_zenoh_cpp REQUIRED) + find_package(example_interfaces REQUIRED) + find_package(std_msgs REQUIRED) find_package(zenoh_cpp_vendor REQUIRED) ament_lint_auto_find_test_dependencies() @@ -27,10 +30,16 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_rmw_zenoh_session test/test_rmw_zenoh_session.cpp ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp) + # Ensure the locally built rmw headers (with type_hashes) take precedence over + # the system rmw headers that rclcpp pulls in transitively. + target_include_directories(test_rmw_zenoh_session BEFORE PRIVATE ${rmw_INCLUDE_DIRS}) target_link_libraries(test_rmw_zenoh_session rclcpp::rclcpp + rmw::rmw rmw_zenoh_cpp::rmw_zenoh_cpp zenohcxx::zenohc + ${example_interfaces_TARGETS} + ${std_msgs_TARGETS} ) endif() diff --git a/test_rmw_zenoh_cpp/package.xml b/test_rmw_zenoh_cpp/package.xml index 8d476ee5..e69387c1 100644 --- a/test_rmw_zenoh_cpp/package.xml +++ b/test_rmw_zenoh_cpp/package.xml @@ -14,7 +14,10 @@ ament_lint_auto ament_lint_common rclcpp + rmw rmw_zenoh_cpp + example_interfaces + std_msgs zenoh_cpp_vendor diff --git a/test_rmw_zenoh_cpp/test/test_rmw_zenoh_session.cpp b/test_rmw_zenoh_cpp/test/test_rmw_zenoh_session.cpp index 4f0a005a..ac59d552 100644 --- a/test_rmw_zenoh_cpp/test/test_rmw_zenoh_session.cpp +++ b/test_rmw_zenoh_cpp/test/test_rmw_zenoh_session.cpp @@ -14,13 +14,27 @@ #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include +#include #include +#include #include #include "rmw_zenoh_cpp/rmw_zenoh.hpp" @@ -39,6 +53,103 @@ class TestRmwZenohSession : public ::testing::Test } }; +namespace +{ +std::string +normalize_name(const std::string & name) +{ + if (!name.empty() && name.front() == '/') { + return name.substr(1); + } + return name; +} + +bool +is_zero_hash(const rosidl_type_hash_t & hash) +{ + if (hash.version != 0) { + return false; + } + for (size_t i = 0; i < ROSIDL_TYPE_HASH_SIZE; ++i) { + if (hash.value[i] != 0) { + return false; + } + } + return true; +} + +bool +names_and_types_has_any_non_zero_hash(const rmw_names_and_types_t & nat) +{ + if (!nat.type_hashes) { + return false; + } + for (size_t i = 0; i < nat.names.size; ++i) { + if (!nat.type_hashes[i]) { + continue; + } + for (size_t j = 0; j < nat.types[i].size; ++j) { + if (!is_zero_hash(nat.type_hashes[i][j])) { + return true; + } + } + } + return false; +} + +bool +endpoint_info_has_non_zero_hash(const rmw_topic_endpoint_info_array_t & arr) +{ + for (size_t i = 0; i < arr.size; ++i) { + if (!is_zero_hash(arr.info_array[i].topic_type_hash)) { + return true; + } + } + return false; +} + +bool +service_endpoint_info_has_non_zero_hash(const rmw_service_endpoint_info_array_t & arr) +{ + for (size_t i = 0; i < arr.size; ++i) { + if (!is_zero_hash(arr.info_array[i].service_type_hash)) { + return true; + } + } + return false; +} + +bool +contains_non_zero_hash_for_type( + const rmw_names_and_types_t & names_and_types, + const std::string & expected_name, + const std::string & expected_type) +{ + if (!names_and_types.type_hashes) { + return false; + } + + const std::string normalized_expected_name = normalize_name(expected_name); + for (size_t i = 0; i < names_and_types.names.size; ++i) { + if (normalize_name(names_and_types.names.data[i]) != normalized_expected_name) { + continue; + } + if (!names_and_types.type_hashes[i]) { + continue; + } + for (size_t j = 0; j < names_and_types.types[i].size; ++j) { + if (expected_type != names_and_types.types[i].data[j]) { + continue; + } + if (!is_zero_hash(names_and_types.type_hashes[i][j])) { + return true; + } + } + } + return false; +} +} // namespace + TEST_F(TestRmwZenohSession, GetZenohSessionFromContext) { // Create a node @@ -94,6 +205,182 @@ TEST_F(TestRmwZenohSession, ZenohSessionDirectAccess) std::this_thread::sleep_for(std::chrono::milliseconds(100)); } +TEST_F(TestRmwZenohSession, NamesAndTypesPopulateTypeHashes) +{ + auto pub_node = std::make_shared("hash_writer"); + auto sub_node = std::make_shared("hash_reader"); + auto query_node = std::make_shared("hash_query"); + + auto pub = pub_node->create_publisher("hash_topic", 10); + auto sub = sub_node->create_subscription( + "hash_topic", 10, [](const std_msgs::msg::String::SharedPtr) {}); + (void)pub; + (void)sub; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(pub_node); + executor.add_node(sub_node); + executor.add_node(query_node); + + auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (std::chrono::steady_clock::now() < deadline) { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + const rcl_node_t * rcl_node = query_node->get_node_base_interface()->get_rcl_node_handle(); + const rmw_node_t * rmw_node = rcl_node_get_rmw_handle(rcl_node); + ASSERT_NE(nullptr, rmw_node); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + rmw_names_and_types_t topic_names_and_types = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_topic_names_and_types( + rmw_node, + &allocator, + false, + &topic_names_and_types)); + + EXPECT_TRUE(contains_non_zero_hash_for_type( + topic_names_and_types, + "/hash_topic", + "std_msgs/msg/String")); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&topic_names_and_types)); + + rmw_names_and_types_t publisher_names_and_types = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_publisher_names_and_types_by_node( + rmw_node, + &allocator, + "hash_writer", + "/", + false, + &publisher_names_and_types)); + + EXPECT_TRUE(contains_non_zero_hash_for_type( + publisher_names_and_types, + "/hash_topic", + "std_msgs/msg/String")); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&publisher_names_and_types)); + + rmw_names_and_types_t subscriber_names_and_types = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_subscriber_names_and_types_by_node( + rmw_node, + &allocator, + "hash_reader", + "/", + false, + &subscriber_names_and_types)); + EXPECT_TRUE(contains_non_zero_hash_for_type( + subscriber_names_and_types, + "/hash_topic", + "std_msgs/msg/String")); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&subscriber_names_and_types)); + + rmw_topic_endpoint_info_array_t pubs_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_publishers_info_by_topic( + rmw_node, &allocator, "/hash_topic", false, &pubs_info)); + EXPECT_TRUE(endpoint_info_has_non_zero_hash(pubs_info)); + ASSERT_EQ(RMW_RET_OK, rmw_topic_endpoint_info_array_fini(&pubs_info, &allocator)); + + rmw_topic_endpoint_info_array_t subs_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_subscriptions_info_by_topic( + rmw_node, &allocator, "/hash_topic", false, &subs_info)); + EXPECT_TRUE(endpoint_info_has_non_zero_hash(subs_info)); + ASSERT_EQ(RMW_RET_OK, rmw_topic_endpoint_info_array_fini(&subs_info, &allocator)); + + executor.remove_node(pub_node); + executor.remove_node(sub_node); + executor.remove_node(query_node); +} + +TEST_F(TestRmwZenohSession, ServiceTypeHashesPopulated) +{ + auto srv_node = std::make_shared("hash_server"); + auto cli_node = std::make_shared("hash_client"); + auto query_node = std::make_shared("hash_svc_query"); + + auto srv = srv_node->create_service( + "hash_service", + []( + const example_interfaces::srv::AddTwoInts::Request::SharedPtr, + example_interfaces::srv::AddTwoInts::Response::SharedPtr) {}); + auto cli = cli_node->create_client("hash_service"); + (void)srv; + (void)cli; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(srv_node); + executor.add_node(cli_node); + executor.add_node(query_node); + + auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (std::chrono::steady_clock::now() < deadline) { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + const rcl_node_t * rcl_node = query_node->get_node_base_interface()->get_rcl_node_handle(); + const rmw_node_t * rmw_node = rcl_node_get_rmw_handle(rcl_node); + ASSERT_NE(nullptr, rmw_node); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + rmw_names_and_types_t svc_nat = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ(RMW_RET_OK, rmw_get_service_names_and_types(rmw_node, &allocator, &svc_nat)); + EXPECT_TRUE(names_and_types_has_any_non_zero_hash(svc_nat)); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&svc_nat)); + + rmw_names_and_types_t srv_nat = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_service_names_and_types_by_node( + rmw_node, &allocator, "hash_server", "/", &srv_nat)); + EXPECT_TRUE(names_and_types_has_any_non_zero_hash(srv_nat)); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&srv_nat)); + + rmw_names_and_types_t cli_nat = rmw_get_zero_initialized_names_and_types(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_client_names_and_types_by_node( + rmw_node, &allocator, "hash_client", "/", &cli_nat)); + EXPECT_TRUE(names_and_types_has_any_non_zero_hash(cli_nat)); + ASSERT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&cli_nat)); + + rmw_service_endpoint_info_array_t servers_info = + rmw_get_zero_initialized_service_endpoint_info_array(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_servers_info_by_service( + rmw_node, &allocator, "/hash_service", false, &servers_info)); + EXPECT_TRUE(service_endpoint_info_has_non_zero_hash(servers_info)); + ASSERT_EQ(RMW_RET_OK, rmw_service_endpoint_info_array_fini(&servers_info, &allocator)); + + rmw_service_endpoint_info_array_t clients_info = + rmw_get_zero_initialized_service_endpoint_info_array(); + ASSERT_EQ( + RMW_RET_OK, + rmw_get_clients_info_by_service( + rmw_node, &allocator, "/hash_service", false, &clients_info)); + EXPECT_TRUE(service_endpoint_info_has_non_zero_hash(clients_info)); + ASSERT_EQ(RMW_RET_OK, rmw_service_endpoint_info_array_fini(&clients_info, &allocator)); + + executor.remove_node(srv_node); + executor.remove_node(cli_node); + executor.remove_node(query_node); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);