diff --git a/README.md b/README.md index cd87b57a9c..5e2504590e 100644 --- a/README.md +++ b/README.md @@ -182,6 +182,8 @@ The Rosbag2 recorder provides the following services for remote control, which c * Start topic discovery to automatically find and subscribe to new topics. Has no effect if discovery is already running or if the recorder is not in recording state. * `~/stop_discovery [rosbag2_interfaces/srv/StopDiscovery]` * Stop topic discovery. Existing subscriptions will be maintained, but new topics will not be discovered automatically. +* `~/get_subscribed_topics [rosbag2_interfaces/srv/GetSubscribedTopics]` + * Returns the list of fully qualified names of topics currently subscribed by the recorder. These services enable full remote control of the recording process, allowing you to start and stop recording sessions, manage topic discovery, and control the recording state without restarting the recorder node. diff --git a/rosbag2_interfaces/CMakeLists.txt b/rosbag2_interfaces/CMakeLists.txt index 1cdf914951..30de9a3870 100644 --- a/rosbag2_interfaces/CMakeLists.txt +++ b/rosbag2_interfaces/CMakeLists.txt @@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/MessagesLostEvent.msg" "srv/Burst.srv" "srv/GetRate.srv" + "srv/GetSubscribedTopics.srv" "srv/IsDiscoveryRunning.srv" "srv/IsPaused.srv" "srv/Pause.srv" @@ -31,6 +32,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetRate.srv" "srv/Snapshot.srv" "srv/SplitBagfile.srv" + "srv/SubscribeToTopics.srv" "srv/StartDiscovery.srv" "srv/Stop.srv" "srv/StopDiscovery.srv" diff --git a/rosbag2_interfaces/srv/GetSubscribedTopics.srv b/rosbag2_interfaces/srv/GetSubscribedTopics.srv new file mode 100644 index 0000000000..7fc059cf12 --- /dev/null +++ b/rosbag2_interfaces/srv/GetSubscribedTopics.srv @@ -0,0 +1,3 @@ +--- +# Fully qualified names of topics currently subscribed by the recorder. +string[] topics diff --git a/rosbag2_interfaces/srv/SubscribeToTopics.srv b/rosbag2_interfaces/srv/SubscribeToTopics.srv new file mode 100644 index 0000000000..2ed250dc65 --- /dev/null +++ b/rosbag2_interfaces/srv/SubscribeToTopics.srv @@ -0,0 +1,35 @@ +# Fully qualified topic names to subscribe the recorder to. +string[] topics + +# Optional list of topic types matching the topics list; must be the same length as `topics` when provided. +# Entries can be empty to request type discovery when the ROS graph has a single type. +string[] topic_types + +# TODO(morlov): The topic_types are defined as an auxiliary list of types in addition to another +# list of topics, aka topic_names. They should match by index. However, in practice, this is often a +# source of numerous errors. Need redesign the message definition to move topic_name and topic_type +# into a simple data structure, aka TopicNameAndType, and use it as an array in the +# SubscribeToTopics.srv request instead of the separate topics and topic_types fields? + +--- + +# TODO(morlov): The return code should return 0 on success and some error code in case of an error. +# Please note that if there are no topics discovered and none of the topics got subscribed, this is +# still a successful call. We put all missed topics into the unavailable_topics list in response. +# However, if some topic has an invalid name or we failed to add the topic to the subscription for +# any other reasons, - this is a failure situation, and the client needs to be aware that something +# is wrong with the request via the return_code. +# Also need to put information into the error_string about failure. Similar to what +# we are logging in case of a failed attempt to do something. + +# Return code. Returns 1 when none of the topics from the request got subscribed to or in case of error, otherwise 0. +int32 return_code + +# Error string. Empty if no error occurred. +string error_string + +# Topics that got subscribed after handling the request. This includes topics that were already subscribed to before the call. +string[] subscribed_topics + +# Topics that could not be subscribed to because they were unavailable, invalid or the type was ambiguous. +string[] unavailable_topics diff --git a/rosbag2_transport/include/rosbag2_transport/recorder.hpp b/rosbag2_transport/include/rosbag2_transport/recorder.hpp index a0c7979da2..9c9231e384 100644 --- a/rosbag2_transport/include/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/include/rosbag2_transport/recorder.hpp @@ -282,6 +282,10 @@ class Recorder : public rclcpp::Node ROSBAG2_TRANSPORT_PUBLIC void set_on_start_recording_callback(OnStartRecordingCallback callback) const; + /// \return List of currently subscribed topics. + ROSBAG2_TRANSPORT_PUBLIC + std::vector get_subscribed_topics() const; + inline constexpr static const auto kPauseResumeToggleKey = KeyboardHandler::KeyCode::SPACE; protected: diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 755d178bb0..3ff8539324 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -38,6 +38,8 @@ #include "rosbag2_cpp/bag_events.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_cpp/writer.hpp" +#include "rosbag2_interfaces/srv/get_subscribed_topics.hpp" +#include "rosbag2_interfaces/srv/subscribe_to_topics.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" #include "rosbag2_storage/qos.hpp" #include "logging.hpp" @@ -194,6 +196,8 @@ class RecorderImpl void read_static_topics() noexcept; + std::vector get_subscribed_topics() const; + /// Public members for access by wrapper std::unordered_set topics_warned_about_incompatibility_; std::shared_ptr writer_; @@ -346,6 +350,32 @@ class RecorderImpl void warn_if_new_qos_for_subscribed_topic(const std::string & topic_name); + /// \brief A helper function to collect requested topics with types from a inner subscribe_ to + /// topics request. + /// \param request The subscribe to topics service request. + /// \param response The subscribe to topics service response. + /// \param available_topics The currently available topics in the system. + /// \return A map of requested topic names to their resolved types. + template + std::unordered_map collect_requested_topics_with_types( + const RequestT & request, + ResponseT & response, + const std::map> & available_topics); + + /// \brief A helper function to resolve the topic type for a subscribe to topics request. + /// \param requested_type The requested type for the topic. + /// \param validated_topic The topic name for which the type is being resolved. + /// \param available_types The list of available types for the topic. + /// \param response The subscribe to topics service response. + /// \return An optional string containing the resolved topic type, or std::nullopt if resolution + /// failed. + template + std::optional resolve_topic_type_for_subscribe_request( + const std::string & requested_type, + const std::string & validated_topic, + const std::vector & available_types, + ResponseT & response) const; + /// \brief Helper wrapper function to set a service response as success. template void set_service_success( @@ -409,6 +439,9 @@ class RecorderImpl std::future discovery_future_; std::unordered_map topic_qos_profile_overrides_; std::unordered_set topic_unknown_types_; + rclcpp::Service::SharedPtr + srv_get_subscribed_topics_; + rclcpp::Service::SharedPtr srv_subscribe_to_topics_; rclcpp::Service::SharedPtr srv_is_discovery_running_; rclcpp::Service::SharedPtr srv_is_paused_; rclcpp::Service::SharedPtr srv_pause_; @@ -422,6 +455,7 @@ class RecorderImpl std::mutex start_stop_transition_mutex_; std::mutex discovery_mutex_; + mutable std::mutex subscriptions_mutex_; std::atomic discovery_running_ = false; std::atomic_uchar paused_ = 0; std::atomic in_recording_ = false; @@ -550,7 +584,10 @@ void RecorderImpl::stop() for (auto & [_, subscription] : subscriptions_) { subscription->disable_callbacks(); } - subscriptions_.clear(); + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + subscriptions_.clear(); + } writer_->close(); // Call writer->close() to finalize current bag file and write metadata { // Clear pending split request if any std::lock_guard lock(pending_bag_split_request_mutex_); @@ -936,6 +973,172 @@ void RecorderImpl::create_control_services() { response->paused = is_paused(); }); + + srv_get_subscribed_topics_ = + node->create_service( + "~/get_subscribed_topics", + [this]( + const std::shared_ptr/* request_header */, + const std::shared_ptr/* request */, + std::shared_ptr response) + { + response->topics = this->get_subscribed_topics(); + }); + + srv_subscribe_to_topics_ = + node->create_service( + "~/subscribe_to_topics", + [this]( + const std::shared_ptr/* request_header */, + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->topics.empty()) { + RCLCPP_WARN(node->get_logger(), + "Received 'SubscribeToTopics' request with empty topic list. Ignoring request."); + set_service_error(response, "No topics specified in request."); + return; + } + + if (!request->topic_types.empty() && request->topic_types.size() != request->topics.size()) { + RCLCPP_WARN(node->get_logger(), + "Received 'SubscribeToTopics' request with %zu topics and %zu topic types. " + "The number of topic types must be zero or match the number of topics.", + request->topics.size(), request->topic_types.size()); + set_service_error(response, "'topic_types' must be empty or the same length as 'topics'."); + return; + } + + { + std::lock_guard state_lock(start_stop_transition_mutex_); + if (!in_recording_.load()) { + RCLCPP_WARN(node->get_logger(), + "Received 'SubscribeToTopics' request while not in recording. Ignoring request."); + // TODO(morlov): More consistent would be to add all requested topics into the + // unavailable_topics list instead of just one. + response->unavailable_topics = request->topics; + set_service_error(response, "Recorder is not currently recording."); + return; + } + } + + const auto available_topics = node->get_topic_names_and_types(); + const auto requested_topics_with_types = + collect_requested_topics_with_types(request, response, available_topics); + + const auto missing_topics = get_missing_topics(requested_topics_with_types); + subscribe_topics(missing_topics); + + response->subscribed_topics.reserve(requested_topics_with_types.size()); + for (const auto & [topic_name, _] : requested_topics_with_types) { + response->subscribed_topics.emplace_back(topic_name); + } + + if (!response->subscribed_topics.empty()) { + set_service_success(response); + } else { + set_service_error(response, "No topics were subscribed."); + } + }); +} + +template +std::unordered_map RecorderImpl::collect_requested_topics_with_types( + const RequestT & request, + ResponseT & response, + const std::map> & available_topics) +{ + std::unordered_map requested_topics_with_types; + requested_topics_with_types.reserve(request->topics.size()); + + for (size_t topic_index = 0; topic_index < request->topics.size(); ++topic_index) { + const auto & requested_topic = request->topics[topic_index]; + const std::string requested_type = + request->topic_types.empty() ? "" : request->topic_types[topic_index]; + + // TODO(morlov): The validated_topic is not a very good name to reflect what this variable is + // assigned to. Consider renaming it to the expanded_topic_name or exp_topic_name + std::string validated_topic; + try { + validated_topic = rclcpp::expand_topic_or_service_name( + requested_topic, node->get_name(), node->get_namespace(), false); + } catch (const rclcpp::exceptions::InvalidTopicNameError & ex) { + RCLCPP_WARN(node->get_logger(), + "Invalid topic '%s' in 'SubscribeToTopics' request: %s", + requested_topic.c_str(), ex.what()); + response->unavailable_topics.emplace_back(requested_topic); + continue; + } // TODO(morlov): Need to add another catch `catch (const std::exception & ex)` or + // catch (...). We don't want to have unhandled exception inside service call handler. + + if (requested_topics_with_types.find(validated_topic) != requested_topics_with_types.end()) { + RCLCPP_WARN(node->get_logger(), + "Topic '%s' appeared multiple times in 'SubscribeToTopics' request. Ignoring duplicates.", + validated_topic.c_str()); + continue; + } + + const auto available_it = available_topics.find(validated_topic); + if (available_it == available_topics.end()) { + RCLCPP_WARN(node->get_logger(), + "Topic '%s' in 'SubscribeToTopics' request is not available. Ignoring.", + validated_topic.c_str()); + response->unavailable_topics.emplace_back(validated_topic); + continue; + } + + const auto topic_type = resolve_topic_type_for_subscribe_request( + requested_type, validated_topic, available_it->second, response); + if (!topic_type.has_value()) { + continue; + } + + requested_topics_with_types.emplace(validated_topic, topic_type.value()); + warn_if_new_qos_for_subscribed_topic(validated_topic); + } + + return requested_topics_with_types; +} + +template +std::optional RecorderImpl::resolve_topic_type_for_subscribe_request( + const std::string & requested_type, + const std::string & validated_topic, + const std::vector & available_types, + ResponseT & response) const +{ + if (!requested_type.empty()) { + if (std::find(available_types.begin(), available_types.end(), + requested_type) == available_types.end()) + { + RCLCPP_WARN(node->get_logger(), + "Requested type '%s' for topic '%s' was not found in ROS graph. Ignoring.", + requested_type.c_str(), validated_topic.c_str()); + response->unavailable_topics.emplace_back(validated_topic); + return std::nullopt; + } + return requested_type; + } + + if (available_types.empty()) { + RCLCPP_WARN(node->get_logger(), + "Topic '%s' in 'SubscribeToTopics' request has no type information in ROS graph. " + "Specify the desired type in 'topic_types' and try again.", + validated_topic.c_str()); + response->unavailable_topics.emplace_back(validated_topic); + return std::nullopt; + } + + if (available_types.size() > 1) { + RCLCPP_WARN(node->get_logger(), + "Topic '%s' in 'SubscribeToTopics' request has multiple types but no type was provided in " + "request. Specify the desired type in 'topic_types' and try again.", + validated_topic.c_str()); + response->unavailable_topics.emplace_back(validated_topic); + return std::nullopt; + } + + return available_types.front(); } std::optional RecorderImpl::optional_time_from_request( @@ -1243,8 +1446,13 @@ void RecorderImpl::topics_discovery() noexcept } bool should_update_subscriptions = true; while (rclcpp::ok() && discovery_running_) { + size_t subscriptions_count = 0u; + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + subscriptions_count = subscriptions_.size(); + } if (!record_options_.topics.empty() && - subscriptions_.size() == record_options_.topics.size()) + subscriptions_count == record_options_.topics.size()) { RCLCPP_INFO( node->get_logger(), "All requested topics are subscribed. Stopping discovery..."); @@ -1297,14 +1505,33 @@ std::unordered_map RecorderImpl::get_missing_topics(const std::unordered_map & all_topics) { std::unordered_map missing_topics; + std::unordered_set subscribed_topics; + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + for (const auto & [topic_name, _] : subscriptions_) { + subscribed_topics.insert(topic_name); + } + } for (const auto & [topic_name, topic_type] : all_topics) { - if (subscriptions_.find(topic_name) == subscriptions_.end()) { + if (subscribed_topics.find(topic_name) == subscribed_topics.end()) { missing_topics.emplace(topic_name, topic_type); } } return missing_topics; } +std::vector RecorderImpl::get_subscribed_topics() const +{ + std::lock_guard subscriptions_lock(subscriptions_mutex_); + std::vector topics; + topics.reserve(subscriptions_.size()); + for (const auto & [topic_name, _] : subscriptions_) { + topics.emplace_back(topic_name); + } + std::sort(topics.begin(), topics.end()); + return topics; +} + void RecorderImpl::subscribe_topics( const std::unordered_map & topics_and_types) @@ -1419,8 +1646,11 @@ uint64_t RecorderImpl::get_total_num_messages_lost_in_transport() const void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) { - if (subscriptions_.find(topic.name) != subscriptions_.end()) { - return; + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + if (subscriptions_.find(topic.name) != subscriptions_.end()) { + return; + } } // Need to create topic in writer before we are trying to create subscription. Since in // callback for subscription we are calling writer_->write(bag_message); and it could happened @@ -1431,7 +1661,10 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) auto subscription = create_subscription(topic.name, topic.type, subscription_qos); if (subscription) { - subscriptions_.insert({topic.name, subscription}); + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + subscriptions_.insert({topic.name, subscription}); + } if (node->get_logger().get_effective_level() == rclcpp::Logger::Level::Debug) { RCLCPP_DEBUG_STREAM(node->get_logger(), "Subscribed to topic '" << topic.name << "' with QoS:\n" << subscription_qos.to_string()); @@ -1573,16 +1806,20 @@ rclcpp::QoS RecorderImpl::subscription_qos_for_topic(const std::string & topic_n void RecorderImpl::warn_if_new_qos_for_subscribed_topic(const std::string & topic_name) { - auto existing_subscription = subscriptions_.find(topic_name); - if (existing_subscription == subscriptions_.end()) { - // Not subscribed yet - return; + std::shared_ptr existing_subscription; + { + std::lock_guard subscriptions_lock(subscriptions_mutex_); + auto iterator = subscriptions_.find(topic_name); + if (iterator == subscriptions_.end()) { + return; + } + existing_subscription = iterator->second; } if (topics_warned_about_incompatibility_.count(topic_name) > 0) { // Already warned about this topic return; } - const auto actual_qos = existing_subscription->second->get_actual_qos(); + const auto actual_qos = existing_subscription->get_actual_qos(); const auto & used_profile = actual_qos.get_rmw_qos_profile(); auto publishers_info = node->get_publishers_info_by_topic(topic_name); for (const auto & info : publishers_info) { @@ -1843,6 +2080,12 @@ void Recorder::set_on_start_recording_callback(OnStartRecordingCallback callback pimpl_->on_start_recording_callback_ = std::move(callback); } +std::vector +Recorder::get_subscribed_topics() const +{ + return pimpl_->get_subscribed_topics(); +} + void Recorder::read_static_topics() noexcept { return pimpl_->read_static_topics(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index af22118c63..947f8b03ac 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -655,6 +656,42 @@ TEST_F(RecordIntegrationTestFixture, add_channel_with_message_definition) recorder->stop(); } +TEST_F(RecordIntegrationTestFixture, get_subscribed_topics_reflects_subscription_state) +{ + auto string_message = get_messages_strings()[0]; + const std::string test_topic = "/get_subscribed_topics_topic"; + + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(test_topic, string_message, 5); + + rosbag2_transport::RecordOptions record_options{}; + record_options.topics = {test_topic}; + record_options.input_serialization_format = "rmw_format"; + record_options.output_serialization_format = "rmw_format"; + record_options.topic_polling_interval = 50ms; + + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + ASSERT_TRUE(pub_manager.wait_for_matched(test_topic.c_str())); + pub_manager.run_publishers(); + + auto observed = rosbag2_test_common::wait_until_condition( + [&recorder, &test_topic]() { + auto topics = recorder->get_subscribed_topics(); + return std::find(topics.begin(), topics.end(), test_topic) != topics.end(); + }, + std::chrono::seconds(5)); + ASSERT_TRUE(observed) << "Recorder did not report expected subscription in time"; + + auto topics = recorder->get_subscribed_topics(); + EXPECT_THAT(topics, Contains(test_topic)); + + recorder->stop(); + EXPECT_THAT(recorder->get_subscribed_topics(), IsEmpty()); +} + TEST_F(RecordIntegrationTestFixture, write_message_writes_to_bag) { rosbag2_transport::RecordOptions record_options{}; diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 3a0dfb1c62..0191515c1a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -26,6 +27,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosgraph_msgs/msg/clock.hpp" +#include "rosbag2_interfaces/srv/get_subscribed_topics.hpp" #include "rosbag2_interfaces/srv/is_discovery_running.hpp" #include "rosbag2_interfaces/srv/is_paused.hpp" #include "rosbag2_interfaces/srv/pause.hpp" @@ -33,6 +35,7 @@ #include "rosbag2_interfaces/srv/resume.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" #include "rosbag2_interfaces/srv/split_bagfile.hpp" +#include "rosbag2_interfaces/srv/subscribe_to_topics.hpp" #include "rosbag2_interfaces/srv/start_discovery.hpp" #include "rosbag2_interfaces/srv/stop_discovery.hpp" #include "rosbag2_interfaces/srv/stop.hpp" @@ -66,6 +69,7 @@ struct type_has_return_code().return_c class RecordSrvsTest : public RecordIntegrationTestFixture { public: + using GetSubscribedTopics = rosbag2_interfaces::srv::GetSubscribedTopics; using IsDiscoveryRunning = rosbag2_interfaces::srv::IsDiscoveryRunning; using IsPaused = rosbag2_interfaces::srv::IsPaused; using Pause = rosbag2_interfaces::srv::Pause; @@ -73,6 +77,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture using Resume = rosbag2_interfaces::srv::Resume; using Snapshot = rosbag2_interfaces::srv::Snapshot; using SplitBagfile = rosbag2_interfaces::srv::SplitBagfile; + using SubscribeToTopics = rosbag2_interfaces::srv::SubscribeToTopics; using StartDiscovery = rosbag2_interfaces::srv::StartDiscovery; using Stop = rosbag2_interfaces::srv::Stop; using StopDiscovery = rosbag2_interfaces::srv::StopDiscovery; @@ -126,6 +131,8 @@ class RecordSrvsTest : public RecordIntegrationTestFixture recorder_->record(); const std::string ns = "/" + recorder_name_; + cli_get_subscribed_topics_ = client_node_->create_client( + ns + "/get_subscribed_topics"); cli_is_discovery_running_ = client_node_->create_client( ns + "/is_discovery_running"); cli_is_paused_ = client_node_->create_client(ns + "/is_paused"); @@ -134,6 +141,8 @@ class RecordSrvsTest : public RecordIntegrationTestFixture cli_resume_ = client_node_->create_client(ns + "/resume"); cli_snapshot_ = client_node_->create_client(ns + "/snapshot"); cli_split_bagfile_ = client_node_->create_client(ns + "/split_bagfile"); + cli_subscribe_to_topics_ = client_node_->create_client( + ns + "/subscribe_to_topics"); cli_start_discovery_ = client_node_->create_client(ns + "/start_discovery"); cli_stop_ = client_node_->create_client(ns + "/stop"); cli_stop_discovery_ = client_node_->create_client(ns + "/stop_discovery"); @@ -171,12 +180,14 @@ class RecordSrvsTest : public RecordIntegrationTestFixture if (snapshot_mode_) { ASSERT_TRUE(cli_snapshot_->wait_for_service(service_wait_timeout_)); } + ASSERT_TRUE(cli_get_subscribed_topics_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_is_discovery_running_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_is_paused_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_pause_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_record_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_resume_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_split_bagfile_->wait_for_service(service_wait_timeout_)); + ASSERT_TRUE(cli_subscribe_to_topics_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_start_discovery_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_stop_->wait_for_service(service_wait_timeout_)); ASSERT_TRUE(cli_stop_discovery_->wait_for_service(service_wait_timeout_)); @@ -257,6 +268,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture // Service clients rclcpp::Node::SharedPtr client_node_; + rclcpp::Client::SharedPtr cli_get_subscribed_topics_; rclcpp::Client::SharedPtr cli_is_discovery_running_; rclcpp::Client::SharedPtr cli_is_paused_; rclcpp::Client::SharedPtr cli_pause_; @@ -264,6 +276,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture rclcpp::Client::SharedPtr cli_resume_; rclcpp::Client::SharedPtr cli_snapshot_; rclcpp::Client::SharedPtr cli_split_bagfile_; + rclcpp::Client::SharedPtr cli_subscribe_to_topics_; rclcpp::Client::SharedPtr cli_start_discovery_; rclcpp::Client::SharedPtr cli_stop_; rclcpp::Client::SharedPtr cli_stop_discovery_; @@ -300,6 +313,178 @@ class RecordSrvsPublishMultipleTopicsTest : public RecordSrvsTest {} }; +TEST_F(RecordSrvsTest, get_subscribed_topics_reports_active_topics) +{ + rosbag2_test_common::PublicationManager pub_manager; + auto string_message = get_messages_strings()[1]; + pub_manager.setup_publisher(test_topic_, string_message, 3); + ASSERT_TRUE(pub_manager.wait_for_matched(test_topic_.c_str())); + pub_manager.run_publishers(); + + auto response = std::make_shared(); + ASSERT_TRUE( + successful_service_request(cli_get_subscribed_topics_, response) + ); + EXPECT_THAT(response->topics, Contains(test_topic_)); + EXPECT_EQ(recorder_->get_subscribed_topics(), response->topics); +} + +TEST_F(RecordSrvsTest, get_subscribed_topics_empty_when_not_recording) +{ + rosbag2_test_common::PublicationManager pub_manager; + auto string_message = get_messages_strings()[1]; + pub_manager.setup_publisher(test_topic_, string_message, 3); + ASSERT_TRUE(pub_manager.wait_for_matched(test_topic_.c_str())); + pub_manager.run_publishers(); + + recorder_->stop(); + auto response = std::make_shared(); + ASSERT_TRUE( + successful_service_request(cli_get_subscribed_topics_, response) + ); + EXPECT_THAT(response->topics, IsEmpty()); + EXPECT_EQ(recorder_->get_subscribed_topics(), response->topics); +} + +class RecordSrvsSubscibeToTopicsTest : public RecordSrvsTest +{ +protected: + RecordSrvsSubscibeToTopicsTest() + : RecordSrvsTest(false /*snapshot_mode*/, false /*is_discovery_disabled*/, + false /*use_sim_time*/, {/*no topics*/}) + {} +}; + +// TODO(morlov): review RecordSrvsSubscibeToTopicsTest cases +TEST_F(RecordSrvsSubscibeToTopicsTest, subscribe_to_topics_adds_requested_topic) +{ + const std::string additional_topic = test_topic_ + "_secondary"; + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(additional_topic, get_messages_strings()[1], 5); + pub_manager.run_publishers(); + + auto request = std::make_shared(); + request->topics = {additional_topic}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(0, response->return_code); + EXPECT_TRUE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, ElementsAre(additional_topic)); + EXPECT_THAT(response->unavailable_topics, IsEmpty()); + EXPECT_THAT(recorder_->get_subscribed_topics(), Contains(additional_topic)); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, subscribe_to_topics_reports_missing_topics) +{ + const std::string missing_topic = "/subscribe_topics_missing"; + auto request = std::make_shared(); + request->topics = {missing_topic}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(1, response->return_code); + EXPECT_FALSE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, IsEmpty()); + EXPECT_THAT(response->unavailable_topics, ElementsAre(missing_topic)); + EXPECT_THAT(recorder_->get_subscribed_topics(), Not(Contains(missing_topic))); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, subscribe_to_topics_rejects_mismatched_topic_types) +{ + const std::string additional_topic = test_topic_ + "_with_types"; + auto request = std::make_shared(); + request->topics = {test_topic_, additional_topic}; + request->topic_types = {"std_msgs/msg/String"}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(1, response->return_code); + EXPECT_FALSE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, IsEmpty()); + EXPECT_THAT(response->unavailable_topics, IsEmpty()); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, subscribe_to_topics_reports_already_subscribed_topics) +{ + rosbag2_test_common::PublicationManager pub_manager; + auto string_message = get_messages_strings()[1]; + pub_manager.setup_publisher(test_topic_, string_message, 3); + // ASSERT_TRUE(pub_manager.wait_for_matched(test_topic_.c_str())); + // pub_manager.run_publishers(); + + auto request = std::make_shared(); + request->topics = {test_topic_}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(0, response->return_code); + EXPECT_TRUE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, ElementsAre(test_topic_)); + EXPECT_THAT(response->unavailable_topics, IsEmpty()); + EXPECT_THAT(recorder_->get_subscribed_topics(), Contains(test_topic_)); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, + subscribe_to_topics_includes_previously_subscribed_topics_in_response) +{ + const std::string additional_topic = test_topic_ + "_tertiary"; + rosbag2_test_common::PublicationManager pub_manager; + auto string_message = get_messages_strings()[1]; + pub_manager.setup_publisher(test_topic_, string_message, 3); + pub_manager.setup_publisher(additional_topic, string_message, 5); + pub_manager.run_publishers(); + + auto request = std::make_shared(); + request->topics = {test_topic_, additional_topic}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(0, response->return_code); + EXPECT_TRUE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, UnorderedElementsAre(test_topic_, additional_topic)); + EXPECT_THAT(response->unavailable_topics, IsEmpty()); + EXPECT_THAT(recorder_->get_subscribed_topics(), + AllOf(Contains(test_topic_), Contains(additional_topic))); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, + subscribe_to_topics_allows_specified_type_when_not_discovered) +{ + const std::string undiscovered_topic = "/subscribe_topics_with_type_only"; + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(undiscovered_topic, get_messages_strings()[1], 5); + pub_manager.run_publishers(); + + auto request = std::make_shared(); + request->topics = {undiscovered_topic}; + request->topic_types = {"test_msgs/msg/Strings"}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(0, response->return_code); + EXPECT_TRUE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, ElementsAre(undiscovered_topic)); + EXPECT_THAT(response->unavailable_topics, IsEmpty()); + EXPECT_THAT(recorder_->get_subscribed_topics(), Contains(undiscovered_topic)); +} + +TEST_F(RecordSrvsSubscibeToTopicsTest, subscribe_to_topics_fails_when_not_recording) +{ + ASSERT_TRUE(successful_service_request(cli_stop_)); + EXPECT_THAT(recorder_->get_subscribed_topics(), IsEmpty()); + + auto request = std::make_shared(); + request->topics = {test_topic_}; + auto response = std::make_shared(); + ASSERT_TRUE(successful_service_request(cli_subscribe_to_topics_, request, + response)); + EXPECT_EQ(1, response->return_code); + EXPECT_FALSE(response->error_string.empty()); + EXPECT_THAT(response->subscribed_topics, IsEmpty()); + EXPECT_THAT(response->unavailable_topics, ElementsAre(test_topic_)); +} + class RecordSrvsDiscoveryTest : public RecordSrvsTest { protected: