diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index f4101cfe..f403c364 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -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 diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp new file mode 100644 index 00000000..2f6ed398 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -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 +#include + +#include +#include +#include + +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(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(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(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; +} diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp new file mode 100644 index 00000000..652b2706 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp @@ -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 + +#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_ diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 35d8fc1f..9b01b904 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -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" @@ -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)