From bc4d1187d36a2c16bbe2ef1f306aa35541b88b3d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 30 Mar 2024 22:46:31 -0700 Subject: [PATCH 01/17] Make service_event_ts_lib as private member again - Motivation: The shared pointer to the service event type support library shall be a member variable to make sure that library loaded during the liveliness of the instance of this class, since we have a raw pointers to its inner members. Signed-off-by: Michael Orlov --- .../include/rosbag2_transport/player_service_client.hpp | 3 +++ .../src/rosbag2_transport/player_service_client.cpp | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index 96476e3c7..1fa3afe45 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -62,6 +62,9 @@ class PlayerServiceClient final std::string service_name_; const rclcpp::Logger logger_; std::shared_ptr player_service_client_manager_; + // Note: The service_event_ts_lib_ shall be a member variable to make sure that library loaded + // during the liveliness of the instance of this class, since we have raw pointers to its members. + std::shared_ptr service_event_ts_lib_; enum class request_info_from { SERVICE = 0, diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index d54ff93ce..5dd730483 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -38,11 +38,11 @@ PlayerServiceClient::PlayerServiceClient( logger_(std::move(logger)), player_service_client_manager_(std::move(player_service_client_manager)) { - auto service_event_ts_lib = rclcpp::get_typesupport_library( - service_event_type, "rosidl_typesupport_cpp"); + service_event_ts_lib_ = + rclcpp::get_typesupport_library(service_event_type, "rosidl_typesupport_cpp"); service_event_type_ts_ = rclcpp::get_message_typesupport_handle( - service_event_type, "rosidl_typesupport_cpp", *service_event_ts_lib); + service_event_type, "rosidl_typesupport_cpp", *service_event_ts_lib_); auto service_event_ts_introspection = get_message_typesupport_handle( service_event_type_ts_, rosidl_typesupport_introspection_cpp::typesupport_identifier); From 5f324c12ec506bb44d63f14dba2a94c6e44baaef Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 30 Mar 2024 21:10:11 -0700 Subject: [PATCH 02/17] Cleanup in PlayerServiceClient::async_send_request(ser_message) - Rewrite raw pointers arithmetic. Assumption that size_t represent size of the void* may not necessarily be true on all platforms. - Use shared pointer with custom deleter for deserialized message. - Assumption that we can take first element from bounded sequence by dereferencing raw pointer to the bounded sequence may not be necessarily be true and up to the underlying rmw and transport layer implementation. Use dedicated request_member.get_function(request_sequence_ptr, 0) function instead. - Add sanity checks `for service_event_members_` in PlayerServiceClient constructor. Signed-off-by: Michael Orlov --- .../player_service_client.cpp | 87 +++++++++++++------ 1 file changed, 62 insertions(+), 25 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index 5dd730483..d9d3c2b08 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -50,6 +50,42 @@ PlayerServiceClient::PlayerServiceClient( service_event_members_ = reinterpret_cast( service_event_ts_introspection->data); + + // Sanity checks for service_event_members_ + if (service_event_members_ == nullptr) { + throw std::invalid_argument("service_event_members_ for `" + service_name_ + "` is nullptr"); + } + if (service_event_members_->member_count_ != 3) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + std::stringstream ss; + ss << "Expected 3 fields in the service introspection message, but got " << + service_event_members_->member_count_; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->members_[1].get_function == nullptr) { + std::stringstream ss; + ss << "get_function() for service request '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->members_[1].size_function == nullptr) { + std::stringstream ss; + ss << "size_function() for service request '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->init_function == nullptr) { + std::stringstream ss; + ss << "service_event_members_->init_function for '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->fini_function == nullptr) { + std::stringstream ss; + ss << "service_event_members_->fini_function for '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } } bool PlayerServiceClient::is_include_request_message(const rcl_serialized_message_t & message) @@ -160,32 +196,33 @@ bool PlayerServiceClient::is_include_request_message(const rcl_serialized_messag void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message) { - int ret = RMW_RET_OK; - - { - auto ros_message = std::make_unique(service_event_members_->size_of_); - - service_event_members_->init_function( - ros_message.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); - - ret = rmw_deserialize( - &message, service_event_type_ts_, ros_message.get()); - if (ret == RMW_RET_OK) { - if (client_->service_is_ready()) { - // members_[0]: info, members_[1]: request, members_[2]: response - auto request_offset = service_event_members_->members_[1].offset_; - auto request_addr = reinterpret_cast(ros_message.get()) + request_offset; - auto future_and_request_id = client_->async_send_request( - reinterpret_cast(*reinterpret_cast(request_addr))); - player_service_client_manager_->register_request_future(future_and_request_id, client_); - } else { - RCLCPP_ERROR( - logger_, "Service request hasn't been sent. The '%s' service isn't ready !", - service_name_.c_str()); - } - } + if (!client_->service_is_ready()) { + RCLCPP_ERROR( + logger_, "Service request hasn't been sent. The '%s' service isn't ready !", + service_name_.c_str()); + return; + } - service_event_members_->fini_function(ros_message.get()); + auto type_erased_ros_message = std::shared_ptr( + new uint8_t[service_event_members_->size_of_], + [fini_function = this->service_event_members_->fini_function](uint8_t * msg) { + fini_function(msg); + delete[] msg; + }); + + service_event_members_->init_function( + type_erased_ros_message.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); + + rmw_ret_t ret = rmw_deserialize(&message, service_event_type_ts_, type_erased_ros_message.get()); + if (ret == RMW_RET_OK) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = service_event_members_->members_[1]; + void * request_sequence_ptr = type_erased_ros_message.get() + request_member.offset_; + if (request_member.is_array_ && request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + auto future_and_request_id = client_->async_send_request(request_ptr); + player_service_client_manager_->register_request_future(future_and_request_id, client_); + } // else { /* No request in the service event */ } } if (ret != RMW_RET_OK) { From 6397df6761f97b17548a6b95252809b8a596ad37 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 31 Mar 2024 12:21:56 -0700 Subject: [PATCH 03/17] Refactoring. Do full deserialization and only once - Rationale: We can't rely on assumption that we can safely partially deserialize service event to the ServiceEventInfo structure. Signed-off-by: Michael Orlov --- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 2 + .../player_service_client.hpp | 25 +++- .../src/rosbag2_transport/player.cpp | 64 +++++++-- .../player_service_client.cpp | 123 +++++++++++++----- 4 files changed, 166 insertions(+), 48 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 62ef91872..333cfd001 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -132,6 +132,8 @@ size_t get_serialization_size_for_service_metadata_event() static_cast( type_support_handle->data); + // TODO(morlov): We shall not rely on this arithmetic!!! It is up to the serialization + // implementation // endian type (4 size) + service event info size + empty request (4 bytes) // + emtpy response (4 bytes) size = 4 + service_event_info->size_of_ + 4 + 4; diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index 1fa3afe45..c89652c1d 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -38,6 +38,9 @@ class PlayerServiceClientManager; class PlayerServiceClient final { public: + using ServiceEventType = service_msgs::msg::ServiceEventInfo::_event_type_type; + using ClientGidType = service_msgs::msg::ServiceEventInfo::_client_gid_type; + explicit PlayerServiceClient( std::shared_ptr generic_client, @@ -46,7 +49,22 @@ class PlayerServiceClient final rclcpp::Logger logger, std::shared_ptr player_service_client_manager); - // Note: Call this function only if is_include_request_message() return true + const std::string & get_service_name(); + + /// \brief Deserialize message to the type erased service event + /// \param message - Serialized message + /// \return Shared pointer to the byte array with deserialized service event if success, + /// otherwise nullptr + std::shared_ptr deserialize_service_event(const rcl_serialized_message_t & message); + + std::tuple + get_service_event_type_and_client_gid(const std::shared_ptr type_erased_service_event); + + bool is_service_event_include_request_message( + const std::shared_ptr type_erased_service_event); + + void async_send_request(const std::shared_ptr type_erased_service_event); + void async_send_request(const rcl_serialized_message_t & message); std::shared_ptr generic_client() @@ -73,16 +91,15 @@ class PlayerServiceClient final }; bool service_set_introspection_content_ = false; - using client_id = service_msgs::msg::ServiceEventInfo::_client_gid_type; // Info on request data from service or client - std::unordered_map request_info_; + std::unordered_map request_info_; const rosidl_message_type_support_t * service_event_type_ts_; const rosidl_typesupport_introspection_cpp::MessageMembers * service_event_members_; rcutils_allocator_t allocator_ = rcutils_get_default_allocator(); - std::tuple + std::tuple get_msg_event_type(const rcl_serialized_message_t & message); }; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 09d1e2376..602351339 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -36,7 +35,6 @@ #include "rosbag2_cpp/clocks/time_controller_clock.hpp" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/service_utils.hpp" -#include "rosbag2_cpp/typesupport_helpers.hpp" #include "rosbag2_storage/storage_filter.hpp" #include "rosbag2_storage/qos.hpp" @@ -1170,13 +1168,11 @@ void PlayerImpl::run_play_msg_post_callbacks( bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) { - bool message_published = false; - auto pub_iter = publishers_.find(message->topic_name); if (pub_iter != publishers_.end()) { // Calling on play message pre-callbacks run_play_msg_pre_callbacks(message); - + bool message_published = false; try { pub_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data)); message_published = true; @@ -1191,17 +1187,61 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr return message_published; } + // Try to publish message as service request auto client_iter = service_clients_.find(message->topic_name); if (client_iter != service_clients_.end()) { - if (!client_iter->second->is_include_request_message(*message->serialized_data)) { - return message_published; + const auto & service_client = client_iter->second; + // TODO(morlov): + // Wrap deserialize_service_event and get_service_event_type_and_client_gid in try-catch + auto service_event = service_client->deserialize_service_event(*message->serialized_data); + if (!service_event) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to deserialize service event message for '" << + service_client->get_service_name() << "' service!\n"); + return false; + } + + auto [service_event_type, client_gid] = + service_client->get_service_event_type_and_client_gid(service_event); + // Ignore response message + if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || + service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) + { + // TODO(morlov): Shall we ignore REQUEST_RECEIVED as well? + return false; + } + + if (!service_client->generic_client()->service_is_ready()) { + RCLCPP_ERROR( + owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", + service_client->get_service_name().c_str()); + return false; + } + + if (!service_client->is_service_event_include_request_message(service_event)) { + if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' was metadata only on service side!", + service_client->get_service_name().c_str()); + } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", + service_client->get_service_name().c_str(), + rosbag2_cpp::client_id_to_string(client_gid).c_str()); + } + return false; } // Calling on play message pre-callbacks run_play_msg_pre_callbacks(message); + bool message_published = false; try { - client_iter->second->async_send_request(*message->serialized_data); + client_iter->second->async_send_request(service_event); message_published = true; } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( @@ -1215,9 +1255,11 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr return message_published; } - RCLCPP_WARN_STREAM( - owner_->get_logger(), "Not find sender for topic '" << message->topic_name << "' topic."); - return message_published; + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Publisher for topic '%s' not found", message->topic_name.c_str()); + + return false; } void PlayerImpl::add_key_callback( diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index d9d3c2b08..4f53bbcda 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -63,9 +63,9 @@ PlayerServiceClient::PlayerServiceClient( throw std::invalid_argument(ss.str()); } - if (service_event_members_->members_[1].get_function == nullptr) { + if (!service_event_members_->members_[1].is_array_) { std::stringstream ss; - ss << "get_function() for service request '" << service_name_ << "' is not set.\n"; + ss << "The service request for '" << service_name_ << "' is not array.\n"; throw std::invalid_argument(ss.str()); } @@ -75,6 +75,12 @@ PlayerServiceClient::PlayerServiceClient( throw std::invalid_argument(ss.str()); } + if (service_event_members_->members_[1].get_function == nullptr) { + std::stringstream ss; + ss << "get_function() for service request '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + if (service_event_members_->init_function == nullptr) { std::stringstream ss; ss << "service_event_members_->init_function for '" << service_name_ << "' is not set.\n"; @@ -194,16 +200,15 @@ bool PlayerServiceClient::is_include_request_message(const rcl_serialized_messag return ret; } -void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message) +const std::string & PlayerServiceClient::get_service_name() { - if (!client_->service_is_ready()) { - RCLCPP_ERROR( - logger_, "Service request hasn't been sent. The '%s' service isn't ready !", - service_name_.c_str()); - return; - } + return service_name_; +} - auto type_erased_ros_message = std::shared_ptr( +std::shared_ptr +PlayerServiceClient::deserialize_service_event(const rcl_serialized_message_t & message) +{ + auto type_erased_service_event = std::shared_ptr( new uint8_t[service_event_members_->size_of_], [fini_function = this->service_event_members_->fini_function](uint8_t * msg) { fini_function(msg); @@ -211,44 +216,99 @@ void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & me }); service_event_members_->init_function( - type_erased_ros_message.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); + type_erased_service_event.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); + + rmw_ret_t ret = + rmw_deserialize(&message, service_event_type_ts_, type_erased_service_event.get()); + if (ret != RMW_RET_OK) { // Failed to deserialize service event message + type_erased_service_event.reset(); + } + return type_erased_service_event; +} + +std::tuple +PlayerServiceClient::get_service_event_type_and_client_gid( + const std::shared_ptr type_erased_service_event) +{ + if (type_erased_service_event) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & info_member = service_event_members_->members_[0]; + + auto service_event_info_ptr = reinterpret_cast( + type_erased_service_event.get() + info_member.offset_); + if (service_event_info_ptr == nullptr) { + throw std::runtime_error("Error: The service_event_info_ptr is nullptr"); + } + return {service_event_info_ptr->event_type, service_event_info_ptr->client_gid}; + } else { + throw std::invalid_argument("Error: The type_erased_service_event is nullptr"); + } +} - rmw_ret_t ret = rmw_deserialize(&message, service_event_type_ts_, type_erased_ros_message.get()); - if (ret == RMW_RET_OK) { +bool PlayerServiceClient::is_service_event_include_request_message( + const std::shared_ptr type_erased_service_event) +{ + if (type_erased_service_event) { // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] const auto & request_member = service_event_members_->members_[1]; - void * request_sequence_ptr = type_erased_ros_message.get() + request_member.offset_; - if (request_member.is_array_ && request_member.size_function(request_sequence_ptr) > 0) { - void * request_ptr = request_member.get_function(request_sequence_ptr, 0); - auto future_and_request_id = client_->async_send_request(request_ptr); - player_service_client_manager_->register_request_future(future_and_request_id, client_); - } // else { /* No request in the service event */ } + void * request_sequence_ptr = type_erased_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + return true; + } // else { /* No service request */ } + } else { + throw std::invalid_argument("Error: The type_erased_service_event is nullptr"); + } + return false; +} + +void PlayerServiceClient::async_send_request( + const std::shared_ptr type_erased_service_event) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = service_event_members_->members_[1]; + void * request_sequence_ptr = type_erased_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + auto future_and_request_id = client_->async_send_request(request_ptr); + player_service_client_manager_->register_request_future(future_and_request_id, client_); + } // else { /* No service request in the service event. Do nothing, just skip it. */ } +} + +void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message) +{ + if (!client_->service_is_ready()) { + RCLCPP_ERROR( + logger_, "Service request hasn't been sent. The '%s' service isn't ready !", + service_name_.c_str()); + return; } - if (ret != RMW_RET_OK) { + auto type_erased_ros_message = deserialize_service_event(message); + + if (type_erased_ros_message) { + async_send_request(type_erased_ros_message); + } else { throw std::runtime_error( "Failed to deserialize service event message for " + service_name_ + " !"); } } -std::tuple +std::tuple PlayerServiceClient::get_msg_event_type(const rcl_serialized_message_t & message) { auto msg = service_msgs::msg::ServiceEventInfo(); const rosidl_message_type_support_t * type_support_info = - rosidl_typesupport_cpp:: - get_message_type_support_handle(); + rosidl_typesupport_cpp::get_message_type_support_handle(); if (type_support_info == nullptr) { - throw std::runtime_error( - "Failed to get message type support handle of service event info !"); + throw std::runtime_error("Failed to get message type support handle of service event info !"); } - auto ret = rmw_deserialize( - &message, - type_support_info, - reinterpret_cast(&msg)); - if (ret != RMW_RET_OK) { + // Partially deserialize service event message. Deserializing only first member ServiceEventInfo + // with assumption that it is going to be the first in serialized message. + // TODO(morlov): We can't rely on this assumption. It is up to the underlying RMW and + // serialization format implementation! + if (rmw_deserialize(&message, type_support_info, reinterpret_cast(&msg)) != RMW_RET_OK) { throw std::runtime_error("Failed to deserialize message !"); } @@ -273,10 +333,7 @@ bool PlayerServiceClientManager::request_future_queue_is_full() return false; } - // Remove complete request future remove_complete_request_future(); - - // Remove all timeout request future remove_all_timeout_request_future(); if (request_futures_list_.size() == maximum_request_future_queue_) { From 94d418f672968aec74cca0f916ee484d8b8abf99 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sun, 7 Apr 2024 15:20:00 +0800 Subject: [PATCH 04/17] Specify service request from which introspection message and fix uncrustify errors Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/play.py | 10 ++ rosbag2_cpp/include/rosbag2_cpp/logging.hpp | 24 ++-- .../rosbag2_cpp/test_sequential_writer.cpp | 2 +- rosbag2_py/rosbag2_py/__init__.py | 2 + rosbag2_py/src/rosbag2_py/_transport.cpp | 6 + .../test/rosbag2_tests/test_reindexer.cpp | 8 +- .../rosbag2_transport/play_options.hpp | 8 ++ .../player_service_client.hpp | 14 --- .../config_options_from_node_params.cpp | 15 +++ .../src/rosbag2_transport/logging.hpp | 24 ++-- .../src/rosbag2_transport/player.cpp | 12 ++ .../player_service_client.cpp | 106 ------------------ .../test/resources/player_node_params.yaml | 1 + .../test/rosbag2_transport/test_burst.cpp | 4 +- .../test_composable_player.cpp | 3 + .../test/rosbag2_transport/test_play.cpp | 98 +++++++++++++++- .../test/rosbag2_transport/test_record.cpp | 12 +- 17 files changed, 190 insertions(+), 159 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index f447e2df7..112862f8d 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -25,6 +25,7 @@ from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import Player from rosbag2_py import PlayOptions +from rosbag2_py import ServiceRequestFrom from rosbag2_py import StorageOptions import yaml @@ -152,6 +153,11 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'By default, if loaned message can be used, messages are published as loaned ' 'message. It can help to reduce the number of data copies, so there is a greater ' 'benefit for sending big data.') + parser.add_argument( + '--service-request-from', default='service_introspection', + choices=['service_introspection', 'client_introspection'], + help='Determine the source of the service request to be replayed. ' + 'By default, the service request is from recorded service introspection message.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -219,6 +225,10 @@ def main(self, *, args): # noqa: D102 play_options.start_offset = args.start_offset play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message + if not args.service_request_from or args.service_request_from == 'service': + play_options.service_request_from = ServiceRequestFrom.SERVICE_INTROSPECTION + else: + play_options.service_request_from = ServiceRequestFrom.CLIENT_INTROSPECTION player = Player() try: diff --git a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp index 38e981f20..6e8a4d200 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp @@ -26,36 +26,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #endif // ROSBAG2_CPP__LOGGING_HPP_ diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp index cb27d87ca..53fea0640 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp @@ -374,7 +374,7 @@ TEST_F( write(An> &>())). WillByDefault( [this, &written_messages] - (const std::vector> & msgs) + (const std::vector> & msgs) { written_messages += msgs.size(); fake_storage_size_.fetch_add(static_cast(msgs.size())); diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py index f848f3404..f11949a14 100644 --- a/rosbag2_py/rosbag2_py/__init__.py +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -56,6 +56,7 @@ from rosbag2_py._transport import ( Player, PlayOptions, + ServiceRequestFrom, Recorder, RecordOptions, bag_rewrite, @@ -94,6 +95,7 @@ 'Info', 'Player', 'PlayOptions', + 'ServiceRequestFrom', 'Recorder', 'RecordOptions', ] diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 6f0a205ac..4e92be4da 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -366,6 +366,12 @@ PYBIND11_MODULE(_transport, m) { &PlayOptions::setPlaybackUntilTimestamp) .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) + .def_readwrite("service_request_from", &PlayOptions::service_request_from) + ; + + py::enum_(m, "ServiceRequestFrom") + .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::SERVICE_INTROSPECTION) + .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::CLIENT_INTROSPECTION) ; py::class_(m, "RecordOptions") diff --git a/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp b/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp index 4bbc3b9d3..95465a2e3 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp @@ -150,10 +150,10 @@ TEST_P(ReindexTestFixture, test_multiple_files) { target_metadata.topics_with_message_count.end(), [&gen_topic](rosbag2_storage::TopicInformation & t) { return (t.topic_metadata.name == gen_topic.topic_metadata.name) && - (t.message_count == gen_topic.message_count) && - (t.topic_metadata.offered_qos_profiles == - gen_topic.topic_metadata.offered_qos_profiles) && - (t.topic_metadata.type == gen_topic.topic_metadata.type); + (t.message_count == gen_topic.message_count) && + (t.topic_metadata.offered_qos_profiles == + gen_topic.topic_metadata.offered_qos_profiles) && + (t.topic_metadata.type == gen_topic.topic_metadata.type); } ) != target_metadata.topics_with_message_count.end() ); diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 18863c685..64909bcf8 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -28,6 +28,11 @@ namespace rosbag2_transport { +enum class ServiceRequestFrom : int +{ + SERVICE_INTROSPECTION = 0, + CLIENT_INTROSPECTION = 1 +}; struct PlayOptions { @@ -112,6 +117,9 @@ struct PlayOptions // Disable to publish as loaned message bool disable_loan_message = false; + + // The source of the service request + ServiceRequestFrom service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index c89652c1d..7a57d1e1c 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -72,9 +71,6 @@ class PlayerServiceClient final return client_; } - // Check if message can be unpacked to get request message - bool is_include_request_message(const rcl_serialized_message_t & message); - private: std::shared_ptr client_; std::string service_name_; @@ -83,16 +79,6 @@ class PlayerServiceClient final // Note: The service_event_ts_lib_ shall be a member variable to make sure that library loaded // during the liveliness of the instance of this class, since we have raw pointers to its members. std::shared_ptr service_event_ts_lib_; - enum class request_info_from - { - SERVICE = 0, - CLIENT, - NO_CONTENT // Only have META info. Not send request. - }; - bool service_set_introspection_content_ = false; - - // Info on request data from service or client - std::unordered_map request_info_; const rosidl_message_type_support_t * service_event_type_ts_; const rosidl_typesupport_introspection_cpp::MessageMembers * service_event_members_; diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index 2454e22c9..dcf9485a8 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -218,6 +218,21 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) play_options.disable_loan_message = node.declare_parameter("play.disable_loan_message", false); + // "SERVICE_INTROSPECTION" or "CLIENT_INTROSPECTION" + auto service_request_from = + node.declare_parameter("play.service_request_from", "SERVICE_INTROSPECTION"); + if (service_request_from == "SERVICE_INTROSPECTION") { + play_options.service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; + } else if (service_request_from == "CLIENT_INTROSPECTION") { + play_options.service_request_from = ServiceRequestFrom::CLIENT_INTROSPECTION; + } else { + RCLCPP_ERROR( + node.get_logger(), + "play.service_request_from doesn't support %s. It must be one of SERVICE_INTROSPECTION" + " and CLIENT_INTROSPECTION. Change it by default value SERVICE_INTROSPECTION.", + service_request_from.c_str()); + } + return play_options; } diff --git a/rosbag2_transport/src/rosbag2_transport/logging.hpp b/rosbag2_transport/src/rosbag2_transport/logging.hpp index 384857b0a..6f7ca5f87 100644 --- a/rosbag2_transport/src/rosbag2_transport/logging.hpp +++ b/rosbag2_transport/src/rosbag2_transport/logging.hpp @@ -26,36 +26,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #endif // ROSBAG2_TRANSPORT__LOGGING_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 602351339..4a18e8297 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1211,6 +1211,18 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr return false; } + if (play_options_.service_request_from == ServiceRequestFrom::SERVICE_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return false; + } + + if (play_options_.service_request_from == ServiceRequestFrom::CLIENT_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) + { + return false; + } + if (!service_client->generic_client()->service_is_ready()) { RCLCPP_ERROR( owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index 4f53bbcda..c8a689838 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -94,112 +94,6 @@ PlayerServiceClient::PlayerServiceClient( } } -bool PlayerServiceClient::is_include_request_message(const rcl_serialized_message_t & message) -{ - auto [type, client_id, sequence_number] = get_msg_event_type(message); - - // Ignore response message - if (type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || - type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) - { - return false; - } - - bool ret = false; - - // For each Client, decide which request data to use based on the first message related to - // the request that is obtained from the record data. - // e.g. - - auto iter = request_info_.find(client_id); - if (type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { - if (!service_set_introspection_content_) { - if (rosbag2_cpp::service_event_include_metadata_and_contents(message.buffer_length)) { - service_set_introspection_content_ = true; - } - } - - if (iter != request_info_.end()) { - switch (iter->second) { - case request_info_from::CLIENT: - { - // Already decide using request data from client. - break; - } - case request_info_from::NO_CONTENT: - { - if (service_set_introspection_content_) { - // introspection type is changed from metadata to metadata + contents - request_info_[client_id] = request_info_from::SERVICE; - ret = true; - } else { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "The configuration of introspection for '%s' is metadata on service side !", - service_name_.c_str()); - } - break; - } - default: // request_info_from::SERVICE: - { - // Already decide using request data from service. - ret = true; - } - } - } else { - if (service_set_introspection_content_) { - request_info_[client_id] = request_info_from::SERVICE; - ret = true; - } else { - request_info_[client_id] = request_info_from::NO_CONTENT; // Only have metadata - } - } - - return ret; - } - - // type is service_msgs::msg::ServiceEventInfo::REQUEST_SENT - if (iter != request_info_.end()) { - switch (iter->second) { - case request_info_from::CLIENT: - { - // Already decide using request data from client. - ret = true; - break; - } - case request_info_from::NO_CONTENT: - { - if (rosbag2_cpp::service_event_include_metadata_and_contents(message.buffer_length)) { - // introspection type is changed from metadata to metadata + contents - request_info_[client_id] = request_info_from::CLIENT; - ret = true; - } else { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "The configuration of introspection for '%s' client [ID: %s]` is metadata !", - rosbag2_cpp::client_id_to_string(client_id).c_str(), - service_name_.c_str()); - } - break; - } - default: // request_info_from::SERVICE: - { - // Already decide using request data from service. - ret = false; - } - } - } else { - if (rosbag2_cpp::service_event_include_metadata_and_contents(message.buffer_length)) { - request_info_[client_id] = request_info_from::CLIENT; - ret = true; - } else { - request_info_[client_id] = request_info_from::NO_CONTENT; - } - } - - return ret; -} - const std::string & PlayerServiceClient::get_service_name() { return service_name_; diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index bbef93ec4..ea18dd027 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -37,6 +37,7 @@ player_params_node: sec: 0 nsec: -999999999 disable_loan_message: false + service_request_from: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION storage: uri: "path/to/some_bag" diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index b518b787f..8c8dae623 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -41,7 +41,7 @@ get_service_event_message_basic_types() { auto msg = std::make_shared(); - msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; test_msgs::srv::BasicTypes_Request request; request.int32_value = 123; request.int64_value = 456; @@ -51,7 +51,7 @@ get_service_event_message_basic_types() { auto msg = std::make_shared(); - msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; test_msgs::srv::BasicTypes_Request request; request.int32_value = 456; request.int64_value = 789; diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index c239b4d2e..4c6e3a0de 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -170,6 +170,9 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.start_offset, 999999999); EXPECT_EQ(play_options.wait_acked_timeout, -999999999); EXPECT_EQ(play_options.disable_loan_message, false); + EXPECT_EQ( + play_options.service_request_from, + rosbag2_transport::ServiceRequestFrom::CLIENT_INTROSPECTION); EXPECT_EQ(storage_options.uri, uri_str); EXPECT_EQ(storage_options.storage_id, GetParam()); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 824fc9593..b63748808 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -54,7 +54,7 @@ get_service_event_message_basic_types() { auto msg = std::make_shared(); - msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; test_msgs::srv::BasicTypes_Request request; request.int32_value = 123; request.int64_value = 456; @@ -64,7 +64,7 @@ get_service_event_message_basic_types() { auto msg = std::make_shared(); - msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; test_msgs::srv::BasicTypes_Request request; request.int32_value = 456; request.int64_value = 789; @@ -72,6 +72,16 @@ get_service_event_message_basic_types() messages.push_back(msg); } + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 789; + request.int64_value = 123; + msg->request.emplace_back(request); + messages.push_back(msg); + } + return messages; } } // namespace @@ -839,6 +849,90 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus player->wait_for_playback_to_finish(); } +TEST_F(RosBag2PlayTestFixture, + player_with_service_request_from_recorded_service_introspection_message) +{ + auto services_types = std::vector{ + {1u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), + serialize_test_message( + "test_service1/_service_event", 300, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message( + "test_service1/_service_event", 300, get_service_event_message_basic_types()[2]), + }; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + std::vector> service1_receive_requests; + + srv_->setup_service("test_service1", service1_receive_requests); + srv_->run_services(); + + ASSERT_TRUE(srv_->all_services_ready()); + + play_options_.service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(300ms); + EXPECT_EQ(service1_receive_requests.size(), 2); +} + +TEST_F(RosBag2PlayTestFixture, + player_with_service_request_from_recorded_client_introspection_message) +{ + auto services_types = std::vector{ + {1u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 300, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), + serialize_test_message( + "test_service1/_service_event", 300, get_service_event_message_basic_types()[2]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + }; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + std::vector> service1_receive_requests; + + srv_->setup_service("test_service1", service1_receive_requests); + srv_->run_services(); + + ASSERT_TRUE(srv_->all_services_ready()); + + play_options_.service_request_from = ServiceRequestFrom::CLIENT_INTROSPECTION; + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(300ms); + EXPECT_EQ(service1_receive_requests.size(), 2); +} + class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index da9a34d40..0cbf743c9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -313,8 +313,8 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { [publisher_volatile, publisher_transient_local]() { // This test is a success if rosbag2 has connected to both publishers return - publisher_volatile->get_subscription_count() && - publisher_transient_local->get_subscription_count(); + publisher_volatile->get_subscription_count() && + publisher_transient_local->get_subscription_count(); }); ASSERT_TRUE(succeeded); } @@ -359,10 +359,10 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe std::chrono::seconds(5), recorder, [publisher_history, publisher_lifespan, publisher_deadline, publisher_liveliness]() { return - publisher_history->get_subscription_count() && - publisher_lifespan->get_subscription_count() && - publisher_deadline->get_subscription_count() && - publisher_liveliness->get_subscription_count(); + publisher_history->get_subscription_count() && + publisher_lifespan->get_subscription_count() && + publisher_deadline->get_subscription_count() && + publisher_liveliness->get_subscription_count(); }); ASSERT_TRUE(succeeded); } From 00bff3e1e2f0810c447b2f5ebe90da0a504fb835 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Mon, 8 Apr 2024 22:45:25 -0700 Subject: [PATCH 05/17] Revert uncrustify changes from previous commit. - Rationale: We are moving to the new version of the uncrustify in rolling and still haven't done it yet fully for the baseline. The discrepancy in style for other untouched files like logging.hpp shall be addressed in a separate PR. Signed-off-by: Michael Orlov --- rosbag2_cpp/include/rosbag2_cpp/logging.hpp | 24 +++++++++---------- .../rosbag2_cpp/test_sequential_writer.cpp | 2 +- .../test/rosbag2_tests/test_reindexer.cpp | 8 +++---- .../src/rosbag2_transport/logging.hpp | 24 +++++++++---------- .../test/rosbag2_transport/test_play.cpp | 6 +++-- .../test/rosbag2_transport/test_record.cpp | 12 +++++----- 6 files changed, 39 insertions(+), 37 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp index 6e8a4d200..38e981f20 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp @@ -26,36 +26,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #endif // ROSBAG2_CPP__LOGGING_HPP_ diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp index 53fea0640..cb27d87ca 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp @@ -374,7 +374,7 @@ TEST_F( write(An> &>())). WillByDefault( [this, &written_messages] - (const std::vector> & msgs) + (const std::vector> & msgs) { written_messages += msgs.size(); fake_storage_size_.fetch_add(static_cast(msgs.size())); diff --git a/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp b/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp index 95465a2e3..4bbc3b9d3 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_reindexer.cpp @@ -150,10 +150,10 @@ TEST_P(ReindexTestFixture, test_multiple_files) { target_metadata.topics_with_message_count.end(), [&gen_topic](rosbag2_storage::TopicInformation & t) { return (t.topic_metadata.name == gen_topic.topic_metadata.name) && - (t.message_count == gen_topic.message_count) && - (t.topic_metadata.offered_qos_profiles == - gen_topic.topic_metadata.offered_qos_profiles) && - (t.topic_metadata.type == gen_topic.topic_metadata.type); + (t.message_count == gen_topic.message_count) && + (t.topic_metadata.offered_qos_profiles == + gen_topic.topic_metadata.offered_qos_profiles) && + (t.topic_metadata.type == gen_topic.topic_metadata.type); } ) != target_metadata.topics_with_message_count.end() ); diff --git a/rosbag2_transport/src/rosbag2_transport/logging.hpp b/rosbag2_transport/src/rosbag2_transport/logging.hpp index 6f7ca5f87..384857b0a 100644 --- a/rosbag2_transport/src/rosbag2_transport/logging.hpp +++ b/rosbag2_transport/src/rosbag2_transport/logging.hpp @@ -26,36 +26,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #endif // ROSBAG2_TRANSPORT__LOGGING_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index b63748808..f8394a0b2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -849,7 +849,8 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus player->wait_for_playback_to_finish(); } -TEST_F(RosBag2PlayTestFixture, +TEST_F( + RosBag2PlayTestFixture, player_with_service_request_from_recorded_service_introspection_message) { auto services_types = std::vector{ @@ -891,7 +892,8 @@ TEST_F(RosBag2PlayTestFixture, EXPECT_EQ(service1_receive_requests.size(), 2); } -TEST_F(RosBag2PlayTestFixture, +TEST_F( + RosBag2PlayTestFixture, player_with_service_request_from_recorded_client_introspection_message) { auto services_types = std::vector{ diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 0cbf743c9..da9a34d40 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -313,8 +313,8 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { [publisher_volatile, publisher_transient_local]() { // This test is a success if rosbag2 has connected to both publishers return - publisher_volatile->get_subscription_count() && - publisher_transient_local->get_subscription_count(); + publisher_volatile->get_subscription_count() && + publisher_transient_local->get_subscription_count(); }); ASSERT_TRUE(succeeded); } @@ -359,10 +359,10 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe std::chrono::seconds(5), recorder, [publisher_history, publisher_lifespan, publisher_deadline, publisher_liveliness]() { return - publisher_history->get_subscription_count() && - publisher_lifespan->get_subscription_count() && - publisher_deadline->get_subscription_count() && - publisher_liveliness->get_subscription_count(); + publisher_history->get_subscription_count() && + publisher_lifespan->get_subscription_count() && + publisher_deadline->get_subscription_count() && + publisher_liveliness->get_subscription_count(); }); ASSERT_TRUE(succeeded); } From 05a3d1d7f2efbf4eb896f9e0f75107c5ba84e768 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 9 Apr 2024 13:27:35 -0700 Subject: [PATCH 06/17] Rename service_request_from to the service_requests_source - Also rename enum class ServiceRequestFrom to the ServiceRequestsSource Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/play.py | 15 ++++++++------- rosbag2_py/rosbag2_py/__init__.py | 4 ++-- rosbag2_py/src/rosbag2_py/_transport.cpp | 8 ++++---- .../include/rosbag2_transport/play_options.hpp | 4 ++-- .../config_options_from_node_params.cpp | 16 ++++++++-------- .../src/rosbag2_transport/player.cpp | 4 ++-- .../test/resources/player_node_params.yaml | 2 +- .../rosbag2_transport/test_composable_player.cpp | 4 ++-- .../test/rosbag2_transport/test_play.cpp | 4 ++-- 9 files changed, 31 insertions(+), 30 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 112862f8d..e92ecb2f9 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -25,7 +25,7 @@ from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import Player from rosbag2_py import PlayOptions -from rosbag2_py import ServiceRequestFrom +from rosbag2_py import ServiceRequestsSource from rosbag2_py import StorageOptions import yaml @@ -154,10 +154,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'message. It can help to reduce the number of data copies, so there is a greater ' 'benefit for sending big data.') parser.add_argument( - '--service-request-from', default='service_introspection', + '--service-requests-source', default='service_introspection', choices=['service_introspection', 'client_introspection'], - help='Determine the source of the service request to be replayed. ' - 'By default, the service request is from recorded service introspection message.') + help='Determine the source of the service requests to be replayed. By default, ' + 'the service requests replaying from recorded service introspection message.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -225,10 +225,11 @@ def main(self, *, args): # noqa: D102 play_options.start_offset = args.start_offset play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message - if not args.service_request_from or args.service_request_from == 'service': - play_options.service_request_from = ServiceRequestFrom.SERVICE_INTROSPECTION + if not args.service_requests_source or \ + args.service_requests_source == 'service_introspection': + play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION else: - play_options.service_request_from = ServiceRequestFrom.CLIENT_INTROSPECTION + play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION player = Player() try: diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py index f11949a14..8338fd104 100644 --- a/rosbag2_py/rosbag2_py/__init__.py +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -56,7 +56,7 @@ from rosbag2_py._transport import ( Player, PlayOptions, - ServiceRequestFrom, + ServiceRequestsSource, Recorder, RecordOptions, bag_rewrite, @@ -95,7 +95,7 @@ 'Info', 'Player', 'PlayOptions', - 'ServiceRequestFrom', + 'ServiceRequestsSource', 'Recorder', 'RecordOptions', ] diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 4e92be4da..301bec874 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -366,12 +366,12 @@ PYBIND11_MODULE(_transport, m) { &PlayOptions::setPlaybackUntilTimestamp) .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) - .def_readwrite("service_request_from", &PlayOptions::service_request_from) + .def_readwrite("service_requests_source", &PlayOptions::service_requests_source) ; - py::enum_(m, "ServiceRequestFrom") - .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::SERVICE_INTROSPECTION) - .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestFrom::CLIENT_INTROSPECTION) + py::enum_(m, "ServiceRequestsSource") + .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::SERVICE_INTROSPECTION) + .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION) ; py::class_(m, "RecordOptions") diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 64909bcf8..cb69ac2a2 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -28,7 +28,7 @@ namespace rosbag2_transport { -enum class ServiceRequestFrom : int +enum class ServiceRequestsSource : int8_t { SERVICE_INTROSPECTION = 0, CLIENT_INTROSPECTION = 1 @@ -119,7 +119,7 @@ struct PlayOptions bool disable_loan_message = false; // The source of the service request - ServiceRequestFrom service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; + ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index dcf9485a8..f64dd1e03 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -219,18 +219,18 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) node.declare_parameter("play.disable_loan_message", false); // "SERVICE_INTROSPECTION" or "CLIENT_INTROSPECTION" - auto service_request_from = - node.declare_parameter("play.service_request_from", "SERVICE_INTROSPECTION"); - if (service_request_from == "SERVICE_INTROSPECTION") { - play_options.service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; - } else if (service_request_from == "CLIENT_INTROSPECTION") { - play_options.service_request_from = ServiceRequestFrom::CLIENT_INTROSPECTION; + auto service_requests_source = + node.declare_parameter("play.service_requests_source", "SERVICE_INTROSPECTION"); + if (service_requests_source == "SERVICE_INTROSPECTION") { + play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + } else if (service_requests_source == "CLIENT_INTROSPECTION") { + play_options.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; } else { RCLCPP_ERROR( node.get_logger(), - "play.service_request_from doesn't support %s. It must be one of SERVICE_INTROSPECTION" + "play.service_requests_source doesn't support %s. It must be one of SERVICE_INTROSPECTION" " and CLIENT_INTROSPECTION. Change it by default value SERVICE_INTROSPECTION.", - service_request_from.c_str()); + service_requests_source.c_str()); } return play_options; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 4a18e8297..eab766f04 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1211,13 +1211,13 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr return false; } - if (play_options_.service_request_from == ServiceRequestFrom::SERVICE_INTROSPECTION && + if (play_options_.service_requests_source == ServiceRequestsSource::SERVICE_INTROSPECTION && service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { return false; } - if (play_options_.service_request_from == ServiceRequestFrom::CLIENT_INTROSPECTION && + if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { return false; diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index ea18dd027..6598e451a 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -37,7 +37,7 @@ player_params_node: sec: 0 nsec: -999999999 disable_loan_message: false - service_request_from: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION + service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION storage: uri: "path/to/some_bag" diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index 4c6e3a0de..d825eb07e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -171,8 +171,8 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.wait_acked_timeout, -999999999); EXPECT_EQ(play_options.disable_loan_message, false); EXPECT_EQ( - play_options.service_request_from, - rosbag2_transport::ServiceRequestFrom::CLIENT_INTROSPECTION); + play_options.service_requests_source, + rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); EXPECT_EQ(storage_options.uri, uri_str); EXPECT_EQ(storage_options.storage_id, GetParam()); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index f8394a0b2..448df0dbf 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -881,7 +881,7 @@ TEST_F( ASSERT_TRUE(srv_->all_services_ready()); - play_options_.service_request_from = ServiceRequestFrom::SERVICE_INTROSPECTION; + play_options_.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; auto player = std::make_shared( std::move( @@ -924,7 +924,7 @@ TEST_F( ASSERT_TRUE(srv_->all_services_ready()); - play_options_.service_request_from = ServiceRequestFrom::CLIENT_INTROSPECTION; + play_options_.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; auto player = std::make_shared( std::move( From 9d98b04e725c7a5e987e930848b9d75d0817d81a Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 01:42:12 -0700 Subject: [PATCH 07/17] Add Player::wait_for_sent_service_requests_to_finish() API - We need this API to be able to write deterministic and non-flaky tests Signed-off-by: Michael Orlov --- .../include/rosbag2_transport/player.hpp | 11 +++++ .../player_service_client.hpp | 9 +++++ .../src/rosbag2_transport/player.cpp | 37 +++++++++++++++++ .../player_service_client.cpp | 40 +++++++++++++++++++ 4 files changed, 97 insertions(+) diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index be623a974..92080087d 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -254,6 +254,17 @@ class Player : public rclcpp::Node ROSBAG2_TRANSPORT_PUBLIC void delete_on_play_message_callback(const callback_handle_t & handle); + /// Wait until sent service requests will receive responses from service servers. + /// \param service_name - Name of the service or service event from what to wait responses. + /// \note is service_name is empty the function will wait until all service requests sent to all + /// service servers will finish. Timeout in this cases will be used for each service name. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + ROSBAG2_TRANSPORT_PUBLIC + bool wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout = std::chrono::seconds(5)); + protected: /// \brief Getter for publishers corresponding to each topic /// \return Hashtable representing topic to publisher map excluding inner clock_publisher diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index 7a57d1e1c..31d8fdebb 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -64,6 +64,13 @@ class PlayerServiceClient final void async_send_request(const std::shared_ptr type_erased_service_event); + /// Wait until sent service requests will receive responses from service servers. + /// \param service_name - Name of the service or service event from what to wait responses. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + bool wait_for_sent_requests_to_finish( + std::chrono::duration timeout = std::chrono::seconds(5)); + void async_send_request(const rcl_serialized_message_t & message); std::shared_ptr generic_client() @@ -103,6 +110,8 @@ class PlayerServiceClientManager final rclcpp::GenericClient::FutureAndRequestId & request_future, std::weak_ptr client); + bool wait_for_all_futures(std::chrono::duration timeout = std::chrono::seconds(5)); + private: using time_point = std::chrono::steady_clock::time_point; using ptr_future_and_request_id = std::unique_ptr; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index eab766f04..211fb5963 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -171,6 +171,14 @@ class PlayerImpl /// #add_on_play_message_post_callback void delete_on_play_message_callback(const callback_handle_t & handle); + /// TBD + /// \param service_name + /// \param timeout + /// \return + bool wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout = std::chrono::seconds(5)); + /// \brief Getter for publishers corresponding to each topic /// \return Hashtable representing topic to publisher map excluding inner clock_publisher std::unordered_map> get_publishers(); @@ -787,6 +795,29 @@ void PlayerImpl::delete_on_play_message_callback(const callback_handle_t & handl }); } +bool PlayerImpl::wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout) +{ + bool is_requests_complete = true; + if (!service_name.empty()) { + auto service_event_name = rosbag2_cpp::service_name_to_service_event_topic_name(service_name); + auto client_iter = service_clients_.find(service_event_name); + if (client_iter != service_clients_.end()) { + is_requests_complete = client_iter->second->wait_for_sent_requests_to_finish(timeout); + } else { + is_requests_complete = false; + } + } else { + for (const auto & [service_event_name, client] : service_clients_) { + if (!client->wait_for_sent_requests_to_finish(timeout)) { + return false; + } + } + } + return is_requests_complete; +} + std::unordered_map> PlayerImpl::get_publishers() { @@ -1630,6 +1661,12 @@ void Player::delete_on_play_message_callback(const Player::callback_handle_t & h pimpl_->delete_on_play_message_callback(handle); } +bool Player::wait_for_sent_service_requests_to_finish( + const std::string & service_name, std::chrono::duration timeout) +{ + return pimpl_->wait_for_sent_service_requests_to_finish(service_name, timeout); +} + std::unordered_map> Player::get_publishers() { return pimpl_->get_publishers(); diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index c8a689838..0afe18064 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -24,6 +24,8 @@ #include "logging.hpp" +using namespace std::chrono_literals; + namespace rosbag2_transport { @@ -168,6 +170,11 @@ void PlayerServiceClient::async_send_request( } // else { /* No service request in the service event. Do nothing, just skip it. */ } } +bool PlayerServiceClient::wait_for_sent_requests_to_finish(std::chrono::duration timeout) +{ + return player_service_client_manager_->wait_for_all_futures(timeout); +} + void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message) { if (!client_->service_is_ready()) { @@ -258,6 +265,39 @@ bool PlayerServiceClientManager::register_request_future( return false; } +bool PlayerServiceClientManager::wait_for_all_futures(std::chrono::duration timeout) +{ + auto is_all_futures_ready = [&]() { + bool is_ready = true; + for (auto & [timestamp, future_request_id_and_client] : request_futures_list_) { + if (!future_request_id_and_client.first->future.valid()) { + std::stringstream ss; + ss << "request's " << future_request_id_and_client.first->request_id << + " future is not valid!\n"; + throw std::runtime_error(ss.str()); + } + if (future_request_id_and_client.first->wait_for(0s) != std::future_status::ready) { + return false; + } + } + return is_ready; + }; + + auto sleep_time = std::chrono::milliseconds(10); + if (timeout < std::chrono::seconds(1)) { + sleep_time = std::chrono::duration_cast(timeout); + } + using clock = std::chrono::system_clock; + auto start = clock::now(); + + std::lock_guard lock(request_futures_list_mutex_); + while (!is_all_futures_ready() && (clock::now() - start) < timeout) { + std::this_thread::sleep_for(sleep_time); + } + + return is_all_futures_ready(); +} + void PlayerServiceClientManager::remove_complete_request_future() { std::vector remove_keys; From b7116b9b3fcdf0fe921a4a927e5a7026929c07e7 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 01:45:49 -0700 Subject: [PATCH 08/17] Mitigate potential issues related to the operations reordering on ARM Signed-off-by: Michael Orlov --- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 60 +++++++++---------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 333cfd001..c689046a2 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -33,33 +33,33 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic { if (topic.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return false; - } - - std::string end_topic_name = topic.substr( - topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); + } else { + std::string end_topic_name = topic.substr( + topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - // Should be "/_service_event" - if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { - return false; + // Should be "/_service_event" + if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { + return false; + } } if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { return false; - } + } else { + // Should include '/srv/' in type + if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + return false; + } - // Should include '/srv/' in type - if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { - return false; - } + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return false; + } - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { - return false; + return topic_type.compare( + topic_type.length() - std::strlen(service_event_topic_type_postfix), + std::strlen(service_event_topic_type_postfix), + service_event_topic_type_postfix) == 0; } - - return topic_type.compare( - topic_type.length() - std::strlen(service_event_topic_type_postfix), - std::strlen(service_event_topic_type_postfix), - service_event_topic_type_postfix) == 0; } std::string service_event_topic_name_to_service_name(const std::string & topic_name) @@ -67,20 +67,20 @@ std::string service_event_topic_name_to_service_name(const std::string & topic_n std::string service_name; if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return service_name; - } + } else { + if (topic_name.substr( + topic_name.length() - + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return service_name; + } + + service_name = topic_name.substr( + 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - if (topic_name.substr( - topic_name.length() - - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != - RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - { return service_name; } - - service_name = topic_name.substr( - 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - - return service_name; } std::string service_event_topic_type_to_service_type(const std::string & topic_type) From b87c944d5933079c2d61213275b915384d0094b2 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 02:12:36 -0700 Subject: [PATCH 09/17] Make tests play_service_requests_from_service(client) deterministic - Get rid of timeout inside tests and use newly added player->wait_for_sent_service_requests_to_finish(service_name) API. Signed-off-by: Michael Orlov --- .../test/rosbag2_transport/test_play.cpp | 118 +++++++++++------- 1 file changed, 74 insertions(+), 44 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 448df0dbf..1c17a73fd 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -58,6 +58,7 @@ get_service_event_message_basic_types() test_msgs::srv::BasicTypes_Request request; request.int32_value = 123; request.int64_value = 456; + request.string_value = "event_type=REQUEST_RECEIVED"; msg->request.emplace_back(request); messages.push_back(msg); } @@ -68,6 +69,7 @@ get_service_event_message_basic_types() test_msgs::srv::BasicTypes_Request request; request.int32_value = 456; request.int64_value = 789; + request.string_value = "event_type=REQUEST_RECEIVED"; msg->request.emplace_back(request); messages.push_back(msg); } @@ -78,6 +80,7 @@ get_service_event_message_basic_types() test_msgs::srv::BasicTypes_Request request; request.int32_value = 789; request.int64_value = 123; + request.string_value = "event_type=REQUEST_SENT"; msg->request.emplace_back(request); messages.push_back(msg); } @@ -849,90 +852,117 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus player->wait_for_playback_to_finish(); } -TEST_F( - RosBag2PlayTestFixture, - player_with_service_request_from_recorded_service_introspection_message) +TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_messages) { + const std::string service_name = "test_service1"; + const std::string service_event_name = service_name + "/_service_event"; auto services_types = std::vector{ - {1u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), - serialize_test_message( - "test_service1/_service_event", 300, get_service_event_message_basic_types()[0]), - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), - serialize_test_message( - "test_service1/_service_event", 300, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 5, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 10, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 20, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 25, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 30, get_service_event_message_basic_types()[2]), }; auto prepared_mock_reader = std::make_unique(); prepared_mock_reader->prepare(messages, services_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); - std::vector> service1_receive_requests; + std::vector> received_service_requests; - srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service(service_name, received_service_requests); srv_->run_services(); - ASSERT_TRUE(srv_->all_services_ready()); play_options_.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; - auto player = std::make_shared( - std::move( - reader), storage_options_, play_options_); + auto player = + std::make_shared(std::move(reader), storage_options_, play_options_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + // We need to spin Player to get responses from services and able to wait for requests to finish + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + player->play(); + player->wait_for_playback_to_finish(); - std::this_thread::sleep_for(300ms); - EXPECT_EQ(service1_receive_requests.size(), 2); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name)); + exec.cancel(); + spin_thread.join(); + EXPECT_EQ(received_service_requests.size(), 2); + // expected_request is ServiceEventInfo::REQUEST_RECEIVED + const auto expected_request = get_service_event_message_basic_types()[0]->request[0]; + for (const auto & service_request : received_service_requests) { + EXPECT_EQ(service_request->int32_value, expected_request.int32_value) << + service_request->string_value; + EXPECT_EQ(service_request->int64_value, expected_request.int64_value) << + service_request->string_value; + } } -TEST_F( - RosBag2PlayTestFixture, - player_with_service_request_from_recorded_client_introspection_message) +TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_messages) { + const std::string service_name = "test_service1"; + const std::string service_event_name = service_name + "/_service_event"; auto services_types = std::vector{ - {1u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { - serialize_test_message( - "test_service1/_service_event", 300, get_service_event_message_basic_types()[0]), - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[2]), - serialize_test_message( - "test_service1/_service_event", 300, get_service_event_message_basic_types()[2]), - serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name, 5, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 10, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name, 20, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 25, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 30, get_service_event_message_basic_types()[1]), }; auto prepared_mock_reader = std::make_unique(); prepared_mock_reader->prepare(messages, services_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); - std::vector> service1_receive_requests; + std::vector> received_service_requests; - srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service(service_name, received_service_requests); srv_->run_services(); - ASSERT_TRUE(srv_->all_services_ready()); play_options_.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; - auto player = std::make_shared( - std::move( - reader), storage_options_, play_options_); + auto player = + std::make_shared(std::move(reader), storage_options_, play_options_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + // We need to spin Player to get responses from services and able to wait for requests to finish + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + player->play(); + player->wait_for_playback_to_finish(); - std::this_thread::sleep_for(300ms); - EXPECT_EQ(service1_receive_requests.size(), 2); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name)); + exec.cancel(); + spin_thread.join(); + + EXPECT_EQ(received_service_requests.size(), 2); + // expected_request is ServiceEventInfo::REQUEST_SENT + const auto expected_request = get_service_event_message_basic_types()[2]->request[0]; + for (const auto & service_request : received_service_requests) { + EXPECT_EQ(service_request->int32_value, expected_request.int32_value) << + service_request->string_value; + EXPECT_EQ(service_request->int64_value, expected_request.int64_value) << + service_request->string_value; + } } class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture From bede8542d28ae802ba7489d6e108e2c64170761d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 02:18:10 -0700 Subject: [PATCH 10/17] Misc findings and improvements 1 Signed-off-by: Michael Orlov --- .../include/rosbag2_transport/player.hpp | 2 ++ .../player_service_client.hpp | 6 ++--- .../config_options_from_node_params.cpp | 3 ++- .../src/rosbag2_transport/player.cpp | 13 +++++----- .../player_service_client.cpp | 24 +++++++++---------- 5 files changed, 25 insertions(+), 23 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 92080087d..1ff2ef64b 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -255,6 +255,8 @@ class Player : public rclcpp::Node void delete_on_play_message_callback(const callback_handle_t & handle); /// Wait until sent service requests will receive responses from service servers. + /// \note The player node shall be spun in the executor in a parallel thread to be able to wait + /// for responses. /// \param service_name - Name of the service or service event from what to wait responses. /// \note is service_name is empty the function will wait until all service requests sent to all /// service servers will finish. Timeout in this cases will be used for each service name. diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index 31d8fdebb..77806fc2a 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -107,7 +107,7 @@ class PlayerServiceClientManager final bool request_future_queue_is_full(); bool register_request_future( - rclcpp::GenericClient::FutureAndRequestId & request_future, + rclcpp::GenericClient::FutureAndRequestId && request_future, std::weak_ptr client); bool wait_for_all_futures(std::chrono::duration timeout = std::chrono::seconds(5)); @@ -115,9 +115,9 @@ class PlayerServiceClientManager final private: using time_point = std::chrono::steady_clock::time_point; using ptr_future_and_request_id = std::unique_ptr; - using request_id_and_client_t = + using future_request_id_and_client_t = std::pair>; - std::map request_futures_list_; + std::map request_futures_list_; std::mutex request_futures_list_mutex_; std::chrono::seconds request_future_timeout_; diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index f64dd1e03..f2054efb6 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -226,10 +226,11 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) } else if (service_requests_source == "CLIENT_INTROSPECTION") { play_options.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; } else { + play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; RCLCPP_ERROR( node.get_logger(), "play.service_requests_source doesn't support %s. It must be one of SERVICE_INTROSPECTION" - " and CLIENT_INTROSPECTION. Change it by default value SERVICE_INTROSPECTION.", + " and CLIENT_INTROSPECTION. Changed it to default value SERVICE_INTROSPECTION.", service_requests_source.c_str()); } diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 211fb5963..45bd79ad4 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -965,7 +965,7 @@ namespace { bool allow_topic( bool is_service, - const std::string topic_name, + const std::string & topic_name, const rosbag2_storage::StorageFilter & storage_filter) { auto & include_topics = storage_filter.topics; @@ -1103,11 +1103,11 @@ void PlayerImpl::prepare_publishers() auto service_name = rosbag2_cpp::service_event_topic_name_to_service_name(topic.name); auto service_type = rosbag2_cpp::service_event_topic_type_to_service_type(topic.type); try { - auto cli = owner_->create_generic_client(service_name, service_type); - auto player_cli = std::make_shared( - std::move(cli), service_name, topic.type, owner_->get_logger(), + auto generic_client = owner_->create_generic_client(service_name, service_type); + auto player_client = std::make_shared( + std::move(generic_client), service_name, topic.type, owner_->get_logger(), player_service_client_manager_); - service_clients_.insert(std::make_pair(topic.name, player_cli)); + service_clients_.insert(std::make_pair(topic.name, player_client)); } catch (const std::runtime_error & e) { RCLCPP_WARN( owner_->get_logger(), @@ -1238,7 +1238,6 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) { - // TODO(morlov): Shall we ignore REQUEST_RECEIVED as well? return false; } @@ -1284,7 +1283,7 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr bool message_published = false; try { - client_iter->second->async_send_request(service_event); + service_client->async_send_request(service_event); message_published = true; } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index 0afe18064..bbf150dd1 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -166,7 +166,8 @@ void PlayerServiceClient::async_send_request( if (request_member.size_function(request_sequence_ptr) > 0) { void * request_ptr = request_member.get_function(request_sequence_ptr, 0); auto future_and_request_id = client_->async_send_request(request_ptr); - player_service_client_manager_->register_request_future(future_and_request_id, client_); + player_service_client_manager_->register_request_future( + std::move(future_and_request_id), client_); } // else { /* No service request in the service event. Do nothing, just skip it. */ } } @@ -245,7 +246,7 @@ bool PlayerServiceClientManager::request_future_queue_is_full() } bool PlayerServiceClientManager::register_request_future( - rclcpp::GenericClient::FutureAndRequestId & request_future, + rclcpp::GenericClient::FutureAndRequestId && request_future, std::weak_ptr client) { auto future_and_request_id = @@ -253,15 +254,14 @@ bool PlayerServiceClientManager::register_request_future( if (!request_future_queue_is_full()) { std::lock_guard lock(request_futures_list_mutex_); - request_futures_list_[std::chrono::steady_clock::now()] = - {std::move(future_and_request_id), client}; - return true; + auto emplace_result = request_futures_list_.emplace( + std::chrono::steady_clock::now(), std::make_pair(std::move(future_and_request_id), client)); + return emplace_result.second; } else { ROSBAG2_TRANSPORT_LOG_WARN( "Client request queue is full. " "Please consider increasing the length of the queue."); } - return false; } @@ -301,13 +301,12 @@ bool PlayerServiceClientManager::wait_for_all_futures(std::chrono::duration remove_keys; - for (auto & [timestamp, request_id_and_client] : request_futures_list_) { - if (request_id_and_client.first->wait_for(std::chrono::seconds(0)) == - std::future_status::ready) - { - auto client = request_id_and_client.second.lock(); + std::lock_guard lock(request_futures_list_mutex_); + for (auto & [timestamp, future_request_id_and_client] : request_futures_list_) { + if (future_request_id_and_client.first->wait_for(0s) == std::future_status::ready) { + auto client = future_request_id_and_client.second.lock(); if (client) { - client->remove_pending_request(request_id_and_client.first->request_id); + client->remove_pending_request(future_request_id_and_client.first->request_id); } remove_keys.emplace_back(timestamp); } @@ -319,6 +318,7 @@ void PlayerServiceClientManager::remove_complete_request_future() void PlayerServiceClientManager::remove_all_timeout_request_future() { + std::lock_guard lock(request_futures_list_mutex_); auto current_time = std::chrono::steady_clock::now(); auto first_iter_without_timeout = request_futures_list_.lower_bound(current_time - request_future_timeout_); From c5f6c132f4df891880215fde249bc63ec39ecea9 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 10:50:44 -0700 Subject: [PATCH 11/17] Rename get_services_clients() to the get_service_clients() Signed-off-by: Michael Orlov --- rosbag2_transport/include/rosbag2_transport/player.hpp | 2 +- rosbag2_transport/src/rosbag2_transport/player.cpp | 8 ++++---- rosbag2_transport/test/rosbag2_transport/mock_player.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 1ff2ef64b..d83bb69d2 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -276,7 +276,7 @@ class Player : public rclcpp::Node /// \brief Getter for clients corresponding to each service name /// \return Hashtable representing service name to client ROSBAG2_TRANSPORT_PUBLIC - std::unordered_map> get_services_clients(); + std::unordered_map> get_service_clients(); /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 45bd79ad4..02bff1145 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -185,7 +185,7 @@ class PlayerImpl /// \brief Getter for clients corresponding to services /// \return Hashtable representing service name to client map - std::unordered_map> get_services_clients(); + std::unordered_map> get_service_clients(); /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher @@ -829,7 +829,7 @@ std::unordered_map> PlayerImpl::get_services_clients() + std::shared_ptr> PlayerImpl::get_service_clients() { std::unordered_map> topic_to_client_map; for (const auto & [service_name, client] : service_clients_) { @@ -1672,9 +1672,9 @@ std::unordered_map> Playe } std::unordered_map> Player::get_services_clients() + std::shared_ptr> Player::get_service_clients() { - return pimpl_->get_services_clients(); + return pimpl_->get_service_clients(); } rclcpp::Publisher::SharedPtr Player::get_clock_publisher() diff --git a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp index bf575cace..9220679b0 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp @@ -59,7 +59,7 @@ class MockPlayer : public rosbag2_transport::Player std::vector get_list_of_clients() { std::vector cli_list; - for (const auto & client : this->get_services_clients()) { + for (const auto & client : this->get_service_clients()) { cli_list.push_back( static_cast( client.second.get())); From a623a23d3c4cf626b36419a574eab210e49918e2 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sun, 7 Apr 2024 23:54:09 -0700 Subject: [PATCH 12/17] Add a new CLI parameter "--publish-service-requests" for Player Also added a new option publish_service_requests to the PlayOptions. Note: By default rosbag2 player will publish service events only. Signed-off-by: Barry Xu Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/play.py | 9 ++- rosbag2_py/src/rosbag2_py/_transport.cpp | 1 + .../rosbag2_transport/play_options.hpp | 3 + .../config_options_from_node_params.cpp | 3 + .../src/rosbag2_transport/player.cpp | 11 +++- .../test/resources/player_node_params.yaml | 1 + .../test/rosbag2_transport/test_burst.cpp | 1 + .../test_composable_player.cpp | 1 + .../test/rosbag2_transport/test_play.cpp | 64 +++++++++++++++++++ 9 files changed, 91 insertions(+), 3 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index e92ecb2f9..fcf866f12 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -153,11 +153,15 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'By default, if loaned message can be used, messages are published as loaned ' 'message. It can help to reduce the number of data copies, so there is a greater ' 'benefit for sending big data.') + parser.add_argument( + '--publish-service-requests', action='store_true', default=False, + help='Publish recorded service requests instead of recorded service events') parser.add_argument( '--service-requests-source', default='service_introspection', choices=['service_introspection', 'client_introspection'], - help='Determine the source of the service requests to be replayed. By default, ' - 'the service requests replaying from recorded service introspection message.') + help='Determine the source of the service requests to be replayed. This option only ' + 'makes sense if the "--publish-service-requests" option is set. By default,' + ' the service requests replaying from recorded service introspection message.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -225,6 +229,7 @@ def main(self, *, args): # noqa: D102 play_options.start_offset = args.start_offset play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message + play_options.publish_service_requests = args.publish_service_requests if not args.service_requests_source or \ args.service_requests_source == 'service_introspection': play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 301bec874..68e6ca268 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -366,6 +366,7 @@ PYBIND11_MODULE(_transport, m) { &PlayOptions::setPlaybackUntilTimestamp) .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) + .def_readwrite("publish_service_requests", &PlayOptions::publish_service_requests) .def_readwrite("service_requests_source", &PlayOptions::service_requests_source) ; diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index cb69ac2a2..e5029c923 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -118,6 +118,9 @@ struct PlayOptions // Disable to publish as loaned message bool disable_loan_message = false; + // Publish service requests instead of service events + bool publish_service_requests = false; + // The source of the service request ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; }; diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index f2054efb6..ea4329c67 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -234,6 +234,9 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) service_requests_source.c_str()); } + play_options.publish_service_requests = + node.declare_parameter("play.publish_service_request", false); + return play_options; } diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 02bff1145..3b78f8d0b 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1089,7 +1089,9 @@ void PlayerImpl::prepare_publishers() auto topics = reader_->get_all_topics_and_types(); std::string topic_without_support_acked; for (const auto & topic : topics) { - if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) { + if (play_options_.publish_service_requests && + rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) + { // Check if sender was created if (service_clients_.find(topic.name) != service_clients_.end()) { continue; @@ -1124,6 +1126,13 @@ void PlayerImpl::prepare_publishers() continue; } + // if not set publish_service_requests, filter service event topic + if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type) && + !allow_topic(true, topic.name, storage_filter)) + { + continue; + } + auto topic_qos = publisher_qos_for_topic( topic, topic_qos_profile_overrides_, owner_->get_logger()); diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index 6598e451a..79e1b47a9 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -37,6 +37,7 @@ player_params_node: sec: 0 nsec: -999999999 disable_loan_message: false + publish_service_requests: false service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION storage: diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 8c8dae623..395ee5f59 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -427,6 +427,7 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_services) { // Filter allows test_service2, blocks test_service1 play_options_.services_to_filter = {"test_service2/_service_event"}; + play_options_.publish_service_requests = true; auto prepared_mock_reader = std::make_unique(); prepared_mock_reader->prepare(messages, services_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index d825eb07e..d400d1fed 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -170,6 +170,7 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.start_offset, 999999999); EXPECT_EQ(play_options.wait_acked_timeout, -999999999); EXPECT_EQ(play_options.disable_loan_message, false); + EXPECT_EQ(play_options.publish_service_requests, false); EXPECT_EQ( play_options.service_requests_source, rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 1c17a73fd..f2ae7710f 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -186,6 +186,7 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) ASSERT_TRUE(srv_->all_services_ready()); + play_options_.publish_service_requests = true; auto player = std::make_shared( std::move( reader), storage_options_, play_options_); @@ -229,6 +230,7 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_topics_and_servi sub_->add_subscription("/topic1", 2); auto await_received_messages = sub_->spin_subscriptions(); + play_options_.publish_service_requests = true; auto player = std::make_shared( std::move( reader), storage_options_, play_options_); @@ -466,6 +468,7 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service prepared_mock_reader->prepare(messages, services_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); + play_options_.publish_service_requests = true; // Filter allows /test_service2, blocks /test_service1 { play_options_.services_to_filter = {"test_service2/_service_event"}; @@ -575,6 +578,7 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ get_service_event_message_basic_types()[0]), }; + play_options_.publish_service_requests = true; // Filter allows all topics, blocks service test_service2 { play_options_.topics_to_filter = {"topic1", "topic2"}; @@ -878,6 +882,7 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_ srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); + play_options_.publish_service_requests = true; play_options_.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; auto player = @@ -934,6 +939,7 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_m srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); + play_options_.publish_service_requests = true; play_options_.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; auto player = @@ -965,6 +971,64 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_m } } +TEST_F(RosBag2PlayTestFixture, play_without_publish_service_requests) +{ + auto all_types = std::vector{ + {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, + {2u, "/topic2", "test_msgs/BasicTypes", "", {}, ""}, + {3u, "/test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {4u, "/test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + + std::vector> messages = + { + serialize_test_message("/topic1", 500, get_messages_basic_types()[0]), + serialize_test_message( + "/test_service1/_service_event", + 520, + get_service_event_message_basic_types()[0]), + serialize_test_message("/topic2", 520, get_messages_basic_types()[0]), + serialize_test_message( + "/test_service2/_service_event", + 550, + get_service_event_message_basic_types()[0]), + }; + + play_options_.publish_service_requests = false; + + sub_ = std::make_shared(); + sub_->add_subscription("/topic1", 1); + sub_->add_subscription("/topic2", 1); + sub_->add_subscription("/test_service1/_service_event", 1); + sub_->add_subscription("/test_service2/_service_event", 1); + + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, all_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto await_received_messages = sub_->spin_subscriptions(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + await_received_messages.get(); + std::this_thread::sleep_for(200ms); + + auto replayed_topic1 = sub_->get_received_messages("/topic1"); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages("/topic2"); + EXPECT_THAT(replayed_topic2, SizeIs(1u)); + auto replayed_service_event_1 = + sub_->get_received_messages("/test_service1/_service_event"); + EXPECT_THAT(replayed_service_event_1, SizeIs(1u)); + auto replayed_service_event_2 = + sub_->get_received_messages("/test_service2/_service_event"); + EXPECT_THAT(replayed_service_event_2, SizeIs(1u)); +} + class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: From 091620cb2271a36b2b5f08c478b996c1278754f4 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 11:21:14 -0700 Subject: [PATCH 13/17] Fix an issue on filtering topic when prepare publishers Co-authored-by: Barry Xu Signed-off-by: Michael Orlov --- rosbag2_transport/src/rosbag2_transport/player.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 3b78f8d0b..785569580 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1089,9 +1089,8 @@ void PlayerImpl::prepare_publishers() auto topics = reader_->get_all_topics_and_types(); std::string topic_without_support_acked; for (const auto & topic : topics) { - if (play_options_.publish_service_requests && - rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) - { + const bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic.name, topic.type); + if (play_options_.publish_service_requests && is_service_event_topic) { // Check if sender was created if (service_clients_.find(topic.name) != service_clients_.end()) { continue; @@ -1122,14 +1121,7 @@ void PlayerImpl::prepare_publishers() } // filter topics to add publishers if necessary - if (!allow_topic(false, topic.name, storage_filter)) { - continue; - } - - // if not set publish_service_requests, filter service event topic - if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type) && - !allow_topic(true, topic.name, storage_filter)) - { + if (!allow_topic(is_service_event_topic, topic.name, storage_filter)) { continue; } From 3c1987394f127fead4492499945e573bbddd3af0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 10 Apr 2024 11:47:20 -0700 Subject: [PATCH 14/17] Cleanup in play_without_publish_service_requests - Long story short: Make it deterministic and run fast. Signed-off-by: Michael Orlov --- .../test/rosbag2_transport/test_play.cpp | 57 ++++++++++--------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index f2ae7710f..ff2f52d7e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -971,61 +971,62 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_m } } -TEST_F(RosBag2PlayTestFixture, play_without_publish_service_requests) +TEST_F(RosBag2PlayTestFixture, play_service_events_and_topics) { + const std::string topic_1_name = "/topic1"; + const std::string topic_2_name = "/topic2"; + const std::string service_event_1_name = "/test_service1/_service_event"; + const std::string service_event_2_name = "/test_service2/_service_event"; + auto all_types = std::vector{ - {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "/topic2", "test_msgs/BasicTypes", "", {}, ""}, - {3u, "/test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, - {4u, "/test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, topic_1_name, "test_msgs/BasicTypes", "", {}, ""}, + {2u, topic_2_name, "test_msgs/BasicTypes", "", {}, ""}, + {3u, service_event_1_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {4u, service_event_2_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; + auto request_received_service_event = get_service_event_message_basic_types()[0]; std::vector> messages = { - serialize_test_message("/topic1", 500, get_messages_basic_types()[0]), - serialize_test_message( - "/test_service1/_service_event", - 520, - get_service_event_message_basic_types()[0]), - serialize_test_message("/topic2", 520, get_messages_basic_types()[0]), - serialize_test_message( - "/test_service2/_service_event", - 550, - get_service_event_message_basic_types()[0]), + serialize_test_message(topic_1_name, 10, get_messages_basic_types()[0]), + serialize_test_message(service_event_1_name, 20, request_received_service_event), + serialize_test_message(topic_2_name, 30, get_messages_basic_types()[0]), + serialize_test_message(service_event_2_name, 40, request_received_service_event), }; play_options_.publish_service_requests = false; sub_ = std::make_shared(); - sub_->add_subscription("/topic1", 1); - sub_->add_subscription("/topic2", 1); - sub_->add_subscription("/test_service1/_service_event", 1); - sub_->add_subscription("/test_service2/_service_event", 1); - + sub_->add_subscription(topic_1_name, 1); + sub_->add_subscription(topic_2_name, 1); + sub_->add_subscription(service_event_1_name, 1); + sub_->add_subscription(service_event_2_name, 1); auto prepared_mock_reader = std::make_unique(); prepared_mock_reader->prepare(messages, all_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + // Wait for discovery to match publishers with subscribers + ASSERT_TRUE( + sub_->spin_and_wait_for_matched(player->get_list_of_publishers(), std::chrono::seconds(30))); auto await_received_messages = sub_->spin_subscriptions(); - auto player = std::make_shared( - std::move( - reader), storage_options_, play_options_); player->play(); + player->wait_for_playback_to_finish(); await_received_messages.get(); - std::this_thread::sleep_for(200ms); - auto replayed_topic1 = sub_->get_received_messages("/topic1"); + auto replayed_topic1 = sub_->get_received_messages(topic_1_name); EXPECT_THAT(replayed_topic1, SizeIs(1u)); - auto replayed_topic2 = sub_->get_received_messages("/topic2"); + auto replayed_topic2 = sub_->get_received_messages(topic_2_name); EXPECT_THAT(replayed_topic2, SizeIs(1u)); auto replayed_service_event_1 = - sub_->get_received_messages("/test_service1/_service_event"); + sub_->get_received_messages(service_event_1_name); EXPECT_THAT(replayed_service_event_1, SizeIs(1u)); auto replayed_service_event_2 = - sub_->get_received_messages("/test_service2/_service_event"); + sub_->get_received_messages(service_event_2_name); EXPECT_THAT(replayed_service_event_2, SizeIs(1u)); } From 98c14ef9dde85684e72588fc1c2fa032dbe222c0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 11 Apr 2024 01:12:23 -0700 Subject: [PATCH 15/17] Wrap code which can throw with try-catch in the publish_message(..) Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player.cpp | 88 ++++++++++--------- 1 file changed, 47 insertions(+), 41 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 785569580..56ed51852 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1221,10 +1221,8 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr // Try to publish message as service request auto client_iter = service_clients_.find(message->topic_name); - if (client_iter != service_clients_.end()) { + if (play_options_.publish_service_requests && client_iter != service_clients_.end()) { const auto & service_client = client_iter->second; - // TODO(morlov): - // Wrap deserialize_service_event and get_service_event_type_and_client_gid in try-catch auto service_event = service_client->deserialize_service_event(*message->serialized_data); if (!service_event) { RCLCPP_ERROR_STREAM( @@ -1233,49 +1231,57 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr return false; } - auto [service_event_type, client_gid] = - service_client->get_service_event_type_and_client_gid(service_event); - // Ignore response message - if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || - service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) - { - return false; - } - - if (play_options_.service_requests_source == ServiceRequestsSource::SERVICE_INTROSPECTION && - service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) - { - return false; - } + try { + auto [service_event_type, client_gid] = + service_client->get_service_event_type_and_client_gid(service_event); + // Ignore response message + if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || + service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) + { + return false; + } - if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && - service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) - { - return false; - } + if (play_options_.service_requests_source == ServiceRequestsSource::SERVICE_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return false; + } - if (!service_client->generic_client()->service_is_ready()) { - RCLCPP_ERROR( - owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", - service_client->get_service_name().c_str()); - return false; - } + if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) + { + return false; + } - if (!service_client->is_service_event_include_request_message(service_event)) { - if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "Can't send service request. " - "The configuration of introspection for '%s' was metadata only on service side!", + if (!service_client->generic_client()->service_is_ready()) { + RCLCPP_ERROR( + owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", service_client->get_service_name().c_str()); - } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { - RCUTILS_LOG_WARN_ONCE_NAMED( - ROSBAG2_TRANSPORT_PACKAGE_NAME, - "Can't send service request. " - "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", - service_client->get_service_name().c_str(), - rosbag2_cpp::client_id_to_string(client_gid).c_str()); + return false; + } + + if (!service_client->is_service_event_include_request_message(service_event)) { + if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' was metadata only on service side!", + service_client->get_service_name().c_str()); + } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", + service_client->get_service_name().c_str(), + rosbag2_cpp::client_id_to_string(client_gid).c_str()); + } + return false; } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send request on '" << + rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << + "' service. \nError: " << e.what()); return false; } From f270ac1f9dac7e2f2fbd7a0abdfbecbe9c32e382 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 11 Apr 2024 02:07:14 -0700 Subject: [PATCH 16/17] Delete some part of the code which became absolute and shall not be used Signed-off-by: Michael Orlov --- .../include/rosbag2_cpp/service_utils.hpp | 10 +- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 108 +++++------------- .../test/rosbag2_cpp/test_service_utils.cpp | 15 --- .../player_service_client.hpp | 5 - .../player_service_client.cpp | 41 ------- 5 files changed, 28 insertions(+), 151 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index d68148d37..11b9f593a 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -26,7 +26,7 @@ namespace rosbag2_cpp { ROSBAG2_CPP_PUBLIC bool -is_service_event_topic(const std::string & topic, const std::string & topic_type); +is_service_event_topic(const std::string & topic_name, const std::string & topic_type); // Call this function after is_service_event_topic() return true ROSBAG2_CPP_PUBLIC @@ -38,18 +38,10 @@ ROSBAG2_CPP_PUBLIC std::string service_event_topic_type_to_service_type(const std::string & topic_type); -ROSBAG2_CPP_PUBLIC -size_t -get_serialization_size_for_service_metadata_event(); - ROSBAG2_CPP_PUBLIC std::string service_name_to_service_event_topic_name(const std::string & service_name); -ROSBAG2_CPP_PUBLIC -bool -service_event_include_metadata_and_contents(size_t message_size); - ROSBAG2_CPP_PUBLIC std::string client_id_to_string(std::array & client_id); diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index c689046a2..07df54a88 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -17,68 +17,60 @@ #include "rcl/service_introspection.h" #include "rosbag2_cpp/service_utils.hpp" -#include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rosidl_typesupport_cpp/message_type_support_dispatch.hpp" -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" - #include "service_msgs/msg/service_event_info.hpp" namespace rosbag2_cpp { -const char * service_event_topic_type_postfix = "_Event"; -const char * service_event_topic_type_middle = "/srv/"; +const char * kServiceEventTopicTypePostfix = "_Event"; +const char * kServiceEventTopicTypeMiddle = "/srv/"; +const size_t kServiceEventTopicPostfixLen = strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); +const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix); -bool is_service_event_topic(const std::string & topic, const std::string & topic_type) +bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type) { - if (topic.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { + if (topic_name.length() <= kServiceEventTopicPostfixLen) { return false; } else { - std::string end_topic_name = topic.substr( - topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - - // Should be "/_service_event" - if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { + // The end of the topic name should be "/_service_event" + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { return false; } } - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + if (topic_type.length() <= kServiceEventTypePostfixLen) { return false; } else { // Should include '/srv/' in type - if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) { return false; } - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + if (topic_type.length() <= kServiceEventTypePostfixLen) { return false; } return topic_type.compare( - topic_type.length() - std::strlen(service_event_topic_type_postfix), - std::strlen(service_event_topic_type_postfix), - service_event_topic_type_postfix) == 0; + topic_type.length() - kServiceEventTypePostfixLen, + kServiceEventTypePostfixLen, + kServiceEventTopicTypePostfix) == 0; } } std::string service_event_topic_name_to_service_name(const std::string & topic_name) { std::string service_name; - if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { + if (topic_name.length() <= kServiceEventTopicPostfixLen) { return service_name; } else { - if (topic_name.substr( - topic_name.length() - - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != + // The end of the topic name should be "/_service_event" + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { return service_name; } - - service_name = topic_name.substr( - 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - + service_name = topic_name.substr(0, topic_name.length() - kServiceEventTopicPostfixLen); return service_name; } } @@ -86,61 +78,24 @@ std::string service_event_topic_name_to_service_name(const std::string & topic_n std::string service_event_topic_type_to_service_type(const std::string & topic_type) { std::string service_type; - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + if (topic_type.length() <= kServiceEventTypePostfixLen) { return service_type; } - // Should include '/srv/' in type - if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) { return service_type; } - if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != - service_event_topic_type_postfix) + if (topic_type.substr(topic_type.length() - kServiceEventTypePostfixLen) != + kServiceEventTopicTypePostfix) { return service_type; } - - service_type = topic_type.substr( - 0, topic_type.length() - strlen(service_event_topic_type_postfix)); + service_type = topic_type.substr(0, topic_type.length() - kServiceEventTypePostfixLen); return service_type; } -size_t get_serialization_size_for_service_metadata_event() -{ - // Since the size is fixed, it only needs to be calculated once. - static size_t size = 0; - - if (size != 0) { - return size; - } - - const rosidl_message_type_support_t * type_support_info = - rosidl_typesupport_cpp::get_message_type_support_handle(); - - // Get the serialized size of service event info - const rosidl_message_type_support_t * type_support_handle = - rosidl_typesupport_cpp::get_message_typesupport_handle_function( - type_support_info, - rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support_handle == nullptr) { - throw std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); - } - - auto service_event_info = - static_cast( - type_support_handle->data); - - // TODO(morlov): We shall not rely on this arithmetic!!! It is up to the serialization - // implementation - // endian type (4 size) + service event info size + empty request (4 bytes) - // + emtpy response (4 bytes) - size = 4 + service_event_info->size_of_ + 4 + 4; - - return size; -} - std::string service_name_to_service_event_topic_name(const std::string & service_name) { if (service_name.empty()) { @@ -148,24 +103,15 @@ std::string service_name_to_service_event_topic_name(const std::string & service } // If the end of string is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX, do nothing - if ((service_name.length() > strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) && - (service_name.substr(service_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) == + if ((service_name.length() > kServiceEventTopicPostfixLen) && + (service_name.substr(service_name.length() - kServiceEventTopicPostfixLen) == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return service_name; } - return service_name + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; } -bool service_event_include_metadata_and_contents(size_t message_size) -{ - if (message_size <= rosbag2_cpp::get_serialization_size_for_service_metadata_event()) { - return false; - } - return true; -} - std::string client_id_to_string(std::array & client_id) { // output format: diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp index b034d8b62..6434a5ed5 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -97,21 +97,6 @@ TEST_F(ServiceUtilsTest, check_service_name_to_service_event_topic_name) } } -TEST_F(ServiceUtilsTest, check_service_event_include_metadata_and_contents) -{ - auto msg = std::make_shared(); - msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; - auto serialized_service_event = memory_management_.serialize_message(msg); - - size_t metadata_event_size = rosbag2_cpp::get_serialization_size_for_service_metadata_event(); - - EXPECT_EQ(serialized_service_event->buffer_length, metadata_event_size); - - EXPECT_FALSE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size - 1)); - EXPECT_FALSE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size)); - EXPECT_TRUE(rosbag2_cpp::service_event_include_metadata_and_contents(metadata_event_size + 1)); -} - TEST_F(ServiceUtilsTest, check_client_id_to_string) { service_msgs::msg::ServiceEventInfo::_client_gid_type client_id = { diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp index 77806fc2a..aead1e268 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -71,8 +71,6 @@ class PlayerServiceClient final bool wait_for_sent_requests_to_finish( std::chrono::duration timeout = std::chrono::seconds(5)); - void async_send_request(const rcl_serialized_message_t & message); - std::shared_ptr generic_client() { return client_; @@ -91,9 +89,6 @@ class PlayerServiceClient final const rosidl_typesupport_introspection_cpp::MessageMembers * service_event_members_; rcutils_allocator_t allocator_ = rcutils_get_default_allocator(); - - std::tuple - get_msg_event_type(const rcl_serialized_message_t & message); }; class PlayerServiceClientManager final diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp index bbf150dd1..fa82c4a97 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -176,47 +176,6 @@ bool PlayerServiceClient::wait_for_sent_requests_to_finish(std::chrono::duration return player_service_client_manager_->wait_for_all_futures(timeout); } -void PlayerServiceClient::async_send_request(const rcl_serialized_message_t & message) -{ - if (!client_->service_is_ready()) { - RCLCPP_ERROR( - logger_, "Service request hasn't been sent. The '%s' service isn't ready !", - service_name_.c_str()); - return; - } - - auto type_erased_ros_message = deserialize_service_event(message); - - if (type_erased_ros_message) { - async_send_request(type_erased_ros_message); - } else { - throw std::runtime_error( - "Failed to deserialize service event message for " + service_name_ + " !"); - } -} - -std::tuple -PlayerServiceClient::get_msg_event_type(const rcl_serialized_message_t & message) -{ - auto msg = service_msgs::msg::ServiceEventInfo(); - - const rosidl_message_type_support_t * type_support_info = - rosidl_typesupport_cpp::get_message_type_support_handle(); - if (type_support_info == nullptr) { - throw std::runtime_error("Failed to get message type support handle of service event info !"); - } - - // Partially deserialize service event message. Deserializing only first member ServiceEventInfo - // with assumption that it is going to be the first in serialized message. - // TODO(morlov): We can't rely on this assumption. It is up to the underlying RMW and - // serialization format implementation! - if (rmw_deserialize(&message, type_support_info, reinterpret_cast(&msg)) != RMW_RET_OK) { - throw std::runtime_error("Failed to deserialize message !"); - } - - return {msg.event_type, msg.client_gid, msg.sequence_number}; -} - PlayerServiceClientManager::PlayerServiceClientManager( std::chrono::seconds requst_future_timeout, size_t maximum_request_future_queue) From 974d687299df1b967ebf9272406d72b6c8581246 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 11 Apr 2024 13:24:34 +0800 Subject: [PATCH 17/17] Update test codes Signed-off-by: Barry Xu --- .../test/rosbag2_transport/test_burst.cpp | 25 +- .../test/rosbag2_transport/test_play.cpp | 282 +++++++++++------- 2 files changed, 186 insertions(+), 121 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 395ee5f59..8de44f02d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -401,32 +401,37 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_topics) { } TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_services) { + const std::string service_name1 = "/test_service1"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + auto services_types = std::vector{ - {1u, "/test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, - {2u, "/test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { serialize_test_message( - "/test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + service_event_name1, 500, get_service_event_message_basic_types()[0]), serialize_test_message( - "/test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + service_event_name2, 600, get_service_event_message_basic_types()[0]), serialize_test_message( - "/test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + service_event_name1, 400, get_service_event_message_basic_types()[1]), serialize_test_message( - "/test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + service_event_name2, 500, get_service_event_message_basic_types()[1]) }; std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); // Filter allows test_service2, blocks test_service1 - play_options_.services_to_filter = {"test_service2/_service_event"}; + play_options_.services_to_filter = {service_event_name2}; play_options_.publish_service_requests = true; auto prepared_mock_reader = std::make_unique(); prepared_mock_reader->prepare(messages, services_types); @@ -449,7 +454,7 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_services) { player->resume(); player_future.get(); - std::this_thread::sleep_for(200ms); + player->wait_for_playback_to_finish(); EXPECT_EQ(service1_receive_requests.size(), 0); EXPECT_EQ(service2_receive_requests.size(), 2); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index ff2f52d7e..26bb25c4b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -87,6 +87,26 @@ get_service_event_message_basic_types() return messages; } + +void spin_thread_and_wait_for_sent_service_requests_to_finish( + std::shared_ptr player, + std::vector service_name_list) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + player->play(); + player->wait_for_playback_to_finish(200ms); + + for (auto & service_name : service_name_list) { + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name, 100ms)); + } + exec.cancel(); + spin_thread.join(); +} } // namespace TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) @@ -156,20 +176,25 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) { + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + auto services_types = std::vector{ - {1u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, - {2u, "test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { serialize_test_message( - "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + service_event_name1, 500, get_service_event_message_basic_types()[0]), serialize_test_message( - "test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + service_event_name2, 600, get_service_event_message_basic_types()[0]), serialize_test_message( - "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + service_event_name1, 400, get_service_event_message_basic_types()[1]), serialize_test_message( - "test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + service_event_name2, 500, get_service_event_message_basic_types()[1]) }; auto prepared_mock_reader = std::make_unique(); @@ -179,8 +204,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); @@ -190,9 +215,9 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); - std::this_thread::sleep_for(300ms); + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name1, service_name2}); + EXPECT_EQ(service1_receive_requests.size(), 2); EXPECT_EQ(service2_receive_requests.size(), 2); } @@ -202,20 +227,24 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_topics_and_servi auto topic_msg = get_messages_basic_types()[0]; topic_msg->int64_value = 1111; + const std::string topic_name = "/topic1"; + const std::string service_name = "/test_service1"; + const std::string service_event_name = service_name + "/_service_event"; + auto services_types = std::vector{ - {1u, "topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, topic_name, "test_msgs/BasicTypes", "", {}, ""}, + {2u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { serialize_test_message( - "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + service_event_name, 500, get_service_event_message_basic_types()[0]), serialize_test_message( - "topic1", 600, topic_msg), + topic_name, 600, topic_msg), serialize_test_message( - "test_service1/_service_event", 550, get_service_event_message_basic_types()[1]), + service_event_name, 550, get_service_event_message_basic_types()[1]), serialize_test_message( - "topic1", 400, topic_msg), + topic_name, 400, topic_msg), }; auto prepared_mock_reader = std::make_unique(); @@ -223,29 +252,40 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_topics_and_servi auto reader = std::make_unique(std::move(prepared_mock_reader)); std::vector> service_receive_requests; - srv_->setup_service("test_service1", service_receive_requests); + srv_->setup_service(service_name, service_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); - sub_->add_subscription("/topic1", 2); + sub_->add_subscription(topic_name, 2); auto await_received_messages = sub_->spin_subscriptions(); play_options_.publish_service_requests = true; auto player = std::make_shared( std::move( reader), storage_options_, play_options_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + player->play(); await_received_messages.get(); auto replayed_topic_msg = sub_->get_received_messages( - "/topic1"); + topic_name); EXPECT_THAT(replayed_topic_msg, SizeIs(Ge(2u))); EXPECT_THAT( replayed_topic_msg, Each(Pointee(Field(&test_msgs::msg::BasicTypes::int64_value, 1111)))); - std::this_thread::sleep_for(200ms); + player->wait_for_playback_to_finish(200ms); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name, 100ms)); + exec.cancel(); + spin_thread.join(); ASSERT_EQ(service_receive_requests.size(), 2); for (size_t i = 0; i < 2; i++) { EXPECT_EQ( @@ -448,20 +488,25 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_services) { + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + auto services_types = std::vector{ - {1u, "/test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, - {2u, "/test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { serialize_test_message( - "/test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + service_event_name1, 500, get_service_event_message_basic_types()[0]), serialize_test_message( - "/test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + service_event_name2, 600, get_service_event_message_basic_types()[0]), serialize_test_message( - "/test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + service_event_name1, 400, get_service_event_message_basic_types()[1]), serialize_test_message( - "/test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + service_event_name2, 500, get_service_event_message_basic_types()[1]) }; auto prepared_mock_reader = std::make_unique(); @@ -469,17 +514,18 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service auto reader = std::make_unique(std::move(prepared_mock_reader)); play_options_.publish_service_requests = true; + // Filter allows /test_service2, blocks /test_service1 { - play_options_.services_to_filter = {"test_service2/_service_event"}; + play_options_.services_to_filter = {service_event_name2}; srv_.reset(); srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -491,23 +537,25 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); - std::this_thread::sleep_for(250ms); + // Only need to wait for sent service 2 request to finish + spin_thread_and_wait_for_sent_service_requests_to_finish( + player, {service_name2}); + EXPECT_EQ(service1_receive_requests.size(), 0); EXPECT_EQ(service2_receive_requests.size(), 2); } // Filter allows /test_service1, blocks /test_service2 { - play_options_.services_to_filter = {"test_service1/_service_event"}; + play_options_.services_to_filter = {service_event_name1}; srv_.reset(); srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -519,9 +567,10 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); + // Only need to wait for sent service 1 request to finish + spin_thread_and_wait_for_sent_service_requests_to_finish( + player, {service_name1}); - std::this_thread::sleep_for(250ms); EXPECT_EQ(service1_receive_requests.size(), 2); EXPECT_EQ(service2_receive_requests.size(), 0); } @@ -534,8 +583,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -547,9 +596,9 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); + spin_thread_and_wait_for_sent_service_requests_to_finish( + player, {service_name1, service_name2}); - std::this_thread::sleep_for(250ms); EXPECT_EQ(service1_receive_requests.size(), 2); EXPECT_EQ(service2_receive_requests.size(), 2); } @@ -557,44 +606,45 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_service TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_and_services) { + const std::string topic_name1 = "/topic1"; + const std::string topic_name2 = "/topic2"; + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + auto all_types = std::vector{ - {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "/topic2", "test_msgs/BasicTypes", "", {}, ""}, - {3u, "/test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, - {4u, "/test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {1u, topic_name1, "test_msgs/BasicTypes", "", {}, ""}, + {2u, topic_name2, "test_msgs/BasicTypes", "", {}, ""}, + {3u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {4u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, }; std::vector> messages = { - serialize_test_message("/topic1", 500, get_messages_basic_types()[0]), - serialize_test_message( - "/test_service1/_service_event", - 520, - get_service_event_message_basic_types()[0]), - serialize_test_message("/topic2", 520, get_messages_basic_types()[0]), - serialize_test_message( - "/test_service2/_service_event", - 550, - get_service_event_message_basic_types()[0]), + serialize_test_message(topic_name1, 500, get_messages_basic_types()[0]), + serialize_test_message(service_event_name1, 520, get_service_event_message_basic_types()[0]), + serialize_test_message(topic_name2, 520, get_messages_basic_types()[0]), + serialize_test_message(service_event_name2, 550, get_service_event_message_basic_types()[0]), }; play_options_.publish_service_requests = true; // Filter allows all topics, blocks service test_service2 { - play_options_.topics_to_filter = {"topic1", "topic2"}; - play_options_.services_to_filter = {"test_service1/_service_event"}; + play_options_.topics_to_filter = {topic_name1, topic_name2}; + play_options_.services_to_filter = {service_event_name1}; sub_.reset(); sub_ = std::make_shared(); - sub_->add_subscription("/topic1", 1); - sub_->add_subscription("/topic2", 1); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 1); srv_.reset(); srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -607,15 +657,26 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + + player->play(); await_received_messages.get(); - std::this_thread::sleep_for(300ms); + player->wait_for_playback_to_finish(200ms); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 100ms)); + exec.cancel(); + spin_thread.join(); // Filter allow all topics - auto replayed_topic1 = sub_->get_received_messages("/topic1"); + auto replayed_topic1 = sub_->get_received_messages(topic_name1); EXPECT_THAT(replayed_topic1, SizeIs(1u)); - auto replayed_topic2 = sub_->get_received_messages("/topic2"); + auto replayed_topic2 = sub_->get_received_messages(topic_name2); EXPECT_THAT(replayed_topic2, SizeIs(1u)); // Filter allow test_service1, block test_service2 @@ -625,21 +686,21 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ // Filter allows all services, blocks topic2 { - play_options_.topics_to_filter = {"topic1"}; + play_options_.topics_to_filter = {topic_name1}; play_options_.services_to_filter = { - "test_service1/_service_event", "test_service2/_service_event"}; + service_event_name1, service_event_name2}; sub_.reset(); sub_ = std::make_shared(); - sub_->add_subscription("/topic1", 1); - sub_->add_subscription("/topic2", 0); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 0); srv_.reset(); srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -652,15 +713,27 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + + player->play(); await_received_messages.get(); - std::this_thread::sleep_for(200ms); + player->wait_for_playback_to_finish(200ms); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 100ms)); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name2, 100ms)); + exec.cancel(); + spin_thread.join(); // Filter allow topic2, block topic1 - auto replayed_topic1 = sub_->get_received_messages("/topic1"); + auto replayed_topic1 = sub_->get_received_messages(topic_name1); EXPECT_THAT(replayed_topic1, SizeIs(1u)); - auto replayed_topic2 = sub_->get_received_messages("/topic2"); + auto replayed_topic2 = sub_->get_received_messages(topic_name2); EXPECT_THAT(replayed_topic2, SizeIs(0u)); // Filter allow all services @@ -670,21 +743,21 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ // Filter allows all services and topics { - play_options_.topics_to_filter = {"topic1", "topic2"}; + play_options_.topics_to_filter = {topic_name1, topic_name2}; play_options_.services_to_filter = { - "test_service1/_service_event", "test_service2/_service_event"}; + service_event_name1, service_event_name2}; sub_.reset(); sub_ = std::make_shared(); - sub_->add_subscription("/topic1", 1); - sub_->add_subscription("/topic2", 1); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 1); srv_.reset(); srv_ = std::make_shared(); std::vector> service1_receive_requests; std::vector> service2_receive_requests; - srv_->setup_service("test_service1", service1_receive_requests); - srv_->setup_service("test_service2", service2_receive_requests); + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); srv_->run_services(); ASSERT_TRUE(srv_->all_services_ready()); @@ -697,10 +770,22 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ auto player = std::make_shared( std::move( reader), storage_options_, play_options_); - player->play(); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + + player->play(); await_received_messages.get(); - std::this_thread::sleep_for(200ms); + player->wait_for_playback_to_finish(200ms); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 100ms)); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name2, 100ms)); + exec.cancel(); + spin_thread.join(); // Filter allow all topics auto replayed_topic1 = sub_->get_received_messages("/topic1"); @@ -858,7 +943,7 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_messages) { - const std::string service_name = "test_service1"; + const std::string service_name = "/test_service1"; const std::string service_event_name = service_name + "/_service_event"; auto services_types = std::vector{ {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, @@ -888,20 +973,8 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_ auto player = std::make_shared(std::move(reader), storage_options_, play_options_); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(player); - // We need to spin Player to get responses from services and able to wait for requests to finish - auto spin_thread = std::thread( - [&exec]() { - exec.spin(); - }); - - player->play(); - player->wait_for_playback_to_finish(); + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name}); - EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name)); - exec.cancel(); - spin_thread.join(); EXPECT_EQ(received_service_requests.size(), 2); // expected_request is ServiceEventInfo::REQUEST_RECEIVED const auto expected_request = get_service_event_message_basic_types()[0]->request[0]; @@ -915,7 +988,7 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_messages) { - const std::string service_name = "test_service1"; + const std::string service_name = "/test_service1"; const std::string service_event_name = service_name + "/_service_event"; auto services_types = std::vector{ {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, @@ -945,20 +1018,7 @@ TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_m auto player = std::make_shared(std::move(reader), storage_options_, play_options_); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(player); - // We need to spin Player to get responses from services and able to wait for requests to finish - auto spin_thread = std::thread( - [&exec]() { - exec.spin(); - }); - - player->play(); - player->wait_for_playback_to_finish(); - - EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name)); - exec.cancel(); - spin_thread.join(); + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name}); EXPECT_EQ(received_service_requests.size(), 2); // expected_request is ServiceEventInfo::REQUEST_SENT