From e2780c2b362055956e98af154cfdfdf616e927bd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 2 Jan 2025 16:39:24 -0500 Subject: [PATCH] Updates to compile against Humble. (#388) * Updates to compile against Humble. There are 5 main changes in here: 1. Humble does not support type hashes. Rather than change all of the liveliness token handling (which will make backports in the future harder), we just remove the code that computes it, and instead set it to a constant string. 2. The definition of RMW_GID_STORAGE_SIZE in Humble is wonky, and set to 24 for some ancient historical reasons. We don't actually want to do that, so just hard-code 16. 3. Some of the events in modern ROS 2 aren't supported in Humble, so remove the code for handling them. 4. There are a few APIs dealing with dynamic messages that aren't supported in Humble. 5. There is at least one QOS policy (best available) which doesn't exist in Humble. With all of these changes in, I'm able to compile and run basic programs against Humble. Signed-off-by: Chris Lalancette * Also update the GitHub workflows. Signed-off-by: Chris Lalancette * Linter fixes. Signed-off-by: Chris Lalancette * Update README Signed-off-by: Yadunund --------- Signed-off-by: Chris Lalancette Signed-off-by: Yadunund Co-authored-by: Yadunund --- .github/workflows/build.yaml | 11 ++-- .github/workflows/style.yaml | 4 +- README.md | 14 +++-- rmw_zenoh_cpp/CMakeLists.txt | 2 +- .../src/detail/attachment_helpers.cpp | 6 +- .../src/detail/attachment_helpers.hpp | 6 +- rmw_zenoh_cpp/src/detail/event.cpp | 5 -- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 18 +----- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 11 ++-- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 6 +- rmw_zenoh_cpp/src/detail/rmw_client_data.cpp | 26 ++------- rmw_zenoh_cpp/src/detail/rmw_client_data.hpp | 2 +- .../src/detail/rmw_publisher_data.cpp | 23 ++------ .../src/detail/rmw_publisher_data.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 32 +++-------- .../src/detail/rmw_subscription_data.cpp | 24 ++------ rmw_zenoh_cpp/src/detail/zenoh_utils.cpp | 2 +- rmw_zenoh_cpp/src/detail/zenoh_utils.hpp | 2 +- rmw_zenoh_cpp/src/rmw_event.cpp | 20 ------- rmw_zenoh_cpp/src/rmw_init.cpp | 10 +++- rmw_zenoh_cpp/src/rmw_init_options.cpp | 19 +------ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 56 +------------------ rmw_zenoh_cpp/src/zenohd/main.cpp | 10 +++- 23 files changed, 79 insertions(+), 232 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 4fa7b445..49241ef1 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -2,7 +2,7 @@ name: build on: pull_request: push: - branches: [ rolling ] + branches: [ humble ] workflow_dispatch: schedule: # Run every morning to detect flakiness and broken dependencies @@ -17,17 +17,14 @@ jobs: fail-fast: false matrix: include: - # Rolling (source) - - ROS_DISTRO: rolling - BUILD_TYPE: source - # Jazzy (binary) - - ROS_DISTRO: jazzy + # Humble (binary) + - ROS_DISTRO: humble BUILD_TYPE: binary env: ROS2_REPOS_FILE_URL: 'https://raw.githubusercontent.com/ros2/ros2/${{ matrix.ROS_DISTRO }}/ros2.repos' runs-on: ubuntu-latest container: - image: ${{ matrix.BUILD_TYPE == 'binary' && format('ros:{0}-ros-base', matrix.ROS_DISTRO) || 'ubuntu:noble' }} + image: ${{ matrix.BUILD_TYPE == 'binary' && format('ros:{0}-ros-base', matrix.ROS_DISTRO) || 'ubuntu:jammy' }} steps: - uses: ros-tooling/setup-ros@v0.7 if: ${{ matrix.BUILD_TYPE == 'source' }} diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 6389ca52..7bade92f 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -2,7 +2,7 @@ name: style on: pull_request: push: - branches: [ rolling ] + branches: [ humble ] defaults: run: shell: bash @@ -12,7 +12,7 @@ jobs: strategy: fail-fast: false matrix: - distro: ['jazzy', 'rolling'] + distro: ['humble'] container: image: ros:${{ matrix.distro }}-ros-base timeout-minutes: 30 diff --git a/README.md b/README.md index 9e56025a..d67a5c31 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # rmw_zenoh -[![build](https://github.com/ros2/rmw_zenoh/actions/workflows/build.yaml/badge.svg)](https://github.com/ros2/rmw_zenoh/actions/workflows/build.yaml) -[![style](https://github.com/ros2/rmw_zenoh/actions/workflows/style.yaml/badge.svg)](https://github.com/ros2/rmw_zenoh/actions/workflows/style.yaml) +[![build](https://github.com/ros2/rmw_zenoh/actions/workflows/build.yaml/badge.svg?branch=humble)](https://github.com/ros2/rmw_zenoh/actions/workflows/build.yaml) +[![style](https://github.com/ros2/rmw_zenoh/actions/workflows/style.yaml/badge.svg?branch=humble)](https://github.com/ros2/rmw_zenoh/actions/workflows/style.yaml) A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-cpp bindings. @@ -10,8 +10,9 @@ A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-cpp bi For information about the Design please visit [design](docs/design.md) page. ## Requirements -- [ROS 2](https://docs.ros.org): Rolling/Jazzy/Iron +- [ROS 2](https://docs.ros.org) +> Note: See available distro branches, eg. `jazzy`, for supported ROS 2 distributions. ## Setup @@ -22,11 +23,12 @@ The `ZENOHC_CARGO_FLAGS` CMake argument may be overwritten with other features i See [zenoh_cpp_vendor/CMakeLists.txt](./zenoh_cpp_vendor/CMakeLists.txt) for more details. ```bash +# replace with ROS 2 distro of choice mkdir ~/ws_rmw_zenoh/src -p && cd ~/ws_rmw_zenoh/src -git clone https://github.com/ros2/rmw_zenoh.git +git clone https://github.com/ros2/rmw_zenoh.git -b cd ~/ws_rmw_zenoh -rosdep install --from-paths src --ignore-src --rosdistro -y # replace with ROS 2 distro of choice -source /opt/ros//setup.bash # replace with ROS 2 distro of choice +rosdep install --from-paths src --ignore-src --rosdistro -y +source /opt/ros//setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 185fa804..d7ee3721 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -94,7 +94,7 @@ if(BUILD_TESTING) set(_linter_excludes src/detail/ordered_hash.hpp src/detail/ordered_map.hpp) - ament_copyright(EXCLUDE src/detail/simplified_xxhash3.cpp src/detail/simplified_xxhash3.hpp) + ament_copyright(EXCLUDE src/detail/simplified_xxhash3.cpp src/detail/simplified_xxhash3.hpp ${_linter_excludes}) ament_cpplint(EXCLUDE ${_linter_excludes}) ament_lint_cmake() ament_uncrustify(EXCLUDE ${_linter_excludes}) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 6f128b47..9a10fc5e 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -32,7 +32,7 @@ namespace rmw_zenoh_cpp AttachmentData::AttachmentData( const int64_t sequence_number, const int64_t source_timestamp, - const std::array source_gid) + const std::array source_gid) : sequence_number_(sequence_number), source_timestamp_(source_timestamp), source_gid_(source_gid), @@ -61,7 +61,7 @@ int64_t AttachmentData::source_timestamp() const } ///============================================================================= -std::array AttachmentData::copy_gid() const +std::array AttachmentData::copy_gid() const { return source_gid_; } @@ -103,7 +103,7 @@ AttachmentData::AttachmentData(const zenoh::Bytes & bytes) if (source_gid_str != "source_gid") { throw std::runtime_error("source_gid is not found in the attachment."); } - this->source_gid_ = deserializer.deserialize>(); + this->source_gid_ = deserializer.deserialize>(); gid_hash_ = hash_gid(this->source_gid_); } } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index 64f444d1..6de90587 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -31,14 +31,14 @@ class AttachmentData final AttachmentData( const int64_t sequence_number, const int64_t source_timestamp, - const std::array source_gid); + const std::array source_gid); explicit AttachmentData(const zenoh::Bytes & bytes); explicit AttachmentData(AttachmentData && data); int64_t sequence_number() const; int64_t source_timestamp() const; - std::array copy_gid() const; + std::array copy_gid() const; size_t gid_hash() const; zenoh::Bytes serialize_to_zbytes(); @@ -46,7 +46,7 @@ class AttachmentData final private: int64_t sequence_number_; int64_t source_timestamp_; - std::array source_gid_; + std::array source_gid_; size_t gid_hash_; }; } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp index dff88118..c54692d3 100644 --- a/rmw_zenoh_cpp/src/detail/event.cpp +++ b/rmw_zenoh_cpp/src/detail/event.cpp @@ -33,11 +33,6 @@ static const std::unordered_map 0) { - update_event_counters(entity, + update_event_counters( + entity, ZENOH_EVENT_PUBLICATION_MATCHED, match_count_for_entity); } @@ -1124,18 +1123,7 @@ 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; - } - } - - memcpy(ep.endpoint_gid, entity->copy_gid().data(), RMW_GID_STORAGE_SIZE); + memcpy(ep.endpoint_gid, entity->copy_gid().data(), 16); endpoints.push_back(ep); } diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index c1792091..cab43512 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -179,8 +179,6 @@ static const std::unordered_map 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::vector split_keyexpr( @@ -438,8 +436,9 @@ Entity::Entity( simplified_XXH128_hash_t keyexpr_gid = simplified_XXH3_128bits(this->liveliness_keyexpr_.c_str(), this->liveliness_keyexpr_.length()); memcpy(this->gid_.data(), &keyexpr_gid.low64, sizeof(keyexpr_gid.low64)); - memcpy(this->gid_.data() + sizeof(keyexpr_gid.low64), &keyexpr_gid.high64, - sizeof(keyexpr_gid.high64)); + memcpy( + this->gid_.data() + sizeof(keyexpr_gid.low64), &keyexpr_gid.high64, + sizeof(keyexpr_gid.high64)); // We also hash the liveliness keyexpression into a size_t that we use to index into our maps. this->keyexpr_hash_ = hash_gid(this->gid_); @@ -631,7 +630,7 @@ std::string Entity::liveliness_keyexpr() const } ///============================================================================= -std::array Entity::copy_gid() const +std::array Entity::copy_gid() const { return gid_; } @@ -672,7 +671,7 @@ std::string demangle_name(const std::string & input) } // namespace liveliness ///============================================================================= -size_t hash_gid(const std::array gid) +size_t hash_gid(const std::array gid) { std::stringstream hash_str; hash_str << std::hex; diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 81880df6..ca7dc6d6 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -173,7 +173,7 @@ class Entity // Two entities are equal if their keyexpr_hash are equal. bool operator==(const Entity & other) const; - std::array copy_gid() const; + std::array copy_gid() const; private: Entity( @@ -192,7 +192,7 @@ class Entity NodeInfo node_info_; std::optional topic_info_; std::string liveliness_keyexpr_; - std::array gid_{}; + std::array gid_{}; }; ///============================================================================= @@ -235,7 +235,7 @@ std::optional keyexpr_to_qos(const std::string & keyexpr); } // namespace liveliness ///============================================================================= -size_t hash_gid(const std::array gid); +size_t hash_gid(const std::array gid); } // namespace rmw_zenoh_cpp ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp index a0463945..497ca869 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -66,9 +66,6 @@ std::shared_ptr ClientData::make( return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; - - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto service_members = static_cast(type_support->data); auto request_members = static_cast( service_members->request_members_->data); @@ -92,20 +89,9 @@ std::shared_ptr ClientData::make( return nullptr; } - // 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) { - // rosidl_stringify_type_hash already set the error - 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); - }); + // Humble doesn't support type hash, but we leave it in place as a constant so we don't have to + // change the graph and liveliness token code. + const char * type_hash_c_str = "TypeHashNotSupported"; std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -203,7 +189,7 @@ bool ClientData::liveliness_is_valid() const } ///============================================================================= -std::array ClientData::copy_gid() const +std::array ClientData::copy_gid() const { return entity_->copy_gid(); } @@ -307,7 +293,7 @@ rmw_ret_t ClientData::take_response( memcpy( request_header->request_id.writer_guid, attachment.copy_gid().data(), - RMW_GID_STORAGE_SIZE); + 16); request_header->received_timestamp = latest_reply->get_received_timestamp(); *taken = true; @@ -364,7 +350,7 @@ rmw_ret_t ClientData::send_request( // Send request zenoh::Session::GetOptions opts = zenoh::Session::GetOptions::create_default(); - std::array local_gid = entity_->copy_gid(); + std::array local_gid = entity_->copy_gid(); opts.attachment = rmw_zenoh_cpp::create_map_and_set_sequence_num(*sequence_id, local_gid); opts.target = Z_QUERY_TARGET_ALL_COMPLETE; // The default timeout for a z_get query is 10 seconds and if a response is not received within diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp index 44b187ba..1bb52992 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp @@ -65,7 +65,7 @@ class ClientData final : public std::enable_shared_from_this bool liveliness_is_valid() const; // Copy the GID of this ClientData into an rmw_gid_t. - std::array copy_gid() const; + std::array copy_gid() const; // Add a new ZenohReply to the queue. void add_new_reply(std::unique_ptr reply); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 3bf62372..3a4be059 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -60,27 +60,12 @@ std::shared_ptr PublisherData::make( return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; - - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto callbacks = static_cast(type_support->data); auto message_type_support = std::make_unique(callbacks); - // 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) { - // rosidl_stringify_type_hash already set the error - return nullptr; - } - auto always_free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + // Humble doesn't support type hash, but we leave it in place as a constant so we don't have to + // change the graph and liveliness token code. + const char * type_hash_c_str = "TypeHashNotSupported"; std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -328,7 +313,7 @@ liveliness::TopicInfo PublisherData::topic_info() const return entity_->topic_info().value(); } -std::array PublisherData::copy_gid() const +std::array PublisherData::copy_gid() const { std::lock_guard lock(mutex_); return entity_->copy_gid(); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 52c22523..b7658adf 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -70,7 +70,7 @@ class PublisherData final liveliness::TopicInfo topic_info() const; // Return a copy of the GID of this publisher. - std::array copy_gid() const; + std::array copy_gid() const; // Returns true if liveliness token is still valid. bool liveliness_is_valid() const; diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index f3df2df6..4b43bf39 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -62,9 +62,6 @@ std::shared_ptr ServiceData::make( return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; - - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto service_members = static_cast(type_support->data); auto request_members = static_cast( service_members->request_members_->data); @@ -88,20 +85,9 @@ std::shared_ptr ServiceData::make( return nullptr; } - // 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) { - // rosidl_stringify_type_hash already set the error - 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); - }); + // Humble doesn't support type hash, but we leave it in place as a constant so we don't have to + // change the graph and liveliness token code. + const char * type_hash_c_str = "TypeHashNotSupported"; std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -329,8 +315,8 @@ rmw_ret_t ServiceData::take_request( return RMW_RET_ERROR; } - std::array writer_guid = attachment.copy_gid(); - memcpy(request_header->request_id.writer_guid, writer_guid.data(), RMW_GID_STORAGE_SIZE); + std::array writer_guid = attachment.copy_gid(); + memcpy(request_header->request_id.writer_guid, writer_guid.data(), 16); request_header->source_timestamp = attachment.source_timestamp(); if (request_header->source_timestamp < 0) { @@ -374,8 +360,8 @@ rmw_ret_t ServiceData::send_response( return RMW_RET_OK; } - std::array writer_guid; - memcpy(writer_guid.data(), request_id->writer_guid, RMW_GID_STORAGE_SIZE); + std::array writer_guid; + memcpy(writer_guid.data(), request_id->writer_guid, 16); // Create the queryable payload const size_t hash = hash_gid(writer_guid); @@ -433,8 +419,8 @@ rmw_ret_t ServiceData::send_response( const zenoh::Query & loaned_query = query->get_query(); zenoh::Query::ReplyOptions options = zenoh::Query::ReplyOptions::create_default(); - std::array writer_gid; - memcpy(writer_gid.data(), request_id->writer_guid, RMW_GID_STORAGE_SIZE); + std::array writer_gid; + memcpy(writer_gid.data(), request_id->writer_guid, 16); options.attachment = create_map_and_set_sequence_num( request_id->sequence_number, writer_gid); diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index 2a070ee8..4950564e 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -73,26 +73,12 @@ std::shared_ptr SubscriptionData::make( return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; - - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto callbacks = static_cast(type_support->data); auto message_type_support = std::make_unique(callbacks); - // 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) { - // rosidl_stringify_type_hash already set the error - 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); - }); + // Humble doesn't support type hash, but we leave it in place as a constant so we don't have to + // change the graph and liveliness token code. + const char * type_hash_c_str = "TypeHashNotSupported"; // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. @@ -520,7 +506,7 @@ rmw_ret_t SubscriptionData::take_one_message( memcpy( message_info->publisher_gid.data, msg_data->attachment.copy_gid().data(), - RMW_GID_STORAGE_SIZE); + 16); message_info->from_intra_process = false; } *taken = true; @@ -577,7 +563,7 @@ rmw_ret_t SubscriptionData::take_serialized_message( memcpy( message_info->publisher_gid.data, msg_data->attachment.copy_gid().data(), - RMW_GID_STORAGE_SIZE); + 16); message_info->from_intra_process = false; } diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index 922a80fb..d097b2e7 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp @@ -28,7 +28,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= zenoh::Bytes create_map_and_set_sequence_num( - int64_t sequence_number, std::array gid) + int64_t sequence_number, std::array gid) { auto now = std::chrono::system_clock::now().time_since_epoch(); auto now_ns = std::chrono::duration_cast(now); diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index 58ed471d..c4f646aa 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -31,7 +31,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= zenoh::Bytes create_map_and_set_sequence_num( - int64_t sequence_number, std::array gid); + int64_t sequence_number, std::array gid); ///============================================================================= // A class to store the replies to service requests. diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index b362a042..6c4e83a3 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -227,26 +227,6 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } - case rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_INCOMPATIBLE_TYPE: - case rmw_zenoh_cpp::ZENOH_EVENT_PUBLISHER_INCOMPATIBLE_TYPE: { - auto ei = static_cast(event_info); - RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); - ei->total_count = static_cast(st.total_count); - ei->total_count_change = static_cast(st.total_count_change); - *taken = true; - return RMW_RET_OK; - } - case rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED: - case rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED: { - auto ei = static_cast(event_info); - RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); - ei->total_count = st.total_count; - ei->total_count_change = st.total_count_change; - ei->current_count = st.current_count; - ei->current_count_change = st.current_count_change; - *taken = true; - return RMW_RET_OK; - } case rmw_zenoh_cpp::ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index cb34697c..c983c0d6 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -84,9 +84,13 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } }); - if (!rcutils_set_env_overwrite(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_WARN_LEVEL_STR, 0)) { - RMW_SET_ERROR_MSG("Error configuring Zenoh logging."); - return RMW_RET_ERROR; + // Equivalent to rcutils_set_env_overwrite(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_WARN_LEVEL_STR, 0) + // in modern ROS 2. + if (getenv(ZENOH_LOG_ENV_VAR_STR) == nullptr) { + if (!rcutils_set_env(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_WARN_LEVEL_STR)) { + RMW_SET_ERROR_MSG("Error configuring Zenoh logging."); + return RMW_RET_ERROR; + } } // Enable the zenoh built-in logger diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 15e06e2b..8ace30a5 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -47,8 +47,8 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all init_options->enclave = nullptr; init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; init_options->security_options = rmw_get_default_security_options(); - init_options->discovery_options = rmw_get_zero_initialized_discovery_options(); - return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; + return RMW_RET_OK; } //============================================================================== @@ -87,19 +87,6 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) [&tmp, allocator]() { rmw_security_options_fini(&tmp.security_options, &allocator); }); - tmp.discovery_options = rmw_get_zero_initialized_discovery_options(); - ret = rmw_discovery_options_copy( - &src->discovery_options, - &allocator, - &tmp.discovery_options); - if (RMW_RET_OK != ret) { - return ret; - } - auto free_discovery_options = rcpputils::make_scope_exit( - [&tmp]() { - rmw_ret_t tmp_ret = rmw_discovery_options_fini(&tmp.discovery_options); - static_cast(tmp_ret); - }); tmp.enclave = nullptr; if (nullptr != src->enclave) { tmp.enclave = rcutils_strdup(src->enclave, allocator); @@ -122,7 +109,6 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) *dst = tmp; free_enclave.cancel(); - free_discovery_options.cancel(); free_security_options.cancel(); return RMW_RET_OK; @@ -152,7 +138,6 @@ rmw_init_options_fini(rmw_init_options_t * init_options) return ret; } - ret = rmw_discovery_options_fini(&init_options->discovery_options); *init_options = rmw_get_zero_initialized_init_options(); return ret; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 23220fdf..ed1d06ef 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -47,7 +47,6 @@ #include "rcutils/types.h" #include "rmw/allocators.h" -#include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" #include "rmw/features.h" #include "rmw/impl/cpp/macros.hpp" @@ -139,10 +138,6 @@ bool rmw_feature_supported(rmw_feature_t feature) return false; case RMW_FEATURE_MESSAGE_INFO_RECEPTION_SEQUENCE_NUMBER: return false; - case RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY: - return true; - case RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE: - return false; default: return false; } @@ -497,51 +492,6 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) return RMW_RET_OK; } -//============================================================================== -rmw_ret_t -rmw_take_dynamic_message( - const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, - bool * taken, - rmw_subscription_allocation_t * allocation) -{ - static_cast(subscription); - static_cast(dynamic_message); - static_cast(taken); - static_cast(allocation); - return RMW_RET_UNSUPPORTED; -} - -//============================================================================== -rmw_ret_t -rmw_take_dynamic_message_with_info( - const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) -{ - static_cast(subscription); - static_cast(dynamic_message); - static_cast(taken); - static_cast(message_info); - static_cast(allocation); - return RMW_RET_UNSUPPORTED; -} - -//============================================================================== -rmw_ret_t -rmw_serialization_support_init( - const char * serialization_lib_name, - rcutils_allocator_t * allocator, - rosidl_dynamic_typesupport_serialization_support_t * serialization_support) -{ - static_cast(serialization_lib_name); - static_cast(allocator); - static_cast(serialization_support); - return RMW_RET_UNSUPPORTED; -} - //============================================================================== /// Borrow a loaned ROS message. rmw_ret_t @@ -2451,7 +2401,7 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy(gid->data, pub_data->copy_gid().data(), RMW_GID_STORAGE_SIZE); + memcpy(gid->data, pub_data->copy_gid().data(), 16); return RMW_RET_OK; } @@ -2468,7 +2418,7 @@ rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy(gid->data, client_data->copy_gid().data(), RMW_GID_STORAGE_SIZE); + memcpy(gid->data, client_data->copy_gid().data(), 16); return RMW_RET_OK; } @@ -2492,7 +2442,7 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); - *result = memcmp(gid1->data, gid2->data, RMW_GID_STORAGE_SIZE) == 0; + *result = memcmp(gid1->data, gid2->data, 16) == 0; return RMW_RET_OK; } diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index bbba50f1..a1e8c3bf 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -57,9 +57,13 @@ int main(int argc, char ** argv) (void)argc; (void)argv; - if (!rcutils_set_env_overwrite(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_INFO_LEVEL_STR, 0)) { - RMW_SET_ERROR_MSG("Error configuring Zenoh logging."); - return 1; + // Equivalent to rcutils_set_env_overwrite(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_WARN_LEVEL_STR, 0) + // in modern ROS 2. + if (getenv(ZENOH_LOG_ENV_VAR_STR) == nullptr) { + if (!rcutils_set_env(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_WARN_LEVEL_STR)) { + RMW_SET_ERROR_MSG("Error configuring Zenoh logging."); + return RMW_RET_ERROR; + } } // Enable the zenoh built-in logger