diff --git a/rclcpp_async/CMakeLists.txt b/rclcpp_async/CMakeLists.txt index 31a71e4..0cd5729 100644 --- a/rclcpp_async/CMakeLists.txt +++ b/rclcpp_async/CMakeLists.txt @@ -149,7 +149,8 @@ if(BUILD_TESTING) target_link_libraries(test_tf_buffer ${PROJECT_NAME}) ament_add_ros_isolated_gtest(test_task_destroy test/test_task_destroy.cpp) - target_link_libraries(test_task_destroy ${PROJECT_NAME} ${example_interfaces_TARGETS}) + target_link_libraries( + test_task_destroy ${PROJECT_NAME} ${example_interfaces_TARGETS} ${std_msgs_TARGETS}) add_subdirectory(test/benchmark) endif() diff --git a/rclcpp_async/include/rclcpp_async/channel.hpp b/rclcpp_async/include/rclcpp_async/channel.hpp index c9e5ac4..06e9198 100644 --- a/rclcpp_async/include/rclcpp_async/channel.hpp +++ b/rclcpp_async/include/rclcpp_async/channel.hpp @@ -36,7 +36,7 @@ class Channel Executor & ctx_; std::mutex mutex_; std::queue queue_; - std::coroutine_handle<> waiter_; + std::weak_ptr waiter_; size_t max_depth_; bool closed_ = false; @@ -55,6 +55,7 @@ class Channel std::stop_token token; std::shared_ptr cancel_cb_; bool cancelled = false; + std::shared_ptr waiting_; void set_token(std::stop_token t) { token = std::move(t); } @@ -86,7 +87,7 @@ class Channel } }; - NextAwaiter next() { return NextAwaiter{*this, {}, {}, false}; } + NextAwaiter next() { return NextAwaiter{*this, {}, {}, false, nullptr}; } }; template @@ -97,26 +98,19 @@ bool Channel::NextAwaiter::await_suspend(std::coroutine_handle<> h) if (!ch.queue_.empty() || ch.closed_) { return false; // don't suspend, data already available } - ch.waiter_ = h; + waiting_ = std::make_shared(h); + ch.waiter_ = waiting_; } // release ch.mutex_ before stop_callback to avoid deadlock - cancel_cb_ = std::make_shared(token, [this, h, &cb = cancel_cb_]() { - std::coroutine_handle<> w; + cancel_cb_ = std::make_shared(token, [this]() { { std::lock_guard lock(ch.mutex_); - if (ch.waiter_ != h) { + if (ch.waiter_.lock() != waiting_) { return; } - w = ch.waiter_; - ch.waiter_ = nullptr; - } - if (w) { - cancelled = true; - ch.ctx_.post([w, weak = std::weak_ptr(cb)]() { - if (weak.lock()) { - w.resume(); - } - }); + ch.waiter_.reset(); } + cancelled = true; + post_resume(ch.ctx_, waiting_); }); return true; } @@ -124,7 +118,7 @@ bool Channel::NextAwaiter::await_suspend(std::coroutine_handle<> h) template void Channel::send(T value) { - std::coroutine_handle<> w; + std::weak_ptr w; { std::lock_guard lock(mutex_); if (closed_) { @@ -134,26 +128,24 @@ void Channel::send(T value) while (queue_.size() > max_depth_) { queue_.pop(); } - w = waiter_; - waiter_ = nullptr; + w = std::exchange(waiter_, {}); } - if (w) { - ctx_.post([w]() { w.resume(); }); + if (!w.expired()) { + post_resume(ctx_, std::move(w)); } } template void Channel::close() { - std::coroutine_handle<> w; + std::weak_ptr w; { std::lock_guard lock(mutex_); closed_ = true; - w = waiter_; - waiter_ = nullptr; + w = std::exchange(waiter_, {}); } - if (w) { - ctx_.post([w]() { w.resume(); }); + if (!w.expired()) { + post_resume(ctx_, std::move(w)); } } diff --git a/rclcpp_async/include/rclcpp_async/co_context.hpp b/rclcpp_async/include/rclcpp_async/co_context.hpp index 2792444..845ef66 100644 --- a/rclcpp_async/include/rclcpp_async/co_context.hpp +++ b/rclcpp_async/include/rclcpp_async/co_context.hpp @@ -259,7 +259,8 @@ class CoContext : public Executor max_depth, {}, {}, - std::make_shared::State>()}; + std::make_shared::State>(), + nullptr}; } template @@ -532,8 +533,10 @@ void SendGoalAwaiter::await_suspend(std::coroutine_handle<> h) // server's accept/reject). When `state->done` is already set by the // cancel path, the callback bails out before touching the (dangling) // coroutine handle. - options.goal_response_callback = [state = state_, client = client, &ctx = ctx, - h](const auto & goal_handle) { + waiting_ = std::make_shared(h); + state_->waiting = waiting_; + options.goal_response_callback = [state = state_, client = client, + &ctx = ctx](const auto & goal_handle) { if (state->done) { return; } @@ -550,7 +553,7 @@ void SendGoalAwaiter::await_suspend(std::coroutine_handle<> h) // (Jazzy bug: https://github.com/ros2/rclcpp/issues/2796). // Resuming synchronously here would deadlock if the coroutine // immediately calls async_send_goal again. - ctx.post([&ctx, h]() { ctx.resume(h); }); + post_resume(ctx, state->waiting); }; client->async_send_goal(goal, options); diff --git a/rclcpp_async/include/rclcpp_async/executor.hpp b/rclcpp_async/include/rclcpp_async/executor.hpp index 24b724a..42344bc 100644 --- a/rclcpp_async/include/rclcpp_async/executor.hpp +++ b/rclcpp_async/include/rclcpp_async/executor.hpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace rclcpp_async { @@ -39,6 +40,23 @@ class Executor virtual void resume(std::coroutine_handle<> h) { h.resume(); } }; +// A suspended coroutine, owned by its awaiter. Whoever resumes it later holds +// a weak_ptr, which expires with the awaiter. +struct Waiting +{ + std::coroutine_handle<> h; +}; + +// Resume `w` on the executor unless its awaiter is gone by then. +inline void post_resume(Executor & ctx, std::weak_ptr w) +{ + ctx.post([w = std::move(w)]() { + if (auto s = w.lock()) { + s->h.resume(); + } + }); +} + template inline void register_cancel( std::shared_ptr & out, std::stop_token token, Executor & ctx, std::coroutine_handle<> h, diff --git a/rclcpp_async/include/rclcpp_async/goal_stream.hpp b/rclcpp_async/include/rclcpp_async/goal_stream.hpp index 3882352..9d6be47 100644 --- a/rclcpp_async/include/rclcpp_async/goal_stream.hpp +++ b/rclcpp_async/include/rclcpp_async/goal_stream.hpp @@ -194,6 +194,7 @@ struct SendGoalAwaiter bool cancelled = false; std::shared_ptr> stream; Result>> result; + std::weak_ptr waiting; }; CoContext & ctx; @@ -203,6 +204,7 @@ struct SendGoalAwaiter std::stop_token token; std::shared_ptr cancel_cb_; std::shared_ptr state_; + std::shared_ptr waiting_; // Same as SendRequestAwaiter: the goal response must not resume a frame // destroyed while suspended here. diff --git a/rclcpp_async/include/rclcpp_async/tf_buffer.hpp b/rclcpp_async/include/rclcpp_async/tf_buffer.hpp index 5c81bc8..6e0e76f 100644 --- a/rclcpp_async/include/rclcpp_async/tf_buffer.hpp +++ b/rclcpp_async/include/rclcpp_async/tf_buffer.hpp @@ -45,7 +45,7 @@ class TfBuffer std::string target_frame; std::string source_frame; tf2::TimePoint time; - std::coroutine_handle<> handle; + std::weak_ptr waiting; std::shared_ptr active; geometry_msgs::msg::TransformStamped result; }; @@ -110,6 +110,7 @@ class TfBuffer std::shared_ptr cancel_cb_; bool cancelled = false; std::shared_ptr request; + std::shared_ptr waiting; // A frame destroyed while suspended here must not stay pending, or the // transform's arrival would resume it. The deferred cancellation no longer @@ -144,7 +145,8 @@ class TfBuffer request->target_frame = target_frame; request->source_frame = source_frame; request->time = time; - request->handle = h; + waiting = std::make_shared(h); + request->waiting = waiting; request->active = std::make_shared(true); tf.add_pending(request); @@ -170,8 +172,9 @@ class TfBuffer LookupTransformAwaiter lookup_transform( const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time) { - return LookupTransformAwaiter{*this, target_frame, source_frame, tf2_ros::fromRclcpp(time), - {}, {}, false, nullptr}; + return LookupTransformAwaiter{*this, target_frame, source_frame, tf2_ros::fromRclcpp(time), + {}, {}, false, nullptr, + nullptr}; } void add_pending(std::shared_ptr req) @@ -213,8 +216,7 @@ class TfBuffer return false; } *req->active = false; - auto h = req->handle; - ctx_.post([h]() { h.resume(); }); + post_resume(ctx_, req->waiting); return true; } return false; diff --git a/rclcpp_async/test/test_channel.cpp b/rclcpp_async/test/test_channel.cpp index 8676165..fe64644 100644 --- a/rclcpp_async/test/test_channel.cpp +++ b/rclcpp_async/test/test_channel.cpp @@ -263,3 +263,31 @@ TEST_F(ChannelTest, CancelFromThread) ASSERT_TRUE(running.handle.done()); EXPECT_TRUE(was_cancelled); } + +TEST_F(ChannelTest, SendThenWaiterDestroyed) +{ + Channel ch(*ctx_); + auto coro = [&]() -> Task { co_await ch.next(); }; + { + auto task = ctx_->create_task(coro()); + ch.send(1); // posts the resumption, destroyed before it runs + } + for (int i = 0; i < 5; i++) { + executor_.spin_some(); + std::this_thread::sleep_for(1ms); + } +} + +TEST_F(ChannelTest, CloseThenWaiterDestroyed) +{ + Channel ch(*ctx_); + auto coro = [&]() -> Task { co_await ch.next(); }; + { + auto task = ctx_->create_task(coro()); + ch.close(); // posts the resumption, destroyed before it runs + } + for (int i = 0; i < 5; i++) { + executor_.spin_some(); + std::this_thread::sleep_for(1ms); + } +} diff --git a/rclcpp_async/test/test_task_destroy.cpp b/rclcpp_async/test/test_task_destroy.cpp index 211de02..b8da11e 100644 --- a/rclcpp_async/test/test_task_destroy.cpp +++ b/rclcpp_async/test/test_task_destroy.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "rclcpp_async/rclcpp_async.hpp" @@ -314,3 +315,82 @@ TEST_F(TaskDestroyTest, ResponseAfterRequesterDestroyed) service->send_response(*pending_header, response); spin_for(200ms); } + +TEST_F(TaskDestroyTest, GoalResponseThenRequesterDestroyed) +{ + // The client answers in its own group, so its goal response can post the + // resumption without executor_ running it. + auto group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::executors::SingleThreadedExecutor client_executor; + client_executor.add_callback_group(group, node_->get_node_base_interface()); + auto client = rclcpp_action::create_client(node_, "test_destroy_action", group); + bool goal_received = false; + action_server_ = rclcpp_action::create_server( + node_, "test_destroy_action", + [&goal_received](const rclcpp_action::GoalUUID &, std::shared_ptr) { + goal_received = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](const std::shared_ptr) { return rclcpp_action::CancelResponse::ACCEPT; }, + [](const std::shared_ptr) {}); + ASSERT_TRUE(client->wait_for_action_server(5s)); + + auto send = [this, &client]() -> Task { + co_await ctx_->send_goal(client, Fibonacci::Goal()); + }; + { + auto task = ctx_->create_task(send()); + auto deadline = std::chrono::steady_clock::now() + 5s; + while (!goal_received && std::chrono::steady_clock::now() < deadline) { + spin_for(10ms); + } + ASSERT_TRUE(goal_received); + // Let the goal response post the resumption, then destroy the task + // before executor_ runs it. + for (int i = 0; i < 20; i++) { + client_executor.spin_some(); + std::this_thread::sleep_for(10ms); + } + } + spin_for(50ms); +} + +// A node-like owner holding the context, what its task waits on, and the task. +struct TaskOwner +{ + TaskOwner(rclcpp::Node & node, std::shared_ptr g) + : ctx(node), event(ctx), guard(std::move(g)) + { + stream = ctx.subscribe("test_destroy_owner_topic", 10); + } + CoContext ctx; + Event event; + std::shared_ptr> stream; + std::shared_ptr guard; + Task task; + + template + Task run(F wait) + { + auto g = guard; + co_await wait(*this); + } +}; + +TEST_F(TaskDestroyTest, OwnerDestroyedWhileItsTaskWaits) +{ + auto pub = node_->create_publisher("test_destroy_owner_topic", 10); + auto check = [&](auto wait) { + auto guard = std::make_shared(0); + auto owner = std::make_unique(*node_, guard); + owner->task = owner->ctx.create_task(owner->run(wait)); + spin_for(50ms); + owner.reset(); + pub->publish(std_msgs::msg::String()); + spin_for(50ms); + EXPECT_EQ(guard.use_count(), 1); // the frame went with its owner + }; + check([](TaskOwner & o) { return as_task(o.event.wait()); }); + check([](TaskOwner & o) { return as_task(o.ctx.sleep(10s)); }); + check([](TaskOwner & o) -> Task { co_await o.stream->next(); }); +} diff --git a/rclcpp_async/test/test_tf_buffer.cpp b/rclcpp_async/test/test_tf_buffer.cpp index c711b32..e08f6bf 100644 --- a/rclcpp_async/test/test_tf_buffer.cpp +++ b/rclcpp_async/test/test_tf_buffer.cpp @@ -244,6 +244,30 @@ TEST_F(TfBufferTest, TransformAfterWaiterDestroyed) } } +TEST_F(TfBufferTest, TransformThenWaiterDestroyed) +{ + auto coro = [&]() -> Task { + co_await tf_buffer_->lookup_transform("r", "s", rclcpp::Time(0)); + }; + auto broadcaster = make_static_broadcaster(node_); + { + auto task = ctx_->create_task(coro()); + // The transform arrives on TfBuffer's own thread, which posts the + // resumption; the task is destroyed before executor_ runs it. + broadcaster.sendTransform(make_transform("r", "s")); + auto deadline = std::chrono::steady_clock::now() + 5s; + while (!tf_buffer_->lookup_transform("r", "s") && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(10ms); + } + ASSERT_TRUE(tf_buffer_->lookup_transform("r", "s")); + std::this_thread::sleep_for(50ms); + } + for (int i = 0; i < 5; i++) { + executor_.spin_some(); + std::this_thread::sleep_for(10ms); + } +} + TEST_F(TfBufferTest, MultipleConcurrentLookups) { geometry_msgs::msg::TransformStamped r1, r2;