Skip to content

Commit

Permalink
Delete some part of the code which became absolute and shall not be used
Browse files Browse the repository at this point in the history
Signed-off-by: Michael Orlov <michael.orlov@apex.ai>
  • Loading branch information
MichaelOrlov committed Apr 11, 2024
1 parent 98c14ef commit f270ac1
Show file tree
Hide file tree
Showing 5 changed files with 28 additions and 151 deletions.
10 changes: 1 addition & 9 deletions rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<uint8_t, 16> & client_id);
Expand Down
108 changes: 27 additions & 81 deletions rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,155 +17,101 @@
#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;
}
}

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<service_msgs::msg::ServiceEventInfo>();

// 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<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
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()) {
return service_name;
}

// 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<uint8_t, 16> & client_id)
{
// output format:
Expand Down
15 changes: 0 additions & 15 deletions rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<test_msgs::srv::BasicTypes_Event>();
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 = {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,6 @@ class PlayerServiceClient final
bool wait_for_sent_requests_to_finish(
std::chrono::duration<double> timeout = std::chrono::seconds(5));

void async_send_request(const rcl_serialized_message_t & message);

std::shared_ptr<rclcpp::GenericClient> generic_client()
{
return client_;
Expand All @@ -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<uint8_t, ClientGidType, int64_t>
get_msg_event_type(const rcl_serialized_message_t & message);
};

class PlayerServiceClientManager final
Expand Down
41 changes: 0 additions & 41 deletions rosbag2_transport/src/rosbag2_transport/player_service_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t, PlayerServiceClient::ClientGidType, int64_t>
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<service_msgs::msg::ServiceEventInfo>();
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<void *>(&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)
Expand Down

0 comments on commit f270ac1

Please sign in to comment.