From 79acdc2c9e23405a4da1dfe0adc009d46fa3825d Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 2 Apr 2024 16:26:14 +0200 Subject: [PATCH 1/6] Added support for KeyedString. Signed-off-by: Miguel Company --- test_communication/CMakeLists.txt | 1 + .../test/subscribe_key_types.cpp | 32 +++++++++++++++++++ .../test/subscribe_key_types.hpp | 30 +++++++++++++++++ test_communication/test/test_publisher.cpp | 2 ++ .../test/test_publisher_subscriber.cpp | 5 +++ test_communication/test/test_subscriber.cpp | 4 +++ 6 files changed, 74 insertions(+) create mode 100644 test_communication/test/subscribe_key_types.cpp create mode 100644 test_communication/test/subscribe_key_types.hpp diff --git a/test_communication/CMakeLists.txt b/test_communication/CMakeLists.txt index 323ca522..9133fbee 100644 --- a/test_communication/CMakeLists.txt +++ b/test_communication/CMakeLists.txt @@ -426,6 +426,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..bee690a0 --- /dev/null +++ b/test_communication/test/subscribe_key_types.cpp @@ -0,0 +1,32 @@ +// Copyright 2015 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 "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); +} diff --git a/test_communication/test/subscribe_key_types.hpp b/test_communication/test/subscribe_key_types.hpp new file mode 100644 index 00000000..692ce195 --- /dev/null +++ b/test_communication/test/subscribe_key_types.hpp @@ -0,0 +1,30 @@ +// Copyright 2015 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/keyed_string.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); + +#endif // SUBSCRIBE_KEY_TYPES_HPP_ diff --git a/test_communication/test/test_publisher.cpp b/test_communication/test/test_publisher.cpp index caa8a882..0d1f151a 100644 --- a/test_communication/test/test_publisher.cpp +++ b/test_communication/test/test_publisher.cpp @@ -107,6 +107,8 @@ 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 { 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..66315534 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,7 @@ 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(); std::thread spin_thread([node]() { rclcpp::spin(node); @@ -148,6 +150,9 @@ 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 { 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..2be507a9 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,7 @@ 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(); rclcpp::SubscriptionBase::SharedPtr subscriber; std::vector received_messages; // collect flags about received messages @@ -85,6 +87,8 @@ 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 { fprintf(stderr, "Unknown message argument '%s'\n", message.c_str()); rclcpp::shutdown(); From 7d318df78cc94fb8e8f04b2f0d5feb8a938b48a2 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 3 Apr 2024 08:17:53 +0200 Subject: [PATCH 2/6] Added support for NonKeyedWithNestedKey. Signed-off-by: Miguel Company --- test_communication/test/subscribe_key_types.cpp | 11 +++++++++++ test_communication/test/subscribe_key_types.hpp | 7 +++++++ test_communication/test/test_publisher.cpp | 3 +++ test_communication/test/test_publisher_subscriber.cpp | 6 ++++++ test_communication/test/test_subscriber.cpp | 4 ++++ 5 files changed, 31 insertions(+) diff --git a/test_communication/test/subscribe_key_types.cpp b/test_communication/test/subscribe_key_types.cpp index bee690a0..2a66febe 100644 --- a/test_communication/test/subscribe_key_types.cpp +++ b/test_communication/test/subscribe_key_types.cpp @@ -17,6 +17,7 @@ #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" @@ -30,3 +31,13 @@ rclcpp::SubscriptionBase::SharedPtr subscribe_keyed_string( 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); +} diff --git a/test_communication/test/subscribe_key_types.hpp b/test_communication/test/subscribe_key_types.hpp index 692ce195..af21a21f 100644 --- a/test_communication/test/subscribe_key_types.hpp +++ b/test_communication/test/subscribe_key_types.hpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.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, @@ -27,4 +28,10 @@ rclcpp::SubscriptionBase::SharedPtr subscribe_keyed_string( 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); + #endif // SUBSCRIBE_KEY_TYPES_HPP_ diff --git a/test_communication/test/test_publisher.cpp b/test_communication/test/test_publisher.cpp index 0d1f151a..5dfb2718 100644 --- a/test_communication/test/test_publisher.cpp +++ b/test_communication/test/test_publisher.cpp @@ -109,6 +109,9 @@ int main(int argc, char ** argv) 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 { 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 66315534..3ebcc6cc 100644 --- a/test_communication/test/test_publisher_subscriber.cpp +++ b/test_communication/test/test_publisher_subscriber.cpp @@ -100,6 +100,7 @@ int main(int argc, char ** argv) 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(); std::thread spin_thread([node]() { rclcpp::spin(node); @@ -153,6 +154,11 @@ int main(int argc, char ** argv) } 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 { 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 2be507a9..7100b411 100644 --- a/test_communication/test/test_subscriber.cpp +++ b/test_communication/test/test_subscriber.cpp @@ -55,6 +55,7 @@ int main(int argc, char ** argv) 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(); rclcpp::SubscriptionBase::SharedPtr subscriber; std::vector received_messages; // collect flags about received messages @@ -89,6 +90,9 @@ int main(int argc, char ** argv) 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 { fprintf(stderr, "Unknown message argument '%s'\n", message.c_str()); rclcpp::shutdown(); From 69e1e97e4d5776a83a9c40a9088a46be0a08cdce Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 3 Apr 2024 08:38:07 +0200 Subject: [PATCH 3/6] Added support for ComplexNestedKey. Signed-off-by: Miguel Company --- test_communication/test/subscribe_key_types.cpp | 10 ++++++++++ test_communication/test/subscribe_key_types.hpp | 7 +++++++ test_communication/test/test_publisher.cpp | 2 ++ test_communication/test/test_publisher_subscriber.cpp | 5 +++++ test_communication/test/test_subscriber.cpp | 4 ++++ 5 files changed, 28 insertions(+) diff --git a/test_communication/test/subscribe_key_types.cpp b/test_communication/test/subscribe_key_types.cpp index 2a66febe..3a90649d 100644 --- a/test_communication/test/subscribe_key_types.cpp +++ b/test_communication/test/subscribe_key_types.cpp @@ -41,3 +41,13 @@ rclcpp::SubscriptionBase::SharedPtr subscribe_non_keyed_with_nested_key( 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 index af21a21f..a1465595 100644 --- a/test_communication/test/subscribe_key_types.hpp +++ b/test_communication/test/subscribe_key_types.hpp @@ -19,6 +19,7 @@ #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" @@ -34,4 +35,10 @@ rclcpp::SubscriptionBase::SharedPtr subscribe_non_keyed_with_nested_key( 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 5dfb2718..dcfbe9d2 100644 --- a/test_communication/test/test_publisher.cpp +++ b/test_communication/test/test_publisher.cpp @@ -112,6 +112,8 @@ int main(int argc, char ** argv) } 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 3ebcc6cc..f682a78a 100644 --- a/test_communication/test/test_publisher_subscriber.cpp +++ b/test_communication/test/test_publisher_subscriber.cpp @@ -101,6 +101,7 @@ int main(int argc, char ** argv) 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); @@ -159,6 +160,10 @@ int main(int argc, char ** argv) 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 7100b411..968e1ceb 100644 --- a/test_communication/test/test_subscriber.cpp +++ b/test_communication/test/test_subscriber.cpp @@ -56,6 +56,7 @@ int main(int argc, char ** argv) 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 @@ -93,6 +94,9 @@ int main(int argc, char ** argv) } 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(); From 1ce36a38cc172bdb20cec0993b16752ae44966ed Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 2 Apr 2024 15:42:14 +0200 Subject: [PATCH 4/6] Make test_communication use idl messages instead of msg ones. Signed-off-by: Miguel Company --- test_communication/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/test_communication/CMakeLists.txt b/test_communication/CMakeLists.txt index 9133fbee..6f62ecf5 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() From 5298d3c5d4d076f27fb0f1abd06134d9b7420add Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 2 Apr 2024 15:44:23 +0200 Subject: [PATCH 5/6] Skip keyed messages for RMWs other than fastrtps ones. Signed-off-by: Miguel Company --- test_communication/CMakeLists.txt | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/test_communication/CMakeLists.txt b/test_communication/CMakeLists.txt index 6f62ecf5..92ca5531 100644 --- a/test_communication/CMakeLists.txt +++ b/test_communication/CMakeLists.txt @@ -241,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( From 24b1c61007a29598fd013c02f457c1c536afe477 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 20 Sep 2024 12:50:23 +0200 Subject: [PATCH 6/6] Changed copyright year on new files. Signed-off-by: Miguel Company --- test_communication/test/subscribe_key_types.cpp | 2 +- test_communication/test/subscribe_key_types.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test_communication/test/subscribe_key_types.cpp b/test_communication/test/subscribe_key_types.cpp index 3a90649d..3333e3df 100644 --- a/test_communication/test/subscribe_key_types.cpp +++ b/test_communication/test/subscribe_key_types.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// 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. diff --git a/test_communication/test/subscribe_key_types.hpp b/test_communication/test/subscribe_key_types.hpp index a1465595..feff54bc 100644 --- a/test_communication/test/subscribe_key_types.hpp +++ b/test_communication/test/subscribe_key_types.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// 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.