From 0fcb2e44ec00eec8aecf955d181b34b14743204c Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 7 May 2019 09:34:28 -0700 Subject: [PATCH] fix compilation against master (#111) * use refactored test messages Signed-off-by: Dirk Thomas * partial update Signed-off-by: Dirk Thomas * fix rsbag2_converter_default_plugins Signed-off-by: Karsten Knese * fix rosbag2_transport Signed-off-by: Karsten Knese * fix rosbag2_tests Signed-off-by: Karsten Knese * add wstring to introspection message Signed-off-by: Karsten Knese * default initialize qos profile Signed-off-by: Karsten Knese * avoid deprecated publish signature Signed-off-by: Karsten Knese --- .../rosbag2/types/introspection_message.cpp | 23 ++ .../test/rosbag2/test_sequential_reader.cpp | 2 +- .../test/rosbag2/test_typesupport_helpers.cpp | 2 +- rosbag2/test/rosbag2/test_writer.cpp | 4 +- .../test/rosbag2/types/test_ros2_message.cpp | 156 +++++--------- .../cdr/test_cdr_converter.cpp | 197 +++++++++++++----- .../rosbag2_test_common/publisher_manager.hpp | 8 +- .../subscription_manager.hpp | 2 +- .../resources/cdr_test/metadata.yaml | 4 +- .../resources/wrong_rmw_test/metadata.yaml | 4 +- .../test/rosbag2_tests/record_fixture.hpp | 4 +- .../test/rosbag2_tests/test_converter.cpp | 16 +- .../test_rosbag2_info_end_to_end.cpp | 4 +- .../test_rosbag2_play_end_to_end.cpp | 18 +- .../test_rosbag2_record_end_to_end.cpp | 10 +- .../rosbag2_transport/generic_publisher.cpp | 2 +- .../test/rosbag2_transport/test_play.cpp | 33 ++- .../rosbag2_transport/test_play_timing.cpp | 8 +- .../test/rosbag2_transport/test_record.cpp | 26 +-- .../rosbag2_transport/test_record_all.cpp | 21 +- .../test_record_all_no_discovery.cpp | 8 +- .../rosbag2_transport/test_rosbag2_node.cpp | 24 +-- 22 files changed, 319 insertions(+), 257 deletions(-) diff --git a/rosbag2/src/rosbag2/types/introspection_message.cpp b/rosbag2/src/rosbag2/types/introspection_message.cpp index 7fa042a4a6..adda96744b 100644 --- a/rosbag2/src/rosbag2/types/introspection_message.cpp +++ b/rosbag2/src/rosbag2/types/introspection_message.cpp @@ -89,6 +89,9 @@ void cleanup_element( if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) { std::string empty; static_cast(data)->swap(empty); + } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) { + std::wstring empty; + static_cast(data)->swap(empty); } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { deallocate_internal_types( data, @@ -106,6 +109,12 @@ void cleanup_array( auto * element = static_cast(member.get_function(data, i)); element->swap(empty); } + } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) { + for (size_t i = 0; i < member.array_size_; ++i) { + std::wstring empty; + auto * element = static_cast(member.get_function(data, i)); + element->swap(empty); + } } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { auto nested_ts = static_cast( @@ -205,6 +214,12 @@ void cleanup_vector( data_vector->swap(empty); break; } + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: { + auto data_vector = static_cast *>(data); + std::vector empty; + data_vector->swap(empty); + break; + } case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: { auto data_vector = static_cast *>(data); @@ -264,6 +279,12 @@ void allocate_array( // This is necessary because initialization of empty strings fails for g++ compiled builds new (element) std::string(""); } + } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) { + for (size_t i = 0; i < member.array_size_; ++i) { + auto * element = static_cast(member.get_function(data, i)); + // This is necessary because initialization of empty strings fails for g++ compiled builds + new (element) std::wstring(); + } } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { auto nested_ts = static_cast( member.members_->data); @@ -280,6 +301,8 @@ void allocate_element( if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) { // This is necessary because initialization of empty strings fails for g++ compiled builds new (data) std::string(""); + } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) { + new (data) std::wstring(); } else if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { allocate_internal_types( data, diff --git a/rosbag2/test/rosbag2/test_sequential_reader.cpp b/rosbag2/test/rosbag2/test_sequential_reader.cpp index 2243e88777..adc90b579d 100644 --- a/rosbag2/test/rosbag2/test_sequential_reader.cpp +++ b/rosbag2/test/rosbag2/test_sequential_reader.cpp @@ -41,7 +41,7 @@ class SequentialReaderTest : public Test rosbag2_storage::TopicMetadata topic_with_type; topic_with_type.name = "topic"; - topic_with_type.type = "test_msgs/Primitives"; + topic_with_type.type = "test_msgs/BasicTypes"; auto topics_and_types = std::vector{topic_with_type}; EXPECT_CALL(*storage_, get_all_topics_and_types()) .Times(AtMost(1)).WillRepeatedly(Return(topics_and_types)); diff --git a/rosbag2/test/rosbag2/test_typesupport_helpers.cpp b/rosbag2/test/rosbag2/test_typesupport_helpers.cpp index 7c110204cf..8914aed4df 100644 --- a/rosbag2/test/rosbag2/test_typesupport_helpers.cpp +++ b/rosbag2/test/rosbag2/test_typesupport_helpers.cpp @@ -55,7 +55,7 @@ TEST(TypesupportHelpersTest, throws_exception_if_library_cannot_be_found) { TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { auto string_typesupport = - rosbag2::get_typesupport("test_msgs/Primitives", "rosidl_typesupport_cpp"); + rosbag2::get_typesupport("test_msgs/BasicTypes", "rosidl_typesupport_cpp"); EXPECT_THAT(std::string(string_typesupport->typesupport_identifier), ContainsRegex("rosidl_typesupport")); diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp index c3150e7cee..091f60e848 100644 --- a/rosbag2/test/rosbag2/test_writer.cpp +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -75,7 +75,7 @@ TEST_F(WriterTest, auto message = std::make_shared(); message->topic_name = "test_topic"; writer_->open(storage_options_, {input_format, storage_serialization_format}); - writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", ""}); writer_->write(message); } @@ -91,7 +91,7 @@ TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_ auto message = std::make_shared(); message->topic_name = "test_topic"; writer_->open(storage_options_, {storage_serialization_format, storage_serialization_format}); - writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", ""}); writer_->write(message); } diff --git a/rosbag2/test/rosbag2/types/test_ros2_message.cpp b/rosbag2/test/rosbag2/types/test_ros2_message.cpp index f2034afd87..c3c8946a01 100644 --- a/rosbag2/test/rosbag2/types/test_ros2_message.cpp +++ b/rosbag2/test/rosbag2/types/test_ros2_message.cpp @@ -21,18 +21,14 @@ #include "rosbag2/typesupport_helpers.hpp" #include "rosbag2/types/introspection_message.hpp" -#include "test_msgs/msg/bounded_array_nested.hpp" -#include "test_msgs/msg/bounded_array_primitives_nested.hpp" -#include "test_msgs/msg/bounded_array_primitives.hpp" -#include "test_msgs/msg/dynamic_array_primitives_nested.hpp" -#include "test_msgs/msg/dynamic_array_nested.hpp" -#include "test_msgs/msg/dynamic_array_primitives.hpp" -#include "test_msgs/msg/dynamic_array_static_array_primitives_nested.hpp" -#include "test_msgs/msg/nested.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_nested.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" #include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/bounded_sequences.hpp" +#include "test_msgs/msg/multi_nested.hpp" +#include "test_msgs/msg/nested.hpp" +#include "test_msgs/msg/strings.hpp" +#include "test_msgs/msg/unbounded_sequences.hpp" using namespace testing; // NOLINT @@ -58,34 +54,29 @@ class Ros2MessageTest : public Test rcutils_allocator_t allocator_; }; -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_primitive_message) { - auto message = get_allocated_message("test_msgs/Primitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_basic_types_message) { + auto message = get_allocated_message("test_msgs/BasicTypes"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); data->bool_value = true; - data->string_value = "content"; data->int16_value = 144; } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_primitive_message_with_empty_string) { - auto message = get_allocated_message("test_msgs/Primitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_strings_message_with_empty_string) { + auto message = get_allocated_message("test_msgs/Strings"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); - data->bool_value = true; data->string_value = ""; - data->int16_value = 144; } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_primitive_message_with_big_string_no_SSO) { - auto message = get_allocated_message("test_msgs/Primitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_strings_message_with_big_string_no_SSO) { + auto message = get_allocated_message("test_msgs/Strings"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); - data->bool_value = true; data->string_value = std::string(1000, 's'); - data->int16_value = 144; } TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_message) { @@ -93,120 +84,81 @@ TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_message) { auto data = static_cast(message->message); - data->primitive_values.bool_value = true; - data->primitive_values.string_value = "content"; - data->primitive_values.int16_value = 143; + data->basic_types_value.bool_value = true; + data->basic_types_value.int16_value = 143; } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_static_array_message) { - auto message = get_allocated_message("test_msgs/StaticArrayPrimitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_arrays_message) { + auto message = get_allocated_message("test_msgs/Arrays"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); data->bool_values = {{true, false, true}}; data->string_values = {{"eins", "zwei", std::string(1000, 'd')}}; data->int32_values = {{11, 22, 33}}; } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_static_array_message_with_empty_string) { - auto message = get_allocated_message("test_msgs/StaticArrayPrimitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_arrays_message_with_empty_string) { + auto message = get_allocated_message("test_msgs/Arrays"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); data->bool_values = {{true, false, true}}; data->string_values = {{"", "zwei", std::string(1000, 'a')}}; data->int32_values = {{11, 22, 33}}; + + data->basic_types_values[0].bool_value = true; + data->basic_types_values[0].int64_value = 123; + data->basic_types_values[1].bool_value = false; + data->basic_types_values[1].int64_value = 123; } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_dynamic_array_message) { - auto message = get_allocated_message("test_msgs/DynamicArrayPrimitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_unbounded_sequences_message) { + auto message = get_allocated_message("test_msgs/UnboundedSequences"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); data->bool_values = {{true, false, true, false}}; data->string_values = {{"eins", std::string(1000, 'b'), ""}}; data->int32_values = {{11, 22, 33, 44, 55}}; + + data->basic_types_values.push_back(*get_messages_basic_types()[1]); + data->basic_types_values.push_back(*get_messages_basic_types()[2]); } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_bounded_array_message) { - auto message = get_allocated_message("test_msgs/BoundedArrayPrimitives"); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_bounded_sequences_message) { + auto message = get_allocated_message("test_msgs/BoundedSequences"); - auto data = static_cast(message->message); + auto data = static_cast(message->message); data->bool_values = {{true, false, true}}; data->string_values = {{"eins", std::string(1000, 'z'), ""}}; data->int32_values = {11}; -} - -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_static_array_nested_message) { - auto message = get_allocated_message("test_msgs/StaticArrayNested"); - auto data = static_cast(message->message); - - data->primitive_values[0].bool_value = true; - data->primitive_values[0].string_value = "eins"; - data->primitive_values[0].int64_value = 123; - data->primitive_values[1].bool_value = false; - data->primitive_values[1].string_value = std::string(1000, '2'); - data->primitive_values[1].int64_value = 123; + data->basic_types_values.push_back(*get_messages_basic_types()[1]); + data->basic_types_values.push_back(*get_messages_basic_types()[2]); } -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_dynamic_array_nested_message) { - auto message = get_allocated_message("test_msgs/DynamicArrayNested"); - - auto data = static_cast(message->message); +TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_multi_nested_message) { + auto message = get_allocated_message("test_msgs/MultiNested"); - data->primitive_values.push_back(*get_messages_primitives()[1]); - data->primitive_values.push_back(*get_messages_primitives()[2]); -} - -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_bounded_array_nested_message) { - auto message = get_allocated_message("test_msgs/BoundedArrayNested"); - - auto data = static_cast(message->message); - - data->primitive_values.push_back(*get_messages_primitives()[1]); - data->primitive_values.push_back(*get_messages_primitives()[2]); -} - -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_static_array_nested_message) { - auto message = get_allocated_message("test_msgs/DynamicArrayStaticArrayPrimitivesNested"); - - auto data = - static_cast(message->message); - - data->dynamic_array_static_array_primitive_values.push_back( - *get_messages_static_array_primitives()[0]); - data->dynamic_array_static_array_primitive_values.push_back( - *get_messages_static_array_primitives()[0]); -} - -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_dynamic_array_nested_message) { - auto message = get_allocated_message("test_msgs/DynamicArrayPrimitivesNested"); - - auto data = - static_cast(message->message); - - data->dynamic_array_primitive_values.push_back( - *get_messages_dynamic_array_primitives()[1]); - data->dynamic_array_primitive_values.push_back( - *get_messages_dynamic_array_primitives()[2]); -} + auto data = static_cast(message->message); -TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_bounded_array_nested_message) { - auto message = get_allocated_message("test_msgs/BoundedArrayPrimitivesNested"); + data->array_of_arrays[0] = *get_messages_arrays()[0]; - auto data = - static_cast(message->message); + data->bounded_sequence_of_bounded_sequences.push_back( + *get_messages_bounded_sequences()[0]); + data->bounded_sequence_of_bounded_sequences.push_back( + *get_messages_bounded_sequences()[1]); - data->bounded_array_primitive_values.push_back( - *get_messages_bounded_array_primitives()[0]); - data->bounded_array_primitive_values.push_back( - *get_messages_bounded_array_primitives()[1]); + data->unbounded_sequence_of_unbounded_sequences.push_back( + *get_messages_unbounded_sequences()[0]); + data->unbounded_sequence_of_unbounded_sequences.push_back( + *get_messages_unbounded_sequences()[0]); } TEST_F(Ros2MessageTest, allocate_ros2_message_cleans_up_topic_name_on_shutdown) { - auto message = get_allocated_message("test_msgs/BoundedArrayNested"); + auto message = get_allocated_message("test_msgs/BoundedSequences"); rosbag2::introspection_message_set_topic_name(message.get(), "Topic name"); @@ -214,7 +166,7 @@ TEST_F(Ros2MessageTest, allocate_ros2_message_cleans_up_topic_name_on_shutdown) } TEST_F(Ros2MessageTest, duplicate_set_topic_does_not_leak) { - auto message = get_allocated_message("test_msgs/BoundedArrayNested"); + auto message = get_allocated_message("test_msgs/BoundedSequences"); rosbag2::introspection_message_set_topic_name(message.get(), "Topic name"); rosbag2::introspection_message_set_topic_name(message.get(), "Topic name"); diff --git a/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp index 1852d9fbbb..91e75895a3 100644 --- a/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp +++ b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp @@ -63,10 +63,7 @@ class CdrConverterTestFixture : public Test }; TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_primitives) { - auto message = get_messages_primitives()[0]; - message->string_value = "test_deserialize"; - message->float64_value = 102.34; - message->int32_value = 10101010; + auto message = get_messages_basic_types()[1]; auto serialized_data = memory_management_->serialize_message(message); auto serialized_message = std::make_shared(); serialized_message->serialized_data = serialized_data; @@ -74,13 +71,13 @@ TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_pr serialized_message->time_stamp = 1; auto ros_message = make_shared_ros_message(); - test_msgs::msg::Primitives primitive_test_msg; + test_msgs::msg::BasicTypes primitive_test_msg; ros_message->message = &primitive_test_msg; - auto type_support = rosbag2::get_typesupport("test_msgs/Primitives", "rosidl_typesupport_cpp"); + auto type_support = rosbag2::get_typesupport("test_msgs/BasicTypes", "rosidl_typesupport_cpp"); converter_->deserialize(serialized_message, type_support, ros_message); - auto cast_message = static_cast(ros_message->message); + auto cast_message = static_cast(ros_message->message); EXPECT_THAT(*cast_message, Eq(*message)); EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); @@ -89,19 +86,96 @@ TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_pr TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_primitives) { auto ros_message = make_shared_ros_message(topic_name_); ros_message->time_stamp = 1; - auto message = get_messages_primitives()[0]; - message->string_value = "test_serialize"; - message->float64_value = 102.34; - message->int32_value = 10101010; + auto message = get_messages_basic_types()[1]; ros_message->message = message.get(); auto serialized_message = std::make_shared(); serialized_message->serialized_data = memory_management_->make_initialized_message(); - auto type_support = rosbag2::get_typesupport("test_msgs/Primitives", "rosidl_typesupport_cpp"); + auto type_support = rosbag2::get_typesupport("test_msgs/BasicTypes", "rosidl_typesupport_cpp"); converter_->serialize(ros_message, type_support, serialized_message); - auto deserialized_msg = memory_management_->deserialize_message( + auto deserialized_msg = memory_management_->deserialize_message( + serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->time_stamp)); +} + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_strings) { + auto message = get_messages_strings()[2]; + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::Strings string_test_msg; + ros_message->message = &string_test_msg; + auto type_support = rosbag2::get_typesupport("test_msgs/Strings", "rosidl_typesupport_cpp"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_strings) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->time_stamp = 1; + auto message = get_messages_strings()[2]; + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport("test_msgs/Strings", "rosidl_typesupport_cpp"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_->deserialize_message( + serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->time_stamp)); +} + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_wstrings) { + auto message = get_messages_wstrings()[0]; + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::WStrings wstring_test_msg; + ros_message->message = &wstring_test_msg; + auto type_support = rosbag2::get_typesupport("test_msgs/WStrings", "rosidl_typesupport_cpp"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_wstrings) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->time_stamp = 1; + auto message = get_messages_wstrings()[0]; + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport("test_msgs/WStrings", "rosidl_typesupport_cpp"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_->deserialize_message( serialized_message->serialized_data); EXPECT_THAT(*deserialized_msg, Eq(*message)); EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); @@ -109,10 +183,7 @@ TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_prim } TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_static_array) { - auto message = get_messages_static_array_primitives()[0]; - message->string_values = {{"test_deserialize", "another string", "the third one"}}; - message->float64_values = {{102.34, 1.9, 1236.011}}; - message->int32_values = {{11, 36, 219}}; + auto message = get_messages_arrays()[0]; auto serialized_data = memory_management_->serialize_message(message); auto serialized_message = std::make_shared(); serialized_message->serialized_data = serialized_data; @@ -120,14 +191,14 @@ TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_st serialized_message->time_stamp = 1; auto ros_message = make_shared_ros_message(); - test_msgs::msg::StaticArrayPrimitives primitive_test_msg; + test_msgs::msg::Arrays primitive_test_msg; ros_message->message = &primitive_test_msg; auto type_support = rosbag2::get_typesupport( - "test_msgs/StaticArrayPrimitives", "rosidl_typesupport_cpp"); + "test_msgs/Arrays", "rosidl_typesupport_cpp"); converter_->deserialize(serialized_message, type_support, ros_message); - auto cast_message = static_cast(ros_message->message); + auto cast_message = static_cast(ros_message->message); EXPECT_THAT(*cast_message, Eq(*message)); EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); @@ -136,36 +207,25 @@ TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_st TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_static_array) { auto ros_message = make_shared_ros_message(topic_name_); ros_message->time_stamp = 1; - auto message = get_messages_static_array_primitives()[0]; - message->string_values = {{"test_deserialize", "another string", "the third one"}}; - message->float64_values = {{102.34, 1.9, 1236.011}}; - message->int32_values = {{11, 36, 219}}; + auto message = get_messages_arrays()[0]; ros_message->message = message.get(); auto serialized_message = std::make_shared(); serialized_message->serialized_data = memory_management_->make_initialized_message(); auto type_support = rosbag2::get_typesupport( - "test_msgs/StaticArrayPrimitives", "rosidl_typesupport_cpp"); + "test_msgs/Arrays", "rosidl_typesupport_cpp"); converter_->serialize(ros_message, type_support, serialized_message); auto deserialized_msg = memory_management_-> - deserialize_message(serialized_message->serialized_data); + deserialize_message(serialized_message->serialized_data); EXPECT_THAT(*deserialized_msg, Eq(*message)); EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->time_stamp)); } -TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_dynamic_array_nest) { - auto message = get_messages_dynamic_array_nested()[0]; - test_msgs::msg::Primitives first_primitive_message; - first_primitive_message.string_value = "I am the first"; - first_primitive_message.float32_value = 35.7f; - test_msgs::msg::Primitives second_primitive_message; - second_primitive_message.string_value = "I am the second"; - second_primitive_message.float32_value = 135.72f; - message->primitive_values.push_back(first_primitive_message); - message->primitive_values.push_back(second_primitive_message); +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_unbounded_sequence) { + auto message = get_messages_unbounded_sequences()[1]; auto serialized_data = memory_management_->serialize_message(message); auto serialized_message = std::make_shared(); serialized_message->serialized_data = serialized_data; @@ -173,42 +233,77 @@ TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_dy serialized_message->time_stamp = 1; auto ros_message = make_shared_ros_message(); - test_msgs::msg::DynamicArrayNested dynamic_nested_message; + test_msgs::msg::UnboundedSequences dynamic_nested_message; ros_message->message = &dynamic_nested_message; auto type_support = rosbag2::get_typesupport( - "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + "test_msgs/UnboundedSequences", "rosidl_typesupport_cpp"); + + converter_->deserialize(serialized_message, type_support, ros_message); + + auto cast_message = static_cast(ros_message->message); + EXPECT_THAT(*cast_message, Eq(*message)); + EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); + EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); +} + +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_unbounded_sequence) { + auto ros_message = make_shared_ros_message(topic_name_); + ros_message->time_stamp = 1; + auto message = get_messages_unbounded_sequences()[1]; + test_msgs::msg::BasicTypes first_primitive_message; + ros_message->message = message.get(); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport( + "test_msgs/UnboundedSequences", "rosidl_typesupport_cpp"); + + converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_-> + deserialize_message(serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->time_stamp)); +} + +TEST_F(CdrConverterTestFixture, deserialize_converts_cdr_into_ros_message_for_multi_nested) { + auto message = get_messages_multi_nested()[0]; + auto serialized_data = memory_management_->serialize_message(message); + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = serialized_data; + serialized_message->topic_name = topic_name_; + serialized_message->time_stamp = 1; + + auto ros_message = make_shared_ros_message(); + test_msgs::msg::MultiNested multi_nested_message; + ros_message->message = &multi_nested_message; + auto type_support = rosbag2::get_typesupport( + "test_msgs/MultiNested", "rosidl_typesupport_cpp"); converter_->deserialize(serialized_message, type_support, ros_message); - auto cast_message = static_cast(ros_message->message); + auto cast_message = static_cast(ros_message->message); EXPECT_THAT(*cast_message, Eq(*message)); EXPECT_THAT(ros_message->time_stamp, Eq(serialized_message->time_stamp)); EXPECT_THAT(ros_message->topic_name, StrEq(serialized_message->topic_name)); } -TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_dynamic_array_nest) { +TEST_F(CdrConverterTestFixture, serialize_converts_ros_message_into_cdr_for_multi_nested) { auto ros_message = make_shared_ros_message(topic_name_); ros_message->time_stamp = 1; - auto message = get_messages_dynamic_array_nested()[0]; - test_msgs::msg::Primitives first_primitive_message; - first_primitive_message.string_value = "I am the first"; - first_primitive_message.float32_value = 35.7f; - test_msgs::msg::Primitives second_primitive_message; - second_primitive_message.string_value = "I am the second"; - second_primitive_message.float32_value = 135.72f; - message->primitive_values.push_back(first_primitive_message); - message->primitive_values.push_back(second_primitive_message); + auto message = get_messages_multi_nested()[0]; ros_message->message = message.get(); auto serialized_message = std::make_shared(); serialized_message->serialized_data = memory_management_->make_initialized_message(); auto type_support = rosbag2::get_typesupport( - "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + "test_msgs/MultiNested", "rosidl_typesupport_cpp"); converter_->serialize(ros_message, type_support, serialized_message); auto deserialized_msg = memory_management_-> - deserialize_message(serialized_message->serialized_data); + deserialize_message(serialized_message->serialized_data); EXPECT_THAT(*deserialized_msg, Eq(*message)); EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->time_stamp)); diff --git a/rosbag2_test_common/include/rosbag2_test_common/publisher_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/publisher_manager.hpp index 3b03222303..07801f98d7 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/publisher_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/publisher_manager.hpp @@ -52,17 +52,17 @@ class PublisherManager CountFunction count_stored_messages) { if (expected_messages != 0) { while (rclcpp::ok() && count_stored_messages(topic_name) < expected_messages) { - publisher->publish(message); + publisher->publish(*message); // rate limiting std::this_thread::sleep_for(50ms); } } else { // Just publish a few messages - they should never be stored - publisher->publish(message); + publisher->publish(*message); std::this_thread::sleep_for(50ms); - publisher->publish(message); + publisher->publish(*message); std::this_thread::sleep_for(50ms); - publisher->publish(message); + publisher->publish(*message); } }); } diff --git a/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp index cfb4c43711..6171c21287 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp @@ -39,7 +39,7 @@ class SubscriptionManager template void add_subscription(const std::string & topic_name, size_t expected_number_of_messages) { - rmw_qos_profile_t qos_profile; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; qos_profile.depth = 4; qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; diff --git a/rosbag2_tests/resources/cdr_test/metadata.yaml b/rosbag2_tests/resources/cdr_test/metadata.yaml index 1130e88892..179d1732b2 100644 --- a/rosbag2_tests/resources/cdr_test/metadata.yaml +++ b/rosbag2_tests/resources/cdr_test/metadata.yaml @@ -11,11 +11,11 @@ rosbag2_bagfile_information: topics_with_message_count: - topic_metadata: name: /test_topic - type: test_msgs/Primitives + type: test_msgs/BasicTypes serialization_format: cdr message_count: 3 - topic_metadata: name: /array_topic - type: test_msgs/StaticArrayPrimitives + type: test_msgs/Arrays serialization_format: cdr message_count: 4 diff --git a/rosbag2_tests/resources/wrong_rmw_test/metadata.yaml b/rosbag2_tests/resources/wrong_rmw_test/metadata.yaml index 32d6e81062..301e3ed2e2 100644 --- a/rosbag2_tests/resources/wrong_rmw_test/metadata.yaml +++ b/rosbag2_tests/resources/wrong_rmw_test/metadata.yaml @@ -11,11 +11,11 @@ rosbag2_bagfile_information: topics_with_message_count: - topic_metadata: name: /test_topic - type: test_msgs/Primitives + type: test_msgs/BasicTypes serialization_format: wrong_format message_count: 3 - topic_metadata: name: /array_topic - type: test_msgs/StaticArrayPrimitives + type: test_msgs/Arrays serialization_format: wrong_format message_count: 4 diff --git a/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp b/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp index fe0fb1cd0f..3c874ca526 100644 --- a/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp +++ b/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp @@ -24,8 +24,8 @@ #include "rclcpp/rclcpp.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" #include "rosbag2_storage/filesystem_helper.hpp" #include "rosbag2_storage_default_plugins/sqlite/sqlite_storage.hpp" diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp index d12ff401b0..5914f5791d 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -40,13 +40,13 @@ class ConverterTestFixture : public Test cdr_serializer_ = factory_->load_serializer("cdr"); cdr_deserializer_ = factory_->load_deserializer("cdr"); type_support_ = - rosbag2::get_typesupport("test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + rosbag2::get_typesupport("test_msgs/MultiNested", "rosidl_typesupport_cpp"); } std::shared_ptr allocate_empty_dynamic_array_message() { auto introspection_type_support = rosbag2::get_typesupport( - "test_msgs/DynamicArrayNested", "rosidl_typesupport_introspection_cpp"); + "test_msgs/MultiNested", "rosidl_typesupport_introspection_cpp"); auto introspection_message = rosbag2::allocate_introspection_message(introspection_type_support, &allocator_); introspection_message->time_stamp = 1; @@ -57,11 +57,9 @@ class ConverterTestFixture : public Test void fill_dynamic_array_message(std::shared_ptr message) { auto correctly_typed_ros_message = - reinterpret_cast(message->message); - auto primitive_msgs = get_messages_primitives(); - for (const auto & primitive_msg : primitive_msgs) { - correctly_typed_ros_message->primitive_values.push_back(*primitive_msg); - } + reinterpret_cast(message->message); + auto multi_nested = get_messages_multi_nested()[0]; + correctly_typed_ros_message->array_of_arrays = multi_nested->array_of_arrays; } std::unique_ptr factory_; @@ -90,8 +88,8 @@ TEST_F(ConverterTestFixture, cdr_converter_plugin_can_serialize_and_deserialize_ serialized_message.reset(); EXPECT_THAT( - *static_cast(final_roundtrip_ros_message->message), - Eq(*static_cast(initial_ros_message->message))); + *static_cast(final_roundtrip_ros_message->message), + Eq(*static_cast(initial_ros_message->message))); EXPECT_THAT(final_roundtrip_ros_message->topic_name, StrEq(initial_ros_message->topic_name)); EXPECT_THAT(final_roundtrip_ros_message->time_stamp, Eq(initial_ros_message->time_stamp)); } diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 54eaeb31d1..fe4aecbf43 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -50,9 +50,9 @@ TEST_F(InfoEndToEndTestFixture, info_end_to_end_test) { "\nMessages: 7" "\nTopic information: ")); EXPECT_THAT(output, HasSubstr( - "Topic: /test_topic | Type: test_msgs/Primitives | Count: 3 | Serialization Format: cdr\n")); + "Topic: /test_topic | Type: test_msgs/BasicTypes | Count: 3 | Serialization Format: cdr\n")); EXPECT_THAT(output, HasSubstr( - "Topic: /array_topic | Type: test_msgs/StaticArrayPrimitives | Count: 4 | " + "Topic: /array_topic | Type: test_msgs/Arrays | Count: 4 | " "Serialization Format: cdr")); } diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_play_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_play_end_to_end.cpp index 6f74136d91..fd9879b6c9 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_play_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_play_end_to_end.cpp @@ -25,8 +25,8 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/process_execution_helpers.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" #include "rosbag2_storage_default_plugins/sqlite/sqlite_storage.hpp" #include "rosbag2_test_common/subscription_manager.hpp" @@ -58,8 +58,8 @@ class PlayEndToEndTestFixture : public Test }; TEST_F(PlayEndToEndTestFixture, play_end_to_end_test) { - sub_->add_subscription("/array_topic", 2); - sub_->add_subscription("/test_topic", 3); + sub_->add_subscription("/array_topic", 2); + sub_->add_subscription("/test_topic", 3); auto subscription_future = sub_->spin_subscriptions(); @@ -67,22 +67,22 @@ TEST_F(PlayEndToEndTestFixture, play_end_to_end_test) { subscription_future.get(); - auto primitive_messages = sub_->get_received_messages("/test_topic"); - auto array_messages = sub_->get_received_messages( + auto primitive_messages = sub_->get_received_messages("/test_topic"); + auto array_messages = sub_->get_received_messages( "/array_topic"); EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); EXPECT_THAT(primitive_messages, SizeIs(Ge(3u))); EXPECT_THAT(primitive_messages, - Each(Pointee(Field(&test_msgs::msg::Primitives::string_value, "test")))); + Each(Pointee(Field(&test_msgs::msg::BasicTypes::string_value, "test")))); EXPECT_THAT(array_messages, SizeIs(Ge(2u))); EXPECT_THAT(array_messages, - Each(Pointee(Field(&test_msgs::msg::StaticArrayPrimitives::bool_values, + Each(Pointee(Field(&test_msgs::msg::Arrays::bool_values, ElementsAre(true, false, true))))); EXPECT_THAT(array_messages, - Each(Pointee(Field(&test_msgs::msg::StaticArrayPrimitives::string_values, + Each(Pointee(Field(&test_msgs::msg::Arrays::string_values, ElementsAre("Complex Hello1", "Complex Hello2", "Complex Hello3"))))); } diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index 9f510b584a..e12740a425 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -22,11 +22,11 @@ #include "rosbag2_storage/metadata_io.hpp" TEST_F(RecordFixture, record_end_to_end_test) { - auto message = get_messages_primitives()[0]; + auto message = get_messages_strings()[0]; message->string_value = "test"; size_t expected_test_messages = 3; - auto wrong_message = get_messages_primitives()[0]; + auto wrong_message = get_messages_strings()[0]; wrong_message->string_value = "wrong_content"; auto process_handle = start_execution("ros2 bag record --output " + bag_path_ + " /test_topic"); @@ -58,13 +58,13 @@ TEST_F(RecordFixture, record_end_to_end_test) { metadata_io.write_metadata(bag_path_, metadata); #endif - auto test_topic_messages = get_messages_for_topic("/test_topic"); + auto test_topic_messages = get_messages_for_topic("/test_topic"); EXPECT_THAT(test_topic_messages, SizeIs(Ge(expected_test_messages))); EXPECT_THAT(test_topic_messages, - Each(Pointee(Field(&test_msgs::msg::Primitives::string_value, "test")))); + Each(Pointee(Field(&test_msgs::msg::Strings::string_value, "test")))); EXPECT_THAT(get_rwm_format_for_topic("/test_topic", db), Eq(rmw_get_serialization_format())); - auto wrong_topic_messages = get_messages_for_topic("/wrong_topic"); + auto wrong_topic_messages = get_messages_for_topic("/wrong_topic"); EXPECT_THAT(wrong_topic_messages, IsEmpty()); } diff --git a/rosbag2_transport/src/rosbag2_transport/generic_publisher.cpp b/rosbag2_transport/src/rosbag2_transport/generic_publisher.cpp index 0f4b53da3f..2884dd6c91 100644 --- a/rosbag2_transport/src/rosbag2_transport/generic_publisher.cpp +++ b/rosbag2_transport/src/rosbag2_transport/generic_publisher.cpp @@ -30,7 +30,7 @@ GenericPublisher::GenericPublisher( void GenericPublisher::publish(std::shared_ptr message) { auto return_code = rcl_publish_serialized_message( - get_publisher_handle(), message.get()); + get_publisher_handle(), message.get(), NULL); if (return_code != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 4ba20c3c26..9a4553a873 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -22,8 +22,8 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_transport/rosbag2_transport.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" #include "rosbag2_transport_test_fixture.hpp" @@ -54,16 +54,16 @@ class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) { - auto primitive_message1 = get_messages_primitives()[0]; - primitive_message1->string_value = "Hello World"; + auto primitive_message1 = get_messages_basic_types()[0]; + primitive_message1->int32_value = 42; - auto complex_message1 = get_messages_static_array_primitives()[0]; - complex_message1->string_values = {{"Complex Hello1", "Complex Hello2", "Complex Hello3"}}; + auto complex_message1 = get_messages_arrays()[0]; + complex_message1->float32_values = {{40.0f, 2.0f, 0.0f}}; complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/Primitives", ""}, - {"topic2", "test_msgs/StaticArrayPrimitives", ""}, + {"topic1", "test_msgs/BasicTypes", ""}, + {"topic2", "test_msgs/Arrays", ""}, }; std::vector> messages = @@ -78,9 +78,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) // Due to a problem related to the subscriber, we play many (3) messages but make the subscriber // node spin only until 2 have arrived. Hence the 2 as `launch_subscriber()` argument. - sub_->add_subscription("/topic1", 2); - sub_->add_subscription( - "/topic2", 2); + sub_->add_subscription("/topic1", 2); + sub_->add_subscription("/topic2", 2); auto await_received_messages = sub_->spin_subscriptions(); @@ -89,19 +88,19 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) await_received_messages.get(); - auto replayed_test_primitives = sub_->get_received_messages( + auto replayed_test_primitives = sub_->get_received_messages( "/topic1"); EXPECT_THAT(replayed_test_primitives, SizeIs(Ge(2u))); EXPECT_THAT(replayed_test_primitives, - Each(Pointee(Field(&test_msgs::msg::Primitives::string_value, "Hello World")))); + Each(Pointee(Field(&test_msgs::msg::BasicTypes::int32_value, 42)))); - auto replayed_test_arrays = sub_->get_received_messages( + auto replayed_test_arrays = sub_->get_received_messages( "/topic2"); EXPECT_THAT(replayed_test_arrays, SizeIs(Ge(2u))); EXPECT_THAT(replayed_test_arrays, - Each(Pointee(Field(&test_msgs::msg::StaticArrayPrimitives::bool_values, + Each(Pointee(Field(&test_msgs::msg::Arrays::bool_values, ElementsAre(true, false, true))))); EXPECT_THAT(replayed_test_arrays, - Each(Pointee(Field(&test_msgs::msg::StaticArrayPrimitives::string_values, - ElementsAre("Complex Hello1", "Complex Hello2", "Complex Hello3"))))); + Each(Pointee(Field(&test_msgs::msg::Arrays::float32_values, + ElementsAre(40.0f, 2.0f, 0.0f))))); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 11a5ebbc8f..dbf7062d51 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_transport/rosbag2_transport.hpp" #include "rosbag2_transport_test_fixture.hpp" -#include "test_msgs/msg/primitives.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" using namespace ::testing; // NOLINT @@ -31,15 +31,15 @@ using namespace rosbag2_transport; // NOLINT TEST_F(Rosbag2TransportTestFixture, playing_respects_relative_timing_of_stored_messages) { rclcpp::init(0, nullptr); - auto primitive_message1 = get_messages_primitives()[0]; + auto primitive_message1 = get_messages_strings()[0]; primitive_message1->string_value = "Hello World 1"; - auto primitive_message2 = get_messages_primitives()[0]; + auto primitive_message2 = get_messages_strings()[0]; primitive_message2->string_value = "Hello World 2"; auto message_time_difference = std::chrono::seconds(1); auto topics_and_types = - std::vector{{"topic1", "test_msgs/Primitives", ""}}; + std::vector{{"topic1", "test_msgs/Strings", ""}}; std::vector> messages = {serialize_test_message("topic1", 0, primitive_message1), serialize_test_message("topic1", 0, primitive_message2)}; diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 2bd311f435..fe6af685c7 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -22,26 +22,23 @@ #include "record_integration_fixture.hpp" #include "rosbag2_transport/rosbag2_transport.hpp" #include "rosbag2/types.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) { - auto array_message = get_messages_static_array_primitives()[0]; - array_message->string_values = {{"Complex Hello1", "Complex Hello2", "Complex Hello3"}}; - array_message->bool_values = {{true, false, true}}; + auto array_message = get_messages_arrays()[0]; std::string array_topic = "/array_topic"; - auto string_message = get_messages_primitives()[0]; - string_message->string_value = "Hello World"; + auto string_message = get_messages_strings()[1]; std::string string_topic = "/string_topic"; start_recording({false, false, {string_topic, array_topic}, "rmw_format", 100ms}); - pub_man_.add_publisher( + pub_man_.add_publisher( string_topic, string_message, 2); - pub_man_.add_publisher( + pub_man_.add_publisher( array_topic, array_message, 2); run_publishers(); stop_recording(); @@ -53,14 +50,13 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are EXPECT_THAT(recorded_topics.at(string_topic).serialization_format, Eq("rmw_format")); EXPECT_THAT(recorded_topics.at(array_topic).serialization_format, Eq("rmw_format")); ASSERT_THAT(recorded_messages, SizeIs(4)); - auto string_messages = filter_messages( + auto string_messages = filter_messages( recorded_messages, string_topic); - auto array_messages = filter_messages( + auto array_messages = filter_messages( recorded_messages, array_topic); ASSERT_THAT(string_messages, SizeIs(2)); ASSERT_THAT(array_messages, SizeIs(2)); - EXPECT_THAT(string_messages[0]->string_value, Eq("Hello World")); - EXPECT_THAT(array_messages[0]->bool_values, ElementsAre(true, false, true)); - EXPECT_THAT(array_messages[0]->string_values, - ElementsAre("Complex Hello1", "Complex Hello2", "Complex Hello3")); + EXPECT_THAT(string_messages[0]->string_value, Eq(string_message->string_value)); + EXPECT_THAT(array_messages[0]->bool_values, Eq(array_message->bool_values)); + EXPECT_THAT(array_messages[0]->float32_values, Eq(array_message->float32_values)); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index f98527423f..fb8dadebff 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -17,23 +17,23 @@ #include #include "record_integration_fixture.hpp" -#include "test_msgs/msg/primitives.hpp" -#include "test_msgs/msg/static_array_primitives.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) { - auto array_message = get_messages_static_array_primitives()[0]; - array_message->string_values = {{"Complex Hello1", "Complex Hello2", "Complex Hello3"}}; + auto array_message = get_messages_arrays()[0]; + array_message->float32_values = {{40.0f, 2.0f, 0.0f}}; array_message->bool_values = {{true, false, true}}; std::string array_topic = "/array_topic"; - auto string_message = get_messages_primitives()[0]; + auto string_message = get_messages_strings()[0]; string_message->string_value = "Hello World"; std::string string_topic = "/string_topic"; - pub_man_.add_publisher(string_topic, string_message, 2); - pub_man_.add_publisher(array_topic, array_message, 2); + pub_man_.add_publisher(string_topic, string_message, 2); + pub_man_.add_publisher(array_topic, array_message, 2); start_recording({true, false, {}, "rmw_format", 100ms}); run_publishers(); @@ -42,14 +42,13 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are auto recorded_messages = writer_->get_messages(); ASSERT_THAT(recorded_messages, SizeIs(4)); - auto string_messages = filter_messages( + auto string_messages = filter_messages( recorded_messages, string_topic); - auto array_messages = filter_messages( + auto array_messages = filter_messages( recorded_messages, array_topic); ASSERT_THAT(string_messages, SizeIs(2)); ASSERT_THAT(array_messages, SizeIs(2)); EXPECT_THAT(string_messages[0]->string_value, Eq("Hello World")); EXPECT_THAT(array_messages[0]->bool_values, ElementsAre(true, false, true)); - EXPECT_THAT(array_messages[0]->string_values, - ElementsAre("Complex Hello1", "Complex Hello2", "Complex Hello3")); + EXPECT_THAT(array_messages[0]->float32_values, ElementsAre(40.0f, 2.0f, 0.0f)); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp index 89fa478a53..14cf488453 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp @@ -19,22 +19,22 @@ #include #include "record_integration_fixture.hpp" -#include "test_msgs/msg/primitives.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_announced_topics) { - auto string_message = get_messages_primitives()[0]; + auto string_message = get_messages_strings()[0]; string_message->string_value = "Hello World"; start_recording({true, true, {}, "rmw_format", 1ms}); std::this_thread::sleep_for(100ms); auto publisher_node = std::make_shared("publisher_for_test"); - auto publisher = publisher_node->create_publisher("/string_topic"); + auto publisher = publisher_node->create_publisher("/string_topic"); for (int i = 0; i < 5; ++i) { std::this_thread::sleep_for(20ms); - publisher->publish(string_message); + publisher->publish(*string_message); } stop_recording(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp index fd23484bc3..454ab705fa 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/memory_management.hpp" #include "test_msgs/message_fixtures.hpp" -#include "test_msgs/msg/primitives.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "../../src/rosbag2_transport/rosbag2_node.hpp" using namespace ::testing; // NOLINT @@ -49,7 +49,7 @@ class RosBag2NodeFixture : public Test void create_publisher(const std::string & topic) { - auto publisher = publisher_node_->create_publisher(topic); + auto publisher = publisher_node_->create_publisher(topic); publishers_.push_back(publisher); } @@ -61,7 +61,7 @@ class RosBag2NodeFixture : public Test auto subscription = node_->create_generic_subscription(topic_name, type, [this, &counter, &messages](std::shared_ptr message) { auto string_message = - memory_management_.deserialize_message(message); + memory_management_.deserialize_message(message); messages.push_back(string_message->string_value); counter++; }); @@ -74,7 +74,7 @@ class RosBag2NodeFixture : public Test std::shared_ptr serialize_string_message(const std::string & message) { - auto string_message = std::make_shared(); + auto string_message = std::make_shared(); string_message->string_value = message; return memory_management_.serialize_message(string_message); } @@ -97,7 +97,7 @@ TEST_F(RosBag2NodeFixture, publisher_and_subscriber_work) // We currently publish more messages because they can get lost std::vector test_messages = {"Hello World", "Hello World"}; std::string topic_name = "string_topic"; - std::string type = "test_msgs/Primitives"; + std::string type = "test_msgs/Strings"; auto publisher = node_->create_generic_publisher(topic_name, type); @@ -134,7 +134,7 @@ TEST_F(RosBag2NodeFixture, auto topics_and_types = node_->get_topics_with_types({"string_topic"}); ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("test_msgs/Primitives")); + EXPECT_THAT(topics_and_types.begin()->second, StrEq("test_msgs/Strings")); } TEST_F(RosBag2NodeFixture, @@ -146,7 +146,7 @@ TEST_F(RosBag2NodeFixture, auto topics_and_types = node_->get_topics_with_types({"/string_topic"}); ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("test_msgs/Primitives")); + EXPECT_THAT(topics_and_types.begin()->second, StrEq("test_msgs/Strings")); } TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_only_specified_topics) { @@ -162,8 +162,8 @@ TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_only_specified_topics) auto topics_and_types = node_->get_topics_with_types({first_topic, second_topic}); ASSERT_THAT(topics_and_types, SizeIs(2)); - EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("test_msgs/Primitives")); - EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("test_msgs/Primitives")); + EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("test_msgs/Strings")); + EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("test_msgs/Strings")); } TEST_F(RosBag2NodeFixture, get_all_topics_with_types_returns_all_topics) @@ -180,7 +180,7 @@ TEST_F(RosBag2NodeFixture, get_all_topics_with_types_returns_all_topics) auto topics_and_types = node_->get_all_topics_with_types(); ASSERT_THAT(topics_and_types, SizeIs(Ge(3u))); - EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("test_msgs/Primitives")); - EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("test_msgs/Primitives")); - EXPECT_THAT(topics_and_types.find(third_topic)->second, StrEq("test_msgs/Primitives")); + EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("test_msgs/Strings")); + EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("test_msgs/Strings")); + EXPECT_THAT(topics_and_types.find(third_topic)->second, StrEq("test_msgs/Strings")); }