diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml
index ff88958f..fb5bd053 100644
--- a/.github/workflows/style.yaml
+++ b/.github/workflows/style.yaml
@@ -21,4 +21,6 @@ jobs:
- name: uncrustify
run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/
- name: cpplint
- run: /ros_entrypoint.sh ament_cpplint rmw_zenoh_cpp/
+ # Ignore logging.hpp since the fmt::sprintf gets incorrectly flagged by ament_cpplint
+ # as a runtime/printf error.
+ run: /ros_entrypoint.sh ament_cpplint rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/logging.hpp
diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt
index 1e4c77db..8b72873d 100644
--- a/rmw_zenoh_cpp/CMakeLists.txt
+++ b/rmw_zenoh_cpp/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(fastcdr CONFIG REQUIRED)
+find_package(fmt CONFIG REQUIRED)
find_package(rcpputils REQUIRED)
find_package(rcutils REQUIRED)
find_package(rosidl_typesupport_fastrtps_c REQUIRED)
@@ -36,6 +37,7 @@ add_library(rmw_zenoh_cpp SHARED
src/detail/graph_cache.cpp
src/detail/guard_condition.cpp
src/detail/liveliness_utils.cpp
+ src/detail/logging.cpp
src/detail/message_type_support.cpp
src/detail/rmw_data_types.cpp
src/detail/service_type_support.cpp
@@ -59,6 +61,7 @@ target_link_libraries(rmw_zenoh_cpp
PRIVATE
ament_index_cpp::ament_index_cpp
fastcdr
+ fmt::fmt
rcpputils::rcpputils
rcutils::rcutils
rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c
@@ -105,12 +108,13 @@ add_executable(rmw_zenohd
src/zenohd/main.cpp
src/detail/zenoh_config.cpp
src/detail/liveliness_utils.cpp
+ src/detail/logging.cpp
)
target_link_libraries(rmw_zenohd
PRIVATE
ament_index_cpp::ament_index_cpp
- rcutils::rcutils
+ fmt::fmt
rcpputils::rcpputils
rmw::rmw
zenohc::lib
diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml
index 6304ab32..72ea95c8 100644
--- a/rmw_zenoh_cpp/package.xml
+++ b/rmw_zenoh_cpp/package.xml
@@ -14,6 +14,7 @@
ament_index_cpp
fastcdr
+ fmt
rcpputils
rcutils
rosidl_typesupport_fastrtps_c
diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp
index 03bec0ec..1185193c 100644
--- a/rmw_zenoh_cpp/src/detail/event.cpp
+++ b/rmw_zenoh_cpp/src/detail/event.cpp
@@ -16,7 +16,7 @@
#include "event.hpp"
-#include "rcutils/logging_macros.h"
+#include "logging_macros.hpp"
#include "rmw/error_handling.h"
@@ -148,7 +148,7 @@ void EventsManager::add_new_event(
std::deque> & event_queue = event_queues_[event_id];
if (event_queue.size() >= event_queue_depth_) {
// Log warning if message is discarded due to hitting the queue depth
- RCUTILS_LOG_DEBUG_NAMED(
+ RMW_ZENOH_LOG_DEBUG_NAMED(
"rmw_zenoh_cpp",
"Event queue depth of %ld reached, discarding oldest message "
"for event type %d",
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
index f23b910e..803383d9 100644
--- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp
+++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -25,7 +25,6 @@
#include "rcpputils/find_and_replace.hpp"
#include "rcpputils/scope_exit.hpp"
-#include "rcutils/logging_macros.h"
#include "rcutils/strdup.h"
#include "rmw/error_handling.h"
@@ -36,6 +35,7 @@
#include "rosidl_runtime_c/type_hash.h"
#include "graph_cache.hpp"
+#include "logging_macros.hpp"
#include "rmw_data_types.hpp"
namespace rmw_zenoh_cpp
@@ -131,7 +131,7 @@ void GraphCache::update_topic_map_for_put(
TopicDataPtr graph_topic_data = TopicData::make(entity);
if (graph_topic_data == nullptr) {
// This should not happen as topic_info should be populated for all non-node entities.
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"update_topic_map_for_put() called for non-node entity without valid TopicInfo. "
"Report this.");
@@ -339,7 +339,7 @@ void GraphCache::parse_put(
return;
}
if (ignore_from_current_session && is_entity_local(*entity)) {
- RCUTILS_LOG_DEBUG_NAMED(
+ RMW_ZENOH_LOG_DEBUG_NAMED(
"rmw_zenoh_cpp",
"Ignoring parse_put for %s from the same session.\n", entity->keyexpr().c_str());
return;
@@ -391,7 +391,7 @@ void GraphCache::parse_put(
update_topic_maps_for_put(node, entity);
total_nodes_in_graph_ += 1;
if (insertion_it == ns_it->second.end()) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to add a new node /%s to an "
"existing namespace %s in the graph. Report this bug.",
@@ -442,7 +442,7 @@ void GraphCache::update_topic_map_for_del(
bool report_events)
{
if (!entity->topic_info().has_value()) {
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"update_topic_maps_for_del() called for non-node entity without valid TopicInfo. "
"Report this.");
@@ -455,7 +455,7 @@ void GraphCache::update_topic_map_for_del(
topic_map.find(topic_info.name_);
if (cache_topic_it == topic_map.end()) {
// This should not happen.
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "topic name %s not found in topic_map. Report this.",
topic_info.name_.c_str());
return;
@@ -465,7 +465,7 @@ void GraphCache::update_topic_map_for_del(
cache_topic_it->second.find(topic_info.type_);
if (cache_topic_type_it == cache_topic_it->second.end()) {
// This should not happen.
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "topic type %s not found in for topic %s. Report this.",
topic_info.type_.c_str(), topic_info.name_.c_str());
return;
@@ -476,7 +476,7 @@ void GraphCache::update_topic_map_for_del(
qos_str);
if (cache_topic_qos_it == cache_topic_type_it->second.end()) {
// This should not happen.
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "qos %s not found in for topic type %s. Report this.",
qos_str.c_str(), topic_info.type_.c_str());
return;
@@ -557,7 +557,7 @@ void GraphCache::parse_del(
return;
}
if (ignore_from_current_session && is_entity_local(*entity)) {
- RCUTILS_LOG_DEBUG_NAMED(
+ RMW_ZENOH_LOG_DEBUG_NAMED(
"rmw_zenoh_cpp",
"Ignoring parse_del for %s from the same session.\n", entity->keyexpr().c_str());
return;
@@ -583,7 +583,7 @@ void GraphCache::parse_del(
});
if (node_it == range.second) {
// Node does not exist.
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token to remove unknown node /%s from the graph. Ignoring...",
entity->node_name().c_str()
@@ -602,7 +602,7 @@ void GraphCache::parse_del(
!graph_node->clients_.empty() ||
!graph_node->services_.empty())
{
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token to remove node /%s from the graph before all pub/subs/"
"clients/services for this node have been removed. Removing all entities first...",
@@ -658,7 +658,7 @@ rmw_ret_t GraphCache::get_node_names(
[node_names]() {
rcutils_ret_t ret = rcutils_string_array_fini(node_names);
if (ret != RCUTILS_RET_OK) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
}
@@ -673,7 +673,7 @@ rmw_ret_t GraphCache::get_node_names(
[node_namespaces]() {
rcutils_ret_t ret = rcutils_string_array_fini(node_namespaces);
if (ret != RCUTILS_RET_OK) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
}
@@ -682,7 +682,7 @@ rmw_ret_t GraphCache::get_node_names(
auto free_enclaves_lambda = [enclaves]() -> void {
rcutils_ret_t ret = rcutils_string_array_fini(enclaves);
if (ret != RCUTILS_RET_OK) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
}
@@ -1231,7 +1231,7 @@ void GraphCache::set_qos_event_callback(
std::lock_guard lock(graph_mutex_);
if (event_type > ZENOH_EVENT_ID_MAX) {
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"set_qos_event_callback() called for unsupported event. Report this.");
return;
diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
index 8810ef13..acd71e58 100644
--- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
+++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
@@ -23,8 +23,9 @@
#include
#include
+#include "logging_macros.hpp"
+
#include "rcpputils/scope_exit.hpp"
-#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
@@ -411,7 +412,7 @@ std::shared_ptr Entity::make(const std::string & keyexpr)
// (ADMIN_SPACE, domain_id, zid, id, entity_type, namespace, node_name).
// Basic validation.
if (parts.size() < KEYEXPR_INDEX_MIN + 1) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received invalid liveliness token with %lu/%d parts: %s",
parts.size(),
@@ -420,7 +421,7 @@ std::shared_ptr Entity::make(const std::string & keyexpr)
}
for (const std::string & p : parts) {
if (p.empty()) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received invalid liveliness token with empty parts: %s", keyexpr.c_str());
return nullptr;
@@ -428,7 +429,7 @@ std::shared_ptr Entity::make(const std::string & keyexpr)
}
if (parts[KeyexprIndex::AdminSpace] != ADMIN_SPACE) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token with invalid admin space.");
return nullptr;
@@ -439,7 +440,7 @@ std::shared_ptr Entity::make(const std::string & keyexpr)
std::unordered_map::const_iterator entity_it =
str_to_entity.find(entity_str);
if (entity_it == str_to_entity.end()) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token with invalid entity %s.", entity_str.c_str());
return nullptr;
@@ -458,14 +459,14 @@ std::shared_ptr Entity::make(const std::string & keyexpr)
// Populate topic_info if we have a token for an entity other than a node.
if (entity_type != EntityType::Node) {
if (parts.size() < KEYEXPR_INDEX_MAX + 1) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token for non-node entity without required parameters.");
return nullptr;
}
std::optional qos = keyexpr_to_qos(parts[KeyexprIndex::TopicQoS]);
if (!qos.has_value()) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Received liveliness token with invalid qos keyexpr");
return nullptr;
diff --git a/rmw_zenoh_cpp/src/detail/logging.cpp b/rmw_zenoh_cpp/src/detail/logging.cpp
new file mode 100644
index 00000000..2a7b2592
--- /dev/null
+++ b/rmw_zenoh_cpp/src/detail/logging.cpp
@@ -0,0 +1,72 @@
+// Copyright 2024 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 "logging.hpp"
+
+#include
+
+namespace rmw_zenoh_cpp
+{
+///=============================================================================
+Logger & Logger::get()
+{
+ static Logger logger(RMW_ZENOH_LOG_LEVEL_INFO);
+ return logger;
+}
+
+///=============================================================================
+Logger::Logger(LogLevel threshold_level)
+: threshold_level_(threshold_level)
+{}
+
+///=============================================================================
+fmt::color Logger::level_to_color(LogLevel level) const
+{
+ switch(level) {
+ case RMW_ZENOH_LOG_LEVEL_DEBUG:
+ return fmt::color::light_green;
+ case RMW_ZENOH_LOG_LEVEL_ERROR:
+ return fmt::color::indian_red;
+ case RMW_ZENOH_LOG_LEVEL_INFO:
+ return fmt::color::light_blue;
+ case RMW_ZENOH_LOG_LEVEL_WARN:
+ return fmt::color::light_yellow;
+ default:
+ return fmt::color::white;
+ }
+}
+
+///=============================================================================
+std::string Logger::level_to_string(LogLevel level) const
+{
+ switch(level) {
+ case RMW_ZENOH_LOG_LEVEL_DEBUG:
+ return "DEBUG";
+ case RMW_ZENOH_LOG_LEVEL_ERROR:
+ return "ERROR";
+ case RMW_ZENOH_LOG_LEVEL_INFO:
+ return "INFO";
+ case RMW_ZENOH_LOG_LEVEL_WARN:
+ return "WARN";
+ default:
+ return "UNKOWN";
+ }
+}
+
+///=============================================================================
+void Logger::set_log_level(LogLevel new_level)
+{
+ threshold_level_ = new_level;
+}
+} // namespace rmw_zenoh_cpp
diff --git a/rmw_zenoh_cpp/src/detail/logging.hpp b/rmw_zenoh_cpp/src/detail/logging.hpp
new file mode 100644
index 00000000..d721f5c9
--- /dev/null
+++ b/rmw_zenoh_cpp/src/detail/logging.hpp
@@ -0,0 +1,83 @@
+// Copyright 2024 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__LOGGING_HPP_
+#define DETAIL__LOGGING_HPP_
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+namespace rmw_zenoh_cpp
+{
+///=============================================================================
+enum LogLevel
+{
+ RMW_ZENOH_LOG_LEVEL_DEBUG,
+ RMW_ZENOH_LOG_LEVEL_INFO,
+ RMW_ZENOH_LOG_LEVEL_WARN,
+ RMW_ZENOH_LOG_LEVEL_ERROR,
+};
+
+///=============================================================================
+class Logger {
+public:
+ // Get a static reference to the logger.
+ static Logger & get();
+
+ // Set the threshold log level.
+ void set_log_level(LogLevel new_level);
+
+ // Log to the console.
+ template
+ void log_named(
+ LogLevel level,
+ const std::string & name,
+ const std::string & message,
+ const Args &... args) const
+ {
+ if (level >= threshold_level_) {
+ const auto time_now = std::chrono::system_clock::now().time_since_epoch();
+ const auto seconds =
+ std::chrono::duration_cast(time_now).count();
+ auto nanoseconds =
+ std::chrono::duration_cast(
+ time_now - std::chrono::seconds(seconds)).count();
+ fmt::print(
+ fmt::fg(level_to_color(level)),
+ "[{}] [{}.{}] [{}]: {}\n",
+ level_to_string(level),
+ seconds,
+ nanoseconds,
+ name,
+ fmt::sprintf(message, args ...)
+ );
+ }
+ }
+
+private:
+ LogLevel threshold_level_;
+ explicit Logger(LogLevel threshold_level);
+ fmt::color level_to_color(LogLevel level) const;
+ std::string level_to_string(LogLevel level) const;
+};
+
+} // namespace rmw_zenoh_cpp
+
+#endif // DETAIL__LOGGING_HPP_
diff --git a/rmw_zenoh_cpp/src/detail/logging_macros.hpp b/rmw_zenoh_cpp/src/detail/logging_macros.hpp
new file mode 100644
index 00000000..4d8cae91
--- /dev/null
+++ b/rmw_zenoh_cpp/src/detail/logging_macros.hpp
@@ -0,0 +1,37 @@
+// Copyright 2024 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__LOGGING_MACROS_HPP_
+#define DETAIL__LOGGING_MACROS_HPP_
+
+#include "logging.hpp"
+
+///=============================================================================
+// We define custom logging marcos to log to console as it was discovered
+// that relying on RCUTILS_LOG_X_NAMED functions also end up logging over /rosout
+// which can lead to deadlocks in rmw_zenoh especially when multiple threads
+// invoke GraphCache::parse_put() and GraphCache::parse_del() functions.
+// See https://github.com/ros2/rmw_zenoh/issues/182 for more details.
+// Note: Consider adding a set_rmw_log_level() API to rmw which can override the
+// default log level at the RMW layer.
+#define RMW_ZENOH_LOG_DEBUG_NAMED(...) { \
+ rmw_zenoh_cpp::Logger::get().log_named(rmw_zenoh_cpp::RMW_ZENOH_LOG_LEVEL_DEBUG, __VA_ARGS__);}
+#define RMW_ZENOH_LOG_ERROR_NAMED(...) { \
+ rmw_zenoh_cpp::Logger::get().log_named(rmw_zenoh_cpp::RMW_ZENOH_LOG_LEVEL_ERROR, __VA_ARGS__);}
+#define RMW_ZENOH_LOG_INFO_NAMED(...) { \
+ rmw_zenoh_cpp::Logger::get().log_named(rmw_zenoh_cpp::RMW_ZENOH_LOG_LEVEL_INFO, __VA_ARGS__);}
+#define RMW_ZENOH_LOG_WARN_NAMED(...) { \
+ rmw_zenoh_cpp::Logger::get().log_named(rmw_zenoh_cpp::RMW_ZENOH_LOG_LEVEL_WARN, __VA_ARGS__);}
+
+#endif // DETAIL__LOGGING_MACROS_HPP_
diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
index c2b94ffb..4ebdfa03 100644
--- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
+++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
@@ -23,8 +23,9 @@
#include
#include
+#include "logging_macros.hpp"
+
#include "rcpputils/scope_exit.hpp"
-#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
@@ -140,7 +141,7 @@ void rmw_subscription_data_t::add_new_message(
if (message_queue_.size() >= adapted_qos_profile.depth) {
// Log warning if message is discarded due to hitting the queue depth
- RCUTILS_LOG_DEBUG_NAMED(
+ RMW_ZENOH_LOG_DEBUG_NAMED(
"rmw_zenoh_cpp",
"Message queue depth of %ld reached, discarding oldest message "
"for subscription for %s",
@@ -237,7 +238,7 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query)
if (query_queue_.size() >= adapted_qos_profile.depth) {
// Log warning if message is discarded due to hitting the queue depth
z_owned_str_t keystr = z_keyexpr_to_string(z_loan(this->keyexpr));
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Query queue depth of %ld reached, discarding oldest Query "
"for service for %s",
@@ -330,7 +331,7 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply)
if (reply_queue_.size() >= adapted_qos_profile.depth) {
// Log warning if message is discarded due to hitting the queue depth
z_owned_str_t keystr = z_keyexpr_to_string(z_loan(this->keyexpr));
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Reply queue depth of %ld reached, discarding oldest reply "
"for client for %s",
@@ -396,7 +397,7 @@ void sub_data_handler(
auto sub_data = static_cast(data);
if (sub_data == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to obtain rmw_subscription_data_t from data for "
"subscription for %s",
@@ -410,7 +411,8 @@ void sub_data_handler(
// We failed to get the GID from the attachment. While this isn't fatal,
// it is unusual and so we should report it.
memset(pub_gid, 0, RMW_GID_STORAGE_SIZE);
- RCUTILS_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to obtain publisher GID from the attachment.");
+ RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp",
+ "Unable to obtain publisher GID from the attachment.");
}
int64_t sequence_number = get_int64_from_attachment(&sample->attachment, "sequence_number");
@@ -418,7 +420,7 @@ void sub_data_handler(
// We failed to get the sequence number from the attachment. While this
// isn't fatal, it is unusual and so we should report it.
sequence_number = 0;
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment.");
}
@@ -427,7 +429,7 @@ void sub_data_handler(
// We failed to get the source timestamp from the attachment. While this
// isn't fatal, it is unusual and so we should report it.
source_timestamp = 0;
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment.");
}
@@ -467,7 +469,7 @@ void service_data_handler(const z_query_t * query, void * data)
rmw_service_data_t * service_data =
static_cast(data);
if (service_data == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to obtain rmw_service_data_t from data for "
"service for %s",
@@ -513,14 +515,14 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
{
auto client_data = static_cast(data);
if (client_data == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to obtain client_data_t "
);
return;
}
if (!z_reply_check(reply)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"z_reply_check returned False"
);
@@ -529,12 +531,12 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
if (!z_reply_is_ok(reply)) {
z_owned_str_t keystr = z_keyexpr_to_string(z_loan(client_data->keyexpr));
z_value_t err = z_reply_err(reply);
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"z_reply_is_ok returned False for keyexpr %s. Reason: %.*s",
z_loan(keystr),
(int)err.payload.len,
- err.payload.start);
+ fmt::ptr(err.payload.start));
z_drop(z_move(keystr));
return;
diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp
index dcf2048c..5cb29172 100644
--- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp
+++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp
@@ -15,11 +15,12 @@
#include "zenoh_config.hpp"
#include
-#include
#include
#include
+#include "logging_macros.hpp"
+
#include
#include
@@ -50,7 +51,7 @@ rmw_ret_t _get_z_config(
// Get the path to the zenoh configuration file from the environment variable.
if (NULL != rcutils_get_env(envar_name, &envar_uri)) {
// NULL is returned if everything is ok.
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "Envar %s cannot be read.", envar_name);
return RMW_RET_ERROR;
}
@@ -66,12 +67,12 @@ rmw_ret_t _get_z_config(
}
// Verify that the configuration is valid.
if (!z_config_check(config)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Invalid configuration file %s", configured_uri);
return RMW_RET_ERROR;
}
- RCUTILS_LOG_DEBUG_NAMED(
+ RMW_ZENOH_LOG_DEBUG_NAMED(
"rmw_zenoh_cpp",
"configured using configuration file %s", configured_uri);
return RMW_RET_OK;
@@ -83,7 +84,7 @@ rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * con
{
auto envar_map_it = envar_map.find(entity);
if (envar_map_it == envar_map.end()) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "get_z_config called with invalid ConfigurableEntity.");
return RMW_RET_ERROR;
}
@@ -104,7 +105,7 @@ std::optional zenoh_router_check_attempts()
if (NULL != rcutils_get_env(router_check_attempts_envar, &envar_value)) {
// NULL is returned if everything is ok.
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.",
router_check_attempts_envar);
return default_value;
diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp
index b23ccb1a..ead5ca5a 100644
--- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp
+++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp
@@ -15,12 +15,12 @@
#include "zenoh_router_check.hpp"
#include
-#include
#include
#include
#include
+#include "logging_macros.hpp"
#include "liveliness_utils.hpp"
namespace rmw_zenoh_cpp
@@ -34,7 +34,7 @@ rmw_ret_t zenoh_router_check(z_session_t session)
// Define callback
auto callback = [](const struct z_id_t * id, void * ctx) {
const std::string id_str = liveliness::zid_to_str(*id);
- RCUTILS_LOG_INFO_NAMED(
+ RMW_ZENOH_LOG_INFO_NAMED(
"rmw_zenoh_cpp",
"Successfully connected to a Zenoh router with id %s.", id_str.c_str());
// Note: Callback is guaranteed to never be called
@@ -45,13 +45,13 @@ rmw_ret_t zenoh_router_check(z_session_t session)
rmw_ret_t ret = RMW_RET_OK;
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(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Failed to evaluate if Zenoh routers are connected to the session.");
ret = RMW_RET_ERROR;
} else {
if (context == 0) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to connect to a Zenoh router. "
"Have you started a router with `ros2 run rmw_zenoh_cpp rmw_zenohd`?");
diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp
index 04126ece..49308a76 100644
--- a/rmw_zenoh_cpp/src/rmw_init.cpp
+++ b/rmw_zenoh_cpp/src/rmw_init.cpp
@@ -26,7 +26,7 @@
#include "detail/zenoh_router_check.hpp"
#include "rcutils/env.h"
-#include "rcutils/logging_macros.h"
+#include "detail/logging_macros.hpp"
#include "rcutils/strdup.h"
#include "rcutils/types.h"
@@ -58,7 +58,7 @@ static void graph_sub_data_handler(
rmw_context_impl_s * context_impl = static_cast(
data);
if (context_impl == nullptr) {
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"[graph_sub_data_handler] Unable to convert data into context_impl"
);
@@ -78,7 +78,7 @@ static void graph_sub_data_handler(
rmw_ret_t rmw_ret = rmw_trigger_guard_condition(context_impl->graph_guard_condition);
if (RMW_RET_OK != rmw_ret) {
- RCUTILS_LOG_WARN_NAMED(
+ RMW_ZENOH_LOG_WARN_NAMED(
"rmw_zenoh_cpp",
"[graph_sub_data_handler] Unable to trigger graph guard condition"
);
diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
index 8aa972b9..a139f191 100644
--- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp
+++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -33,6 +33,7 @@
#include "detail/graph_cache.hpp"
#include "detail/identifier.hpp"
#include "detail/liveliness_utils.hpp"
+#include "detail/logging_macros.hpp"
#include "detail/message_type_support.hpp"
#include "detail/rmw_data_types.hpp"
#include "detail/serialization_format.hpp"
@@ -41,7 +42,6 @@
#include "rcpputils/scope_exit.hpp"
#include "rcutils/env.h"
-#include "rcutils/logging_macros.h"
#include "rcutils/strdup.h"
#include "rcutils/types.h"
@@ -312,7 +312,7 @@ rmw_create_node(
rmw_zenoh_cpp::liveliness::NodeInfo{context->actual_domain_id, namespace_, name,
context->impl->enclave});
if (node_data->entity == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to generate keyexpr for liveliness token for the node.");
return nullptr;
@@ -327,7 +327,7 @@ rmw_create_node(
z_drop(z_move(node_data->token));
});
if (!z_check(node_data->token)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to create liveliness token for the node.");
return nullptr;
@@ -693,7 +693,7 @@ rmw_create_publisher(
publisher_data->adapted_qos_profile}
);
if (publisher_data->entity == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to generate keyexpr for liveliness token for the publisher.");
return nullptr;
@@ -710,7 +710,7 @@ rmw_create_publisher(
}
});
if (!z_check(publisher_data->token)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to create liveliness token for the publisher.");
return nullptr;
@@ -1535,7 +1535,7 @@ rmw_create_subscription(
sub_data->adapted_qos_profile}
);
if (sub_data->entity == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to generate keyexpr for liveliness token for the subscription.");
return nullptr;
@@ -1552,7 +1552,7 @@ rmw_create_subscription(
}
});
if (!z_check(sub_data->token)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to create liveliness token for the subscription.");
return nullptr;
@@ -2235,7 +2235,7 @@ rmw_create_client(
if (std::string::npos != suffix_substring_position) {
service_type = service_type.substr(0, suffix_substring_position);
} else {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unexpected type %s for client %s. Report this bug",
service_type.c_str(), rmw_client->service_name);
@@ -2287,7 +2287,7 @@ rmw_create_client(
client_data->adapted_qos_profile}
);
if (client_data->entity == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to generate keyexpr for liveliness token for the client.");
return nullptr;
@@ -2304,7 +2304,7 @@ rmw_create_client(
}
});
if (!z_check(client_data->token)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to create liveliness token for the client.");
return nullptr;
@@ -2800,7 +2800,7 @@ rmw_create_service(
if (std::string::npos != suffix_substring_position) {
service_type = service_type.substr(0, suffix_substring_position);
} else {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unexpected type %s for service %s. Report this bug",
service_type.c_str(), rmw_service->service_name);
@@ -2875,7 +2875,7 @@ rmw_create_service(
service_data->adapted_qos_profile}
);
if (service_data->entity == nullptr) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to generate keyexpr for liveliness token for the service.");
return nullptr;
@@ -2892,7 +2892,7 @@ rmw_create_service(
}
});
if (!z_check(service_data->token)) {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unable to create liveliness token for the service.");
return nullptr;
@@ -3858,7 +3858,7 @@ rmw_service_server_is_available(
if (std::string::npos != suffix_substring_position) {
service_type = service_type.substr(0, suffix_substring_position);
} else {
- RCUTILS_LOG_ERROR_NAMED(
+ RMW_ZENOH_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
"Unexpected type %s for client %s. Report this bug",
service_type.c_str(), client->service_name);