Skip to content

Commit

Permalink
Verify a zenoh router is available for the session (#61)
Browse files Browse the repository at this point in the history
* Verify zenoh router presence at rmw_init.

* Fail when an error in z_info_routers_zid is found.

Signed-off-by: Franco Cipollone <[email protected]>
  • Loading branch information
francocipollone authored Nov 17, 2023
1 parent d3d68dc commit 9752924
Show file tree
Hide file tree
Showing 4 changed files with 124 additions and 0 deletions.
1 change: 1 addition & 0 deletions rmw_zenoh_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ add_library(rmw_zenoh_cpp SHARED
src/detail/type_support.cpp
src/detail/type_support_common.cpp
src/detail/zenoh_config.cpp
src/detail/zenoh_router_check.cpp
src/rmw_event.cpp
src/rmw_get_network_flow_endpoints.cpp
src/rmw_get_node_info_and_types.cpp
Expand Down
89 changes: 89 additions & 0 deletions rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "zenoh_router_check.hpp"

#include <rcutils/env.h>
#include <rcutils/logging_macros.h>

#include <iomanip>
#include <sstream>
#include <string>

namespace
{

// Convert a Zenoh Id to a string
// Zenoh IDs are LSB-first 128bit unsigned and non-zero integers in hexadecimal lowercase.
// @param pid Zenoh Id to convert
std::string zid_to_str(const z_id_t & pid)
{
std::stringstream ss;
int len = 0;
for (size_t i = 0; i < sizeof(pid.id); ++i) {
if (pid.id[i]) {
len = static_cast<int>(i) + 1;
}
}
if (!len) {
ss << "";
} else {
for (int i = len - 1; i >= 0; --i) {
ss << std::hex << std::setfill('0') << std::setw(2) << static_cast<int>(pid.id[i]);
}
}
return ss.str();
}

} // namespace

rmw_ret_t zenoh_router_check(z_session_t session)
{
// Initialize context for callback
int context = 0;

// Define callback
auto callback = [](const struct z_id_t * id, void * ctx) {
const std::string id_str = zid_to_str(*id);
RCUTILS_LOG_INFO_NAMED(
"ZenohRouterCheck",
"A Zenoh router connected to the session with id '%s'", id_str.c_str());
// Note: Callback is guaranteed to never be called
// concurrently according to z_info_routers_zid docstring
(*(static_cast<int *>(ctx)))++;
};

rmw_ret_t ret;
z_owned_closure_zid_t router_callback = z_closure(callback, nullptr /* drop */, &context);
if (z_info_routers_zid(session, z_move(router_callback))) {
RCUTILS_LOG_ERROR_NAMED(
"ZenohRouterCheck",
"Failed to evaluate if Zenoh routers are connected to the session");
ret = RMW_RET_ERROR;
} else {
if (context == 0) {
RCUTILS_LOG_ERROR_NAMED(
"ZenohRouterCheck",
"No Zenoh router connected to the session");
ret = RMW_RET_ERROR;
} else {
RCUTILS_LOG_INFO_NAMED(
"ZenohRouterCheck",
"There are %d Zenoh routers connected to the session", context);
ret = RMW_RET_OK;
}
}

return ret;
}
27 changes: 27 additions & 0 deletions rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef DETAIL__ZENOH_ROUTER_CHECK_HPP_
#define DETAIL__ZENOH_ROUTER_CHECK_HPP_

#include <zenoh.h>

#include "rmw/ret_types.h"

/// Check if a Zenoh router is connected to the session.
/// @param session Zenoh session to check.
/// @return RMW_RET_OK if a Zenoh router is connected to the session.
rmw_ret_t zenoh_router_check(z_session_t session);

#endif // DETAIL__ZENOH_ROUTER_CHECK_HPP_
7 changes: 7 additions & 0 deletions rmw_zenoh_cpp/src/rmw_init.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "detail/identifier.hpp"
#include "detail/rmw_data_types.hpp"
#include "detail/zenoh_config.hpp"
#include "detail/zenoh_router_check.hpp"

#include "rcutils/env.h"
#include "rcutils/logging_macros.h"
Expand Down Expand Up @@ -173,6 +174,12 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
z_close(z_move(context->impl->session));
});

// Verify if the zenoh router is running.
if ((ret = zenoh_router_check(z_loan(context->impl->session))) != RMW_RET_OK) {
RMW_SET_ERROR_MSG("Error while checking for Zenoh router");
return ret;
}

// Initialize the shm manager if shared_memory is enabled in the config.
if (shm_enabled._cstr != nullptr &&
strcmp(shm_enabled._cstr, "true") == 0)
Expand Down

0 comments on commit 9752924

Please sign in to comment.