diff --git a/src/agnocastlib/include/agnocast.hpp b/src/agnocastlib/include/agnocast.hpp index 4865d3da..dee65704 100644 --- a/src/agnocastlib/include/agnocast.hpp +++ b/src/agnocastlib/include/agnocast.hpp @@ -24,8 +24,6 @@ namespace agnocast extern "C" void * initialize_agnocast(const uint64_t shm_size); -bool ok(); - template typename Publisher::SharedPtr create_publisher( rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos) diff --git a/src/agnocastlib/src/agnocast.cpp b/src/agnocastlib/src/agnocast.cpp index 7835d1bf..7a57cb8c 100644 --- a/src/agnocastlib/src/agnocast.cpp +++ b/src/agnocastlib/src/agnocast.cpp @@ -14,18 +14,12 @@ namespace agnocast { int agnocast_fd = -1; -std::atomic is_running = true; std::vector threads; extern mqd_t mq_new_publisher; std::vector shm_fds; std::mutex shm_fds_mtx; -bool ok() -{ - return is_running.load(); -} - bool already_mapped(const pid_t pid) { static pthread_mutex_t mapped_pid_mtx = PTHREAD_MUTEX_INITIALIZER; @@ -130,7 +124,6 @@ void * initialize_agnocast(const uint64_t shm_size) static void shutdown_agnocast() { printf("[INFO] [Agnocast]: shutdown_agnocast started\n"); - is_running.store(false); const pid_t pid = getpid(); diff --git a/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp b/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp index 37c16a35..2c17d169 100644 --- a/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp +++ b/src/agnocastlib/src/agnocast_multi_threaded_executor.cpp @@ -76,7 +76,7 @@ void MultiThreadedAgnocastExecutor::ros2_spin() { std::lock_guard wait_lock{wait_mutex_}; - if (!rclcpp::ok(this->context_) || !agnocast::ok()) { + if (!rclcpp::ok(this->context_)) { return; } if (!get_next_executable(any_executable, ros2_next_exec_timeout_)) { @@ -106,6 +106,10 @@ void MultiThreadedAgnocastExecutor::agnocast_spin() agnocast::AgnocastExecutables agnocast_executables; + if (!rclcpp::ok(this->context_)) { + 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_executables() can block indefinitely without a diff --git a/src/agnocastlib/src/agnocast_single_threaded_executor.cpp b/src/agnocastlib/src/agnocast_single_threaded_executor.cpp index 521224f7..f34fedd2 100644 --- a/src/agnocastlib/src/agnocast_single_threaded_executor.cpp +++ b/src/agnocastlib/src/agnocast_single_threaded_executor.cpp @@ -25,7 +25,7 @@ void SingleThreadedAgnocastExecutor::spin() // TODO(sykwer): Transient local - while (rclcpp::ok(this->context_) && agnocast::ok() && spinning.load()) { + while (rclcpp::ok(this->context_) && spinning.load()) { if (need_epoll_updates.exchange(false)) { prepare_epoll(); } diff --git a/src/agnocastlib/src/agnocast_subscription.cpp b/src/agnocastlib/src/agnocast_subscription.cpp index 68367139..f4810e77 100644 --- a/src/agnocastlib/src/agnocast_subscription.cpp +++ b/src/agnocastlib/src/agnocast_subscription.cpp @@ -49,7 +49,7 @@ static void wait_for_new_publisher(const pid_t subscriber_pid) // Create a thread that maps the areas for publishers afterwards auto th = std::thread([=]() { - while (agnocast::ok()) { + while (true) { MqMsgNewPublisher mq_msg = {}; auto ret = mq_receive(mq, reinterpret_cast(&mq_msg), sizeof(mq_msg), nullptr); if (ret == -1) {