diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index b1ccb78b1e..c07c3ffc08 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -147,11 +147,11 @@ class ServerGoalHandle : public ServerGoalHandleBase public: /// Send an update about the progress of a goal. /** - * This must only be called when the goal is executing. + * This must only be called when the goal is executing or canceling. * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called * first. * - * If the goal is in any state besides executing, a warning will be logged and the feedback + * If the goal is in any state besides executing or canceling, a warning will be logged and the feedback * will not be published. * * \param[in] feedback_msg the message to publish to clients. @@ -159,10 +159,10 @@ class ServerGoalHandle : public ServerGoalHandleBase void publish_feedback(std::shared_ptr feedback_msg) { - if (!is_executing()) { + if (!is_executing() && !is_canceling()) { RCLCPP_WARN( rclcpp::get_logger("rclcpp_action"), - "publish_feedback() called on a goal handle not in executing state, ignoring"); + "publish_feedback() called on a goal handle not in executing or canceling state, ignoring"); return; } auto feedback_message = std::make_shared(); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 1061ff9071..7402306ce8 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -884,6 +884,69 @@ TEST_F(TestServer, publish_feedback) ASSERT_EQ(sent_message->sequence, msg->feedback.sequence); } +TEST_F(TestServer, publish_feedback_canceling) +{ + auto node = std::make_shared( + "pub_feedback_canceling", "/rclcpp_action/pub_feedback_canceling"); + const GoalUUID uuid{{2, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to feedback messages + using FeedbackT = Fibonacci::Impl::FeedbackMessage; + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::ConstSharedPtr msg) + { + received_msgs.push_back(msg); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + ASSERT_TRUE(received_handle->is_canceling()); + + auto sent_message = std::make_shared(); + sent_message->sequence = {1, 1, 2, 3, 5, 8}; + received_handle->publish_feedback(sent_message); + + // 10 seconds + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + } + + ASSERT_EQ(1u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(sent_message->sequence, msg->feedback.sequence); +} + TEST_F(TestServer, get_result) { auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); diff --git a/rclcpp_action/test/test_server_goal_handle.cpp b/rclcpp_action/test/test_server_goal_handle.cpp index f5faaabea3..386ffb2370 100644 --- a/rclcpp_action/test/test_server_goal_handle.cpp +++ b/rclcpp_action/test/test_server_goal_handle.cpp @@ -78,13 +78,16 @@ class TestServerGoalHandle : public ::testing::Test auto on_terminal_state = [](const rclcpp_action::GoalUUID &, std::shared_ptr) {}; auto on_executing = [](const rclcpp_action::GoalUUID &) {}; auto publish_feedback = - [](std::shared_ptr) {}; + [this](std::shared_ptr) { + ++this->feedback_count_; + }; handle_ = std::make_unique( rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback); } protected: std::unique_ptr handle_; + size_t feedback_count_{0}; }; TEST_F(TestServerGoalHandle, construct_destruct) { @@ -178,6 +181,7 @@ TEST_F(TestServerGoalHandle, publish_feedback_not_executing) { // Goal is in accepted (not executing) state, publish_feedback should warn and ignore auto feedback = std::make_shared(); EXPECT_NO_THROW(handle_->publish_feedback(feedback)); + EXPECT_EQ(0u, feedback_count_); } TEST_F(TestServerGoalHandle, publish_feedback_executing) { @@ -185,6 +189,17 @@ TEST_F(TestServerGoalHandle, publish_feedback_executing) { handle_->execute(); auto feedback = std::make_shared(); EXPECT_NO_THROW(handle_->publish_feedback(feedback)); + EXPECT_EQ(1u, feedback_count_); +} + +TEST_F(TestServerGoalHandle, publish_feedback_canceling) { + // Goal is in canceling state, publish_feedback should succeed + handle_->execute(); + handle_->cancel_goal(); + EXPECT_TRUE(handle_->is_canceling()); + auto feedback = std::make_shared(); + EXPECT_NO_THROW(handle_->publish_feedback(feedback)); + EXPECT_EQ(1u, feedback_count_); } TEST_F(TestServerGoalHandle, publish_feedback_after_succeed) { @@ -194,6 +209,21 @@ TEST_F(TestServerGoalHandle, publish_feedback_after_succeed) { handle_->succeed(result); auto feedback = std::make_shared(); EXPECT_NO_THROW(handle_->publish_feedback(feedback)); + EXPECT_EQ(0u, feedback_count_); +} + +TEST_F(TestServerGoalHandle, publish_feedback_after_canceled) { + // Goal reached terminal canceled state, publish_feedback should warn and ignore + handle_->execute(); + handle_->cancel_goal(); + EXPECT_TRUE(handle_->is_canceling()); + auto result = std::make_shared(); + handle_->canceled(result); + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_FALSE(handle_->is_active()); + auto feedback = std::make_shared(); + EXPECT_NO_THROW(handle_->publish_feedback(feedback)); + EXPECT_EQ(0u, feedback_count_); } TEST_F(TestServerGoalHandle, rcl_action_goal_handle_get_status_error) {