From f270ac1f9dac7e2f2fbd7a0abdfbecbe9c32e382 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 11 Apr 2024 02:07:14 -0700 Subject: [PATCH] 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)