diff --git a/ddsrecorder/src/cpp/tool/DdsRecorder.cpp b/ddsrecorder/src/cpp/tool/DdsRecorder.cpp index 7e7b4e4e5..63ba0aa1e 100644 --- a/ddsrecorder/src/cpp/tool/DdsRecorder.cpp +++ b/ddsrecorder/src/cpp/tool/DdsRecorder.cpp @@ -72,7 +72,8 @@ DdsRecorder::DdsRecorder( configuration_.log_publish_time, configuration_.only_with_type, configuration_.mcap_writer_options, - configuration_.record_types); + configuration_.record_types, + configuration_.ros2_types); // Create MCAP Handler mcap_handler_ = std::make_shared( diff --git a/ddsrecorder/test/blackbox/mcap/CMakeLists.txt b/ddsrecorder/test/blackbox/mcap/CMakeLists.txt index 26fea97a6..c90567fe8 100644 --- a/ddsrecorder/test/blackbox/mcap/CMakeLists.txt +++ b/ddsrecorder/test/blackbox/mcap/CMakeLists.txt @@ -27,7 +27,8 @@ set(TEST_SOURCES set(TEST_LIST mcap_data_msgs - mcap_data_topic + mcap_dds_topic + mcap_ros2_topic mcap_data_num_msgs mcap_data_num_msgs_downsampling transition_running diff --git a/ddsrecorder/test/blackbox/mcap/McapFileCreationTest.cpp b/ddsrecorder/test/blackbox/mcap/McapFileCreationTest.cpp index 056bc1e79..b0c327ce8 100644 --- a/ddsrecorder/test/blackbox/mcap/McapFileCreationTest.cpp +++ b/ddsrecorder/test/blackbox/mcap/McapFileCreationTest.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -66,13 +67,16 @@ namespace test { const unsigned int DOMAIN = 222; -std::string topic = "TypeIntrospectionTopic"; -std::string data_type_name = "HelloWorld"; +const std::string dds_topic_name = "TypeIntrospectionTopic"; +const std::string dds_type_name = "HelloWorld"; -unsigned int n_msgs = 3; -std::string send_message = "Hello World"; -unsigned int index = 6; -unsigned int downsampling = 3; +const std::string ros2_topic_name = "rt/hello"; +const std::string ros2_type_name = "std_msgs::msg::dds_::String_"; + +const unsigned int n_msgs = 3; +const std::string send_message = "Hello World"; +const unsigned int index = 6; +const unsigned int downsampling = 3; eprosima::fastdds::dds::DataWriter* writer_; eprosima::fastrtps::types::DynamicType_ptr dynamic_type_; @@ -80,10 +84,11 @@ eprosima::fastrtps::types::DynamicType_ptr dynamic_type_; } // test std::unique_ptr create_recorder( - std::string file_name, - int downsampling, + const std::string file_name, + const int downsampling, DdsRecorderState recorder_state = DdsRecorderState::RUNNING, - unsigned int event_window = 20) + const unsigned int event_window = 20, + const bool ros2_types = false) { YAML::Node yml; @@ -96,6 +101,7 @@ std::unique_ptr create_recorder( eprosima::ddspipe::core::types::DomainId domainId; domainId.domain_id = test::DOMAIN; configuration.simple_configuration->domain = domainId; + configuration.ros2_types = ros2_types; return std::make_unique( configuration, @@ -105,9 +111,9 @@ std::unique_ptr create_recorder( } void create_publisher( - std::string topic_name, - unsigned int domain, - DataTypeKind data_type_kind) + const std::string topic_name, + const std::string type_name, + const unsigned int domain) { eprosima::fastdds::dds::DomainParticipantQos pqos; pqos.name("TypeIntrospectionExample_Participant_Publisher"); @@ -121,7 +127,7 @@ void create_publisher( // Register the type registerHelloWorldTypes(); test::dynamic_type_ = eprosima::fastrtps::types::TypeObjectFactory::get_instance()->build_dynamic_type( - test::data_type_name, + type_name, GetHelloWorldIdentifier(true), GetHelloWorldObject(true)); @@ -136,7 +142,7 @@ void create_publisher( eprosima::fastdds::dds::Publisher* publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT, nullptr); // Create the DDS Topic - eprosima::fastdds::dds::Topic* topic_ = participant_->create_topic(topic_name, test::data_type_name, + eprosima::fastdds::dds::Topic* topic_ = participant_->create_topic(topic_name, type_name, TOPIC_QOS_DEFAULT); // Create the DDS DataWriter @@ -144,8 +150,8 @@ void create_publisher( } eprosima::fastrtps::types::DynamicData_ptr send_sample( - unsigned int index = 1, - unsigned int time_sleep = 100) + const unsigned int index = 1, + const unsigned int time_sleep = 100) { // Create and initialize new dynamic data eprosima::fastrtps::types::DynamicData_ptr dynamic_data_; @@ -165,32 +171,32 @@ eprosima::fastrtps::types::DynamicData_ptr send_sample( } eprosima::fastrtps::types::DynamicData_ptr record( - std::string file_name, - unsigned int num_msgs = 1, - unsigned int downsampling = 1) + const std::string file_name, + const unsigned int num_msgs = 1, + const unsigned int downsampling = 1, + const bool ros2_types = false) { eprosima::fastrtps::types::DynamicData_ptr send_data; + { + // Create Recorder + auto recorder = create_recorder(file_name, downsampling, DdsRecorderState::RUNNING, 20, ros2_types); - // Create Recorder - auto recorder = create_recorder(file_name, downsampling); - - // Create Publisher - create_publisher( - test::topic, - test::DOMAIN, - DataTypeKind::HELLO_WORLD); + // Create Publisher + ros2_types ? create_publisher(test::ros2_topic_name, test::ros2_type_name, test::DOMAIN) : create_publisher( + test::dds_topic_name, test::dds_type_name, test::DOMAIN); - // Send data - for (unsigned int i = 0; i < num_msgs; i++) - { - send_data = send_sample(test::index); + // Send data + for (unsigned int i = 0; i < num_msgs; i++) + { + send_data = send_sample(test::index); + } } return send_data; } mcap::LinearMessageView get_msgs_mcap( - std::string file_name, + const std::string file_name, mcap::McapReader& mcap_reader_) { auto status = mcap_reader_.open(file_name); @@ -201,26 +207,26 @@ mcap::LinearMessageView get_msgs_mcap( } std::tuple record_with_transitions( - std::string file_name, + const std::string file_name, DdsRecorderState init_state, - unsigned int first_round, - unsigned int secound_round, + const unsigned int first_round, + const unsigned int secound_round, DdsRecorderState current_state, EventKind event = EventKind::NO_EVENT, - unsigned int event_window = 20, + const unsigned int event_window = 20, unsigned int time_sleep = 0, - unsigned int downsampling = 1) + const unsigned int downsampling = 1, + const bool ros2_types = false) { uint64_t current_time; { // Create Publisher - create_publisher( - test::topic, - test::DOMAIN, - DataTypeKind::HELLO_WORLD); + ros2_types ? create_publisher(test::ros2_topic_name, test::ros2_type_name, test::DOMAIN) : create_publisher( + test::dds_topic_name, test::dds_type_name, test::DOMAIN); // Create Recorder - std::unique_ptr recorder = create_recorder(file_name, downsampling, init_state, event_window); + std::unique_ptr recorder = + create_recorder(file_name, downsampling, init_state, event_window, ros2_types); // Send data for (unsigned int i = 0; i < first_round; i++) @@ -303,7 +309,7 @@ std::tuple record_with_transitions( TEST(McapFileCreationTest, mcap_data_msgs) { - std::string file_name = "output_mcap_data_msgs.mcap"; + const std::string file_name = "output_mcap_data_msgs.mcap"; eprosima::fastrtps::types::DynamicData_ptr send_data; send_data = record(file_name); @@ -332,18 +338,44 @@ TEST(McapFileCreationTest, mcap_data_msgs) } -TEST(McapFileCreationTest, mcap_data_topic) +TEST(McapFileCreationTest, mcap_dds_topic) { - std::string file_name = "output_mcap_data_topic.mcap"; + const std::string file_name = "output_mcap_dds_topic.mcap"; record(file_name); mcap::McapReader mcap_reader; auto messages = get_msgs_mcap(file_name, mcap_reader); - std::string received_topic; - std::string received_data_type_name; + std::string received_topic = ""; + std::string received_data_type_name = ""; + + for (auto it = messages.begin(); it != messages.end(); it++) + { + received_topic = it->channel->topic; + received_data_type_name = it->schema->name; + } + mcap_reader.close(); + + // Test data + ASSERT_EQ(received_topic, test::dds_topic_name); + ASSERT_EQ(received_data_type_name, test::dds_type_name); + +} + +TEST(McapFileCreationTest, mcap_ros2_topic) +{ + + const std::string file_name = "output_mcap_ros2_topic.mcap"; + + record(file_name, 1, 1, true); + + mcap::McapReader mcap_reader; + auto messages = get_msgs_mcap(file_name, mcap_reader); + + std::string received_topic = ""; + std::string received_data_type_name = ""; for (auto it = messages.begin(); it != messages.end(); it++) { @@ -353,15 +385,15 @@ TEST(McapFileCreationTest, mcap_data_topic) mcap_reader.close(); // Test data - ASSERT_EQ(received_topic, test::topic); - ASSERT_EQ(received_data_type_name, test::data_type_name); + ASSERT_EQ(received_topic, eprosima::utils::demangle_if_ros_topic(test::ros2_topic_name)); + ASSERT_EQ(received_data_type_name, eprosima::utils::demangle_if_ros_type(test::ros2_type_name)); } TEST(McapFileCreationTest, mcap_data_num_msgs) { - std::string file_name = "output_mcap_data_num_msgs.mcap"; + const std::string file_name = "output_mcap_data_num_msgs.mcap"; record(file_name, test::n_msgs); @@ -383,7 +415,7 @@ TEST(McapFileCreationTest, mcap_data_num_msgs) TEST(McapFileCreationTest, mcap_data_num_msgs_downsampling) { - std::string file_name = "output_mcap_data_num_msgs_downsampling.mcap"; + const std::string file_name = "output_mcap_data_num_msgs_downsampling.mcap"; record(file_name, test::n_msgs, test::downsampling); @@ -413,7 +445,7 @@ TEST(McapFileCreationTest, mcap_data_num_msgs_downsampling) TEST(McapFileCreationTest, transition_running) { - std::string file_name = "output_transition_running.mcap"; + const std::string file_name = "output_transition_running.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -432,7 +464,7 @@ TEST(McapFileCreationTest, transition_running) TEST(McapFileCreationTest, transition_paused) { - std::string file_name = "output_transition_paused.mcap"; + const std::string file_name = "output_transition_paused.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -451,7 +483,7 @@ TEST(McapFileCreationTest, transition_paused) TEST(McapFileCreationTest, transition_stopped) { - std::string file_name = "output_transition_stopped.mcap"; + const std::string file_name = "output_transition_stopped.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -470,7 +502,7 @@ TEST(McapFileCreationTest, transition_stopped) TEST(McapFileCreationTest, transition_suspended) { - std::string file_name = "output_transition_suspended.mcap"; + const std::string file_name = "output_transition_suspended.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -489,7 +521,7 @@ TEST(McapFileCreationTest, transition_suspended) TEST(McapFileCreationTest, transition_running_paused) { - std::string file_name = "output_transition_running_paused.mcap"; + const std::string file_name = "output_transition_running_paused.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -508,7 +540,7 @@ TEST(McapFileCreationTest, transition_running_paused) TEST(McapFileCreationTest, transition_running_stopped) { - std::string file_name = "output_transition_running_stopped.mcap"; + const std::string file_name = "output_transition_running_stopped.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -527,7 +559,7 @@ TEST(McapFileCreationTest, transition_running_stopped) TEST(McapFileCreationTest, transition_running_suspended) { - std::string file_name = "output_transition_running_suspended.mcap"; + const std::string file_name = "output_transition_running_suspended.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -546,7 +578,7 @@ TEST(McapFileCreationTest, transition_running_suspended) TEST(McapFileCreationTest, transition_paused_running) { - std::string file_name = "output_transition_paused_running.mcap"; + const std::string file_name = "output_transition_paused_running.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -565,7 +597,7 @@ TEST(McapFileCreationTest, transition_paused_running) TEST(McapFileCreationTest, transition_paused_stopped) { - std::string file_name = "output_transition_paused_stopped.mcap"; + const std::string file_name = "output_transition_paused_stopped.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -584,7 +616,7 @@ TEST(McapFileCreationTest, transition_paused_stopped) TEST(McapFileCreationTest, transition_paused_suspended) { - std::string file_name = "output_transition_paused_suspended.mcap"; + const std::string file_name = "output_transition_paused_suspended.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -603,7 +635,7 @@ TEST(McapFileCreationTest, transition_paused_suspended) TEST(McapFileCreationTest, transition_stopped_running) { - std::string file_name = "output_transition_stopped_running.mcap"; + const std::string file_name = "output_transition_stopped_running.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -622,7 +654,7 @@ TEST(McapFileCreationTest, transition_stopped_running) TEST(McapFileCreationTest, transition_stopped_paused) { - std::string file_name = "output_transition_stopped_paused.mcap"; + const std::string file_name = "output_transition_stopped_paused.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -641,7 +673,7 @@ TEST(McapFileCreationTest, transition_stopped_paused) TEST(McapFileCreationTest, transition_stopped_suspended) { - std::string file_name = "output_transition_stopped_suspended.mcap"; + const std::string file_name = "output_transition_stopped_suspended.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -660,7 +692,7 @@ TEST(McapFileCreationTest, transition_stopped_suspended) TEST(McapFileCreationTest, transition_suspended_running) { - std::string file_name = "output_transition_suspended_running.mcap"; + const std::string file_name = "output_transition_suspended_running.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -679,7 +711,7 @@ TEST(McapFileCreationTest, transition_suspended_running) TEST(McapFileCreationTest, transition_suspended_paused) { - std::string file_name = "output_transition_suspended_paused.mcap"; + const std::string file_name = "output_transition_suspended_paused.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -698,7 +730,7 @@ TEST(McapFileCreationTest, transition_suspended_paused) TEST(McapFileCreationTest, transition_suspended_stopped) { - std::string file_name = "output_transition_suspended_stopped.mcap"; + const std::string file_name = "output_transition_suspended_stopped.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -718,7 +750,7 @@ TEST(McapFileCreationTest, transition_suspended_stopped) // can fail due to two race conditions but is very unlikely TEST(McapFileCreationTest, transition_paused_event_less_window) { - std::string file_name = "output_transition_paused_event_less_window.mcap"; + const std::string file_name = "output_transition_paused_event_less_window.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -741,7 +773,7 @@ TEST(McapFileCreationTest, transition_paused_event_less_window) TEST(McapFileCreationTest, transition_paused_event_max_window) { - std::string file_name = "output_transition_paused_event_max_window.mcap"; + const std::string file_name = "output_transition_paused_event_max_window.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -764,7 +796,7 @@ TEST(McapFileCreationTest, transition_paused_event_max_window) TEST(McapFileCreationTest, transition_paused_event_start) { - std::string file_name = "output_transition_paused_event_start.mcap"; + const std::string file_name = "output_transition_paused_event_start.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -787,7 +819,7 @@ TEST(McapFileCreationTest, transition_paused_event_start) TEST(McapFileCreationTest, transition_paused_event_stop) { - std::string file_name = "output_transition_paused_event_stop.mcap"; + const std::string file_name = "output_transition_paused_event_stop.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; @@ -810,7 +842,7 @@ TEST(McapFileCreationTest, transition_paused_event_stop) TEST(McapFileCreationTest, transition_paused_event_suspend) { - std::string file_name = "output_transition_paused_event_suspend.mcap"; + const std::string file_name = "output_transition_paused_event_suspend.mcap"; unsigned int n_data_1 = rand() % 10 + 1; unsigned int n_data_2 = rand() % 10 + 1; diff --git a/ddsrecorder/test/labels/XTSAN.list b/ddsrecorder/test/labels/XTSAN.list index 433a99edb..657faccbb 100644 --- a/ddsrecorder/test/labels/XTSAN.list +++ b/ddsrecorder/test/labels/XTSAN.list @@ -1,5 +1,6 @@ McapFileCreationTest.mcap_data_msgs -McapFileCreationTest.mcap_data_topic +McapFileCreationTest.mcap_dds_topic +McapFileCreationTest.mcap_ros2_topic McapFileCreationTest.mcap_data_num_msgs McapFileCreationTest.mcap_data_num_msgs_downsampling McapFileCreationTest.transition_running diff --git a/ddsrecorder_participants/include/ddsrecorder_participants/constants.hpp b/ddsrecorder_participants/include/ddsrecorder_participants/constants.hpp index f9117ba61..4afef20ab 100644 --- a/ddsrecorder_participants/include/ddsrecorder_participants/constants.hpp +++ b/ddsrecorder_participants/include/ddsrecorder_participants/constants.hpp @@ -32,6 +32,9 @@ constexpr const char* QOS_SERIALIZATION_KEYED("keyed"); // Dynamic types serialization constexpr const char* DYNAMIC_TYPES_ATTACHMENT_NAME("dynamic_types"); +// ROS 2 Types metadata +constexpr const char* ROS2_TYPES("ros2-types"); + // Version metadata constexpr const char* VERSION_METADATA_NAME("version"); constexpr const char* VERSION_METADATA_RELEASE("release"); diff --git a/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandler.hpp b/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandler.hpp index b7728d86b..c1bc87f83 100644 --- a/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandler.hpp +++ b/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandler.hpp @@ -133,7 +133,7 @@ class McapHandler : public ddspipe::participants::ISchemaHandler ~McapHandler(); /** - * @brief Create and store in \c schemas_ an OMG IDL schema (.idl format). + * @brief Create and store in \c schemas_ an OMG IDL (.idl format) or ROS 2 (.msg format) schema. * Any samples following this schema that were received before the schema itself are moved to the memory buffer * to be written with the next batch. * Previously created channels (for this type) associated with a blank schema are updated to use the new one. diff --git a/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandlerConfiguration.hpp b/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandlerConfiguration.hpp index 62a039639..bff591935 100644 --- a/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandlerConfiguration.hpp +++ b/ddsrecorder_participants/include/ddsrecorder_participants/recorder/mcap/McapHandlerConfiguration.hpp @@ -57,7 +57,8 @@ struct McapHandlerConfiguration const bool& log_publishTime, const bool& only_with_schema, const mcap::McapWriterOptions& mcap_writer_options, - const bool& record_types) + const bool& record_types, + const bool& ros2_types) : mcap_output_settings(mcap_output_settings) , max_pending_samples(max_pending_samples) , buffer_size(buffer_size) @@ -67,6 +68,7 @@ struct McapHandlerConfiguration , only_with_schema(only_with_schema) , mcap_writer_options(mcap_writer_options) , record_types(record_types) + , ros2_types(ros2_types) { } @@ -96,6 +98,9 @@ struct McapHandlerConfiguration //! Whether to store received dynamic types in output MCAP file bool record_types; + + //! Whether to generate schemas as OMG IDL or ROS2 msg + bool ros2_types; }; } /* namespace participants */ diff --git a/ddsrecorder_participants/src/cpp/recorder/mcap/McapHandler.cpp b/ddsrecorder_participants/src/cpp/recorder/mcap/McapHandler.cpp index 4175aa751..286f1281e 100644 --- a/ddsrecorder_participants/src/cpp/recorder/mcap/McapHandler.cpp +++ b/ddsrecorder_participants/src/cpp/recorder/mcap/McapHandler.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -111,6 +112,7 @@ void McapHandler::add_schema( // NOTE: Process schemas even if in STOPPED state to avoid losing them (only sent/received once in discovery) assert(nullptr != dynamic_type); + std::string type_name = dynamic_type->get_name(); // Check if it exists already @@ -120,12 +122,17 @@ void McapHandler::add_schema( } // Schema not found, generate from dynamic type and store - std::string schema_text = idl::generate_idl_schema(dynamic_type); + std::string schema_text = + configuration_.ros2_types ? msg::generate_ros2_schema(dynamic_type) : idl::generate_idl_schema( + dynamic_type); logInfo(DDSRECORDER_MCAP_HANDLER, "\nAdding schema with name " << type_name << " :\n" << schema_text << "\n"); // Create schema and add it to writer and to schemas map - mcap::Schema new_schema(type_name, "omgidl", schema_text); + std::string encoding = configuration_.ros2_types ? "ros2msg" : "omgidl"; + mcap::Schema new_schema(configuration_.ros2_types ? utils::demangle_if_ros_type(dynamic_type->get_name()) : + dynamic_type + ->get_name(), encoding, schema_text); // WARNING: passing as non-const to MCAP library mcap_writer_.addSchema(new_schema); @@ -837,7 +844,8 @@ mcap::ChannelId McapHandler::create_channel_id_nts_( logInfo(DDSRECORDER_MCAP_HANDLER, "Schema not found for type: " << topic.type_name << ". Creating blank schema..."); - mcap::Schema blank_schema(topic.type_name, "omgidl", ""); + std::string encoding = configuration_.ros2_types ? "ros2msg" : "omgidl"; + mcap::Schema blank_schema(topic.type_name, encoding, ""); mcap_writer_.addSchema(blank_schema); schemas_.insert({topic.type_name, std::move(blank_schema)}); @@ -853,7 +861,11 @@ mcap::ChannelId McapHandler::create_channel_id_nts_( // Create new channel mcap::KeyValueMap metadata = {}; metadata[QOS_SERIALIZATION_QOS] = serialize_qos_(topic.topic_qos); - mcap::Channel new_channel(topic.m_topic_name, "cdr", schema_id, metadata); + std::string topic_name = + configuration_.ros2_types ? utils::demangle_if_ros_topic(topic.m_topic_name) : topic.m_topic_name; + // Set ROS2_TYPES to "false" if the given topic_name is equal to topic.m_topic_name, otherwise set it to "true". + metadata[ROS2_TYPES] = topic_name.compare(topic.m_topic_name) ? "true" : "false"; + mcap::Channel new_channel(topic_name, "cdr", schema_id, metadata); mcap_writer_.addChannel(new_channel); auto channel_id = new_channel.id; channels_.insert({topic, std::move(new_channel)}); diff --git a/ddsrecorder_participants/src/cpp/replayer/McapReaderParticipant.cpp b/ddsrecorder_participants/src/cpp/replayer/McapReaderParticipant.cpp index 217afd64f..02643e03b 100644 --- a/ddsrecorder_participants/src/cpp/replayer/McapReaderParticipant.cpp +++ b/ddsrecorder_participants/src/cpp/replayer/McapReaderParticipant.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -30,6 +31,7 @@ #include #include +#include #include namespace eprosima { @@ -199,8 +201,10 @@ void McapReaderParticipant::process_mcap() // Create topic on which this message should be published DdsTopic channel_topic; - channel_topic.m_topic_name = it->channel->topic; - channel_topic.type_name = it->schema->name; + channel_topic.m_topic_name = it->channel->metadata[ROS2_TYPES] == "true" ? utils::mangle_if_ros_topic( + it->channel->topic) : it->channel->topic; + channel_topic.type_name = it->channel->metadata[ROS2_TYPES] == "true" ? utils::mangle_if_ros_type( + it->schema->name) : it->schema->name; auto readers_it = readers_.find(channel_topic); if (readers_it == readers_.end()) diff --git a/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/YamlReaderConfiguration.hpp b/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/YamlReaderConfiguration.hpp index 03414e691..4a4c3f277 100644 --- a/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/YamlReaderConfiguration.hpp +++ b/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/YamlReaderConfiguration.hpp @@ -74,6 +74,7 @@ class DDSRECORDER_YAML_DllAPI RecorderConfiguration bool only_with_type = false; mcap::McapWriterOptions mcap_writer_options{"ros2"}; bool record_types = true; + bool ros2_types = false; // Remote controller configuration bool enable_remote_controller = true; diff --git a/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/yaml_configuration_tags.hpp b/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/yaml_configuration_tags.hpp index 7c813bee0..14ac21017 100644 --- a/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/yaml_configuration_tags.hpp +++ b/ddsrecorder_yaml/include/ddsrecorder_yaml/recorder/yaml_configuration_tags.hpp @@ -48,6 +48,7 @@ constexpr const char* RECORDER_EVENT_WINDOW_TAG("event-window"); constexpr const char* RECORDER_LOG_PUBLISH_TIME_TAG("log-publish-time"); constexpr const char* RECORDER_ONLY_WITH_TYPE_TAG("only-with-type"); constexpr const char* RECORDER_RECORD_TYPES_TAG("record-types"); +constexpr const char* RECORDER_ROS2_TYPES_TAG("ros2-types"); // Compression settings constexpr const char* RECORDER_COMPRESSION_SETTINGS_TAG("compression"); diff --git a/ddsrecorder_yaml/src/cpp/recorder/YamlReaderConfiguration.cpp b/ddsrecorder_yaml/src/cpp/recorder/YamlReaderConfiguration.cpp index 7972dc07c..aa47f292d 100644 --- a/ddsrecorder_yaml/src/cpp/recorder/YamlReaderConfiguration.cpp +++ b/ddsrecorder_yaml/src/cpp/recorder/YamlReaderConfiguration.cpp @@ -232,6 +232,13 @@ void RecorderConfiguration::load_recorder_configuration_( { record_types = YamlReader::get(yml, RECORDER_RECORD_TYPES_TAG, version); } + + ///// + // Get optional ros2_types + if (YamlReader::is_tag_present(yml, RECORDER_ROS2_TYPES_TAG)) + { + ros2_types = YamlReader::get(yml, RECORDER_ROS2_TYPES_TAG, version); + } } void RecorderConfiguration::load_controller_configuration_( diff --git a/ddsreplayer/src/cpp/tool/DdsReplayer.cpp b/ddsreplayer/src/cpp/tool/DdsReplayer.cpp index 765dd4244..7f917dd9b 100644 --- a/ddsreplayer/src/cpp/tool/DdsReplayer.cpp +++ b/ddsreplayer/src/cpp/tool/DdsReplayer.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -279,8 +280,10 @@ std::set> DdsReplayer::generate_builtin_topic for (auto it = channels.begin(); it != channels.end(); it++) { - std::string topic_name = it->second->topic; - std::string type_name = schemas[it->second->schemaId]->name; // TODO: assert exists beforehand + std::string topic_name = it->second->metadata[ROS2_TYPES] == "true" ? utils::mangle_if_ros_topic( + it->second->topic) : it->second->topic; + std::string type_name = it->second->metadata[ROS2_TYPES] == "true" ? utils::mangle_if_ros_type( + schemas[it->second->schemaId]->name) : schemas[it->second->schemaId]->name; // TODO: assert exists beforehand auto channel_topic = utils::Heritable::make_heritable(); channel_topic->m_topic_name = topic_name; diff --git a/ddsreplayer/test/blackbox/mcap/CMakeLists.txt b/ddsreplayer/test/blackbox/mcap/CMakeLists.txt index 2434c03ea..e879f8e60 100644 --- a/ddsreplayer/test/blackbox/mcap/CMakeLists.txt +++ b/ddsreplayer/test/blackbox/mcap/CMakeLists.txt @@ -27,7 +27,7 @@ set(TEST_SOURCES set(TEST_LIST trivial - data_to_check + dds_data_to_check less_playback_rate more_playback_rate begin_time @@ -69,9 +69,12 @@ set(TEST_SOURCES set(TEST_LIST trivial - data_to_check - less_playback_rate - more_playback_rate + dds_data_to_check + ros2_data_to_check + dds_less_playback_rate + ros2_less_playback_rate + dds_more_playback_rate + ros2_more_playback_rate begin_time end_time start_replay_time_earlier @@ -79,6 +82,7 @@ set(TEST_LIST set(TEST_NEEDED_SOURCES resources/helloworld_withtype_file.mcap + resources/ros2_file.mcap resources/config_file.yaml resources/config_file_less_hz.yaml resources/config_file_more_hz.yaml diff --git a/ddsreplayer/test/blackbox/mcap/McapFileReadTest.cpp b/ddsreplayer/test/blackbox/mcap/McapFileReadTest.cpp index 406606d78..981e1c0a8 100644 --- a/ddsreplayer/test/blackbox/mcap/McapFileReadTest.cpp +++ b/ddsreplayer/test/blackbox/mcap/McapFileReadTest.cpp @@ -36,14 +36,14 @@ namespace test { const unsigned int DOMAIN = 110; -std::string topic_name = "/dds/topic"; +const std::string topic_name = "/dds/topic"; } // test void create_subscriber_replayer( DataToCheck& data, - std::string configuration_path = "resources/config_file.yaml", + const std::string& configuration_path = "resources/config_file.yaml", std::string input_file = "resources/helloworld_file.mcap") { { @@ -105,7 +105,7 @@ TEST(McapFileReadTest, trivial) ASSERT_TRUE(true); } -TEST(McapFileReadTest, data_to_check) +TEST(McapFileReadTest, dds_data_to_check) { // info to check DataToCheck data; @@ -124,7 +124,7 @@ TEST(McapFileReadTest, more_playback_rate) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_more_hz.yaml"; + const std::string configuration = "resources/config_file_more_hz.yaml"; create_subscriber_replayer(data, configuration); // ms ~ 100 ASSERT_GT(data.mean_ms_between_msgs, 97.5); @@ -135,7 +135,7 @@ TEST(McapFileReadTest, less_playback_rate) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_less_hz.yaml"; + const std::string configuration = "resources/config_file_less_hz.yaml"; create_subscriber_replayer(data, configuration); // ms ~ 400 ASSERT_GT(data.mean_ms_between_msgs, 397.5); @@ -146,7 +146,7 @@ TEST(McapFileReadTest, begin_time) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_begin_time.yaml"; + const std::string configuration = "resources/config_file_begin_time.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 3); ASSERT_EQ(data.min_index_msg, 8); @@ -157,7 +157,7 @@ TEST(McapFileReadTest, end_time) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_end_time.yaml"; + const std::string configuration = "resources/config_file_end_time.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 8); ASSERT_EQ(data.min_index_msg, 0); @@ -168,7 +168,7 @@ TEST(McapFileReadTest, start_replay_time_earlier) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_start_replay_time_earlier.yaml"; + const std::string configuration = "resources/config_file_start_replay_time_earlier.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 11); ASSERT_EQ(data.min_index_msg, 0); diff --git a/ddsreplayer/test/blackbox/mcap/McapFileReadWithTypeTest.cpp b/ddsreplayer/test/blackbox/mcap/McapFileReadWithTypeTest.cpp index 6c34dd753..1b4de3942 100644 --- a/ddsreplayer/test/blackbox/mcap/McapFileReadWithTypeTest.cpp +++ b/ddsreplayer/test/blackbox/mcap/McapFileReadWithTypeTest.cpp @@ -34,7 +34,12 @@ namespace test { const unsigned int DOMAIN = 110; -std::string topic_name = "/dds/topic"; +const std::string dds_topic_name = "/dds/topic"; +const std::string ros2_topic_name = "rt/topic"; + +const std::string dds_type_name = "HelloWorld"; +const std::string ros2_type_name = "std_msgs::msg::dds_::String_"; + } // test @@ -49,13 +54,16 @@ std::string topic_name = "/dds/topic"; */ void create_subscriber_replayer( DataToCheck& data, - std::string configuration_path = "resources/config_file.yaml", - std::string input_file = "resources/helloworld_withtype_file.mcap") + const std::string& configuration_path = "resources/config_file.yaml", + std::string input_file = "resources/helloworld_withtype_file.mcap", + bool ros2 = false) { { + std::string topic_name = ros2 ? test::ros2_topic_name : test::dds_topic_name; + // Create Subscriber HelloWorldDynTypesSubscriber subscriber( - test::topic_name, + topic_name, static_cast(test::DOMAIN), data); @@ -109,13 +117,13 @@ TEST(McapFileReadWithTypeTest, trivial) ASSERT_TRUE(true); } -TEST(McapFileReadWithTypeTest, data_to_check) +TEST(McapFileReadWithTypeTest, dds_data_to_check) { // info to check DataToCheck data; create_subscriber_replayer(data); ASSERT_EQ(data.n_received_msgs, 13); - ASSERT_EQ(data.type_msg, "HelloWorld"); + ASSERT_EQ(data.type_msg, test::dds_type_name); ASSERT_EQ(data.message_msg, "Hello World"); ASSERT_EQ(data.min_index_msg, 0); ASSERT_EQ(data.max_index_msg, 12); @@ -124,33 +132,72 @@ TEST(McapFileReadWithTypeTest, data_to_check) ASSERT_LT(data.mean_ms_between_msgs, 202.5); } -TEST(McapFileReadWithTypeTest, more_playback_rate) +TEST(McapFileReadWithTypeTest, ros2_data_to_check) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_more_hz.yaml"; + const std::string ros2_mcap = "resources/ros2_file.mcap"; + create_subscriber_replayer(data, "resources/config_file.yaml", ros2_mcap, true); + ASSERT_EQ(data.n_received_msgs, 17); + ASSERT_EQ(data.type_msg, test::ros2_type_name); + const std::regex base_regex("Hello, world! \\d*"); + ASSERT_TRUE(std::regex_match(data.message_msg, base_regex)); + // ms ~ 500 + ASSERT_GT(data.mean_ms_between_msgs, 497.5); + ASSERT_LT(data.mean_ms_between_msgs, 502.5); +} + +TEST(McapFileReadWithTypeTest, dds_more_playback_rate) +{ + // info to check + DataToCheck data; + const std::string configuration = "resources/config_file_more_hz.yaml"; create_subscriber_replayer(data, configuration); // ms ~ 100 ASSERT_GT(data.mean_ms_between_msgs, 97.5); ASSERT_LT(data.mean_ms_between_msgs, 102.5); } -TEST(McapFileReadWithTypeTest, less_playback_rate) +TEST(McapFileReadWithTypeTest, ros2_more_playback_rate) +{ + // info to check + DataToCheck data; + const std::string configuration = "resources/config_file_more_hz.yaml"; + const std::string ros2_mcap = "resources/ros2_file.mcap"; + create_subscriber_replayer(data, configuration, ros2_mcap, true); + // ms ~ 250 + ASSERT_GT(data.mean_ms_between_msgs, 247.5); + ASSERT_LT(data.mean_ms_between_msgs, 252.5); +} + +TEST(McapFileReadWithTypeTest, dds_less_playback_rate) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_less_hz.yaml"; + const std::string configuration = "resources/config_file_less_hz.yaml"; create_subscriber_replayer(data, configuration); // ms ~ 400 ASSERT_GT(data.mean_ms_between_msgs, 397.5); ASSERT_LT(data.mean_ms_between_msgs, 402.5); } +TEST(McapFileReadWithTypeTest, ros2_less_playback_rate) +{ + // info to check + DataToCheck data; + const std::string configuration = "resources/config_file_less_hz.yaml"; + const std::string ros2_mcap = "resources/ros2_file.mcap"; + create_subscriber_replayer(data, configuration, ros2_mcap, true); + // ms ~ 1000 + ASSERT_GT(data.mean_ms_between_msgs, 997.5); + ASSERT_LT(data.mean_ms_between_msgs, 1002.5); +} + TEST(McapFileReadWithTypeTest, begin_time) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_begin_time_with_types.yaml"; + const std::string configuration = "resources/config_file_begin_time_with_types.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 6); ASSERT_EQ(data.min_index_msg, 7); @@ -161,7 +208,7 @@ TEST(McapFileReadWithTypeTest, end_time) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_end_time_with_types.yaml"; + const std::string configuration = "resources/config_file_end_time_with_types.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 7); ASSERT_EQ(data.min_index_msg, 0); @@ -172,7 +219,7 @@ TEST(McapFileReadWithTypeTest, start_replay_time_earlier) { // info to check DataToCheck data; - std::string configuration = "resources/config_file_start_replay_time_earlier_with_types.yaml"; + const std::string configuration = "resources/config_file_start_replay_time_earlier_with_types.yaml"; create_subscriber_replayer(data, configuration); ASSERT_EQ(data.n_received_msgs, 13); ASSERT_EQ(data.min_index_msg, 0); diff --git a/ddsreplayer/test/blackbox/mcap/dds/HelloWorldDynTypesSubscriber.cpp b/ddsreplayer/test/blackbox/mcap/dds/HelloWorldDynTypesSubscriber.cpp index d7e446748..aa40faeab 100644 --- a/ddsreplayer/test/blackbox/mcap/dds/HelloWorldDynTypesSubscriber.cpp +++ b/ddsreplayer/test/blackbox/mcap/dds/HelloWorldDynTypesSubscriber.cpp @@ -142,10 +142,19 @@ void HelloWorldDynTypesSubscriber::on_data_available( samples_++; - int32_t index = new_dynamic_data->get_uint32_value(0); - std::string message = new_dynamic_data->get_string_value(1); + if (new_dynamic_data->get_name() == "std_msgs::msg::dds_::String_") + { + std::string message = new_dynamic_data->get_string_value(0); - fill_info(static_cast(index), message, current_time); + fill_info(static_cast(0), message, current_time); + } + else if (new_dynamic_data->get_name() == "HelloWorld") + { + int32_t index = new_dynamic_data->get_uint32_value(0); + std::string message = new_dynamic_data->get_string_value(1); + + fill_info(static_cast(index), message, current_time); + } std::cout << "Message " << samples_ << " received:\n" << std::endl; eprosima::fastrtps::types::DynamicDataHelper::print(new_dynamic_data); diff --git a/ddsreplayer/test/blackbox/mcap/resources/ros2_file.mcap b/ddsreplayer/test/blackbox/mcap/resources/ros2_file.mcap new file mode 100644 index 000000000..4a44b37e8 Binary files /dev/null and b/ddsreplayer/test/blackbox/mcap/resources/ros2_file.mcap differ diff --git a/ddsreplayer/test/labels/XTSAN.list b/ddsreplayer/test/labels/XTSAN.list index 45ca339d8..ef2da0207 100644 --- a/ddsreplayer/test/labels/XTSAN.list +++ b/ddsreplayer/test/labels/XTSAN.list @@ -1,14 +1,17 @@ McapFileReadTest.trivial -McapFileReadTest.data_to_check +McapFileReadTest.dds_data_to_check McapFileReadTest.less_playback_rate McapFileReadTest.more_playback_rate McapFileReadTest.begin_time McapFileReadTest.end_time McapFileReadTest.start_replay_time_earlier McapFileReadWithTypeTest.trivial -McapFileReadWithTypeTest.data_to_check -McapFileReadWithTypeTest.less_playback_rate -McapFileReadWithTypeTest.more_playback_rate +McapFileReadWithTypeTest.dds_data_to_check +McapFileReadWithTypeTest.ros2_data_to_check +McapFileReadWithTypeTest.dds_less_playback_rate +McapFileReadWithTypeTest.ros2_less_playback_rate +McapFileReadWithTypeTest.dds_more_playback_rate +McapFileReadWithTypeTest.ros2_more_playback_rate McapFileReadWithTypeTest.begin_time McapFileReadWithTypeTest.end_time McapFileReadWithTypeTest.start_replay_time_earlier diff --git a/docs/rst/notes/forthcoming_version.rst b/docs/rst/notes/forthcoming_version.rst index 8f10f22d5..d8916290e 100644 --- a/docs/rst/notes/forthcoming_version.rst +++ b/docs/rst/notes/forthcoming_version.rst @@ -14,8 +14,8 @@ Next release will include the following **DDS Recorder & Replay internal adjustm * Store *DDS Record & Replay* version in metadata record of the generated MCAP files. * Move dynamic types storage from metadata to attachments MCAP section. -* Store schemas in OMG IDL format (instead of ROS 2 msg). * Set `app_id` and `app_metadata` attributes on *DDS Record & Replay* participants. +* Store schemas in OMG IDL and ROS 2 msg format. .. warning:: @@ -28,9 +28,11 @@ Next release will include the following **DDS Recorder tool configuration featur * New configuration options (``timestamp-format`` and ``local-timestamp``) available for :ref:`output file ` settings. * New configuration option (``topics``) to configure the :ref:`Manual Topics `. * Rename ``max-reception-rate`` to ``max-rx-rate``. +* Record data in either ROS 2 format or the raw DDS format (see :ref:`Topic Type Format `). Next release will include the following **DDS Replayer tool configuration features**: * New configuration option (``topics``) to configure the :ref:`Manual Topics `. * New configuration option (``max-tx-rate``) to configure the :ref:`Max transmission rate `. * Remove the support for `Built-in Topics `_. +* Read data in either ROS 2 format or the raw DDS format. diff --git a/docs/rst/recording/usage/configuration.rst b/docs/rst/recording/usage/configuration.rst index b417234b9..4b4d19645 100644 --- a/docs/rst/recording/usage/configuration.rst +++ b/docs/rst/recording/usage/configuration.rst @@ -431,6 +431,16 @@ By default, all type information received during execution is stored in a dedica This information is then leveraged by |ddsreplayer| on playback, publishing recorded types in addition to data samples, which may be required for receiver applications relying on :term:`Dynamic Types` (see :ref:`Replay Types `). However, a user may choose to disable this feature by setting ``record-types: false``. +.. _recorder_usage_configuration_topictypeformat: + +Topic type format +^^^^^^^^^^^^^^^^^ + +The optional ``ros2-types`` tag enables specification of the format for storing schemas. +When set to ``true``, schemas are stored in ROS 2 message format (.msg). +If set to ``false``, schemas are stored in OMG IDL format (.idl). +By default it is set to ``false``. + .. _recorder_usage_configuration_remote_controller: Remote Controller @@ -606,6 +616,7 @@ A complete example of all the configurations described on this page can be found level: slowest force: true record-types: true + ros2-types: false remote-controller: enable: true diff --git a/docs/rst/spelling_wordlist.txt b/docs/rst/spelling_wordlist.txt index c41eec06e..a79544a62 100644 --- a/docs/rst/spelling_wordlist.txt +++ b/docs/rst/spelling_wordlist.txt @@ -33,6 +33,7 @@ gMock Gtest guid IDL +idl IPv kubernetes localhost diff --git a/resources/configurations/recorder/complete_config.yaml b/resources/configurations/recorder/complete_config.yaml index d42b47841..c2511d7ac 100644 --- a/resources/configurations/recorder/complete_config.yaml +++ b/resources/configurations/recorder/complete_config.yaml @@ -42,6 +42,7 @@ recorder: level: slowest force: true record-types: true + ros2-types: false remote-controller: enable: true