fix(rclcpp_action): allow publishing feedback in canceling state (fixes #3283)

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -147,22 +147,22 @@ 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.
    */
   void
   publish_feedback(std::shared_ptr<typename ActionT::Feedback> 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<typename ActionT::Impl::FeedbackMessage>();

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<rclcpp::Node>(
+    "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<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(
+    node, "fibonacci",
+    handle_goal,
+    handle_cancel,
+    handle_accepted);
+  (void)as;
+
+  // Subscribe to feedback messages
+  using FeedbackT = Fibonacci::Impl::FeedbackMessage;
+  std::vector<FeedbackT::ConstSharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<FeedbackT>(
+    "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<Fibonacci::Feedback>();
+  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<rclcpp::Node>("get_result", "/rclcpp_action/get_result");

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<void>) {};
     auto on_executing = [](const rclcpp_action::GoalUUID &) {};
     auto publish_feedback =
-      [](std::shared_ptr<test_msgs::action::Fibonacci::Impl::FeedbackMessage>) {};
+      [this](std::shared_ptr<test_msgs::action::Fibonacci::Impl::FeedbackMessage>) {
+        ++this->feedback_count_;
+      };
     handle_ = std::make_unique<FibonacciServerGoalHandle>(
       rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback);
   }
 
 protected:
   std::unique_ptr<FibonacciServerGoalHandle> handle_;
+  size_t feedback_count_{0};
 };
 
 TEST_F(TestServerGoalHandle, construct_destruct) {
@@ -178,13 +181,25 @@ 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<test_msgs::action::Fibonacci::Feedback>();
   EXPECT_NO_THROW(handle_->publish_feedback(feedback));
+  EXPECT_EQ(0u, feedback_count_);
 }
 
 TEST_F(TestServerGoalHandle, publish_feedback_executing) {
   // Goal is in executing state, publish_feedback should succeed
   handle_->execute();
   auto feedback = std::make_shared<test_msgs::action::Fibonacci::Feedback>();
   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<test_msgs::action::Fibonacci::Feedback>();
+  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<test_msgs::action::Fibonacci::Feedback>();
   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<test_msgs::action::Fibonacci::Result>();
+  handle_->canceled(result);
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_FALSE(handle_->is_active());
+  auto feedback = std::make_shared<test_msgs::action::Fibonacci::Feedback>();
+  EXPECT_NO_THROW(handle_->publish_feedback(feedback));
+  EXPECT_EQ(0u, feedback_count_);
 }
 
 TEST_F(TestServerGoalHandle, rcl_action_goal_handle_get_status_error) {