From cdf8404769018f66a71e66c1e031f9fcdaa542a2 Mon Sep 17 00:00:00 2001 From: Alexander Sherikov Date: Fri, 10 Jan 2025 19:35:16 +0400 Subject: [PATCH] Refactoring. --- .utils/qa/scspell.dict | 3 + README.md | 16 +- frontend/include/intrometry/backend/utils.h | 1 + frontend/src/backend.cpp | 18 +- .../intrometry/pjmsg_mcap/pjmsg_mcap.h | 11 ++ .../include/intrometry/pjmsg_mcap/sink.h | 10 +- pjmsg_mcap/src/intrometry.cpp | 176 +++++++++--------- pjmsg_mcap/src/messages.h | 102 ++++++++++ pjmsg_mcap/src/schema_names.h | 48 ----- pjmsg_mcap/src/schema_values.h | 48 ----- pjmsg_mcap/test/common.h | 3 +- .../intrometry/pjmsg_topic/pjmsg_topic.h | 11 ++ .../include/intrometry/pjmsg_topic/sink.h | 2 +- 13 files changed, 261 insertions(+), 188 deletions(-) create mode 100644 pjmsg_mcap/include/intrometry/pjmsg_mcap/pjmsg_mcap.h create mode 100644 pjmsg_mcap/src/messages.h delete mode 100644 pjmsg_mcap/src/schema_names.h delete mode 100644 pjmsg_mcap/src/schema_values.h create mode 100644 pjmsg_topic/include/intrometry/pjmsg_topic/pjmsg_topic.h diff --git a/.utils/qa/scspell.dict b/.utils/qa/scspell.dict index fc797d5..649844c 100644 --- a/.utils/qa/scspell.dict +++ b/.utils/qa/scspell.dict @@ -14,12 +14,14 @@ cppcheck defgroup eprosima fastcdr +gmtime google gtest https ingroup intrometry intrometryfixture +iomanip killall mcap msgs @@ -33,6 +35,7 @@ rclcpp rdparty rosout sherikov +sstream stringstream strstream yaml diff --git a/README.md b/README.md index 4511512..7866461 100644 --- a/README.md +++ b/README.md @@ -148,12 +148,20 @@ does NOT depend on any ROS components. The resulting files can also be viewed by `PlotJuggler`. -Using backends with cmake -------------------------- +Using library +------------- + +### cmake + +``` +find_package(intrometry_ REQUIRED) +target_link_libraries(my_library intrometry::) +``` + +### C++ ``` -find_package(intrometry_pjmsg_mcap REQUIRED) -target_link_libraries(my_library intrometry::pjmsg_mcap) +#include /.h> ``` diff --git a/frontend/include/intrometry/backend/utils.h b/frontend/include/intrometry/backend/utils.h index 42657ff..29a542a 100644 --- a/frontend/include/intrometry/backend/utils.h +++ b/frontend/include/intrometry/backend/utils.h @@ -26,6 +26,7 @@ namespace intrometry::backend std::string getRandomId(const std::size_t length); std::string normalizeId(const std::string &input_id); + std::string getDateString(); class RateTimer diff --git a/frontend/src/backend.cpp b/frontend/src/backend.cpp index e7de59d..1e579ed 100644 --- a/frontend/src/backend.cpp +++ b/frontend/src/backend.cpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include #include @@ -20,7 +22,7 @@ namespace namespace intrometry_private::backend { const std::string valid_chars = "0123456789abcdefghijklmnopqrstuvwxyz"; - } // namespace intrometry_private::backend + } // namespace intrometry_private::backend } // namespace @@ -94,6 +96,20 @@ namespace intrometry::backend } + std::string getDateString() + { + const std::time_t date_now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::stringstream date_stream; + // thread-unsafe + date_stream << std::put_time(std::gmtime(&date_now), "%Y%m%d_%H%M%S"); // NOLINT + + return (date_stream.str()); + } +} // namespace intrometry::backend + + +namespace intrometry::backend +{ class RateTimer::Implementation { public: diff --git a/pjmsg_mcap/include/intrometry/pjmsg_mcap/pjmsg_mcap.h b/pjmsg_mcap/include/intrometry/pjmsg_mcap/pjmsg_mcap.h new file mode 100644 index 0000000..e7fbf1c --- /dev/null +++ b/pjmsg_mcap/include/intrometry/pjmsg_mcap/pjmsg_mcap.h @@ -0,0 +1,11 @@ +/** + @file + @author Alexander Sherikov + @copyright 2025 Alexander Sherikov. Licensed under the Apache License, + Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0) +*/ + +#pragma once + +#include +#include "sink.h" diff --git a/pjmsg_mcap/include/intrometry/pjmsg_mcap/sink.h b/pjmsg_mcap/include/intrometry/pjmsg_mcap/sink.h index a973ed6..9f78469 100644 --- a/pjmsg_mcap/include/intrometry/pjmsg_mcap/sink.h +++ b/pjmsg_mcap/include/intrometry/pjmsg_mcap/sink.h @@ -9,7 +9,9 @@ #pragma once -#include "intrometry/sink.h" +#include + +#include namespace intrometry::pjmsg_mcap @@ -27,12 +29,18 @@ namespace intrometry::pjmsg_mcap /// id of the sink, disables publishing if empty std::string id_; + + /// output directory + std::filesystem::path directory_; + + public: Parameters(const std::string &id = ""); // NOLINT Parameters(const char *id = ""); // NOLINT Parameters &rate(const std::size_t value); Parameters &id(const std::string &value); + Parameters &directory(const std::filesystem::path &value); }; class Implementation; diff --git a/pjmsg_mcap/src/intrometry.cpp b/pjmsg_mcap/src/intrometry.cpp index 4de8689..0b3bf7a 100644 --- a/pjmsg_mcap/src/intrometry.cpp +++ b/pjmsg_mcap/src/intrometry.cpp @@ -25,8 +25,7 @@ #include "intrometry/backend/utils.h" #include "intrometry/pjmsg_mcap/sink.h" -#include "schema_names.h" -#include "schema_values.h" +#include "messages.h" namespace @@ -95,111 +94,109 @@ namespace { class McapCDRWriter { - public: - mcap::Message mcap_message_names_; - mcap::Message mcap_message_values_; - - std::vector buffer_; - eprosima::fastcdr::CdrSizeCalculator cdr_size_calculator_; - mcap::McapWriter writer_; - - public: - McapCDRWriter() : cdr_size_calculator_(eprosima::fastcdr::CdrVersion::XCDRv1) + protected: + template + class Channel { - } + protected: + mcap::Message message_; + eprosima::fastcdr::CdrSizeCalculator cdr_size_calculator_; - ~McapCDRWriter() - { - writer_.close(); - } + protected: + uint32_t getSize(const t_Message &message) + { + size_t current_alignment{ 0 }; + return (cdr_size_calculator_.calculate_serialized_size(message, current_alignment) + + 4u /*encapsulation*/); + } - void initialize(const std::string &filename, const std::string &topic_prefix) // NOLINT - { + public: + Channel() : cdr_size_calculator_(eprosima::fastcdr::CdrVersion::XCDRv1) { - const mcap::McapWriterOptions options = mcap::McapWriterOptions("ros2msg"); - const mcap::Status res = writer_.open(filename, options); - if (not res.ok()) - { - throw std::runtime_error(intrometry::backend::str_concat( - "Failed to open ", filename, " for writing: ", res.message)); - } } + void initialize(mcap::McapWriter &writer, const std::string_view &msg_topic) { mcap::Schema schema( - "plotjuggler_msgs/msg/StatisticsNames", + intrometry_private::pjmsg_mcap::Message::type, "ros2msg", - intrometry_private::pjmsg_mcap::schema::names); - writer_.addSchema(schema); + intrometry_private::pjmsg_mcap::Message::schema); + writer.addSchema(schema); - mcap::Channel channel(intrometry::backend::str_concat(topic_prefix, "/names"), "ros2msg", schema.id); - writer_.addChannel(channel); + mcap::Channel channel(msg_topic, "ros2msg", schema.id); + writer.addChannel(channel); - mcap_message_names_.channelId = channel.id; + message_.channelId = channel.id; } + void write(mcap::McapWriter &writer, std::vector &buffer, const t_Message &message) { - mcap::Schema schema( - "plotjuggler_msgs/msg/StatisticsValues", - "ros2msg", - intrometry_private::pjmsg_mcap::schema::values); - writer_.addSchema(schema); - - mcap::Channel channel(intrometry::backend::str_concat(topic_prefix, "/values"), "ros2msg", schema.id); - writer_.addChannel(channel); + buffer.resize(getSize(message)); + message_.data = buffer.data(); - mcap_message_values_.channelId = channel.id; - } - } + { + eprosima::fastcdr::FastBuffer cdr_buffer( + reinterpret_cast(buffer.data()), buffer.size()); // NOLINT + eprosima::fastcdr::Cdr ser( + cdr_buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::CdrVersion::XCDRv1); + ser.set_encoding_flag(eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); - template - uint32_t getSize(const t_Message &message) - { - size_t current_alignment{ 0 }; - return (cdr_size_calculator_.calculate_serialized_size(message, current_alignment) + 4u /*encapsulation*/); - } + ser.serialize_encapsulation(); + ser << message; + ser.set_dds_cdr_options({ 0, 0 }); - template - void write(mcap::Message &mcap_message, const t_Message &message) - { - buffer_.resize(getSize(message)); - mcap_message.data = buffer_.data(); + message_.dataSize = ser.get_serialized_data_length(); + } - { - eprosima::fastcdr::FastBuffer cdr_buffer( - reinterpret_cast(buffer_.data()), buffer_.size()); // NOLINT - eprosima::fastcdr::Cdr ser( - cdr_buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::CdrVersion::XCDRv1); - ser.set_encoding_flag(eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + message_.logTime = intrometry::backend::now(); + message_.publishTime = message_.logTime; - ser.serialize_encapsulation(); - ser << message; - ser.set_dds_cdr_options({ 0, 0 }); - mcap_message.dataSize = ser.get_serialized_data_length(); + const mcap::Status res = writer.write(message_); + if (not res.ok()) + { + throw std::runtime_error( + intrometry::backend::str_concat("Failed to write a message: ", res.message)); + } } + }; - mcap_message.logTime = intrometry::backend::now(); - mcap_message.publishTime = mcap_message.logTime; + public: + std::tuple, Channel> + channels_; + std::vector buffer_; + mcap::McapWriter writer_; - const mcap::Status res = writer_.write(mcap_message); - if (not res.ok()) - { - throw std::runtime_error(intrometry::backend::str_concat("Failed to write a message: ", res.message)); - } + public: + ~McapCDRWriter() + { + writer_.close(); } - template - void writeValues(const t_Message &message) + void initialize(const std::filesystem::path &filename, const std::string &topic_prefix) { - write(mcap_message_values_, message); + { + const mcap::McapWriterOptions options = mcap::McapWriterOptions("ros2msg"); + const mcap::Status res = writer_.open(filename.native(), options); + if (not res.ok()) + { + throw std::runtime_error(intrometry::backend::str_concat( + "Failed to open ", filename.native(), " for writing: ", res.message)); + } + } + + std::get>(channels_).initialize( + writer_, intrometry::backend::str_concat(topic_prefix, "/names")); + + std::get>(channels_).initialize( + writer_, intrometry::backend::str_concat(topic_prefix, "/values")); } template - void writeNames(const t_Message &message) + void write(const t_Message &message) { - write(mcap_message_names_, message); + std::get>(channels_).write(writer_, buffer_, message); } }; } // namespace @@ -241,11 +238,11 @@ namespace { if (data_->new_names_version_) { - mcap_writer.writeNames(data_->names_); + mcap_writer.write(data_->names_); data_->new_names_version_ = false; } - mcap_writer.writeValues(data_->values_); + mcap_writer.write(data_->values_); serialized_ = true; } mutex_.unlock(); @@ -292,6 +289,12 @@ namespace intrometry::pjmsg_mcap::sink id_ = value; return (*this); } + + Parameters &Parameters::directory(const std::filesystem::path &value) + { + directory_ = value; + return (*this); + } } // namespace intrometry::pjmsg_mcap::sink namespace intrometry::pjmsg_mcap::sink @@ -314,15 +317,22 @@ namespace intrometry::pjmsg_mcap::sink McapCDRWriter mcap_writer_; public: - Implementation(const std::string &sink_id, const std::size_t rate) + Implementation(const std::filesystem::path &directory, const std::string &sink_id, const std::size_t rate) { const std::string node_id = intrometry::backend::normalizeId(sink_id); const std::string random_id = intrometry::backend::getRandomId(8); - const std::string id = - node_id.empty() ? random_id : intrometry::backend::str_concat(node_id, "_", random_id); const std::string topic_prefix = intrometry::backend::str_concat("/intrometry/", node_id.empty() ? random_id : node_id); - const std::string filename = intrometry::backend::str_concat(id, ".mcap"); + + std::filesystem::create_directories(directory); + const std::filesystem::path filename = directory + / intrometry::backend::str_concat( + node_id, + node_id.empty() ? "" : "_", + random_id, + "_", + intrometry::backend::getDateString(), + ".mcap"); mcap_writer_.initialize(filename, topic_prefix); @@ -431,7 +441,7 @@ namespace intrometry::pjmsg_mcap { return (false); } - make_pimpl(parameters_.id_, parameters_.rate_); + make_pimpl(parameters_.directory_, parameters_.id_, parameters_.rate_); return (true); } diff --git a/pjmsg_mcap/src/messages.h b/pjmsg_mcap/src/messages.h new file mode 100644 index 0000000..053e24a --- /dev/null +++ b/pjmsg_mcap/src/messages.h @@ -0,0 +1,102 @@ +/** + @file + @author Alexander Sherikov + @copyright 2025 Alexander Sherikov. Licensed under the Apache License, + Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0) + @brief +*/ + +#pragma once + +namespace // NOLINT +{ + namespace intrometry_private::pjmsg_mcap + { + template + class Message + { + }; + + + template <> + class Message + { + public: + inline static const char *const type = "plotjuggler_msgs/msg/StatisticsNames"; // NOLINT + + inline static const char *const schema = // NOLINT + R"SCHEMA( +# header +std_msgs/Header header + +# Statistics names +string[] names +uint32 names_version #This is increased each time names change + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. + +# Two-integer timestamp that is expressed as seconds and nanoseconds. +builtin_interfaces/Time stamp + +# Transform frame with which this data is associated. +string frame_id + +================================================================================ +MSG: builtin_interfaces/Time +# This message communicates ROS Time defined here: +# https://design.ros2.org/articles/clock_and_time.html + +# The seconds component, valid over all int32 values. +int32 sec + +# The nanoseconds component, valid in the range [0, 10e9). +uint32 nanosec +)SCHEMA"; + }; + + + template <> + class Message + { + public: + inline static const char *const type = "plotjuggler_msgs/msg/StatisticsValues"; // NOLINT + + inline static const char *const schema = // NOLINT + R"SCHEMA( +# header +std_msgs/Header header + +# Statistics +float64[] values +uint32 names_version # The values vector corresponds to the name vector with the same name + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. + +# Two-integer timestamp that is expressed as seconds and nanoseconds. +builtin_interfaces/Time stamp + +# Transform frame with which this data is associated. +string frame_id + +================================================================================ +MSG: builtin_interfaces/Time +# This message communicates ROS Time defined here: +# https://design.ros2.org/articles/clock_and_time.html + +# The seconds component, valid over all int32 values. +int32 sec + +# The nanoseconds component, valid in the range [0, 10e9). +uint32 nanosec +)SCHEMA"; + }; + } // namespace intrometry_private::pjmsg_mcap +} // namespace diff --git a/pjmsg_mcap/src/schema_names.h b/pjmsg_mcap/src/schema_names.h deleted file mode 100644 index a461d8d..0000000 --- a/pjmsg_mcap/src/schema_names.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - @file - @author Alexander Sherikov - @copyright 2025 Alexander Sherikov. Licensed under the Apache License, - Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0) - @brief -*/ - -#pragma once - -namespace // NOLINT -{ - namespace intrometry_private::pjmsg_mcap::schema - { - const char *const names = // NOLINT - R"SCHEMA( -# header -std_msgs/Header header - -# Statistics names -string[] names -uint32 names_version #This is increased each time names change - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. - -# Two-integer timestamp that is expressed as seconds and nanoseconds. -builtin_interfaces/Time stamp - -# Transform frame with which this data is associated. -string frame_id - -================================================================================ -MSG: builtin_interfaces/Time -# This message communicates ROS Time defined here: -# https://design.ros2.org/articles/clock_and_time.html - -# The seconds component, valid over all int32 values. -int32 sec - -# The nanoseconds component, valid in the range [0, 10e9). -uint32 nanosec -)SCHEMA"; - } // namespace intrometry_private::pjmsg_mcap::schema -} // namespace diff --git a/pjmsg_mcap/src/schema_values.h b/pjmsg_mcap/src/schema_values.h deleted file mode 100644 index 85dce42..0000000 --- a/pjmsg_mcap/src/schema_values.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - @file - @author Alexander Sherikov - @copyright 2025 Alexander Sherikov. Licensed under the Apache License, - Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0) - @brief -*/ - -#pragma once - -namespace // NOLINT -{ - namespace intrometry_private::pjmsg_mcap::schema - { - const char *const values = // NOLINT - R"SCHEMA( -# header -std_msgs/Header header - -# Statistics -float64[] values -uint32 names_version # The values vector corresponds to the name vector with the same name - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. - -# Two-integer timestamp that is expressed as seconds and nanoseconds. -builtin_interfaces/Time stamp - -# Transform frame with which this data is associated. -string frame_id - -================================================================================ -MSG: builtin_interfaces/Time -# This message communicates ROS Time defined here: -# https://design.ros2.org/articles/clock_and_time.html - -# The seconds component, valid over all int32 values. -int32 sec - -# The nanoseconds component, valid in the range [0, 10e9). -uint32 nanosec -)SCHEMA"; - } // namespace intrometry_private::pjmsg_mcap::schema -} // namespace diff --git a/pjmsg_mcap/test/common.h b/pjmsg_mcap/test/common.h index 4a1fdc7..0af90f2 100644 --- a/pjmsg_mcap/test/common.h +++ b/pjmsg_mcap/test/common.h @@ -7,8 +7,7 @@ */ -#include "intrometry/intrometry.h" -#include "intrometry/pjmsg_mcap/sink.h" +#include "intrometry/pjmsg_mcap/pjmsg_mcap.h" #include #include diff --git a/pjmsg_topic/include/intrometry/pjmsg_topic/pjmsg_topic.h b/pjmsg_topic/include/intrometry/pjmsg_topic/pjmsg_topic.h new file mode 100644 index 0000000..e7fbf1c --- /dev/null +++ b/pjmsg_topic/include/intrometry/pjmsg_topic/pjmsg_topic.h @@ -0,0 +1,11 @@ +/** + @file + @author Alexander Sherikov + @copyright 2025 Alexander Sherikov. Licensed under the Apache License, + Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0) +*/ + +#pragma once + +#include +#include "sink.h" diff --git a/pjmsg_topic/include/intrometry/pjmsg_topic/sink.h b/pjmsg_topic/include/intrometry/pjmsg_topic/sink.h index e05e823..73adb84 100644 --- a/pjmsg_topic/include/intrometry/pjmsg_topic/sink.h +++ b/pjmsg_topic/include/intrometry/pjmsg_topic/sink.h @@ -9,7 +9,7 @@ #pragma once -#include "intrometry/sink.h" +#include namespace intrometry::pjmsg_topic