diff --git a/test_communication/CMakeLists.txt b/test_communication/CMakeLists.txt index 323ca522..92ca5531 100644 --- a/test_communication/CMakeLists.txt +++ b/test_communication/CMakeLists.txt @@ -41,8 +41,8 @@ if(BUILD_TESTING) foreach(interface_file ${interface_files}) get_filename_component(interface_ns "${interface_file}" DIRECTORY) get_filename_component(interface_ns "${interface_ns}" NAME) - string_ends_with("${interface_file}" ".msg" is_message) - if(is_message AND interface_ns STREQUAL "msg") + string_ends_with("${interface_file}" ".idl" is_idl) + if(is_idl AND interface_ns STREQUAL "msg") list(APPEND message_files "${interface_file}") continue() endif() @@ -51,8 +51,7 @@ if(BUILD_TESTING) list(APPEND service_files "${interface_file}") continue() endif() - string_ends_with("${interface_file}" ".idl" is_action) - if(is_action AND interface_ns STREQUAL "action") + if(is_idl AND interface_ns STREQUAL "action") list(APPEND action_files "${interface_file}") continue() endif() @@ -242,6 +241,26 @@ if(BUILD_TESTING) set(TEST_MESSAGE_TYPES "") foreach(message_file ${message_files}) get_filename_component(message_type "${message_file}" NAME_WE) + set(message_has_keys FALSE) + if( + "${message_type}" STREQUAL "KeyedString" OR + "${message_type}" STREQUAL "ComplexNestedKey" + ) + set(message_has_keys TRUE) + endif() + + # TODO(MiguelCompany): Only fastrtps RMWs interoperate for keyed messages + if( + message_has_keys AND + (NOT "${rmw_implementation1}" STREQUAL "${rmw_implementation2}") AND + ( + (NOT rmw_implementation1_is_fastrtps) OR + (NOT rmw_implementation2_is_fastrtps) + ) + ) + continue() + endif() + # TODO(dirk-thomas) WStrings published by FastRTPS can't be received # correctly by Connext on macOS if( @@ -426,6 +445,7 @@ if(BUILD_TESTING) add_library(subscribe_types STATIC "test/subscribe_array_types.cpp" "test/subscribe_basic_types.cpp" + "test/subscribe_key_types.cpp" "test/subscribe_string_types.cpp") target_link_libraries(subscribe_types rclcpp::rclcpp diff --git a/test_communication/test/subscribe_key_types.cpp b/test_communication/test/subscribe_key_types.cpp new file mode 100644 index 00000000..3333e3df --- /dev/null +++ b/test_communication/test/subscribe_key_types.cpp @@ -0,0 +1,53 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/keyed_string.hpp" +#include "test_msgs/msg/non_keyed_with_nested_key.hpp" + +#include "subscribe_helper.hpp" +#include "subscribe_key_types.hpp" + +rclcpp::SubscriptionBase::SharedPtr subscribe_keyed_string( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages) +{ + return subscribe( + node, message_type, expected_messages, received_messages); +} + +rclcpp::SubscriptionBase::SharedPtr subscribe_non_keyed_with_nested_key( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages) +{ + return subscribe( + node, message_type, expected_messages, received_messages); +} + +rclcpp::SubscriptionBase::SharedPtr subscribe_complex_nested_key( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages) +{ + return subscribe( + node, message_type, expected_messages, received_messages); +} diff --git a/test_communication/test/subscribe_key_types.hpp b/test_communication/test/subscribe_key_types.hpp new file mode 100644 index 00000000..feff54bc --- /dev/null +++ b/test_communication/test/subscribe_key_types.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SUBSCRIBE_KEY_TYPES_HPP_ +#define SUBSCRIBE_KEY_TYPES_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/complex_nested_key.hpp" +#include "test_msgs/msg/keyed_string.hpp" +#include "test_msgs/msg/non_keyed_with_nested_key.hpp" + +rclcpp::SubscriptionBase::SharedPtr subscribe_keyed_string( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages); + +rclcpp::SubscriptionBase::SharedPtr subscribe_non_keyed_with_nested_key( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages); + +rclcpp::SubscriptionBase::SharedPtr subscribe_complex_nested_key( + rclcpp::Node::SharedPtr node, + const std::string & message_type, + const std::vector & expected_messages, + std::vector & received_messages); + +#endif // SUBSCRIBE_KEY_TYPES_HPP_ diff --git a/test_communication/test/test_publisher.cpp b/test_communication/test/test_publisher.cpp index caa8a882..dcfbe9d2 100644 --- a/test_communication/test/test_publisher.cpp +++ b/test_communication/test/test_publisher.cpp @@ -107,6 +107,13 @@ int main(int argc, char ** argv) publish(node, message, get_messages_strings()); } else if (message == "WStrings") { publish(node, message, get_messages_wstrings()); + } else if (message == "KeyedString") { + publish(node, message, get_messages_keyed_string()); + } else if (message == "NonKeyedWithNestedKey") { + publish( + node, message, get_messages_non_keyed_with_nested_key()); + } else if (message == "ComplexNestedKey") { + publish(node, message, get_messages_complex_nested_key()); } else { fprintf(stderr, "Unknown message argument '%s'\n", message.c_str()); rclcpp::shutdown(); diff --git a/test_communication/test/test_publisher_subscriber.cpp b/test_communication/test/test_publisher_subscriber.cpp index 9d49041e..f682a78a 100644 --- a/test_communication/test/test_publisher_subscriber.cpp +++ b/test_communication/test/test_publisher_subscriber.cpp @@ -25,6 +25,7 @@ #include "subscribe_array_types.hpp" #include "subscribe_basic_types.hpp" +#include "subscribe_key_types.hpp" #include "subscribe_string_types.hpp" template @@ -98,6 +99,9 @@ int main(int argc, char ** argv) auto messages_defaults = get_messages_defaults(); auto messages_strings = get_messages_strings(); auto messages_wstrings = get_messages_wstrings(); + auto messages_keyed_string = get_messages_keyed_string(); + auto messages_non_keyed_with_nested_key = get_messages_non_keyed_with_nested_key(); + auto messages_complex_nested_key = get_messages_complex_nested_key(); std::thread spin_thread([node]() { rclcpp::spin(node); @@ -148,6 +152,18 @@ int main(int argc, char ** argv) } else if (message == "WStrings") { subscriber = subscribe_wstrings(node, message, messages_wstrings, received_messages); publish(node, message, messages_wstrings); + } else if (message == "KeyedString") { + subscriber = subscribe_keyed_string(node, message, messages_keyed_string, received_messages); + publish(node, message, messages_keyed_string); + } else if (message == "NonKeyedWithNestedKey") { + subscriber = subscribe_non_keyed_with_nested_key( + node, message, messages_non_keyed_with_nested_key, received_messages); + publish( + node, message, messages_non_keyed_with_nested_key); + } else if (message == "ComplexNestedKey") { + subscriber = subscribe_complex_nested_key( + node, message, messages_complex_nested_key, received_messages); + publish(node, message, messages_complex_nested_key); } else { fprintf(stderr, "Unknown message argument '%s'\n", message.c_str()); return 1; diff --git a/test_communication/test/test_subscriber.cpp b/test_communication/test/test_subscriber.cpp index 148c5266..968e1ceb 100644 --- a/test_communication/test/test_subscriber.cpp +++ b/test_communication/test/test_subscriber.cpp @@ -23,6 +23,7 @@ #include "subscribe_array_types.hpp" #include "subscribe_basic_types.hpp" +#include "subscribe_key_types.hpp" #include "subscribe_string_types.hpp" int main(int argc, char ** argv) @@ -53,6 +54,9 @@ int main(int argc, char ** argv) auto messages_defaults = get_messages_defaults(); auto messages_strings = get_messages_strings(); auto messages_wstrings = get_messages_wstrings(); + auto messages_keyed_string = get_messages_keyed_string(); + auto messages_non_keyed_with_nested_key = get_messages_non_keyed_with_nested_key(); + auto messages_complex_nested_key = get_messages_complex_nested_key(); rclcpp::SubscriptionBase::SharedPtr subscriber; std::vector received_messages; // collect flags about received messages @@ -85,6 +89,14 @@ int main(int argc, char ** argv) subscriber = subscribe_strings(node, message, messages_strings, received_messages); } else if (message == "WStrings") { subscriber = subscribe_wstrings(node, message, messages_wstrings, received_messages); + } else if (message == "KeyedString") { + subscriber = subscribe_keyed_string(node, message, messages_keyed_string, received_messages); + } else if (message == "NonKeyedWithNestedKey") { + subscriber = subscribe_non_keyed_with_nested_key( + node, message, messages_non_keyed_with_nested_key, received_messages); + } else if (message == "ComplexNestedKey") { + subscriber = subscribe_complex_nested_key( + node, message, messages_complex_nested_key, received_messages); } else { fprintf(stderr, "Unknown message argument '%s'\n", message.c_str()); rclcpp::shutdown();