From 3b23acce01b3dda84b8949281039508d7fa7bf39 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Tue, 11 Sep 2018 18:07:27 +0200 Subject: [PATCH] Record all topics (#30) * GH-23 Get all topics from node and sanitize * GH-23 Move methods to node for better interface * GH-23 Use rmw_serialized_message_t consistently * GH-23 Improve santization of topics * GH-65 Introduce and use better logging macros * GH-23 Use publisher to serialized message directly * GH-23 Improve readability of sanitizing topics and types * GH-23 Allow to write all available topics * GH-23 Add test for record all * GH-23 Cleanup: add missing const ref to record interface * Cleanup for doxygen * Improve topic sanitization - correctly expand topic names using rcl - do not check type correctness (supposed to be done internally) * Pass topic_name by reference --- rosbag2/CMakeLists.txt | 21 +-- rosbag2/include/rosbag2/logging.hpp | 61 ++++++++ rosbag2/include/rosbag2/rosbag2.hpp | 22 ++- rosbag2/src/rosbag2/demo_record.cpp | 8 +- rosbag2/src/rosbag2/generic_subscription.cpp | 7 +- rosbag2/src/rosbag2/generic_subscription.hpp | 8 +- rosbag2/src/rosbag2/rosbag2.cpp | 47 ++---- rosbag2/src/rosbag2/rosbag2_node.cpp | 116 ++++++++++++++- rosbag2/src/rosbag2/rosbag2_node.hpp | 14 +- .../test/rosbag2/rosbag2_integration_test.cpp | 107 -------------- .../test/rosbag2/rosbag2_rosbag_node_test.cpp | 78 ++++++++++ .../rosbag2_write_all_integration_test.cpp | 56 ++++++++ .../rosbag2_write_integration_fixture.hpp | 136 ++++++++++++++++++ .../rosbag2_write_integration_test.cpp | 98 +------------ .../include/rosbag2_storage/logging.hpp | 61 ++++++++ .../src/impl/storage_factory_impl.hpp | 31 ++-- .../src/rosbag2_storage/ros_helper.cpp | 7 +- .../logging.hpp | 61 ++++++++ .../sqlite/sqlite_storage.cpp | 7 +- 19 files changed, 651 insertions(+), 295 deletions(-) create mode 100644 rosbag2/include/rosbag2/logging.hpp delete mode 100644 rosbag2/test/rosbag2/rosbag2_integration_test.cpp create mode 100644 rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp create mode 100644 rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp create mode 100644 rosbag2_storage/include/rosbag2_storage/logging.hpp create mode 100644 rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/logging.hpp diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index b8fcd949ea..32f863a314 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -86,6 +86,14 @@ if(BUILD_TESTING) target_link_libraries(rosbag2_write_integration_test librosbag2) endif() + ament_add_gmock(rosbag2_write_all_integration_test + test/rosbag2/rosbag2_write_all_integration_test.cpp + test/rosbag2/test_memory_management.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET rosbag2_write_all_integration_test) + target_link_libraries(rosbag2_write_all_integration_test librosbag2) + endif() + ament_add_gmock(rosbag2_read_integration_test test/rosbag2/rosbag2_read_integration_test.cpp test/rosbag2/test_memory_management.cpp @@ -110,14 +118,6 @@ if(BUILD_TESTING) ) endif() - ament_add_gmock(rosbag2_integration_test - test/rosbag2/rosbag2_integration_test.cpp - src/rosbag2/typesupport_helpers.cpp - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) - if(TARGET rosbag2_integration_test) - target_link_libraries(rosbag2_integration_test librosbag2) - endif() - ament_add_gmock(rosbag2_rosbag_node_test test/rosbag2/rosbag2_rosbag_node_test.cpp test/rosbag2/test_memory_management.cpp @@ -127,6 +127,11 @@ if(BUILD_TESTING) src/rosbag2/typesupport_helpers.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) if(TARGET rosbag2_rosbag_node_test) + target_include_directories(rosbag2_rosbag_node_test + PUBLIC + $ + $ + ) ament_target_dependencies(rosbag2_rosbag_node_test ament_index_cpp Poco diff --git a/rosbag2/include/rosbag2/logging.hpp b/rosbag2/include/rosbag2/logging.hpp new file mode 100644 index 0000000000..f90ce225db --- /dev/null +++ b/rosbag2/include/rosbag2/logging.hpp @@ -0,0 +1,61 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2__LOGGING_HPP_ +#define ROSBAG2__LOGGING_HPP_ + +#include +#include + +#include "rcutils/logging_macros.h" + +#define ROSBAG2_PACKAGE_NAME "rosbag2" + +#define ROSBAG2_LOG_INFO(...) \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_LOG_INFO_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_LOG_ERROR(...) \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_LOG_ERROR_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_LOG_WARN(...) \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_LOG_WARN_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_LOG_DEBUG(...) \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_LOG_DEBUG_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#endif // ROSBAG2__LOGGING_HPP_ diff --git a/rosbag2/include/rosbag2/rosbag2.hpp b/rosbag2/include/rosbag2/rosbag2.hpp index 14ced520ad..22fbb2253a 100644 --- a/rosbag2/include/rosbag2/rosbag2.hpp +++ b/rosbag2/include/rosbag2/rosbag2.hpp @@ -37,19 +37,31 @@ class Rosbag2Node; class Rosbag2 { public: + /** + * Records topics to a bagfile. Subscription happens at startup time, hence the topics must + * exist when "record" is called. + * + * \param file_name Name of the bagfile to write + * \param topic_names Vector of topics to subscribe to. Topics must exist at startup time. If + * the vector is empty, all topics will be subscribed. + * \param after_write_action This function will be executed after each write to the database + * where the input parameter is the topic name of the topic written Currently needed for testing. + * Might be removed later. + */ ROSBAG2_PUBLIC void record( const std::string & file_name, - std::vector topic_names, + const std::vector & topic_names, std::function after_write_action = nullptr); + /** + * Replay all topics in a bagfile. + * + * \param file_name Name of the bagfile to replay + */ ROSBAG2_PUBLIC void play(const std::string & file_name); - ROSBAG2_PUBLIC - std::map get_topics_with_types( - const std::vector & topic_names, std::shared_ptr node); - private: void prepare_publishers( std::shared_ptr node, diff --git a/rosbag2/src/rosbag2/demo_record.cpp b/rosbag2/src/rosbag2/demo_record.cpp index 314f9c6e3e..ae37f5ebf9 100644 --- a/rosbag2/src/rosbag2/demo_record.cpp +++ b/rosbag2/src/rosbag2/demo_record.cpp @@ -23,13 +23,15 @@ int main(int argc, const char ** argv) { if (argc < 2) { - std::cerr << "\nThe names of topics to record must be given as parameter!\n"; + std::cerr << "\nThe names of topics or `--all` must be given to record as parameter!\n"; return 0; } std::vector topics; - for (int i = 1; i < argc; i++) { - topics.emplace_back(argv[i]); + if (strcmp(argv[1], "--all") != 0) { + for (int i = 1; i < argc; i++) { + topics.emplace_back(argv[i]); + } } // TODO(anhosi): allow output file to be specified by cli argument and do proper checking if diff --git a/rosbag2/src/rosbag2/generic_subscription.cpp b/rosbag2/src/rosbag2/generic_subscription.cpp index 7e0e8b26f0..43e27d1bdd 100644 --- a/rosbag2/src/rosbag2/generic_subscription.cpp +++ b/rosbag2/src/rosbag2/generic_subscription.cpp @@ -20,6 +20,8 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/subscription.hpp" +#include "rosbag2/logging.hpp" + namespace rosbag2 { @@ -98,9 +100,8 @@ GenericSubscription::borrow_serialized_message(size_t capacity) auto fini_return = rmw_serialized_message_fini(msg); delete msg; if (fini_return != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rosbag2", - "failed to destroy serialized message: %s", rcl_get_error_string_safe()); + ROSBAG2_LOG_ERROR_STREAM( + "Failed to destroy serialized message: " << rcl_get_error_string_safe()); } }); diff --git a/rosbag2/src/rosbag2/generic_subscription.hpp b/rosbag2/src/rosbag2/generic_subscription.hpp index f43f3f060b..84e1351e33 100644 --- a/rosbag2/src/rosbag2/generic_subscription.hpp +++ b/rosbag2/src/rosbag2/generic_subscription.hpp @@ -39,10 +39,10 @@ class GenericSubscription : public rclcpp::SubscriptionBase * Constructor. In order to properly subscribe to a topic, this subscription needs to be added to * the node_topic_interface of the node passed into this constructor. * - * @param node_handle Node handle to the node to create the subscription to - * @param ts Type support handle - * @param topic_name Topic name - * @param callback Callback for new messages of serialized form + * \param node_handle Node handle to the node to create the subscription to + * \param ts Type support handle + * \param topic_name Topic name + * \param callback Callback for new messages of serialized form */ GenericSubscription( std::shared_ptr node_handle, diff --git a/rosbag2/src/rosbag2/rosbag2.cpp b/rosbag2/src/rosbag2/rosbag2.cpp index 7359c91391..a601d141ac 100644 --- a/rosbag2/src/rosbag2/rosbag2.cpp +++ b/rosbag2/src/rosbag2/rosbag2.cpp @@ -24,6 +24,7 @@ #include "rclcpp/rclcpp.hpp" #include "rcutils/logging_macros.h" +#include "rosbag2/logging.hpp" #include "rosbag2_storage/serialized_bag_message.hpp" #include "rosbag2_storage/storage_factory.hpp" #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp" @@ -42,7 +43,7 @@ const char * ROS_PACKAGE_NAME = "rosbag2"; void Rosbag2::record( const std::string & file_name, - std::vector topic_names, + const std::vector & topic_names, std::function after_write_action) { rosbag2_storage::StorageFactory factory; @@ -50,12 +51,13 @@ void Rosbag2::record( if (!storage) { throw std::runtime_error("No storage could be initialized. Abort"); - return; } auto node = std::make_shared("rosbag2"); - auto topics_and_types = get_topics_with_types(topic_names, node); + auto topics_and_types = topic_names.empty() ? + node->get_all_topics_with_types() : + node->get_topics_with_types(topic_names); if (topics_and_types.empty()) { throw std::runtime_error("No topics found. Abort"); @@ -77,45 +79,15 @@ void Rosbag2::record( if (subscriptions_.empty()) { throw std::runtime_error("No topics could be subscribed. Abort"); - return; } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Waiting for messages..."); + ROSBAG2_LOG_INFO("Waiting for messages..."); while (rclcpp::ok()) { rclcpp::spin(node); } subscriptions_.clear(); } -std::map Rosbag2::get_topics_with_types( - const std::vector & topic_names, std::shared_ptr node) -{ - // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic - // This should be replaced by an auto-discovery system in the future - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto topics = node->get_topic_names_and_types(); - - std::map topic_names_and_types; - for (const auto & topic_name : topic_names) { - std::string complete_topic_name = topic_name; - if (topic_name[0] != '/') { - complete_topic_name = "/" + topic_name; - } - auto position = topics.find(complete_topic_name); - if (position != topics.end()) { - if (position->second.size() > 1) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "Topic '%s' has several types associated. Only topics with one type are supported.", - position->first.c_str()); - } else { - topic_names_and_types.insert({position->first, position->second[0]}); - } - } - } - return topic_names_and_types; -} - std::shared_ptr Rosbag2::create_subscription( const std::function & after_write_action, @@ -133,9 +105,8 @@ Rosbag2::create_subscription( rcutils_time_point_value_t time_stamp; int error = rcutils_system_time_now(&time_stamp); if (error != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "Error getting current time. Error: %s", rcutils_get_error_string_safe()); + ROSBAG2_LOG_ERROR_STREAM( + "Error getting current time. Error:" << rcutils_get_error_string_safe()); } bag_message->time_stamp = time_stamp; @@ -161,7 +132,7 @@ void Rosbag2::play(const std::string & file_name) // without the sleep_for() many messages are lost. std::this_thread::sleep_for(std::chrono::milliseconds(50)); publishers_[message->topic_name]->publish(message->serialized_data); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "published message"); + ROSBAG2_LOG_INFO("Published message"); } } } diff --git a/rosbag2/src/rosbag2/rosbag2_node.cpp b/rosbag2/src/rosbag2/rosbag2_node.cpp index d87e8b3eb0..a9ab8432a0 100644 --- a/rosbag2/src/rosbag2/rosbag2_node.cpp +++ b/rosbag2/src/rosbag2/rosbag2_node.cpp @@ -14,9 +14,15 @@ #include "rosbag2_node.hpp" +#include +#include #include #include +#include +#include +#include "rcl/expand_topic_name.h" +#include "rosbag2/logging.hpp" #include "typesupport_helpers.hpp" namespace rosbag2 @@ -36,7 +42,7 @@ std::shared_ptr Rosbag2Node::create_generic_publisher( std::shared_ptr Rosbag2Node::create_generic_subscription( const std::string & topic, const std::string & type, - std::function)> callback) + std::function)> callback) { auto type_support = get_typesupport(type); @@ -51,11 +57,115 @@ std::shared_ptr Rosbag2Node::create_generic_subscription( get_node_topics_interface()->add_subscription(subscription, nullptr); } catch (const std::runtime_error & ex) { - RCUTILS_LOG_ERROR_NAMED( - "rosbag2", "Error subscribing to topic %s. Error: %s", topic.c_str(), ex.what()); + ROSBAG2_LOG_ERROR_STREAM("Error subscribing to topic '" << topic << "'. Error: " << ex.what()); } return subscription; } +std::shared_ptr get_initialized_string_map() +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + auto substitutions_map = new rcutils_string_map_t; + *substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t map_init = rcutils_string_map_init(substitutions_map, 0, allocator); + if (map_init != RCUTILS_RET_OK) { + ROSBAG2_LOG_ERROR("Failed to initialize string map within rcutils."); + return std::shared_ptr(); + } + return std::shared_ptr(substitutions_map, + [](rcutils_string_map_t * map) { + rcl_ret_t cleanup = rcutils_string_map_fini(map); + delete map; + if (cleanup != RCL_RET_OK) { + ROSBAG2_LOG_ERROR("Failed to deallocate string map when expanding topic."); + } + }); +} + +std::string Rosbag2Node::expand_topic_name(const std::string & topic_name) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto substitutions_map = get_initialized_string_map(); + if (!substitutions_map) { + ROSBAG2_LOG_ERROR("Failed to initialize string map within rcutils."); + return ""; + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(substitutions_map.get()); + if (ret != RCL_RET_OK) { + ROSBAG2_LOG_ERROR("Failed to initialize map with default values."); + return ""; + } + char * expanded_topic_name = nullptr; + ret = rcl_expand_topic_name( + topic_name.c_str(), + get_name(), + get_namespace(), + substitutions_map.get(), + allocator, + &expanded_topic_name); + + if (ret != RCL_RET_OK) { + ROSBAG2_LOG_ERROR_STREAM( + "Failed to expand topic name " << topic_name << " with error: " << + rcutils_get_error_string_safe()); + return ""; + } + std::string expanded_topic_name_std(expanded_topic_name); + allocator.deallocate(expanded_topic_name, allocator.state); + return expanded_topic_name_std; +} + +std::map Rosbag2Node::get_topics_with_types( + const std::vector & topic_names) +{ + std::vector sanitized_topic_names; + for (const auto & topic_name : topic_names) { + auto sanitized_topic_name = expand_topic_name(topic_name); + if (!sanitized_topic_name.empty()) { + sanitized_topic_names.push_back(sanitized_topic_name); + } + } + + // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic + // This should be replaced by an auto-discovery system in the future + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + auto topics_and_types = this->get_topic_names_and_types(); + + std::map> filtered_topics_and_types; + for (const auto & topic_and_type : topics_and_types) { + if (std::find(sanitized_topic_names.begin(), sanitized_topic_names.end(), + topic_and_type.first) != sanitized_topic_names.end()) + { + filtered_topics_and_types.insert(topic_and_type); + } + } + + return filter_topics_with_more_than_one_type(filtered_topics_and_types); +} + +std::map +Rosbag2Node::get_all_topics_with_types() +{ + // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic + // This should be replaced by an auto-discovery system in the future + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + return filter_topics_with_more_than_one_type(this->get_topic_names_and_types()); +} + +std::map Rosbag2Node::filter_topics_with_more_than_one_type( + std::map> topics_and_types) +{ + std::map filtered_topics_and_types; + for (const auto & topic_and_type : topics_and_types) { + if (topic_and_type.second.size() > 1) { + ROSBAG2_LOG_ERROR_STREAM("Topic '" << topic_and_type.first << + "' has several types associated. Only topics with one type are supported"); + } else { + filtered_topics_and_types.insert({topic_and_type.first, topic_and_type.second[0]}); + } + } + return filtered_topics_and_types; +} + } // namespace rosbag2 diff --git a/rosbag2/src/rosbag2/rosbag2_node.hpp b/rosbag2/src/rosbag2/rosbag2_node.hpp index f25c71f13b..efbb5d4c7a 100644 --- a/rosbag2/src/rosbag2/rosbag2_node.hpp +++ b/rosbag2/src/rosbag2/rosbag2_node.hpp @@ -15,8 +15,10 @@ #ifndef ROSBAG2__ROSBAG2_NODE_HPP_ #define ROSBAG2__ROSBAG2_NODE_HPP_ +#include #include #include +#include #include "rclcpp/node.hpp" #include "rcutils/types.h" @@ -39,7 +41,17 @@ class Rosbag2Node : public rclcpp::Node std::shared_ptr create_generic_subscription( const std::string & topic, const std::string & type, - std::function)> callback); + std::function)> callback); + + std::map + get_topics_with_types(const std::vector & topic_names); + + std::string expand_topic_name(const std::string & topic_name); + + std::map get_all_topics_with_types(); + + std::map filter_topics_with_more_than_one_type( + std::map> topics_and_types); }; } // namespace rosbag2 diff --git a/rosbag2/test/rosbag2/rosbag2_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_integration_test.cpp deleted file mode 100644 index d5e993a572..0000000000 --- a/rosbag2/test/rosbag2/rosbag2_integration_test.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rosbag2/rosbag2.hpp" -#include "std_msgs/msg/string.hpp" - -#include "rosbag2_test_fixture.hpp" - -using namespace ::testing; // NOLINT - -class RosBag2IntegrationFixture : public Test -{ -public: - RosBag2IntegrationFixture() - { - node_ = std::make_shared("rosbag2"); - publisher_node_ = std::make_shared("publisher_node"); - rosbag2_ = rosbag2::Rosbag2(); - } - - void create_publisher(std::string topic) - { - auto publisher = publisher_node_->create_publisher(topic); - publishers_.push_back(publisher); - } - - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - rosbag2::Rosbag2 rosbag2_; - std::shared_ptr node_; - std::shared_ptr publisher_node_; - std::vector> publishers_; -}; - -TEST_F(RosBag2IntegrationFixture, - get_topic_returns_with_topic_string_if_topic_is_specified_without_slash) -{ - create_publisher("string_topic"); - - auto topics_and_types = rosbag2_.get_topics_with_types({"string_topic"}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); -} - -TEST_F(RosBag2IntegrationFixture, - get_topic_returns_with_topic_string_if_topic_is_specified_with_slash) -{ - create_publisher("string_topic"); - - auto topics_and_types = rosbag2_.get_topics_with_types({"/string_topic"}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); -} - -TEST_F(RosBag2IntegrationFixture, returns_multiple_topics_for_multiple_inputs) -{ - std::string first_topic("/string_topic"); - std::string second_topic("/other_topic"); - std::string third_topic("/wrong_topic"); - - create_publisher(first_topic); - create_publisher(second_topic); - create_publisher(third_topic); - - auto topics_and_types = rosbag2_.get_topics_with_types({first_topic, second_topic}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(2)); - EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("std_msgs/String")); - EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("std_msgs/String")); -} - -TEST_F(RosBag2IntegrationFixture, get_topic_returns_empty_if_topic_does_not_exist) { - create_publisher("string_topic"); - - auto topics_and_types = rosbag2_.get_topics_with_types({"/wrong_topic"}, node_); - - ASSERT_THAT(topics_and_types, IsEmpty()); -} diff --git a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp index 4adc67f191..bb439d6380 100644 --- a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp +++ b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -34,6 +35,7 @@ class RosBag2NodeFixture : public Test RosBag2NodeFixture() { node_ = std::make_shared("rosbag2"); + publisher_node_ = std::make_shared("publisher_node"); } static void SetUpTestCase() @@ -46,6 +48,12 @@ class RosBag2NodeFixture : public Test rclcpp::shutdown(); } + void create_publisher(std::string topic) + { + auto publisher = publisher_node_->create_publisher(topic); + publishers_.push_back(publisher); + } + std::vector subscribe_raw_messages( size_t expected_messages_number, const std::string & topic_name, const std::string & type) { @@ -74,6 +82,8 @@ class RosBag2NodeFixture : public Test test_helpers::TestMemoryManagement memory_management_; std::shared_ptr node_; + rclcpp::Node::SharedPtr publisher_node_; + std::vector> publishers_; }; @@ -100,3 +110,71 @@ TEST_F(RosBag2NodeFixture, publisher_and_subscriber_work) EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); } + +TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_empty_if_topic_does_not_exist) { + create_publisher("string_topic"); + + auto topics_and_types = node_->get_topics_with_types({"/wrong_topic"}); + + ASSERT_THAT(topics_and_types, IsEmpty()); +} + +TEST_F(RosBag2NodeFixture, + get_topics_with_types_returns_with_topic_string_if_topic_is_specified_without_slash) +{ + create_publisher("string_topic"); + + 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("std_msgs/String")); +} + +TEST_F(RosBag2NodeFixture, + get_topics_with_types_returns_with_topic_string_if_topic_is_specified_with_slash) +{ + create_publisher("string_topic"); + + 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("std_msgs/String")); +} + +TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_only_specified_topics) { + std::string first_topic("/string_topic"); + std::string second_topic("/other_topic"); + std::string third_topic("/wrong_topic"); + + create_publisher(first_topic); + create_publisher(second_topic); + create_publisher(third_topic); + + 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("std_msgs/String")); + EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("std_msgs/String")); +} + +TEST_F(RosBag2NodeFixture, get_all_topics_with_types_returns_all_topics) +{ + std::string first_topic("/string_topic"); + std::string second_topic("/other_topic"); + std::string third_topic("/wrong_topic"); + + create_publisher(first_topic); + create_publisher(second_topic); + create_publisher(third_topic); + + auto topics_and_types = node_->get_all_topics_with_types(); + + ASSERT_THAT(topics_and_types, SizeIs(5)); + EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("std_msgs/String")); + EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("std_msgs/String")); + EXPECT_THAT(topics_and_types.find(third_topic)->second, StrEq("std_msgs/String")); + // The latter two topics can usually be found on any node, so they should be subscribed here + EXPECT_THAT(topics_and_types.find("/clock")->second, StrEq("rosgraph_msgs/Clock")); + EXPECT_THAT( + topics_and_types.find("/parameter_events")->second, StrEq("rcl_interfaces/ParameterEvent")); +} diff --git a/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp new file mode 100644 index 0000000000..3c0d19f786 --- /dev/null +++ b/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp @@ -0,0 +1,56 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rosbag2/rosbag2.hpp" +#include "rosbag2_storage/serialized_bag_message.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/u_int8.hpp" + +#include "rosbag2_write_integration_fixture.hpp" +#include "test_memory_management.hpp" + +// TODO(Martin-Idel-SI): merge with other write and read tests once signal handling is sorted out +TEST_F(RosBag2IntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) +{ + std::string int_topic = "/int_topic"; + auto serialized_int_bag_message = serialize_message(int_topic, 10); + + std::string string_topic = "/string_topic"; + auto serialized_string_bag_message = serialize_message( + string_topic, "test_message"); + + start_publishing(serialized_string_bag_message, string_topic, 2); + start_publishing(serialized_int_bag_message, int_topic, 2); + + start_recording_all_topics(); + wait_for_publishers(); + stop_recording(); + + auto recorded_messages = get_messages(database_name_); + + ASSERT_THAT(recorded_messages, SizeIs(4)); + auto string_messages = filter_messages(recorded_messages, string_topic); + auto int_messages = filter_messages(recorded_messages, int_topic); + ASSERT_THAT(string_messages, SizeIs(2)); + ASSERT_THAT(int_messages, SizeIs(2)); + EXPECT_THAT(string_messages[0]->data, Eq("test_message")); + EXPECT_THAT(int_messages[0]->data, Eq(10)); +} diff --git a/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp b/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp new file mode 100644 index 0000000000..d662c906c4 --- /dev/null +++ b/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp @@ -0,0 +1,136 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rosbag2/rosbag2.hpp" +#include "rosbag2_storage/serialized_bag_message.hpp" + +#include "rosbag2_test_fixture.hpp" +#include "test_memory_management.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2; // NOLINT +using namespace std::chrono_literals; // NOLINT + +#ifndef ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ +#define ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ + +class RosBag2IntegrationTestFixture : public Rosbag2TestFixture +{ +public: + RosBag2IntegrationTestFixture() + : Rosbag2TestFixture() + {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + publisher_node_ = rclcpp::Node::make_shared("publisher_node"); + } + + void record_message(const std::string & db_name, std::vector topics) + { + rosbag2::Rosbag2 rosbag2; + rosbag2.record(db_name, topics, [this](std::string topic_name) { + messages_stored_counter_[topic_name]++; + }); + } + + void start_recording(std::vector topics) + { + // the future object returned from std::async needs to be stored not to block the execution + future_ = std::async( + std::launch::async, [this, topics]() { + record_message(database_name_, topics); + }); + } + + void start_recording_all_topics() + { + // the future object returned from std::async needs to be stored not to block the execution + future_ = std::async( + std::launch::async, [this]() { + record_message(database_name_, {}); + }); + } + + void stop_recording() + { + rclcpp::shutdown(); + future_.get(); + } + + void wait_for_publishers() + { + for (auto & future : publisher_futures_) { + future.get(); + } + } + + void start_publishing( + std::shared_ptr message, + std::string topic_name, size_t number_expected_messages) + { + messages_stored_counter_.insert({topic_name, 0}); + publisher_futures_.push_back(std::async( + std::launch::async, [this, message, topic_name, number_expected_messages]() { + auto publisher = publisher_node_->create_publisher(topic_name); + + while (rclcpp::ok() && messages_stored_counter_[topic_name] < number_expected_messages) { + publisher->publish(message->serialized_data); + // We need to wait a bit, in order for record to process the messages + std::this_thread::sleep_for(50ms); + } + } + )); + } + + template + std::shared_ptr deserialize_message( + std::shared_ptr message) + { + return memory_.deserialize_message(message->serialized_data); + } + + template + std::vector> filter_messages( + std::vector> messages, + std::string topic) + { + std::vector> filtered_messages; + for (const auto & message : messages) { + if (message->topic_name == topic) { + filtered_messages.push_back(deserialize_message(message)); + } + } + return filtered_messages; + } + + rclcpp::Node::SharedPtr publisher_node_; + test_helpers::TestMemoryManagement memory_; + std::map messages_stored_counter_; + std::vector> publisher_futures_; + std::future future_; +}; + +#endif // ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ diff --git a/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp index 3280d8cc74..d0806c692e 100644 --- a/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp +++ b/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp @@ -14,12 +14,9 @@ #include -#include -#include #include #include #include -#include #include "rclcpp/rclcpp.hpp" #include "rosbag2/rosbag2.hpp" @@ -27,103 +24,10 @@ #include "std_msgs/msg/string.hpp" #include "std_msgs/msg/u_int8.hpp" -#include "rosbag2_test_fixture.hpp" +#include "rosbag2_write_integration_fixture.hpp" #include "test_memory_management.hpp" -using namespace ::testing; // NOLINT -using namespace rosbag2; // NOLINT -using namespace std::chrono_literals; // NOLINT - // TODO(Martin-Idel-SI): merge with other write and read tests once signal handling is sorted out -class RosBag2IntegrationTestFixture : public Rosbag2TestFixture -{ -public: - RosBag2IntegrationTestFixture() - : Rosbag2TestFixture() - {} - - void SetUp() override - { - rclcpp::init(0, nullptr); - publisher_node_ = rclcpp::Node::make_shared("publisher_node"); - } - - void record_message(const std::string & db_name, std::vector topics) - { - rosbag2::Rosbag2 rosbag2; - rosbag2.record(db_name, topics, [this](std::string topic_name) { - messages_stored_counter_[topic_name]++; - }); - } - - void start_recording(std::vector topics) - { - // the future object returned from std::async needs to be stored not to block the execution - future_ = std::async( - std::launch::async, [this, topics]() { - record_message(database_name_, topics); - }); - } - - void stop_recording() - { - rclcpp::shutdown(); - future_.get(); - } - - void wait_for_publishers() - { - for (auto & future : publisher_futures_) { - future.get(); - } - } - - void start_publishing( - std::shared_ptr message, - std::string topic_name, size_t number_expected_messages) - { - messages_stored_counter_.insert({topic_name, 0}); - publisher_futures_.push_back(std::async( - std::launch::async, [this, message, topic_name, number_expected_messages]() { - auto publisher = publisher_node_->create_publisher(topic_name); - - while (rclcpp::ok() && messages_stored_counter_[topic_name] < number_expected_messages) { - publisher->publish(message->serialized_data.get()); - // We need to wait a bit, in order for record to process the messages - std::this_thread::sleep_for(50ms); - } - } - )); - } - - template - std::shared_ptr deserialize_message( - std::shared_ptr message) - { - return memory_.deserialize_message(message->serialized_data); - } - - template - std::vector> filter_messages( - std::vector> messages, - std::string topic) - { - std::vector> filtered_messages; - for (const auto & message : messages) { - if (message->topic_name == topic) { - filtered_messages.push_back(deserialize_message(message)); - } - } - return filtered_messages; - } - - rclcpp::Node::SharedPtr publisher_node_; - test_helpers::TestMemoryManagement memory_; - std::map messages_stored_counter_; - std::vector> publisher_futures_; - std::future future_; -}; - TEST_F(RosBag2IntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) { std::string int_topic = "/int_topic"; diff --git a/rosbag2_storage/include/rosbag2_storage/logging.hpp b/rosbag2_storage/include/rosbag2_storage/logging.hpp new file mode 100644 index 0000000000..5787026e93 --- /dev/null +++ b/rosbag2_storage/include/rosbag2_storage/logging.hpp @@ -0,0 +1,61 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_STORAGE__LOGGING_HPP_ +#define ROSBAG2_STORAGE__LOGGING_HPP_ + +#include +#include + +#include "rcutils/logging_macros.h" + +#define ROSBAG2_STORAGE_PACKAGE_NAME "rosbag2_storage" + +#define ROSBAG2_STORAGE_LOG_INFO(...) \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_LOG_INFO_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_LOG_ERROR(...) \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_LOG_ERROR_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_LOG_WARN(...) \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_LOG_WARN_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_LOG_DEBUG(...) \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_LOG_DEBUG_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_STORAGE_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#endif // ROSBAG2_STORAGE__LOGGING_HPP_ diff --git a/rosbag2_storage/src/impl/storage_factory_impl.hpp b/rosbag2_storage/src/impl/storage_factory_impl.hpp index fcfc035c03..e6f2d89187 100644 --- a/rosbag2_storage/src/impl/storage_factory_impl.hpp +++ b/rosbag2_storage/src/impl/storage_factory_impl.hpp @@ -22,19 +22,17 @@ #include #include "pluginlib/class_loader.hpp" -#include "rcutils/logging_macros.h" #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp" #include "rosbag2_storage/storage_interfaces/read_write_interface.hpp" #include "rosbag2_storage/storage_factory.hpp" #include "rosbag2_storage/storage_traits.hpp" +#include "rosbag2_storage/logging.hpp" namespace rosbag2_storage { -constexpr const char * ROS_PACKAGE_NAME = "rosbag2_storage"; - using storage_interfaces::ReadOnlyInterface; using storage_interfaces::ReadWriteInterface; @@ -43,7 +41,7 @@ std::shared_ptr> get_class_loader() { const char * lookup_name = StorageTraits::name; - return std::make_shared>(ROS_PACKAGE_NAME, lookup_name); + return std::make_shared>("rosbag2_storage", lookup_name); } template< @@ -59,8 +57,7 @@ get_interface_instance( const auto & registered_classes = class_loader->getDeclaredClasses(); auto class_exists = std::find(registered_classes.begin(), registered_classes.end(), storage_id); if (class_exists == registered_classes.end()) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, - "Requested storage id %s does not exist", storage_id.c_str()); + ROSBAG2_STORAGE_LOG_DEBUG_STREAM("Requested storage id '" << storage_id << "' does not exist"); return nullptr; } @@ -69,8 +66,8 @@ get_interface_instance( auto unmanaged_instance = class_loader->createUnmanagedInstance(storage_id); instance = std::shared_ptr(unmanaged_instance); } catch (const std::runtime_error & ex) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, - "unable to load instance of read write interface: %s", ex.what()); + ROSBAG2_STORAGE_LOG_ERROR_STREAM( + "Unable to load instance of read write interface: " << ex.what()); return nullptr; } @@ -78,8 +75,8 @@ get_interface_instance( instance->open(uri, flag); return instance; } catch (const std::runtime_error & ex) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, - "Could not open '%s' with %s. Error: %s", uri.c_str(), storage_id.c_str(), ex.what()); + ROSBAG2_STORAGE_LOG_ERROR_STREAM( + "Could not open '" << uri << "' with '" << storage_id << "'. Error: " << ex.what()); return nullptr; } } @@ -92,16 +89,14 @@ class StorageFactoryImpl try { read_write_class_loader_ = get_class_loader(); } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "unable to create class load instance: %s", e.what()); + ROSBAG2_STORAGE_LOG_ERROR_STREAM("Unable to create class load instance: " << e.what()); throw e; } try { read_only_class_loader_ = get_class_loader(); } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "unable to create class load instance: %s", e.what()); + ROSBAG2_STORAGE_LOG_ERROR_STREAM("Unable to create class load instance: " << e.what()); throw e; } } @@ -113,8 +108,8 @@ class StorageFactoryImpl { auto instance = get_interface_instance(read_write_class_loader_, storage_id, uri); - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Could not load/open plugin with storage id '%s'", storage_id.c_str()) + ROSBAG2_STORAGE_LOG_ERROR_STREAM( + "Could not load/open plugin with storage id '" << storage_id << "'."); return instance; } @@ -130,8 +125,8 @@ class StorageFactoryImpl read_write_class_loader_, storage_id, uri); } - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Could not load/open plugin with storage id '%s'", storage_id.c_str()) + ROSBAG2_STORAGE_LOG_ERROR_STREAM( + "Could not load/open plugin with storage id '" << storage_id << "'."); return instance; } diff --git a/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp b/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp index 8a4d8d99a9..9648f4896e 100644 --- a/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp +++ b/rosbag2_storage/src/rosbag2_storage/ros_helper.cpp @@ -17,8 +17,8 @@ #include #include -#include "rcutils/logging_macros.h" #include "rcutils/types.h" +#include "rosbag2_storage/logging.hpp" namespace rosbag2_storage { @@ -41,9 +41,8 @@ make_serialized_message(const void * data, size_t size) int error = rcutils_char_array_fini(msg); delete msg; if (error != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rosbag2_storage", - "Leaking memory. Error: %s", rcutils_get_error_string_safe()); + ROSBAG2_STORAGE_LOG_ERROR_STREAM( + "Leaking memory. Error: " << rcutils_get_error_string_safe()); } }); diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/logging.hpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/logging.hpp new file mode 100644 index 0000000000..696cec5019 --- /dev/null +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/logging.hpp @@ -0,0 +1,61 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_STORAGE_DEFAULT_PLUGINS__LOGGING_HPP_ +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS__LOGGING_HPP_ + +#include +#include + +#include "rcutils/logging_macros.h" + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME "rosbag2_storage" + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_INFO(...) \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_INFO_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR(...) \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_WARN(...) \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_WARN_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_DEBUG(...) \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __VA_ARGS__) + +#define ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_DEBUG_STREAM(args) do { \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_STORAGE_DEFAULT_PLUGINS_PACKAGE_NAME, __ss.str().c_str()); \ +} while (0) + +#endif // ROSBAG2_STORAGE_DEFAULT_PLUGINS__LOGGING_HPP_ diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index e017ccf4e7..f3009ec124 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -23,13 +23,12 @@ #include #include -#include "rcutils/logging_macros.h" #include "rosbag2_storage/serialized_bag_message.hpp" +#include "../logging.hpp" #include "sqlite_statement_wrapper.hpp" namespace rosbag2_storage_plugins { -const char * ROS_PACKAGE_NAME = "rosbag2_storage_default_plugins"; SqliteStorage::SqliteStorage() : database_(), @@ -58,7 +57,7 @@ void SqliteStorage::open( initialize(); } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Opened database '%s'.", uri.c_str()); + ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_INFO_STREAM("Opened database '" << uri << "'."); } void SqliteStorage::write(std::shared_ptr message) @@ -75,7 +74,7 @@ void SqliteStorage::write(std::shared_ptrbind(message->time_stamp, topic_entry->second, message->serialized_data); write_statement_->execute_and_reset(); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Stored message."); + ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_INFO("Stored message"); } bool SqliteStorage::has_next()