Skip to content

Commit

Permalink
Updates to compile against Humble. (#388)
Browse files Browse the repository at this point in the history
* 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 <[email protected]>

* Also update the GitHub workflows.

Signed-off-by: Chris Lalancette <[email protected]>

* Linter fixes.

Signed-off-by: Chris Lalancette <[email protected]>

* Update README

Signed-off-by: Yadunund <[email protected]>

---------

Signed-off-by: Chris Lalancette <[email protected]>
Signed-off-by: Yadunund <[email protected]>
Co-authored-by: Yadunund <[email protected]>
  • Loading branch information
clalancette and Yadunund authored Jan 2, 2025
1 parent fb06619 commit e2780c2
Show file tree
Hide file tree
Showing 23 changed files with 79 additions and 232 deletions.
11 changes: 4 additions & 7 deletions .github/workflows/build.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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/[email protected]
if: ${{ matrix.BUILD_TYPE == 'source' }}
Expand Down
4 changes: 2 additions & 2 deletions .github/workflows/style.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ name: style
on:
pull_request:
push:
branches: [ rolling ]
branches: [ humble ]
defaults:
run:
shell: bash
Expand All @@ -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
Expand Down
14 changes: 8 additions & 6 deletions README.md
Original file line number Diff line number Diff line change
@@ -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.

Expand All @@ -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

Expand All @@ -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 <DISTRO> 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 <DISTRO>
cd ~/ws_rmw_zenoh
rosdep install --from-paths src --ignore-src --rosdistro <DISTRO> -y # replace <DISTRO> with ROS 2 distro of choice
source /opt/ros/<DISTRO>/setup.bash # replace <DISTRO> with ROS 2 distro of choice
rosdep install --from-paths src --ignore-src --rosdistro <DISTRO> -y
source /opt/ros/<DISTRO>/setup.bash
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
```

Expand Down
2 changes: 1 addition & 1 deletion rmw_zenoh_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/attachment_helpers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ namespace rmw_zenoh_cpp
AttachmentData::AttachmentData(
const int64_t sequence_number,
const int64_t source_timestamp,
const std::array<uint8_t, RMW_GID_STORAGE_SIZE> source_gid)
const std::array<uint8_t, 16> source_gid)
: sequence_number_(sequence_number),
source_timestamp_(source_timestamp),
source_gid_(source_gid),
Expand Down Expand Up @@ -61,7 +61,7 @@ int64_t AttachmentData::source_timestamp() const
}

///=============================================================================
std::array<uint8_t, RMW_GID_STORAGE_SIZE> AttachmentData::copy_gid() const
std::array<uint8_t, 16> AttachmentData::copy_gid() const
{
return source_gid_;
}
Expand Down Expand Up @@ -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<std::array<uint8_t, RMW_GID_STORAGE_SIZE>>();
this->source_gid_ = deserializer.deserialize<std::array<uint8_t, 16>>();
gid_hash_ = hash_gid(this->source_gid_);
}
} // namespace rmw_zenoh_cpp
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/attachment_helpers.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,22 +31,22 @@ class AttachmentData final
AttachmentData(
const int64_t sequence_number,
const int64_t source_timestamp,
const std::array<uint8_t, RMW_GID_STORAGE_SIZE> source_gid);
const std::array<uint8_t, 16> source_gid);

explicit AttachmentData(const zenoh::Bytes & bytes);
explicit AttachmentData(AttachmentData && data);

int64_t sequence_number() const;
int64_t source_timestamp() const;
std::array<uint8_t, RMW_GID_STORAGE_SIZE> copy_gid() const;
std::array<uint8_t, 16> copy_gid() const;
size_t gid_hash() const;

zenoh::Bytes serialize_to_zbytes();

private:
int64_t sequence_number_;
int64_t source_timestamp_;
std::array<uint8_t, RMW_GID_STORAGE_SIZE> source_gid_;
std::array<uint8_t, 16> source_gid_;
size_t gid_hash_;
};
} // namespace rmw_zenoh_cpp
Expand Down
5 changes: 0 additions & 5 deletions rmw_zenoh_cpp/src/detail/event.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,6 @@ 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}
// TODO(clalancette): Implement remaining events
};
} // namespace
Expand Down
18 changes: 3 additions & 15 deletions rmw_zenoh_cpp/src/detail/graph_cache.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@
#include "rmw/validate_namespace.h"
#include "rmw/validate_node_name.h"

#include "rosidl_runtime_c/type_hash.h"

#include "graph_cache.hpp"
#include "logging_macros.hpp"

Expand Down Expand Up @@ -233,7 +231,8 @@ void GraphCache::handle_matched_events_for_put(
}
// Update event counters for the new entity.
if (is_entity_local(*entity) && match_count_for_entity > 0) {
update_event_counters(entity,
update_event_counters(
entity,
ZENOH_EVENT_PUBLICATION_MATCHED,
match_count_for_entity);
}
Expand Down Expand Up @@ -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);
}
Expand Down
11 changes: 5 additions & 6 deletions rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -179,8 +179,6 @@ 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::vector<std::string> split_keyexpr(
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -631,7 +630,7 @@ std::string Entity::liveliness_keyexpr() const
}

///=============================================================================
std::array<uint8_t, RMW_GID_STORAGE_SIZE> Entity::copy_gid() const
std::array<uint8_t, 16> Entity::copy_gid() const
{
return gid_;
}
Expand Down Expand Up @@ -672,7 +671,7 @@ std::string demangle_name(const std::string & input)
} // namespace liveliness

///=============================================================================
size_t hash_gid(const std::array<uint8_t, RMW_GID_STORAGE_SIZE> gid)
size_t hash_gid(const std::array<uint8_t, 16> gid)
{
std::stringstream hash_str;
hash_str << std::hex;
Expand Down
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/liveliness_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ class Entity
// Two entities are equal if their keyexpr_hash are equal.
bool operator==(const Entity & other) const;

std::array<uint8_t, RMW_GID_STORAGE_SIZE> copy_gid() const;
std::array<uint8_t, 16> copy_gid() const;

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

///=============================================================================
Expand Down Expand Up @@ -235,7 +235,7 @@ std::optional<rmw_qos_profile_t> keyexpr_to_qos(const std::string & keyexpr);
} // namespace liveliness

///=============================================================================
size_t hash_gid(const std::array<uint8_t, RMW_GID_STORAGE_SIZE> gid);
size_t hash_gid(const std::array<uint8_t, 16> gid);
} // namespace rmw_zenoh_cpp

///=============================================================================
Expand Down
26 changes: 6 additions & 20 deletions rmw_zenoh_cpp/src/detail/rmw_client_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,6 @@ std::shared_ptr<ClientData> 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<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 @@ -92,20 +89,9 @@ std::shared_ptr<ClientData> 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(
Expand Down Expand Up @@ -203,7 +189,7 @@ bool ClientData::liveliness_is_valid() const
}

///=============================================================================
std::array<uint8_t, RMW_GID_STORAGE_SIZE> ClientData::copy_gid() const
std::array<uint8_t, 16> ClientData::copy_gid() const
{
return entity_->copy_gid();
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -364,7 +350,7 @@ rmw_ret_t ClientData::send_request(

// Send request
zenoh::Session::GetOptions opts = zenoh::Session::GetOptions::create_default();
std::array<uint8_t, RMW_GID_STORAGE_SIZE> local_gid = entity_->copy_gid();
std::array<uint8_t, 16> 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
Expand Down
2 changes: 1 addition & 1 deletion rmw_zenoh_cpp/src/detail/rmw_client_data.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ class ClientData final : public std::enable_shared_from_this<ClientData>
bool liveliness_is_valid() const;

// Copy the GID of this ClientData into an rmw_gid_t.
std::array<uint8_t, RMW_GID_STORAGE_SIZE> copy_gid() const;
std::array<uint8_t, 16> copy_gid() const;

// Add a new ZenohReply to the queue.
void add_new_reply(std::unique_ptr<rmw_zenoh_cpp::ZenohReply> reply);
Expand Down
23 changes: 4 additions & 19 deletions rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,27 +60,12 @@ std::shared_ptr<PublisherData> 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<const message_type_support_callbacks_t *>(type_support->data);
auto message_type_support = std::make_unique<MessageTypeSupport>(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(
Expand Down Expand Up @@ -328,7 +313,7 @@ liveliness::TopicInfo PublisherData::topic_info() const
return entity_->topic_info().value();
}

std::array<uint8_t, RMW_GID_STORAGE_SIZE> PublisherData::copy_gid() const
std::array<uint8_t, 16> PublisherData::copy_gid() const
{
std::lock_guard<std::mutex> lock(mutex_);
return entity_->copy_gid();
Expand Down
2 changes: 1 addition & 1 deletion rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ class PublisherData final
liveliness::TopicInfo topic_info() const;

// Return a copy of the GID of this publisher.
std::array<uint8_t, RMW_GID_STORAGE_SIZE> copy_gid() const;
std::array<uint8_t, 16> copy_gid() const;

// Returns true if liveliness token is still valid.
bool liveliness_is_valid() const;
Expand Down
Loading

0 comments on commit e2780c2

Please sign in to comment.