Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Backward Compatibility with ROS 2 Humble #335

Closed
wants to merge 7 commits into from
Closed

Backward Compatibility with ROS 2 Humble #335

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
5a477ed
refactor: wrap the zenoh session with a shared pointer
YuanYuYuan Nov 29, 2024
d19af76
fixup! refactor: wrap the zenoh session with a shared pointer
YuanYuYuan Dec 6, 2024
edef550
chore: remove the automatically added headers
YuanYuYuan Dec 6, 2024
9fdb567
fix: apply the suggested change
YuanYuYuan Dec 9, 2024
4f34079
fix: hold the session at the right place
YuanYuYuan Dec 10, 2024
a442bc7
feat: support ROS2 humble
YuanYuYuan Dec 10, 2024
5d19984
chore: comment out the unused code
YuanYuYuan Dec 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions rmw_zenoh_cpp/src/detail/attachment_helpers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ namespace rmw_zenoh_cpp
AttachmentData::AttachmentData(
const int64_t sequence_number,
const int64_t source_timestamp,
const uint8_t source_gid[RMW_GID_STORAGE_SIZE])
const uint8_t source_gid[16])
: sequence_number_(sequence_number),
source_timestamp_(source_timestamp)
{
memcpy(source_gid_, source_gid, RMW_GID_STORAGE_SIZE);
memcpy(source_gid_, source_gid, 16);
gid_hash_ = hash_gid(source_gid_);
}

Expand All @@ -45,7 +45,7 @@ AttachmentData::AttachmentData(AttachmentData && data)
source_timestamp_(std::move(data.source_timestamp_)),
gid_hash_(std::move(data.gid_hash_))
{
memcpy(source_gid_, data.source_gid_, RMW_GID_STORAGE_SIZE);
memcpy(source_gid_, data.source_gid_, 16);
}

///=============================================================================
Expand Down Expand Up @@ -90,7 +90,7 @@ AttachmentData::AttachmentData(const z_loaned_bytes_t * attachment)
if (ze_deserializer_deserialize_slice(&deserializer, &slice)) {
throw std::runtime_error("Failed to deserialize the source_gid.");
}
if (z_slice_len(z_loan(slice)) != RMW_GID_STORAGE_SIZE) {
if (z_slice_len(z_loan(slice)) != 16) {
throw std::runtime_error("The length of source_gid mismatched.");
}
memcpy(this->source_gid_, z_slice_data(z_loan(slice)), z_slice_len(z_loan(slice)));
Expand All @@ -111,9 +111,9 @@ int64_t AttachmentData::source_timestamp() const
}

///=============================================================================
void AttachmentData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const
void AttachmentData::copy_gid(uint8_t out_gid[16]) const
{
memcpy(out_gid, source_gid_, RMW_GID_STORAGE_SIZE);
memcpy(out_gid, source_gid_, 16);
}

///=============================================================================
Expand All @@ -132,7 +132,7 @@ void AttachmentData::serialize_to_zbytes(z_owned_bytes_t * attachment)
ze_serializer_serialize_str(z_loan_mut(serializer), "source_timestamp");
ze_serializer_serialize_int64(z_loan_mut(serializer), this->source_timestamp_);
ze_serializer_serialize_str(z_loan_mut(serializer), "source_gid");
ze_serializer_serialize_buf(z_loan_mut(serializer), this->source_gid_, RMW_GID_STORAGE_SIZE);
ze_serializer_serialize_buf(z_loan_mut(serializer), this->source_gid_, 16);
ze_serializer_finish(z_move(serializer), attachment);
}
} // namespace rmw_zenoh_cpp
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/attachment_helpers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,21 +28,21 @@ class AttachmentData final
AttachmentData(
const int64_t sequence_number,
const int64_t source_timestamp,
const uint8_t source_gid[RMW_GID_STORAGE_SIZE]);
const uint8_t source_gid[16]);
explicit AttachmentData(const z_loaned_bytes_t *);
explicit AttachmentData(AttachmentData && data);

int64_t sequence_number() const;
int64_t source_timestamp() const;
void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const;
void copy_gid(uint8_t out_gid[16]) const;
size_t gid_hash() const;

void serialize_to_zbytes(z_owned_bytes_t *);

private:
int64_t sequence_number_;
int64_t source_timestamp_;
uint8_t source_gid_[RMW_GID_STORAGE_SIZE];
uint8_t source_gid_[16];
size_t gid_hash_;
};
} // namespace rmw_zenoh_cpp
Expand Down
10 changes: 5 additions & 5 deletions rmw_zenoh_cpp/src/detail/event.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ static const std::unordered_map<rmw_event_type_t, rmw_zenoh_cpp::rmw_zenoh_event
{RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE, rmw_zenoh_cpp::ZENOH_EVENT_REQUESTED_QOS_INCOMPATIBLE},
{RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, rmw_zenoh_cpp::ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE},
{RMW_EVENT_MESSAGE_LOST, rmw_zenoh_cpp::ZENOH_EVENT_MESSAGE_LOST},
{RMW_EVENT_SUBSCRIPTION_MATCHED, rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED},
{RMW_EVENT_PUBLICATION_MATCHED, rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED},
{RMW_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE,
rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE},
{RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE, rmw_zenoh_cpp::ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE}
// {RMW_EVENT_SUBSCRIPTION_MATCHED, rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED},
// {RMW_EVENT_PUBLICATION_MATCHED, rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED},
// {RMW_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE,
// rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE},
// {RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE, rmw_zenoh_cpp::ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE}
// TODO(clalancette): Implement remaining events
};
} // namespace
Expand Down
26 changes: 13 additions & 13 deletions rmw_zenoh_cpp/src/detail/graph_cache.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
#include "rmw/validate_namespace.h"
#include "rmw/validate_node_name.h"

#include "rosidl_runtime_c/type_hash.h"
// #include "rosidl_runtime_c/type_hash.h"

#include "graph_cache.hpp"
#include "logging_macros.hpp"
Expand Down Expand Up @@ -1148,18 +1148,18 @@ rmw_ret_t GraphCache::get_entities_info_by_topic(
return ret;
}

rosidl_type_hash_t type_hash;
rcutils_ret_t rc_ret = rosidl_parse_type_hash_string(
topic_data->info_.type_hash_.c_str(),
&type_hash);
if (RCUTILS_RET_OK == rc_ret) {
ret = rmw_topic_endpoint_info_set_topic_type_hash(&ep, &type_hash);
if (RMW_RET_OK != ret) {
return ret;
}
}

memset(ep.endpoint_gid, 0, RMW_GID_STORAGE_SIZE);
// rosidl_type_hash_t type_hash;
// rcutils_ret_t rc_ret = rosidl_parse_type_hash_string(
// topic_data->info_.type_hash_.c_str(),
// &type_hash);
// if (RCUTILS_RET_OK == rc_ret) {
// ret = rmw_topic_endpoint_info_set_topic_type_hash(&ep, &type_hash);
// if (RMW_RET_OK != ret) {
// return ret;
// }
// }

memset(ep.endpoint_gid, 0, 16);
entity->copy_gid(ep.endpoint_gid);

endpoints.push_back(ep);
Expand Down
13 changes: 7 additions & 6 deletions rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,8 +177,8 @@ static const std::unordered_map<std::string, rmw_qos_liveliness_policy_e> str_to
{std::to_string(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC),
RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC},
{std::to_string(RMW_QOS_POLICY_LIVELINESS_UNKNOWN), RMW_QOS_POLICY_LIVELINESS_UNKNOWN},
{std::to_string(RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE),
RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE}
// {std::to_string(RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE),
// RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE}
};

std::vector<std::string> split_keyexpr(
Expand Down Expand Up @@ -638,9 +638,9 @@ std::string Entity::liveliness_keyexpr() const
}

///=============================================================================
void Entity::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const
void Entity::copy_gid(uint8_t out_gid[16]) const
{
memcpy(out_gid, gid_, RMW_GID_STORAGE_SIZE);
memcpy(out_gid, gid_, 16);
}

///=============================================================================
Expand Down Expand Up @@ -679,12 +679,13 @@ std::string demangle_name(const std::string & input)
} // namespace liveliness

///=============================================================================
size_t hash_gid(const uint8_t gid[RMW_GID_STORAGE_SIZE])
size_t hash_gid(const uint8_t gid[16])
// size_t hash_gid(const int8_t gid[16])
{
std::stringstream hash_str;
hash_str << std::hex;
size_t i = 0;
for (; i < (RMW_GID_STORAGE_SIZE - 1); i++) {
for (; i < (16 - 1); i++) {
hash_str << static_cast<int>(gid[i]);
}
return std::hash<std::string>{}(hash_str.str());
Expand Down
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/liveliness_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ class Entity
// Two entities are equal if their keyexpr_hash are equal.
bool operator==(const Entity & other) const;

void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const;
void copy_gid(uint8_t out_gid[16]) const;

private:
Entity(
Expand All @@ -191,7 +191,7 @@ class Entity
NodeInfo node_info_;
std::optional<TopicInfo> topic_info_;
std::string liveliness_keyexpr_;
uint8_t gid_[RMW_GID_STORAGE_SIZE];
uint8_t gid_[16];
};

///=============================================================================
Expand Down Expand Up @@ -239,7 +239,7 @@ std::string zid_to_str(const z_id_t & id);

///=============================================================================
/// Generate a hash for a given GID.
size_t hash_gid(const uint8_t gid[RMW_GID_STORAGE_SIZE]);
size_t hash_gid(const uint8_t gid[16]);
} // namespace rmw_zenoh_cpp

///=============================================================================
Expand Down
58 changes: 35 additions & 23 deletions rmw_zenoh_cpp/src/detail/rmw_client_data.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ namespace rmw_zenoh_cpp
{
///=============================================================================
std::shared_ptr<ClientData> ClientData::make(
const z_loaned_session_t * session,
std::shared_ptr<ZenohSession> session,
const rmw_node_t * const node,
const rmw_client_t * client,
liveliness::NodeInfo node_info,
Expand All @@ -124,9 +124,9 @@ std::shared_ptr<ClientData> ClientData::make(
return nullptr;
}

rcutils_allocator_t * allocator = &node->context->options.allocator;
// rcutils_allocator_t * allocator = &node->context->options.allocator;

const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support);
// const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support);
auto service_members = static_cast<const service_type_support_callbacks_t *>(type_support->data);
auto request_members = static_cast<const message_type_support_callbacks_t *>(
service_members->request_members_->data);
Expand All @@ -151,23 +151,24 @@ std::shared_ptr<ClientData> ClientData::make(
}

// Convert the type hash to a string so that it can be included in the keyexpr.
char * type_hash_c_str = nullptr;
rcutils_ret_t stringify_ret = rosidl_stringify_type_hash(
type_hash,
*allocator,
&type_hash_c_str);
if (RCUTILS_RET_BAD_ALLOC == stringify_ret) {
RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str.");
return nullptr;
}
auto free_type_hash_c_str = rcpputils::make_scope_exit(
[&allocator, &type_hash_c_str]() {
allocator->deallocate(type_hash_c_str, allocator->state);
});
const char * type_hash_c_str = "TypeHashNotSupported";
// char * type_hash_c_str = nullptr;
// rcutils_ret_t stringify_ret = rosidl_stringify_type_hash(
// type_hash,
// *allocator,
// &type_hash_c_str);
// if (RCUTILS_RET_BAD_ALLOC == stringify_ret) {
// RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str.");
// return nullptr;
// }
// auto free_type_hash_c_str = rcpputils::make_scope_exit(
// [&allocator, &type_hash_c_str]() {
// allocator->deallocate(type_hash_c_str, allocator->state);
// });

std::size_t domain_id = node_info.domain_id_;
auto entity = liveliness::Entity::make(
z_info_zid(session),
z_info_zid(session->loan()),
std::to_string(node_id),
std::to_string(service_id),
liveliness::EntityType::Client,
Expand All @@ -192,6 +193,7 @@ std::shared_ptr<ClientData> ClientData::make(
node,
client,
entity,
session,
request_members,
response_members,
request_type_support,
Expand All @@ -211,13 +213,15 @@ ClientData::ClientData(
const rmw_node_t * rmw_node,
const rmw_client_t * rmw_client,
std::shared_ptr<liveliness::Entity> entity,
std::shared_ptr<ZenohSession> sess,
const void * request_type_support_impl,
const void * response_type_support_impl,
std::shared_ptr<RequestTypeSupport> request_type_support,
std::shared_ptr<ResponseTypeSupport> response_type_support)
: rmw_node_(rmw_node),
rmw_client_(rmw_client),
entity_(std::move(entity)),
sess_(std::move(sess)),
request_type_support_impl_(request_type_support_impl),
response_type_support_impl_(response_type_support_impl),
request_type_support_(request_type_support),
Expand All @@ -232,7 +236,7 @@ ClientData::ClientData(
}

///=============================================================================
bool ClientData::init(const z_loaned_session_t * session)
bool ClientData::init(std::shared_ptr<ZenohSession> session)
{
if (z_keyexpr_from_str(
&this->keyexpr_,
Expand All @@ -250,7 +254,7 @@ bool ClientData::init(const z_loaned_session_t * session)
z_view_keyexpr_t liveliness_ke;
z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str());
if (z_liveliness_declare_token(
session,
session->loan(),
&this->token_,
z_loan(liveliness_ke),
NULL
Expand All @@ -266,6 +270,7 @@ bool ClientData::init(const z_loaned_session_t * session)
z_drop(z_move(this->token_));
});

sess_ = session;
initialized_ = true;

free_ros_keyexpr.cancel();
Expand All @@ -292,7 +297,7 @@ bool ClientData::liveliness_is_valid() const
}

///=============================================================================
void ClientData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const
void ClientData::copy_gid(uint8_t out_gid[16]) const
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
entity_->copy_gid(out_gid);
Expand Down Expand Up @@ -382,7 +387,13 @@ rmw_ret_t ClientData::take_response(
RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment");
return RMW_RET_ERROR;
}
attachment.copy_gid(request_header->request_id.writer_guid);

// attachment.copy_gid(request_header->request_id.writer_guid);
uint8_t tmp_writer_guid[16];
attachment.copy_gid(tmp_writer_guid);
for (size_t i = 0; i < 16; ++i) {
request_header->request_id.writer_guid[i] = static_cast<int8_t>(tmp_writer_guid[i]);
}
request_header->received_timestamp = latest_reply->get_received_timestamp();

z_drop(z_move(payload));
Expand Down Expand Up @@ -442,7 +453,7 @@ rmw_ret_t ClientData::send_request(
z_get_options_t opts;
z_get_options_default(&opts);
z_owned_bytes_t attachment;
uint8_t local_gid[RMW_GID_STORAGE_SIZE];
uint8_t local_gid[16];
entity_->copy_gid(local_gid);
create_map_and_set_sequence_num(
&attachment, *sequence_id,
Expand Down Expand Up @@ -470,7 +481,7 @@ rmw_ret_t ClientData::send_request(
z_owned_closure_reply_t zn_closure_reply;
z_closure(&zn_closure_reply, client_data_handler, client_data_drop, this);
z_get(
context_impl->session(),
sess_->loan(),
z_loan(keyexpr_), "",
z_move(zn_closure_reply),
&opts);
Expand Down Expand Up @@ -535,6 +546,7 @@ void ClientData::_shutdown()
z_drop(z_move(keyexpr_));
}

sess_.reset();
is_shutdown_ = true;
}

Expand Down
Loading
Loading