diff --git a/rclcpp_async/include/rclcpp_async/channel.hpp b/rclcpp_async/include/rclcpp_async/channel.hpp index c9e5ac4..8f1904e 100644 --- a/rclcpp_async/include/rclcpp_async/channel.hpp +++ b/rclcpp_async/include/rclcpp_async/channel.hpp @@ -37,6 +37,7 @@ class Channel std::mutex mutex_; std::queue queue_; std::coroutine_handle<> waiter_; + std::weak_ptr waiter_alive_; size_t max_depth_; bool closed_ = false; @@ -55,6 +56,7 @@ class Channel std::stop_token token; std::shared_ptr cancel_cb_; bool cancelled = false; + std::shared_ptr alive_; void set_token(std::stop_token t) { token = std::move(t); } @@ -86,7 +88,7 @@ class Channel } }; - NextAwaiter next() { return NextAwaiter{*this, {}, {}, false}; } + NextAwaiter next() { return NextAwaiter{*this, {}, {}, false, {}}; } }; template @@ -97,7 +99,9 @@ bool Channel::NextAwaiter::await_suspend(std::coroutine_handle<> h) if (!ch.queue_.empty() || ch.closed_) { return false; // don't suspend, data already available } + alive_ = std::make_shared(); ch.waiter_ = h; + ch.waiter_alive_ = alive_; } // release ch.mutex_ before stop_callback to avoid deadlock cancel_cb_ = std::make_shared(token, [this, h, &cb = cancel_cb_]() { std::coroutine_handle<> w; @@ -125,6 +129,7 @@ template void Channel::send(T value) { std::coroutine_handle<> w; + std::weak_ptr alive; { std::lock_guard lock(mutex_); if (closed_) { @@ -136,9 +141,10 @@ void Channel::send(T value) } w = waiter_; waiter_ = nullptr; + alive = std::move(waiter_alive_); } if (w) { - ctx_.post([w]() { w.resume(); }); + post_resume(ctx_, w, std::move(alive)); } } @@ -146,14 +152,16 @@ template void Channel::close() { std::coroutine_handle<> w; + std::weak_ptr alive; { std::lock_guard lock(mutex_); closed_ = true; w = waiter_; waiter_ = nullptr; + alive = std::move(waiter_alive_); } if (w) { - ctx_.post([w]() { w.resume(); }); + post_resume(ctx_, w, std::move(alive)); } } diff --git a/rclcpp_async/include/rclcpp_async/co_context.hpp b/rclcpp_async/include/rclcpp_async/co_context.hpp index 6ca2d46..2ed715a 100644 --- a/rclcpp_async/include/rclcpp_async/co_context.hpp +++ b/rclcpp_async/include/rclcpp_async/co_context.hpp @@ -252,7 +252,8 @@ class CoContext : public Executor max_depth, {}, {}, - std::make_shared::State>()}; + std::make_shared::State>(), + {}}; } template @@ -500,6 +501,7 @@ template void SendGoalAwaiter::await_suspend(std::coroutine_handle<> h) { state_->stream = std::make_shared>(ctx, max_depth); + alive_ = std::make_shared(); typename rclcpp_action::Client::SendGoalOptions options; @@ -523,8 +525,8 @@ 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) { + options.goal_response_callback = [state = state_, client = client, &ctx = ctx, h, + alive = std::weak_ptr(alive_)](const auto & goal_handle) { if (state->done) { return; } @@ -541,7 +543,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, h, alive); }; 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..d88044b 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 { @@ -61,4 +62,15 @@ inline void register_cancel( }); } +// Resume h later on the executor thread, unless the awaiter owning `alive` is +// gone by then: its frame may be destroyed between the post and the resume. +inline void post_resume(Executor & ctx, std::coroutine_handle<> h, std::weak_ptr alive) +{ + ctx.post([&ctx, h, alive = std::move(alive)]() { + if (alive.lock()) { + ctx.resume(h); + } + }); +} + } // namespace rclcpp_async diff --git a/rclcpp_async/include/rclcpp_async/goal_stream.hpp b/rclcpp_async/include/rclcpp_async/goal_stream.hpp index 3882352..5c72f86 100644 --- a/rclcpp_async/include/rclcpp_async/goal_stream.hpp +++ b/rclcpp_async/include/rclcpp_async/goal_stream.hpp @@ -203,6 +203,7 @@ struct SendGoalAwaiter std::stop_token token; std::shared_ptr cancel_cb_; std::shared_ptr state_; + std::shared_ptr alive_; // 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..b01d951 100644 --- a/rclcpp_async/include/rclcpp_async/tf_buffer.hpp +++ b/rclcpp_async/include/rclcpp_async/tf_buffer.hpp @@ -47,6 +47,7 @@ class TfBuffer tf2::TimePoint time; std::coroutine_handle<> handle; std::shared_ptr active; + std::weak_ptr alive; geometry_msgs::msg::TransformStamped result; }; @@ -110,6 +111,7 @@ class TfBuffer std::shared_ptr cancel_cb_; bool cancelled = false; std::shared_ptr request; + std::shared_ptr alive_; // A frame destroyed while suspended here must not stay pending, or the // transform's arrival would resume it. The deferred cancellation no longer @@ -146,6 +148,8 @@ class TfBuffer request->time = time; request->handle = h; request->active = std::make_shared(true); + alive_ = std::make_shared(); + request->alive = alive_; tf.add_pending(request); register_cancel( @@ -170,8 +174,8 @@ 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, {}}; } void add_pending(std::shared_ptr req) @@ -213,8 +217,7 @@ class TfBuffer return false; } *req->active = false; - auto h = req->handle; - ctx_.post([h]() { h.resume(); }); + post_resume(ctx_, req->handle, req->alive); 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..f65e1c8 100644 --- a/rclcpp_async/test/test_task_destroy.cpp +++ b/rclcpp_async/test/test_task_destroy.cpp @@ -314,3 +314,42 @@ 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); +} 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;