From be3bb3b9d8597f3a433ab84e9778c8c11bc24ccb Mon Sep 17 00:00:00 2001
From: Koichi <koichi.imai.2@tier4.jp>
Date: Sat, 28 Feb 2026 16:14:57 +0900
Subject: [PATCH 01/11] fix error handling in agnocastlib

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>
---
 .../agnocast/agnocast_callback_info.hpp       |   6 +-
 .../include/agnocast/agnocast_publisher.hpp   |   4 +-
 .../agnocast/agnocast_subscription.hpp        |   9 +-
 .../include/agnocast/agnocast_utils.hpp       |   6 +-
 src/agnocastlib/src/agnocast.cpp              | 107 +++++++-----------
 .../src/agnocast_callback_info.cpp            |   5 +-
 src/agnocastlib/src/agnocast_client.cpp       |   5 +-
 src/agnocastlib/src/agnocast_publisher.cpp    |  25 ++--
 .../src/agnocast_smart_pointer.cpp            |   5 +-
 src/agnocastlib/src/agnocast_subscription.cpp |  19 ++--
 src/agnocastlib/src/agnocast_utils.cpp        |   7 +-
 11 files changed, 71 insertions(+), 127 deletions(-)

diff --git a/src/agnocastlib/include/agnocast/agnocast_callback_info.hpp b/src/agnocastlib/include/agnocast/agnocast_callback_info.hpp
index 13b66fbf7..879e16e70 100644
--- a/src/agnocastlib/include/agnocast/agnocast_callback_info.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_callback_info.hpp
@@ -69,10 +69,8 @@ TypeErasedCallback get_erased_callback(Func && callback)
       auto && typed_arg = static_cast<TypedMessagePtr<T> &&>(arg);
       callback(std::move(typed_arg).get());
     } else {
-      RCLCPP_ERROR(
-        logger, "Agnocast internal implementation error: bad allocation when callback is called");
-      close(agnocast_fd);
-      exit(EXIT_FAILURE);
+      throw std::runtime_error(
+        "Agnocast internal implementation error: bad allocation when callback is called");
     }
   };
 }
diff --git a/src/agnocastlib/include/agnocast/agnocast_publisher.hpp b/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
index b08a561f3..d70c49304 100644
--- a/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
@@ -175,9 +175,7 @@ class BasicPublisher
   void publish(ipc_shared_ptr<MessageT> && message)
   {
     if (!message || topic_name_ != message.get_topic_name()) {
-      RCLCPP_ERROR(logger, "Invalid message to publish.");
-      close(agnocast_fd);
-      exit(EXIT_FAILURE);
+      throw std::runtime_error("Invalid message to publish.");
     }
 
     // Capture raw pointer BEFORE invalidation (get() returns nullptr after invalidation).
diff --git a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
index 641462f4f..9d3b6e2f1 100644
--- a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
@@ -60,9 +60,7 @@ rclcpp::CallbackGroup::SharedPtr get_valid_callback_group(
 
   if (callback_group) {
     if (!node->get_node_base_interface()->callback_group_in_node(callback_group)) {
-      RCLCPP_ERROR(logger, "Cannot create agnocast subscription, callback group not in node.");
-      close(agnocast_fd);
-      exit(EXIT_FAILURE);
+      throw std::runtime_error("Cannot create agnocast subscription, callback group not in node.");
     }
   } else {
     callback_group = node->get_node_base_interface()->get_default_callback_group();
@@ -296,9 +294,8 @@ class BasicTakeSubscription : public SubscriptionBase
       std::lock_guard<std::mutex> lock(mmap_mtx);
 
       if (ioctl(agnocast_fd, AGNOCAST_TAKE_MSG_CMD, &take_args) < 0) {
-        RCLCPP_ERROR(logger, "AGNOCAST_TAKE_MSG_CMD failed: %s", strerror(errno));
-        close(agnocast_fd);
-        exit(EXIT_FAILURE);
+        throw std::runtime_error(
+          std::string("AGNOCAST_TAKE_MSG_CMD failed: ") + strerror(errno));
       }
 
       for (uint32_t i = 0; i < take_args.ret_pub_shm_num; i++) {
diff --git a/src/agnocastlib/include/agnocast/agnocast_utils.hpp b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
index e5d1f6775..9442913f8 100644
--- a/src/agnocastlib/include/agnocast/agnocast_utils.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
@@ -3,6 +3,7 @@
 #include "agnocast/agnocast_ioctl.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <stdexcept>
 #include <string>
 
 namespace agnocast
@@ -15,9 +16,8 @@ extern bool is_bridge_process;
 inline void validate_qos(const rclcpp::QoS & qos)
 {
   if (qos.history() == rclcpp::HistoryPolicy::KeepAll) {
-    RCLCPP_ERROR(logger, "Agnocast does not support KeepAll history policy. Use KeepLast instead.");
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      "Agnocast does not support KeepAll history policy. Use KeepLast instead.");
   }
 
   const auto & rmw_qos = qos.get_rmw_qos_profile();
diff --git a/src/agnocastlib/src/agnocast.cpp b/src/agnocastlib/src/agnocast.cpp
index 53288eb03..453a30562 100644
--- a/src/agnocastlib/src/agnocast.cpp
+++ b/src/agnocastlib/src/agnocast.cpp
@@ -49,9 +49,7 @@ void * map_area(
   const int shm_mode = 0666;
   int shm_fd = shm_open(shm_name.c_str(), oflag, shm_mode);
   if (shm_fd == -1) {
-    RCLCPP_ERROR(logger, "shm_open failed: %s", strerror(errno));
-    close(agnocast_fd);
-    return nullptr;
+    throw std::runtime_error(std::string("shm_open failed: ") + strerror(errno));
   }
 
   {
@@ -72,18 +70,14 @@ void * map_area(
 
   if (writable) {
     if (ftruncate(shm_fd, static_cast<off_t>(shm_size)) == -1) {
-      RCLCPP_ERROR(logger, "ftruncate failed: %s", strerror(errno));
       cleanup_shm_fd();
-      close(agnocast_fd);
-      return nullptr;
+      throw std::runtime_error(std::string("ftruncate failed: ") + strerror(errno));
     }
 
     const int new_shm_mode = 0444;
     if (fchmod(shm_fd, new_shm_mode) == -1) {
-      RCLCPP_ERROR(logger, "fchmod failed: %s", strerror(errno));
       cleanup_shm_fd();
-      close(agnocast_fd);
-      return nullptr;
+      throw std::runtime_error(std::string("fchmod failed: ") + strerror(errno));
     }
   }
 
@@ -93,10 +87,8 @@ void * map_area(
     0);
 
   if (ret == MAP_FAILED) {
-    RCLCPP_ERROR(logger, "mmap failed: %s", strerror(errno));
     cleanup_shm_fd();
-    close(agnocast_fd);
-    return nullptr;
+    throw std::runtime_error(std::string("mmap failed: ") + strerror(errno));
   }
 
   return ret;
@@ -109,9 +101,7 @@ void * map_writable_area(const pid_t pid, const uint64_t shm_addr, const uint64_
 
 void map_read_only_area(const pid_t pid, const uint64_t shm_addr, const uint64_t shm_size)
 {
-  if (map_area(pid, shm_addr, shm_size, false) == nullptr) {
-    exit(EXIT_FAILURE);
-  }
+  map_area(pid, shm_addr, shm_size, false);
 }
 
 // Initializes the child allocator for bridge functionality.
@@ -154,10 +144,6 @@ initialize_agnocast_result acquire_agnocast_resources_for_bridge()
   void * mempool_ptr =
     map_writable_area(getpid(), add_process_args.ret_addr, add_process_args.ret_shm_size);
 
-  if (mempool_ptr == nullptr) {
-    throw std::runtime_error("map_writable_area failed.");
-  }
-
   return {
     mempool_ptr,
     add_process_args.ret_shm_size,
@@ -167,9 +153,8 @@ initialize_agnocast_result acquire_agnocast_resources_for_bridge()
 void poll_for_unlink()
 {
   if (setsid() == -1) {
-    RCLCPP_ERROR(logger, "setsid failed for unlink daemon: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("setsid failed for unlink daemon: ") + strerror(errno));
   }
 
   while (true) {
@@ -179,9 +164,8 @@ void poll_for_unlink()
     do {
       get_exit_process_args = {};
       if (ioctl(agnocast_fd, AGNOCAST_GET_EXIT_PROCESS_CMD, &get_exit_process_args) < 0) {
-        RCLCPP_ERROR(logger, "AGNOCAST_GET_EXIT_PROCESS_CMD failed: %s", strerror(errno));
-        close(agnocast_fd);
-        exit(EXIT_FAILURE);
+        throw std::runtime_error(
+          std::string("AGNOCAST_GET_EXIT_PROCESS_CMD failed: ") + strerror(errno));
       }
 
       if (get_exit_process_args.ret_pid > 0) {
@@ -211,30 +195,21 @@ void poll_for_unlink()
 void poll_for_bridge_manager([[maybe_unused]] pid_t target_pid)
 {
   if (setsid() == -1) {
-    RCLCPP_ERROR(logger, "setsid failed for unlink daemon: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
-  }
-
-  try {
-    const auto resources = acquire_agnocast_resources_for_bridge();
-    initialize_bridge_allocator(resources.mempool_ptr, resources.mempool_size);
-
-    auto bridge_mode = get_bridge_mode();
-    if (bridge_mode == BridgeMode::Standard) {
-      StandardBridgeManager manager(target_pid);
-      manager.run();
-    } else if (bridge_mode == BridgeMode::Performance) {
-      {
-        PerformanceBridgeManager manager;
-        manager.run();
-      }
-    }
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(logger, "BridgeManager crashed: %s", e.what());
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("setsid failed for bridge manager: ") + strerror(errno));
+  }
+
+  const auto resources = acquire_agnocast_resources_for_bridge();
+  initialize_bridge_allocator(resources.mempool_ptr, resources.mempool_size);
+
+  auto bridge_mode = get_bridge_mode();
+  if (bridge_mode == BridgeMode::Standard) {
+    StandardBridgeManager manager(target_pid);
+    manager.run();
+  } else if (bridge_mode == BridgeMode::Performance) {
+    PerformanceBridgeManager manager;
+    manager.run();
   }
-  exit(0);
 }
 
 struct semver
@@ -366,9 +341,7 @@ pid_t spawn_daemon_process(Func && func)
 {
   pid_t pid = fork();
   if (pid < 0) {
-    RCLCPP_ERROR(logger, "fork failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(std::string("fork failed: ") + strerror(errno));
   }
   if (pid == 0) {
     agnocast::is_bridge_process = true;
@@ -384,7 +357,12 @@ pid_t spawn_daemon_process(Func && func)
       close(devnull);
     }
 
-    func();
+    try {
+      func();
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(logger, "Daemon process failed: %s", e.what());
+      exit(EXIT_FAILURE);
+    }
     exit(0);
   }
 
@@ -396,37 +374,32 @@ struct initialize_agnocast_result initialize_agnocast(
   const unsigned char * heaphook_version_ptr, const size_t heaphook_version_str_len)
 {
   if (agnocast_fd >= 0) {
-    RCLCPP_ERROR(logger, "Agnocast is already open");
-    exit(EXIT_FAILURE);
+    throw std::runtime_error("Agnocast is already open");
   }
 
   agnocast_fd = open("/dev/agnocast", O_RDWR);
   if (agnocast_fd < 0) {
     if (errno == ENOENT) {
-      RCLCPP_ERROR(logger, "%s", AGNOCAST_DEVICE_NOT_FOUND_MSG);
+      throw std::runtime_error(AGNOCAST_DEVICE_NOT_FOUND_MSG);
     } else {
-      RCLCPP_ERROR(logger, "Failed to open /dev/agnocast: %s", strerror(errno));
+      throw std::runtime_error(std::string("Failed to open /dev/agnocast: ") + strerror(errno));
     }
-    exit(EXIT_FAILURE);
   }
 
   struct ioctl_get_version_args get_version_args = {};
   if (ioctl(agnocast_fd, AGNOCAST_GET_VERSION_CMD, &get_version_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_GET_VERSION_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_GET_VERSION_CMD failed: ") + strerror(errno));
   }
 
   if (!is_version_consistent(heaphook_version_ptr, heaphook_version_str_len, get_version_args)) {
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error("Agnocast version mismatch");
   }
 
   union ioctl_add_process_args add_process_args = {};
   if (ioctl(agnocast_fd, AGNOCAST_ADD_PROCESS_CMD, &add_process_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_ADD_PROCESS_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_ADD_PROCESS_CMD failed: ") + strerror(errno));
   }
 
   pid_t target_pid = 0;
@@ -455,10 +428,6 @@ struct initialize_agnocast_result initialize_agnocast(
 
   void * mempool_ptr =
     map_writable_area(getpid(), add_process_args.ret_addr, add_process_args.ret_shm_size);
-  if (mempool_ptr == nullptr) {
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
-  }
 
   struct initialize_agnocast_result result = {};
   result.mempool_ptr = mempool_ptr;
diff --git a/src/agnocastlib/src/agnocast_callback_info.cpp b/src/agnocastlib/src/agnocast_callback_info.cpp
index 244889f7b..474aacca4 100644
--- a/src/agnocastlib/src/agnocast_callback_info.cpp
+++ b/src/agnocastlib/src/agnocast_callback_info.cpp
@@ -43,9 +43,8 @@ void receive_and_execute_message(
     std::lock_guard<std::mutex> lock(mmap_mtx);
 
     if (ioctl(agnocast_fd, AGNOCAST_RECEIVE_MSG_CMD, &receive_args) < 0) {
-      RCLCPP_ERROR(logger, "AGNOCAST_RECEIVE_MSG_CMD failed: %s", strerror(errno));
-      close(agnocast_fd);
-      exit(EXIT_FAILURE);
+      throw std::runtime_error(
+        std::string("AGNOCAST_RECEIVE_MSG_CMD failed: ") + strerror(errno));
     }
 
     // Map the shared memory region with read permissions whenever a new publisher is discovered.
diff --git a/src/agnocastlib/src/agnocast_client.cpp b/src/agnocastlib/src/agnocast_client.cpp
index 62d89250a..f6c0ae5d8 100644
--- a/src/agnocastlib/src/agnocast_client.cpp
+++ b/src/agnocastlib/src/agnocast_client.cpp
@@ -21,9 +21,8 @@ uint32_t get_agnocast_sub_count(const std::string & topic_name)
   topic_info_args.topic_info_ret_buffer_addr = reinterpret_cast<uint64_t>(topic_info_buffer.data());
   topic_info_args.topic_info_ret_buffer_size = MAX_TOPIC_INFO_RET_NUM;
   if (ioctl(agnocast_fd, AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD, &topic_info_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_GET_TOPIC_SUBSCRIBER_INFO_CMD failed: ") + strerror(errno));
   }
 
   return topic_info_args.ret_topic_info_ret_num;
diff --git a/src/agnocastlib/src/agnocast_publisher.cpp b/src/agnocastlib/src/agnocast_publisher.cpp
index 00f9538a5..3c94d3913 100644
--- a/src/agnocastlib/src/agnocast_publisher.cpp
+++ b/src/agnocastlib/src/agnocast_publisher.cpp
@@ -32,11 +32,8 @@ void increment_borrowed_publisher_num()
 void decrement_borrowed_publisher_num()
 {
   if (borrowed_publisher_num == 0) {
-    RCLCPP_ERROR(
-      logger,
+    throw std::runtime_error(
       "The number of publish() called exceeds the number of borrow_loaned_message() called.");
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
   }
   borrowed_publisher_num--;
 }
@@ -54,9 +51,8 @@ topic_local_id_t initialize_publisher(
   pub_args.qos_is_transient_local = qos.durability() == rclcpp::DurabilityPolicy::TransientLocal;
   pub_args.is_bridge = is_bridge;
   if (ioctl(agnocast_fd, AGNOCAST_ADD_PUBLISHER_CMD, &pub_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_ADD_PUBLISHER_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_ADD_PUBLISHER_CMD failed: ") + strerror(errno));
   }
 
   return pub_args.ret_id;
@@ -80,9 +76,8 @@ union ioctl_publish_msg_args publish_core(
   publish_msg_args.subscriber_ids_buffer_size = MAX_SUBSCRIBER_NUM;
 
   if (ioctl(agnocast_fd, AGNOCAST_PUBLISH_MSG_CMD, &publish_msg_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_PUBLISH_MSG_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_PUBLISH_MSG_CMD failed: ") + strerror(errno));
   }
 
   TRACEPOINT(agnocast_publish, publisher_handle, publish_msg_args.ret_entry_id);
@@ -152,9 +147,8 @@ uint32_t get_subscription_count_core(const std::string & topic_name)
   union ioctl_get_subscriber_num_args args = {};
   args.topic_name = {topic_name.c_str(), topic_name.size()};
   if (ioctl(agnocast_fd, AGNOCAST_GET_SUBSCRIBER_NUM_CMD, &args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_GET_SUBSCRIBER_NUM_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_GET_SUBSCRIBER_NUM_CMD failed: ") + strerror(errno));
   }
 
   uint32_t inter_count = args.ret_other_process_subscriber_num;
@@ -177,9 +171,8 @@ uint32_t get_intra_subscription_count_core(const std::string & topic_name)
   union ioctl_get_subscriber_num_args get_subscriber_count_args = {};
   get_subscriber_count_args.topic_name = {topic_name.c_str(), topic_name.size()};
   if (ioctl(agnocast_fd, AGNOCAST_GET_SUBSCRIBER_NUM_CMD, &get_subscriber_count_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_GET_SUBSCRIBER_NUM_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_GET_SUBSCRIBER_NUM_CMD failed: ") + strerror(errno));
   }
 
   return get_subscriber_count_args.ret_same_process_subscriber_num;
diff --git a/src/agnocastlib/src/agnocast_smart_pointer.cpp b/src/agnocastlib/src/agnocast_smart_pointer.cpp
index 19a938592..3dab347d2 100644
--- a/src/agnocastlib/src/agnocast_smart_pointer.cpp
+++ b/src/agnocastlib/src/agnocast_smart_pointer.cpp
@@ -11,9 +11,8 @@ void release_subscriber_reference(
   entry_args.pubsub_id = pubsub_id;
   entry_args.entry_id = entry_id;
   if (ioctl(agnocast_fd, AGNOCAST_RELEASE_SUB_REF_CMD, &entry_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_RELEASE_SUB_REF_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_RELEASE_SUB_REF_CMD failed: ") + strerror(errno));
   }
 }
 
diff --git a/src/agnocastlib/src/agnocast_subscription.cpp b/src/agnocastlib/src/agnocast_subscription.cpp
index 28548221f..1b600ce32 100644
--- a/src/agnocastlib/src/agnocast_subscription.cpp
+++ b/src/agnocastlib/src/agnocast_subscription.cpp
@@ -32,9 +32,8 @@ union ioctl_add_subscriber_args SubscriptionBase::initialize(
   add_subscriber_args.ignore_local_publications = ignore_local_publications;
   add_subscriber_args.is_bridge = is_bridge;
   if (ioctl(agnocast_fd, AGNOCAST_ADD_SUBSCRIBER_CMD, &add_subscriber_args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_ADD_SUBSCRIBER_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_ADD_SUBSCRIBER_CMD failed: ") + strerror(errno));
   }
 
   return add_subscriber_args;
@@ -45,9 +44,8 @@ uint32_t get_publisher_count_core(const std::string & topic_name)
   union ioctl_get_publisher_num_args args = {};
   args.topic_name = {topic_name.c_str(), topic_name.size()};
   if (ioctl(agnocast_fd, AGNOCAST_GET_PUBLISHER_NUM_CMD, &args) < 0) {
-    RCLCPP_ERROR(logger, "AGNOCAST_GET_PUBLISHER_NUM_CMD failed: %s", strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      std::string("AGNOCAST_GET_PUBLISHER_NUM_CMD failed: ") + strerror(errno));
   }
 
   uint32_t count = args.ret_publisher_num;
@@ -79,12 +77,9 @@ mqd_t open_mq_for_subscription(
   const int mq_mode = 0666;
   mqd_t mq = mq_open(mq_name.c_str(), O_CREAT | O_RDONLY | O_NONBLOCK, mq_mode, &attr);
   if (mq == -1) {
-    RCLCPP_ERROR_STREAM(
-      logger, "mq_open failed for topic '" << topic_name << "' (subscriber_id=" << subscriber_id
-                                           << ", mq_name='" << mq_name
-                                           << "'): " << strerror(errno));
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error(
+      "mq_open failed for topic '" + topic_name + "' (subscriber_id=" +
+      std::to_string(subscriber_id) + ", mq_name='" + mq_name + "'): " + strerror(errno));
   }
   mq_subscription = std::make_pair(mq, mq_name);
 
diff --git a/src/agnocastlib/src/agnocast_utils.cpp b/src/agnocastlib/src/agnocast_utils.cpp
index e0ca277d7..41aaa079a 100644
--- a/src/agnocastlib/src/agnocast_utils.cpp
+++ b/src/agnocastlib/src/agnocast_utils.cpp
@@ -19,8 +19,7 @@ void validate_ld_preload()
   if (
     ld_preload_cstr == nullptr ||
     std::strstr(ld_preload_cstr, "libagnocast_heaphook.so") == nullptr) {
-    RCLCPP_ERROR(logger, "libagnocast_heaphook.so not found in LD_PRELOAD.");
-    exit(EXIT_FAILURE);
+    throw std::runtime_error("libagnocast_heaphook.so not found in LD_PRELOAD.");
   }
 
   std::string ld_preload(ld_preload_cstr);
@@ -46,9 +45,7 @@ static std::string create_mq_name(
   const std::string & header, const std::string & topic_name, const topic_local_id_t id)
 {
   if (topic_name.length() == 0 || topic_name[0] != '/') {
-    RCLCPP_ERROR(logger, "create_mq_name failed");
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
+    throw std::runtime_error("create_mq_name failed: invalid topic_name");
   }
 
   std::string mq_name = topic_name;

From 0a97919e754b3c6f9f7ec918e4e3043efbcd90e8 Mon Sep 17 00:00:00 2001
From: Koichi <koichi.imai.2@tier4.jp>
Date: Sat, 28 Feb 2026 16:45:51 +0900
Subject: [PATCH 02/11] fix

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>
---
 .../src/agnocast_component_container.cpp             |  6 +++---
 .../src/agnocast_component_container_mt.cpp          |  6 +++---
 src/agnocastlib/src/agnocast_component_container.cpp |  6 +++---
 .../src/agnocast_component_container_mt.cpp          |  6 +++---
 src/agnocastlib/src/agnocast_publisher.cpp           |  5 ++++-
 src/agnocastlib/src/agnocast_smart_pointer.cpp       | 12 ++++++++++--
 6 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/src/agnocast_components/src/agnocast_component_container.cpp b/src/agnocast_components/src/agnocast_component_container.cpp
index 09f464d9b..efe29eca0 100644
--- a/src/agnocast_components/src/agnocast_component_container.cpp
+++ b/src/agnocast_components/src/agnocast_component_container.cpp
@@ -29,12 +29,12 @@ int main(int argc, char * argv[])
     executor->spin();
 
     rclcpp::shutdown();
-  } catch (rclcpp_components::ComponentManagerException & ex) {
-    std::cerr << "Exception caught in main: " << ex.what() << std::endl;
+  } catch (const std::exception & e) {
+    std::cerr << "Exception caught in main: " << e.what() << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main: " << std::endl;
+    std::cerr << "Unknown exception caught in main" << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocast_components/src/agnocast_component_container_mt.cpp b/src/agnocast_components/src/agnocast_component_container_mt.cpp
index e61e83e92..bde081b17 100644
--- a/src/agnocast_components/src/agnocast_component_container_mt.cpp
+++ b/src/agnocast_components/src/agnocast_component_container_mt.cpp
@@ -44,12 +44,12 @@ int main(int argc, char * argv[])
     executor->spin();
 
     rclcpp::shutdown();
-  } catch (rclcpp_components::ComponentManagerException & ex) {
-    std::cerr << "Exception caught in main: " << ex.what() << std::endl;
+  } catch (const std::exception & e) {
+    std::cerr << "Exception caught in main: " << e.what() << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main: " << std::endl;
+    std::cerr << "Unknown exception caught in main" << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_component_container.cpp b/src/agnocastlib/src/agnocast_component_container.cpp
index 12d65b92c..e14dea926 100644
--- a/src/agnocastlib/src/agnocast_component_container.cpp
+++ b/src/agnocastlib/src/agnocast_component_container.cpp
@@ -34,12 +34,12 @@ int main(int argc, char * argv[])
     executor->spin();
 
     rclcpp::shutdown();
-  } catch (rclcpp_components::ComponentManagerException & ex) {
-    std::cerr << "Exception caught in main: " << ex.what() << std::endl;
+  } catch (const std::exception & e) {
+    std::cerr << "Exception caught in main: " << e.what() << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main: " << std::endl;
+    std::cerr << "Unknown exception caught in main" << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_component_container_mt.cpp b/src/agnocastlib/src/agnocast_component_container_mt.cpp
index b4173f022..b9ee1f7d5 100644
--- a/src/agnocastlib/src/agnocast_component_container_mt.cpp
+++ b/src/agnocastlib/src/agnocast_component_container_mt.cpp
@@ -49,12 +49,12 @@ int main(int argc, char * argv[])
     executor->spin();
 
     rclcpp::shutdown();
-  } catch (rclcpp_components::ComponentManagerException & ex) {
-    std::cerr << "Exception caught in main: " << ex.what() << std::endl;
+  } catch (const std::exception & e) {
+    std::cerr << "Exception caught in main: " << e.what() << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main: " << std::endl;
+    std::cerr << "Unknown exception caught in main" << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_publisher.cpp b/src/agnocastlib/src/agnocast_publisher.cpp
index 3c94d3913..8dc8e24d5 100644
--- a/src/agnocastlib/src/agnocast_publisher.cpp
+++ b/src/agnocastlib/src/agnocast_publisher.cpp
@@ -32,8 +32,11 @@ void increment_borrowed_publisher_num()
 void decrement_borrowed_publisher_num()
 {
   if (borrowed_publisher_num == 0) {
-    throw std::runtime_error(
+    RCLCPP_ERROR(
+      logger,
       "The number of publish() called exceeds the number of borrow_loaned_message() called.");
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
   borrowed_publisher_num--;
 }
diff --git a/src/agnocastlib/src/agnocast_smart_pointer.cpp b/src/agnocastlib/src/agnocast_smart_pointer.cpp
index 3dab347d2..a92928e52 100644
--- a/src/agnocastlib/src/agnocast_smart_pointer.cpp
+++ b/src/agnocastlib/src/agnocast_smart_pointer.cpp
@@ -1,8 +1,15 @@
 #include "agnocast/agnocast_smart_pointer.hpp"
 
+#include "rclcpp/rclcpp.hpp"
+
 namespace agnocast
 {
 
+namespace
+{
+rclcpp::Logger logger = rclcpp::get_logger("agnocast_smart_pointer");
+}
+
 void release_subscriber_reference(
   const std::string & topic_name, const topic_local_id_t pubsub_id, const int64_t entry_id)
 {
@@ -11,8 +18,9 @@ void release_subscriber_reference(
   entry_args.pubsub_id = pubsub_id;
   entry_args.entry_id = entry_id;
   if (ioctl(agnocast_fd, AGNOCAST_RELEASE_SUB_REF_CMD, &entry_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_RELEASE_SUB_REF_CMD failed: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "AGNOCAST_RELEASE_SUB_REF_CMD failed: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 }
 

From 55deee8dbf84d66bd0f17c5891f823fdb5bfe8b1 Mon Sep 17 00:00:00 2001
From: Koichi <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 01:08:27 +0900
Subject: [PATCH 03/11] fix

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>
---
 .../src/agnocast_component_container.cpp                   | 2 +-
 .../src/agnocast_component_container_mt.cpp                | 2 +-
 src/agnocastlib/src/agnocast_component_container.cpp       | 2 +-
 src/agnocastlib/src/agnocast_component_container_mt.cpp    | 2 +-
 src/agnocastlib/src/agnocast_smart_pointer.cpp             | 7 -------
 5 files changed, 4 insertions(+), 11 deletions(-)

diff --git a/src/agnocast_components/src/agnocast_component_container.cpp b/src/agnocast_components/src/agnocast_component_container.cpp
index efe29eca0..11a3ff06f 100644
--- a/src/agnocast_components/src/agnocast_component_container.cpp
+++ b/src/agnocast_components/src/agnocast_component_container.cpp
@@ -34,7 +34,7 @@ int main(int argc, char * argv[])
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main" << std::endl;
+    std::cerr << "Unknown exception caught in main: " << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocast_components/src/agnocast_component_container_mt.cpp b/src/agnocast_components/src/agnocast_component_container_mt.cpp
index bde081b17..8ce7b1d82 100644
--- a/src/agnocast_components/src/agnocast_component_container_mt.cpp
+++ b/src/agnocast_components/src/agnocast_component_container_mt.cpp
@@ -49,7 +49,7 @@ int main(int argc, char * argv[])
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main" << std::endl;
+    std::cerr << "Unknown exception caught in main: " << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_component_container.cpp b/src/agnocastlib/src/agnocast_component_container.cpp
index e14dea926..e76428591 100644
--- a/src/agnocastlib/src/agnocast_component_container.cpp
+++ b/src/agnocastlib/src/agnocast_component_container.cpp
@@ -39,7 +39,7 @@ int main(int argc, char * argv[])
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main" << std::endl;
+    std::cerr << "Unknown exception caught in main: " << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_component_container_mt.cpp b/src/agnocastlib/src/agnocast_component_container_mt.cpp
index b9ee1f7d5..8f2de381f 100644
--- a/src/agnocastlib/src/agnocast_component_container_mt.cpp
+++ b/src/agnocastlib/src/agnocast_component_container_mt.cpp
@@ -54,7 +54,7 @@ int main(int argc, char * argv[])
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   } catch (...) {
-    std::cerr << "Unknown exception caught in main" << std::endl;
+    std::cerr << "Unknown exception caught in main: " << std::endl;
     close(agnocast::agnocast_fd);
     return EXIT_FAILURE;
   }
diff --git a/src/agnocastlib/src/agnocast_smart_pointer.cpp b/src/agnocastlib/src/agnocast_smart_pointer.cpp
index a92928e52..19a938592 100644
--- a/src/agnocastlib/src/agnocast_smart_pointer.cpp
+++ b/src/agnocastlib/src/agnocast_smart_pointer.cpp
@@ -1,15 +1,8 @@
 #include "agnocast/agnocast_smart_pointer.hpp"
 
-#include "rclcpp/rclcpp.hpp"
-
 namespace agnocast
 {
 
-namespace
-{
-rclcpp::Logger logger = rclcpp::get_logger("agnocast_smart_pointer");
-}
-
 void release_subscriber_reference(
   const std::string & topic_name, const topic_local_id_t pubsub_id, const int64_t entry_id)
 {

From 50f792ef72a5d101413942a7c769ffb03a907369 Mon Sep 17 00:00:00 2001
From: Koichi <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 14:18:15 +0900
Subject: [PATCH 04/11] fix for agnocast.cpp

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/src/agnocast.cpp | 107 ++++++++++++++++++++-----------
 1 file changed, 69 insertions(+), 38 deletions(-)

diff --git a/src/agnocastlib/src/agnocast.cpp b/src/agnocastlib/src/agnocast.cpp
index 453a30562..53288eb03 100644
--- a/src/agnocastlib/src/agnocast.cpp
+++ b/src/agnocastlib/src/agnocast.cpp
@@ -49,7 +49,9 @@ void * map_area(
   const int shm_mode = 0666;
   int shm_fd = shm_open(shm_name.c_str(), oflag, shm_mode);
   if (shm_fd == -1) {
-    throw std::runtime_error(std::string("shm_open failed: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "shm_open failed: %s", strerror(errno));
+    close(agnocast_fd);
+    return nullptr;
   }
 
   {
@@ -70,14 +72,18 @@ void * map_area(
 
   if (writable) {
     if (ftruncate(shm_fd, static_cast<off_t>(shm_size)) == -1) {
+      RCLCPP_ERROR(logger, "ftruncate failed: %s", strerror(errno));
       cleanup_shm_fd();
-      throw std::runtime_error(std::string("ftruncate failed: ") + strerror(errno));
+      close(agnocast_fd);
+      return nullptr;
     }
 
     const int new_shm_mode = 0444;
     if (fchmod(shm_fd, new_shm_mode) == -1) {
+      RCLCPP_ERROR(logger, "fchmod failed: %s", strerror(errno));
       cleanup_shm_fd();
-      throw std::runtime_error(std::string("fchmod failed: ") + strerror(errno));
+      close(agnocast_fd);
+      return nullptr;
     }
   }
 
@@ -87,8 +93,10 @@ void * map_area(
     0);
 
   if (ret == MAP_FAILED) {
+    RCLCPP_ERROR(logger, "mmap failed: %s", strerror(errno));
     cleanup_shm_fd();
-    throw std::runtime_error(std::string("mmap failed: ") + strerror(errno));
+    close(agnocast_fd);
+    return nullptr;
   }
 
   return ret;
@@ -101,7 +109,9 @@ void * map_writable_area(const pid_t pid, const uint64_t shm_addr, const uint64_
 
 void map_read_only_area(const pid_t pid, const uint64_t shm_addr, const uint64_t shm_size)
 {
-  map_area(pid, shm_addr, shm_size, false);
+  if (map_area(pid, shm_addr, shm_size, false) == nullptr) {
+    exit(EXIT_FAILURE);
+  }
 }
 
 // Initializes the child allocator for bridge functionality.
@@ -144,6 +154,10 @@ initialize_agnocast_result acquire_agnocast_resources_for_bridge()
   void * mempool_ptr =
     map_writable_area(getpid(), add_process_args.ret_addr, add_process_args.ret_shm_size);
 
+  if (mempool_ptr == nullptr) {
+    throw std::runtime_error("map_writable_area failed.");
+  }
+
   return {
     mempool_ptr,
     add_process_args.ret_shm_size,
@@ -153,8 +167,9 @@ initialize_agnocast_result acquire_agnocast_resources_for_bridge()
 void poll_for_unlink()
 {
   if (setsid() == -1) {
-    throw std::runtime_error(
-      std::string("setsid failed for unlink daemon: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "setsid failed for unlink daemon: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   while (true) {
@@ -164,8 +179,9 @@ void poll_for_unlink()
     do {
       get_exit_process_args = {};
       if (ioctl(agnocast_fd, AGNOCAST_GET_EXIT_PROCESS_CMD, &get_exit_process_args) < 0) {
-        throw std::runtime_error(
-          std::string("AGNOCAST_GET_EXIT_PROCESS_CMD failed: ") + strerror(errno));
+        RCLCPP_ERROR(logger, "AGNOCAST_GET_EXIT_PROCESS_CMD failed: %s", strerror(errno));
+        close(agnocast_fd);
+        exit(EXIT_FAILURE);
       }
 
       if (get_exit_process_args.ret_pid > 0) {
@@ -195,21 +211,30 @@ void poll_for_unlink()
 void poll_for_bridge_manager([[maybe_unused]] pid_t target_pid)
 {
   if (setsid() == -1) {
-    throw std::runtime_error(
-      std::string("setsid failed for bridge manager: ") + strerror(errno));
-  }
-
-  const auto resources = acquire_agnocast_resources_for_bridge();
-  initialize_bridge_allocator(resources.mempool_ptr, resources.mempool_size);
-
-  auto bridge_mode = get_bridge_mode();
-  if (bridge_mode == BridgeMode::Standard) {
-    StandardBridgeManager manager(target_pid);
-    manager.run();
-  } else if (bridge_mode == BridgeMode::Performance) {
-    PerformanceBridgeManager manager;
-    manager.run();
+    RCLCPP_ERROR(logger, "setsid failed for unlink daemon: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
+  }
+
+  try {
+    const auto resources = acquire_agnocast_resources_for_bridge();
+    initialize_bridge_allocator(resources.mempool_ptr, resources.mempool_size);
+
+    auto bridge_mode = get_bridge_mode();
+    if (bridge_mode == BridgeMode::Standard) {
+      StandardBridgeManager manager(target_pid);
+      manager.run();
+    } else if (bridge_mode == BridgeMode::Performance) {
+      {
+        PerformanceBridgeManager manager;
+        manager.run();
+      }
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(logger, "BridgeManager crashed: %s", e.what());
+    exit(EXIT_FAILURE);
   }
+  exit(0);
 }
 
 struct semver
@@ -341,7 +366,9 @@ pid_t spawn_daemon_process(Func && func)
 {
   pid_t pid = fork();
   if (pid < 0) {
-    throw std::runtime_error(std::string("fork failed: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "fork failed: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
   if (pid == 0) {
     agnocast::is_bridge_process = true;
@@ -357,12 +384,7 @@ pid_t spawn_daemon_process(Func && func)
       close(devnull);
     }
 
-    try {
-      func();
-    } catch (const std::exception & e) {
-      RCLCPP_ERROR(logger, "Daemon process failed: %s", e.what());
-      exit(EXIT_FAILURE);
-    }
+    func();
     exit(0);
   }
 
@@ -374,32 +396,37 @@ struct initialize_agnocast_result initialize_agnocast(
   const unsigned char * heaphook_version_ptr, const size_t heaphook_version_str_len)
 {
   if (agnocast_fd >= 0) {
-    throw std::runtime_error("Agnocast is already open");
+    RCLCPP_ERROR(logger, "Agnocast is already open");
+    exit(EXIT_FAILURE);
   }
 
   agnocast_fd = open("/dev/agnocast", O_RDWR);
   if (agnocast_fd < 0) {
     if (errno == ENOENT) {
-      throw std::runtime_error(AGNOCAST_DEVICE_NOT_FOUND_MSG);
+      RCLCPP_ERROR(logger, "%s", AGNOCAST_DEVICE_NOT_FOUND_MSG);
     } else {
-      throw std::runtime_error(std::string("Failed to open /dev/agnocast: ") + strerror(errno));
+      RCLCPP_ERROR(logger, "Failed to open /dev/agnocast: %s", strerror(errno));
     }
+    exit(EXIT_FAILURE);
   }
 
   struct ioctl_get_version_args get_version_args = {};
   if (ioctl(agnocast_fd, AGNOCAST_GET_VERSION_CMD, &get_version_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_GET_VERSION_CMD failed: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "AGNOCAST_GET_VERSION_CMD failed: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   if (!is_version_consistent(heaphook_version_ptr, heaphook_version_str_len, get_version_args)) {
-    throw std::runtime_error("Agnocast version mismatch");
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   union ioctl_add_process_args add_process_args = {};
   if (ioctl(agnocast_fd, AGNOCAST_ADD_PROCESS_CMD, &add_process_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_ADD_PROCESS_CMD failed: ") + strerror(errno));
+    RCLCPP_ERROR(logger, "AGNOCAST_ADD_PROCESS_CMD failed: %s", strerror(errno));
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   pid_t target_pid = 0;
@@ -428,6 +455,10 @@ struct initialize_agnocast_result initialize_agnocast(
 
   void * mempool_ptr =
     map_writable_area(getpid(), add_process_args.ret_addr, add_process_args.ret_shm_size);
+  if (mempool_ptr == nullptr) {
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
+  }
 
   struct initialize_agnocast_result result = {};
   result.mempool_ptr = mempool_ptr;

From 7f3926abfad077319dcc2702f9cf111a44e468d8 Mon Sep 17 00:00:00 2001
From: Koichi <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 14:29:08 +0900
Subject: [PATCH 05/11] use invalid_argument

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/include/agnocast/agnocast_publisher.hpp    | 2 +-
 src/agnocastlib/include/agnocast/agnocast_subscription.hpp | 3 ++-
 src/agnocastlib/include/agnocast/agnocast_utils.hpp        | 2 +-
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/agnocastlib/include/agnocast/agnocast_publisher.hpp b/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
index d70c49304..0d868ef8a 100644
--- a/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_publisher.hpp
@@ -175,7 +175,7 @@ class BasicPublisher
   void publish(ipc_shared_ptr<MessageT> && message)
   {
     if (!message || topic_name_ != message.get_topic_name()) {
-      throw std::runtime_error("Invalid message to publish.");
+      throw std::invalid_argument("Invalid message to publish.");
     }
 
     // Capture raw pointer BEFORE invalidation (get() returns nullptr after invalidation).
diff --git a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
index 9d3b6e2f1..0eb611a16 100644
--- a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
@@ -60,7 +60,8 @@ rclcpp::CallbackGroup::SharedPtr get_valid_callback_group(
 
   if (callback_group) {
     if (!node->get_node_base_interface()->callback_group_in_node(callback_group)) {
-      throw std::runtime_error("Cannot create agnocast subscription, callback group not in node.");
+      throw std::invalid_argument(
+        "Cannot create agnocast subscription, callback group not in node.");
     }
   } else {
     callback_group = node->get_node_base_interface()->get_default_callback_group();
diff --git a/src/agnocastlib/include/agnocast/agnocast_utils.hpp b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
index 9442913f8..12cc16266 100644
--- a/src/agnocastlib/include/agnocast/agnocast_utils.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
@@ -16,7 +16,7 @@ extern bool is_bridge_process;
 inline void validate_qos(const rclcpp::QoS & qos)
 {
   if (qos.history() == rclcpp::HistoryPolicy::KeepAll) {
-    throw std::runtime_error(
+    throw std::invalid_argument(
       "Agnocast does not support KeepAll history policy. Use KeepLast instead.");
   }
 

From d55a4e425ae6f863fcfa1bf547f58a00d00c5615 Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:02:00 +0900
Subject: [PATCH 06/11] fix clang-format

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/include/agnocast/agnocast_subscription.hpp | 3 +--
 src/agnocastlib/src/agnocast_callback_info.cpp             | 3 +--
 src/agnocastlib/src/agnocast_publisher.cpp                 | 6 ++----
 src/agnocastlib/src/agnocast_subscription.cpp              | 3 +--
 4 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
index 0eb611a16..c3ba930f4 100644
--- a/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_subscription.hpp
@@ -295,8 +295,7 @@ class BasicTakeSubscription : public SubscriptionBase
       std::lock_guard<std::mutex> lock(mmap_mtx);
 
       if (ioctl(agnocast_fd, AGNOCAST_TAKE_MSG_CMD, &take_args) < 0) {
-        throw std::runtime_error(
-          std::string("AGNOCAST_TAKE_MSG_CMD failed: ") + strerror(errno));
+        throw std::runtime_error(std::string("AGNOCAST_TAKE_MSG_CMD failed: ") + strerror(errno));
       }
 
       for (uint32_t i = 0; i < take_args.ret_pub_shm_num; i++) {
diff --git a/src/agnocastlib/src/agnocast_callback_info.cpp b/src/agnocastlib/src/agnocast_callback_info.cpp
index 474aacca4..92b9ceba5 100644
--- a/src/agnocastlib/src/agnocast_callback_info.cpp
+++ b/src/agnocastlib/src/agnocast_callback_info.cpp
@@ -43,8 +43,7 @@ void receive_and_execute_message(
     std::lock_guard<std::mutex> lock(mmap_mtx);
 
     if (ioctl(agnocast_fd, AGNOCAST_RECEIVE_MSG_CMD, &receive_args) < 0) {
-      throw std::runtime_error(
-        std::string("AGNOCAST_RECEIVE_MSG_CMD failed: ") + strerror(errno));
+      throw std::runtime_error(std::string("AGNOCAST_RECEIVE_MSG_CMD failed: ") + strerror(errno));
     }
 
     // Map the shared memory region with read permissions whenever a new publisher is discovered.
diff --git a/src/agnocastlib/src/agnocast_publisher.cpp b/src/agnocastlib/src/agnocast_publisher.cpp
index 8dc8e24d5..c7b3c2a2a 100644
--- a/src/agnocastlib/src/agnocast_publisher.cpp
+++ b/src/agnocastlib/src/agnocast_publisher.cpp
@@ -54,8 +54,7 @@ topic_local_id_t initialize_publisher(
   pub_args.qos_is_transient_local = qos.durability() == rclcpp::DurabilityPolicy::TransientLocal;
   pub_args.is_bridge = is_bridge;
   if (ioctl(agnocast_fd, AGNOCAST_ADD_PUBLISHER_CMD, &pub_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_ADD_PUBLISHER_CMD failed: ") + strerror(errno));
+    throw std::runtime_error(std::string("AGNOCAST_ADD_PUBLISHER_CMD failed: ") + strerror(errno));
   }
 
   return pub_args.ret_id;
@@ -79,8 +78,7 @@ union ioctl_publish_msg_args publish_core(
   publish_msg_args.subscriber_ids_buffer_size = MAX_SUBSCRIBER_NUM;
 
   if (ioctl(agnocast_fd, AGNOCAST_PUBLISH_MSG_CMD, &publish_msg_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_PUBLISH_MSG_CMD failed: ") + strerror(errno));
+    throw std::runtime_error(std::string("AGNOCAST_PUBLISH_MSG_CMD failed: ") + strerror(errno));
   }
 
   TRACEPOINT(agnocast_publish, publisher_handle, publish_msg_args.ret_entry_id);
diff --git a/src/agnocastlib/src/agnocast_subscription.cpp b/src/agnocastlib/src/agnocast_subscription.cpp
index 1b600ce32..7961a565c 100644
--- a/src/agnocastlib/src/agnocast_subscription.cpp
+++ b/src/agnocastlib/src/agnocast_subscription.cpp
@@ -32,8 +32,7 @@ union ioctl_add_subscriber_args SubscriptionBase::initialize(
   add_subscriber_args.ignore_local_publications = ignore_local_publications;
   add_subscriber_args.is_bridge = is_bridge;
   if (ioctl(agnocast_fd, AGNOCAST_ADD_SUBSCRIBER_CMD, &add_subscriber_args) < 0) {
-    throw std::runtime_error(
-      std::string("AGNOCAST_ADD_SUBSCRIBER_CMD failed: ") + strerror(errno));
+    throw std::runtime_error(std::string("AGNOCAST_ADD_SUBSCRIBER_CMD failed: ") + strerror(errno));
   }
 
   return add_subscriber_args;

From 3da0bcff44455997c7cf9ca10aff12ea9ae87a11 Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:09:35 +0900
Subject: [PATCH 07/11] fix for borrow_loaned_message

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/src/agnocast_publisher.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/agnocastlib/src/agnocast_publisher.cpp b/src/agnocastlib/src/agnocast_publisher.cpp
index c7b3c2a2a..6ee058945 100644
--- a/src/agnocastlib/src/agnocast_publisher.cpp
+++ b/src/agnocastlib/src/agnocast_publisher.cpp
@@ -32,11 +32,8 @@ void increment_borrowed_publisher_num()
 void decrement_borrowed_publisher_num()
 {
   if (borrowed_publisher_num == 0) {
-    RCLCPP_ERROR(
-      logger,
+    throw std::logic_error(
       "The number of publish() called exceeds the number of borrow_loaned_message() called.");
-    close(agnocast_fd);
-    exit(EXIT_FAILURE);
   }
   borrowed_publisher_num--;
 }

From 20e62f7ad40438396599957b05f7c43d7065bd74 Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:12:36 +0900
Subject: [PATCH 08/11] delete fix in agnocast_utils.cpp,hpp

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/include/agnocast/agnocast_utils.hpp | 6 +++---
 src/agnocastlib/src/agnocast_utils.cpp              | 7 +++++--
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/agnocastlib/include/agnocast/agnocast_utils.hpp b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
index 12cc16266..e5d1f6775 100644
--- a/src/agnocastlib/include/agnocast/agnocast_utils.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_utils.hpp
@@ -3,7 +3,6 @@
 #include "agnocast/agnocast_ioctl.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include <stdexcept>
 #include <string>
 
 namespace agnocast
@@ -16,8 +15,9 @@ extern bool is_bridge_process;
 inline void validate_qos(const rclcpp::QoS & qos)
 {
   if (qos.history() == rclcpp::HistoryPolicy::KeepAll) {
-    throw std::invalid_argument(
-      "Agnocast does not support KeepAll history policy. Use KeepLast instead.");
+    RCLCPP_ERROR(logger, "Agnocast does not support KeepAll history policy. Use KeepLast instead.");
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   const auto & rmw_qos = qos.get_rmw_qos_profile();
diff --git a/src/agnocastlib/src/agnocast_utils.cpp b/src/agnocastlib/src/agnocast_utils.cpp
index 41aaa079a..e0ca277d7 100644
--- a/src/agnocastlib/src/agnocast_utils.cpp
+++ b/src/agnocastlib/src/agnocast_utils.cpp
@@ -19,7 +19,8 @@ void validate_ld_preload()
   if (
     ld_preload_cstr == nullptr ||
     std::strstr(ld_preload_cstr, "libagnocast_heaphook.so") == nullptr) {
-    throw std::runtime_error("libagnocast_heaphook.so not found in LD_PRELOAD.");
+    RCLCPP_ERROR(logger, "libagnocast_heaphook.so not found in LD_PRELOAD.");
+    exit(EXIT_FAILURE);
   }
 
   std::string ld_preload(ld_preload_cstr);
@@ -45,7 +46,9 @@ static std::string create_mq_name(
   const std::string & header, const std::string & topic_name, const topic_local_id_t id)
 {
   if (topic_name.length() == 0 || topic_name[0] != '/') {
-    throw std::runtime_error("create_mq_name failed: invalid topic_name");
+    RCLCPP_ERROR(logger, "create_mq_name failed");
+    close(agnocast_fd);
+    exit(EXIT_FAILURE);
   }
 
   std::string mq_name = topic_name;

From 5218950fcc13e2dbf60c603c43e6947f8b7f5c1e Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:17:21 +0900
Subject: [PATCH 09/11] fix in unit tests

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/test/unit/test_mocked_agnocast.cpp | 12 +++---------
 1 file changed, 3 insertions(+), 9 deletions(-)

diff --git a/src/agnocastlib/test/unit/test_mocked_agnocast.cpp b/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
index 175040e2a..c72867fb4 100644
--- a/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
+++ b/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
@@ -107,9 +107,7 @@ TEST_F(AgnocastPublisherTest, test_publish_null_message)
   agnocast::ipc_shared_ptr<std_msgs::msg::Int32> message;
 
   // Act & Assert
-  EXPECT_EXIT(
-    dummy_publisher->publish(std::move(message)), ::testing::ExitedWithCode(EXIT_FAILURE),
-    "Invalid message to publish.");
+  EXPECT_THROW(dummy_publisher->publish(std::move(message)), std::invalid_argument);
 }
 
 TEST_F(AgnocastPublisherTest, test_publish_already_published_message)
@@ -119,9 +117,7 @@ TEST_F(AgnocastPublisherTest, test_publish_already_published_message)
   dummy_publisher->publish(std::move(message));
 
   // Act & Assert
-  EXPECT_EXIT(
-    dummy_publisher->publish(std::move(message)), ::testing::ExitedWithCode(EXIT_FAILURE),
-    "Invalid message to publish.");
+  EXPECT_THROW(dummy_publisher->publish(std::move(message)), std::invalid_argument);
 }
 
 TEST_F(AgnocastPublisherTest, test_publish_different_message)
@@ -135,9 +131,7 @@ TEST_F(AgnocastPublisherTest, test_publish_different_message)
   agnocast::ipc_shared_ptr<std_msgs::msg::Int32> message = dummy_publisher->borrow_loaned_message();
 
   // Act & Assert
-  EXPECT_EXIT(
-    dummy_publisher->publish(std::move(diff_message)), ::testing::ExitedWithCode(EXIT_FAILURE),
-    "Invalid message to publish.");
+  EXPECT_THROW(dummy_publisher->publish(std::move(diff_message)), std::invalid_argument);
 }
 
 TEST_F(AgnocastPublisherTest, test_publish_loan_num_and_pub_num_mismatch)

From 7671ed5db8682e1b607d0ab36ab6fafd9d7f65fe Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:36:46 +0900
Subject: [PATCH 10/11] catch in each thread

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 .../agnocast_callback_isolated_executor.hpp   |  2 +
 .../agnocast_multi_threaded_executor.hpp      |  3 +
 ...nocast_only_callback_isolated_executor.hpp |  2 +
 .../agnocast_only_multi_threaded_executor.hpp |  2 +
 .../agnocast_callback_isolated_executor.cpp   | 14 +++-
 .../src/agnocast_multi_threaded_executor.cpp  | 48 ++++++++------
 ...nocast_only_callback_isolated_executor.cpp | 14 +++-
 .../agnocast_only_multi_threaded_executor.cpp | 64 +++++++++++--------
 8 files changed, 99 insertions(+), 50 deletions(-)

diff --git a/src/agnocastlib/include/agnocast/agnocast_callback_isolated_executor.hpp b/src/agnocastlib/include/agnocast/agnocast_callback_isolated_executor.hpp
index 3ecc2c2e3..0f139db96 100644
--- a/src/agnocastlib/include/agnocast/agnocast_callback_isolated_executor.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_callback_isolated_executor.hpp
@@ -27,6 +27,8 @@ class CallbackIsolatedAgnocastExecutor : public rclcpp::Executor
     std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
     weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  std::atomic<bool> worker_thread_failed_{false};
+
   // Mutex to protect weak_child_executors_ and child_threads_
   mutable std::mutex child_resources_mutex_;
 
diff --git a/src/agnocastlib/include/agnocast/agnocast_multi_threaded_executor.hpp b/src/agnocastlib/include/agnocast/agnocast_multi_threaded_executor.hpp
index 90b0dae75..89115b937 100644
--- a/src/agnocastlib/include/agnocast/agnocast_multi_threaded_executor.hpp
+++ b/src/agnocastlib/include/agnocast/agnocast_multi_threaded_executor.hpp
@@ -3,6 +3,7 @@
 #include "agnocast/agnocast_executor.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <atomic>
 #include <chrono>
 
 namespace agnocast
@@ -19,6 +20,8 @@ class MultiThreadedAgnocastExecutor : public agnocast::AgnocastExecutor
   std::chrono::nanoseconds ros2_next_exec_timeout_;
   const int agnocast_next_exec_timeout_ms_;
 
+  std::atomic<bool> worker_thread_failed_{false};
+
   void ros2_spin();
   void agnocast_spin();
   bool validate_callback_group(const rclcpp::CallbackGroup::SharedPtr & group) const override;
diff --git a/src/agnocastlib/include/agnocast/node/agnocast_only_callback_isolated_executor.hpp b/src/agnocastlib/include/agnocast/node/agnocast_only_callback_isolated_executor.hpp
index 3c755ee32..b10186de8 100644
--- a/src/agnocastlib/include/agnocast/node/agnocast_only_callback_isolated_executor.hpp
+++ b/src/agnocastlib/include/agnocast/node/agnocast_only_callback_isolated_executor.hpp
@@ -32,6 +32,8 @@ class AgnocastOnlyCallbackIsolatedExecutor : public AgnocastOnlyExecutor
   std::mutex callback_group_created_cv_mutex_;
   bool callback_group_created_{false};
 
+  std::atomic<bool> worker_thread_failed_{false};
+
   // Mutex to protect weak_child_executors_ and child_threads_
   mutable std::mutex child_resources_mutex_;
 
diff --git a/src/agnocastlib/include/agnocast/node/agnocast_only_multi_threaded_executor.hpp b/src/agnocastlib/include/agnocast/node/agnocast_only_multi_threaded_executor.hpp
index 93901a194..2e386ab96 100644
--- a/src/agnocastlib/include/agnocast/node/agnocast_only_multi_threaded_executor.hpp
+++ b/src/agnocastlib/include/agnocast/node/agnocast_only_multi_threaded_executor.hpp
@@ -14,6 +14,8 @@ class AgnocastOnlyMultiThreadedExecutor : public AgnocastOnlyExecutor
   bool yield_before_execute_;
   const int next_exec_timeout_ms_;
 
+  std::atomic<bool> worker_thread_failed_{false};
+
   void agnocast_spin();
 
 public:
diff --git a/src/agnocastlib/src/agnocast_callback_isolated_executor.cpp b/src/agnocastlib/src/agnocast_callback_isolated_executor.cpp
index 86125be41..f04098f76 100644
--- a/src/agnocastlib/src/agnocast_callback_isolated_executor.cpp
+++ b/src/agnocastlib/src/agnocast_callback_isolated_executor.cpp
@@ -89,7 +89,7 @@ void CallbackIsolatedAgnocastExecutor::spin()
 
       weak_child_executors_.push_back(executor);
 
-      child_threads_.emplace_back([executor, callback_group_id = std::move(callback_group_id),
+      child_threads_.emplace_back([this, executor, callback_group_id = std::move(callback_group_id),
                                    &client_publisher, &client_publisher_mutex]() {
         auto tid = static_cast<pid_t>(syscall(SYS_gettid));
 
@@ -98,7 +98,13 @@ void CallbackIsolatedAgnocastExecutor::spin()
           agnocast::publish_callback_group_info(client_publisher, tid, callback_group_id);
         }
 
-        executor->spin();
+        try {
+          executor->spin();
+        } catch (const std::exception & e) {
+          RCLCPP_ERROR(logger, "Exception in agnocast child executor thread: %s", e.what());
+          worker_thread_failed_.store(true);
+          spinning.store(false);
+        }
       });
     };
 
@@ -173,6 +179,10 @@ void CallbackIsolatedAgnocastExecutor::spin()
   }
   child_threads_.clear();
   weak_child_executors_.clear();
+
+  if (worker_thread_failed_.load()) {
+    throw std::runtime_error("Exception occurred in agnocast child executor thread");
+  }
 }
 
 void CallbackIsolatedAgnocastExecutor::add_callback_group(
diff --git a/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp b/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp
index 9f7c53b61..7aa6bfd68 100644
--- a/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp
+++ b/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp
@@ -111,6 +111,10 @@ void MultiThreadedAgnocastExecutor::spin()
   for (auto & thread : threads) {
     thread.join();
   }
+
+  if (worker_thread_failed_.load()) {
+    throw std::runtime_error("Exception occurred in agnocast worker thread");
+  }
 }
 
 void MultiThreadedAgnocastExecutor::ros2_spin()
@@ -155,30 +159,36 @@ void MultiThreadedAgnocastExecutor::ros2_spin()
 
 void MultiThreadedAgnocastExecutor::agnocast_spin()
 {
-  while (rclcpp::ok(this->context_) && spinning.load()) {
-    if (need_epoll_updates.load()) {
-      prepare_epoll();
-    }
+  try {
+    while (rclcpp::ok(this->context_) && spinning.load()) {
+      if (need_epoll_updates.load()) {
+        prepare_epoll();
+      }
 
-    agnocast::AgnocastExecutable agnocast_executable;
+      agnocast::AgnocastExecutable agnocast_executable;
 
-    if (!rclcpp::ok(this->context_) || !spinning.load()) {
-      return;
-    }
-
-    // Unlike a single-threaded executor, in a multi-threaded executor, each thread is dedicated to
-    // handling either ROS 2 callbacks or Agnocast callbacks exclusively.
-    // Given this separation, get_next_agnocast_executable() can block indefinitely without a
-    // timeout. However, since we need to periodically check for epoll updates, we should implement
-    // a long timeout period instead of an infinite block.
-    if (get_next_agnocast_executable(
-          agnocast_executable, agnocast_next_exec_timeout_ms_ /* timed-blocking*/)) {
-      if (yield_before_execute_) {
-        std::this_thread::yield();
+      if (!rclcpp::ok(this->context_) || !spinning.load()) {
+        return;
       }
 
-      execute_agnocast_executable(agnocast_executable);
+      // Unlike a single-threaded executor, in a multi-threaded executor, each thread is dedicated
+      // to handling either ROS 2 callbacks or Agnocast callbacks exclusively. Given this
+      // separation, get_next_agnocast_executable() can block indefinitely without a timeout.
+      // However, since we need to periodically check for epoll updates, we should implement a long
+      // timeout period instead of an infinite block.
+      if (get_next_agnocast_executable(
+            agnocast_executable, agnocast_next_exec_timeout_ms_ /* timed-blocking*/)) {
+        if (yield_before_execute_) {
+          std::this_thread::yield();
+        }
+
+        execute_agnocast_executable(agnocast_executable);
+      }
     }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(logger, "Exception in agnocast worker thread: %s", e.what());
+    worker_thread_failed_.store(true);
+    spinning.store(false);
   }
 }
 
diff --git a/src/agnocastlib/src/node/agnocast_only_callback_isolated_executor.cpp b/src/agnocastlib/src/node/agnocast_only_callback_isolated_executor.cpp
index 1952cb577..c207161bd 100644
--- a/src/agnocastlib/src/node/agnocast_only_callback_isolated_executor.cpp
+++ b/src/agnocastlib/src/node/agnocast_only_callback_isolated_executor.cpp
@@ -92,7 +92,7 @@ void AgnocastOnlyCallbackIsolatedExecutor::spin()
 
       weak_child_executors_.push_back(agnocast_executor);
 
-      child_threads_.emplace_back([executor = std::move(agnocast_executor),
+      child_threads_.emplace_back([this, executor = std::move(agnocast_executor),
                                    callback_group_id = std::move(callback_group_id),
                                    &client_publisher, &client_publisher_mutex]() {
         auto tid = static_cast<pid_t>(syscall(SYS_gettid));
@@ -102,7 +102,13 @@ void AgnocastOnlyCallbackIsolatedExecutor::spin()
           agnocast::publish_callback_group_info(client_publisher, tid, callback_group_id);
         }
 
-        executor->spin();
+        try {
+          executor->spin();
+        } catch (const std::exception & e) {
+          RCLCPP_ERROR(logger, "Exception in agnocast child executor thread: %s", e.what());
+          worker_thread_failed_.store(true);
+          spinning_.store(false);
+        }
       });
     };
 
@@ -182,6 +188,10 @@ void AgnocastOnlyCallbackIsolatedExecutor::spin()
   }
   child_threads_.clear();
   weak_child_executors_.clear();
+
+  if (worker_thread_failed_.load()) {
+    throw std::runtime_error("Exception occurred in agnocast child executor thread");
+  }
 }
 
 void AgnocastOnlyCallbackIsolatedExecutor::cancel()
diff --git a/src/agnocastlib/src/node/agnocast_only_multi_threaded_executor.cpp b/src/agnocastlib/src/node/agnocast_only_multi_threaded_executor.cpp
index ace74a504..f145f2399 100644
--- a/src/agnocastlib/src/node/agnocast_only_multi_threaded_executor.cpp
+++ b/src/agnocastlib/src/node/agnocast_only_multi_threaded_executor.cpp
@@ -42,43 +42,53 @@ void AgnocastOnlyMultiThreadedExecutor::spin()
   for (auto & thread : threads) {
     thread.join();
   }
+
+  if (worker_thread_failed_.load()) {
+    throw std::runtime_error("Exception occurred in agnocast worker thread");
+  }
 }
 
 void AgnocastOnlyMultiThreadedExecutor::agnocast_spin()
 {
-  while (spinning_.load()) {
-    if (need_epoll_updates.load()) {
-      add_callback_groups_from_nodes_associated_to_executor();
-      agnocast::prepare_epoll_impl(
-        epoll_fd_, my_pid_, ready_agnocast_executables_mutex_, ready_agnocast_executables_,
-        [this](const rclcpp::CallbackGroup::SharedPtr & group) {
-          return is_callback_group_associated(group);
-        });
-    }
+  try {
+    while (spinning_.load()) {
+      if (need_epoll_updates.load()) {
+        add_callback_groups_from_nodes_associated_to_executor();
+        agnocast::prepare_epoll_impl(
+          epoll_fd_, my_pid_, ready_agnocast_executables_mutex_, ready_agnocast_executables_,
+          [this](const rclcpp::CallbackGroup::SharedPtr & group) {
+            return is_callback_group_associated(group);
+          });
+      }
 
-    agnocast::AgnocastExecutable agnocast_executable;
+      agnocast::AgnocastExecutable agnocast_executable;
 
-    if (!spinning_.load()) {
-      return;
-    }
-
-    // As each thread is dedicated to handling Agnocast callbacks, get_next_agnocast_executable()
-    // can block indefinitely without a timeout. However, since we need to periodically check for
-    // epoll updates, we should implement a long timeout period instead of an infinite block.
-    bool shutdown_detected = false;
-    if (get_next_agnocast_executable(
-          agnocast_executable, next_exec_timeout_ms_ /* timed-blocking*/, shutdown_detected)) {
-      if (yield_before_execute_) {
-        std::this_thread::yield();
+      if (!spinning_.load()) {
+        return;
       }
 
-      execute_agnocast_executable(agnocast_executable);
-    }
+      // As each thread is dedicated to handling Agnocast callbacks, get_next_agnocast_executable()
+      // can block indefinitely without a timeout. However, since we need to periodically check for
+      // epoll updates, we should implement a long timeout period instead of an infinite block.
+      bool shutdown_detected = false;
+      if (get_next_agnocast_executable(
+            agnocast_executable, next_exec_timeout_ms_ /* timed-blocking*/, shutdown_detected)) {
+        if (yield_before_execute_) {
+          std::this_thread::yield();
+        }
+
+        execute_agnocast_executable(agnocast_executable);
+      }
 
-    if (shutdown_detected) {
-      spinning_.store(false);
-      return;
+      if (shutdown_detected) {
+        spinning_.store(false);
+        return;
+      }
     }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(logger, "Exception in agnocast worker thread: %s", e.what());
+    worker_thread_failed_.store(true);
+    spinning_.store(false);
   }
 }
 

From a10b283f7d7c447ae12ae4f28780cee09540b951 Mon Sep 17 00:00:00 2001
From: Koichi Imai <koichi.imai.2@tier4.jp>
Date: Sun, 1 Mar 2026 15:47:22 +0900
Subject: [PATCH 11/11] fix unit tests

Signed-off-by: Koichi Imai <koichi.imai.2@tier4.jp>
---
 src/agnocastlib/test/unit/test_agnocast_subscription.cpp | 5 +----
 src/agnocastlib/test/unit/test_mocked_agnocast.cpp       | 4 +---
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/agnocastlib/test/unit/test_agnocast_subscription.cpp b/src/agnocastlib/test/unit/test_agnocast_subscription.cpp
index 9c787fe8b..14eec5ad1 100644
--- a/src/agnocastlib/test/unit/test_agnocast_subscription.cpp
+++ b/src/agnocastlib/test/unit/test_agnocast_subscription.cpp
@@ -34,10 +34,7 @@ TEST_F(GetValidCallbackGroupTest, get_valid_callback_group_not_in_node)
   options.callback_group =
     other_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
-  EXPECT_EXIT(
-    agnocast::get_valid_callback_group(node.get(), options),
-    ::testing::ExitedWithCode(EXIT_FAILURE),
-    "Cannot create agnocast subscription, callback group not in node.");
+  EXPECT_THROW(agnocast::get_valid_callback_group(node.get(), options), std::invalid_argument);
 }
 
 TEST_F(GetValidCallbackGroupTest, get_valid_callback_group_nullptr)
diff --git a/src/agnocastlib/test/unit/test_mocked_agnocast.cpp b/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
index c72867fb4..45b2ba03e 100644
--- a/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
+++ b/src/agnocastlib/test/unit/test_mocked_agnocast.cpp
@@ -536,9 +536,7 @@ TEST_F(AgnocastCallbackInfoTest, get_erased_callback_invalid_type)
   // Act & Assert
   agnocast::TypeErasedCallback erased_callback =
     agnocast::get_erased_callback<float>(float_callback);
-  EXPECT_EXIT(
-    erased_callback(std::move(int_arg)), ::testing::ExitedWithCode(EXIT_FAILURE),
-    "Agnocast internal implementation error: bad allocation when callback is called");
+  EXPECT_THROW(erased_callback(std::move(int_arg)), std::runtime_error);
 }
 
 TEST_F(AgnocastCallbackInfoTest, get_erased_callback_const_ptr)