Support feedback subscription content filter for action client

lib/action/client.js

@@ -51,6 +51,12 @@ class ActionClient extends Entity {
    * @param {QoS} options.qos.feedbackSubQosProfile - Quality of service option for the feedback subscription,
    *                                                  default: new QoS(QoS.HistoryPolicy.RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 10).
    * @param {QoS} options.qos.statusSubQosProfile - Quality of service option for the status subscription, default: QoS.profileActionStatusDefault.
+   * @param {boolean} options.enableFeedbackMsgOptimization - Enable feedback subscription content filter to
+   *    optimize the handling of feedback messages. When enabled, the content filter is used to configure
+   *    the goal ID for the subscription, which helps avoid the reception of irrelevant feedback messages.
+   *    An action client can handle up to 6 goals simultaneously with this optimization. If the number
+   *    of goals exceeds the limit or the RMW doesn't support content filter, optimization is automatically
+   *    disabled. Default: false.
    */
   constructor(node, typeClass, actionName, options) {
     super(null, null, options);
@@ -87,6 +93,15 @@ class ActionClient extends Entity {
       checkTypes: true,
     };
 
+    // Enable feedback subscription content filter optimization.
+    // Only supported on ROS2 Rolling and only effective when the native
+    // binding provides the required functions AND the RMW implementation
+    // actually supports content filtering on the feedback subscription.
+    this._enableFeedbackMsgOptimization =
+      this._options.enableFeedbackMsgOptimization === true &&
+      DistroUtils.getDistroId() >= DistroUtils.DistroId.ROLLING &&
+      typeof rclnodejs.actionConfigureFeedbackSubFilterAddGoalId === 'function';
+
     let type = this.typeClass.type();
 
     this._handle = rclnodejs.actionCreateClient(
@@ -126,6 +141,7 @@ class ActionClient extends Entity {
         }
 
         this._goalHandles.set(uuid, goalHandle);
+        this._feedbackSubFilterAddGoalId(goalHandle.goalId);
       } else {
         // Clean up feedback callback for rejected goals
         let uuid = ActionUuid.fromMessage(
@@ -205,6 +221,9 @@ class ActionClient extends Entity {
             status === ActionInterfaces.GoalStatus.STATUS_ABORTED
           ) {
             this._goalHandles.delete(uuid);
+            this._feedbackSubFilterRemoveGoalId(
+              statusMessage.goal_info.goal_id
+            );
           }
         }
       } else {
@@ -393,6 +412,8 @@ class ActionClient extends Entity {
       this._removePendingCancelRequest(sequenceNumber)
     );
 
+    this._feedbackSubFilterRemoveGoalId(goalHandle.goalId);
+
     return deferred.promise;
   }
 
@@ -442,9 +463,10 @@ class ActionClient extends Entity {
       goalHandle.status = result.status;
       return result.result;
     });
-    deferred.setDoneCallback(() =>
-      this._removePendingResultRequest(sequenceNumber)
-    );
+    deferred.setDoneCallback(() => {
+      this._removePendingResultRequest(sequenceNumber);
+      this._feedbackSubFilterRemoveGoalId(goalHandle.goalId);
+    });
 
     this._pendingResultRequests.set(sequenceNumber, deferred);
 
@@ -464,6 +486,42 @@ class ActionClient extends Entity {
     this._pendingCancelRequests.delete(sequenceNumber);
   }
 
+  /**
+   * Add a goal ID to the feedback subscription content filter.
+   * @ignore
+   * @param {object} goalId - The goal UUID message.
+   */
+  _feedbackSubFilterAddGoalId(goalId) {
+    if (!this._enableFeedbackMsgOptimization) return;
+    try {
+      rclnodejs.actionConfigureFeedbackSubFilterAddGoalId(
+        this.handle,
+        Buffer.from(goalId.uuid)
+      );
+    } catch (e) {
+      this._enableFeedbackMsgOptimization = false;
+      this._node.getLogger().warn(`${e.message}`);
+    }
+  }
+
+  /**
+   * Remove a goal ID from the feedback subscription content filter.
+   * @ignore
+   * @param {object} goalId - The goal UUID message.
+   */
+  _feedbackSubFilterRemoveGoalId(goalId) {
+    if (!this._enableFeedbackMsgOptimization) return;
+    try {
+      rclnodejs.actionConfigureFeedbackSubFilterRemoveGoalId(
+        this.handle,
+        Buffer.from(goalId.uuid)
+      );
+    } catch (e) {
+      this._enableFeedbackMsgOptimization = false;
+      this._node.getLogger().warn(`${e.message}`);
+    }
+  }
+
   /**
    * Destroy the underlying action client handle.
    * @return {undefined}

src/executor.cpp

@@ -48,7 +48,8 @@ Executor::Executor(Napi::Env env, HandleManager* handle_manager,
       handle_manager_(handle_manager),
       delegate_(delegate),
       context_(nullptr),
-      env_(env) {
+      env_(env),
+      work_pending_(false) {
   running_.store(false);
 }
 
@@ -105,6 +106,8 @@ void Executor::Stop() {
     // Stop thread first, and then uv_close
     // Make sure async_ is not used anymore
     running_.store(false);
+    // Wake the background thread in case it is waiting on the condvar.
+    work_done_cv_.notify_all();
     handle_manager_->StopWaitingHandles();
     uv_thread_join(&background_thread_);
 
@@ -133,6 +136,21 @@ bool Executor::IsMainThread() {
 
 void Executor::DoWork(uv_async_t* handle) {
   Executor* executor = reinterpret_cast<Executor*>(handle->data);
+
+  // RAII guard: always clear work_pending_ and notify the background thread,
+  // even if ExecuteReadyHandles() throws (e.g. from N-API callbacks).
+  // Without this, the background thread would block forever on work_done_cv_.
+  struct WorkDoneGuard {
+    Executor* exec;
+    ~WorkDoneGuard() {
+      {
+        std::lock_guard<std::mutex> lock(exec->work_done_mutex_);
+        exec->work_pending_ = false;
+      }
+      exec->work_done_cv_.notify_one();
+    }
+  } guard{executor};
+
   executor->ExecuteReadyHandles();
 }
 
@@ -159,7 +177,23 @@ void Executor::Run(void* arg) {
 
       if (!uv_is_closing(reinterpret_cast<uv_handle_t*>(executor->async_)) &&
           handle_manager->ready_handles_count() > 0) {
+        // Tell the main thread there is work to do, then wait for it to
+        // finish before re-entering rcl_wait.  This prevents a data race
+        // where the background thread holds subscriptions in the wait set
+        // while the main thread modifies their state (e.g. content filter).
+        {
+          std::lock_guard<std::mutex> lock(executor->work_done_mutex_);
+          executor->work_pending_ = true;
+        }
         uv_async_send(executor->async_);
+
+        // Wait until DoWork() signals completion.
+        {
+          std::unique_lock<std::mutex> lock(executor->work_done_mutex_);
+          executor->work_done_cv_.wait(lock, [executor] {
+            return !executor->work_pending_ || !executor->running_.load();
+          });
+        }
       }
     }
 

src/executor.h

@@ -20,7 +20,9 @@
 #include <uv.h>
 
 #include <atomic>
+#include <condition_variable>
 #include <exception>
+#include <mutex>
 #include <vector>
 
 #include "rcl_handle.h"
@@ -72,6 +74,15 @@ class Executor {
   Napi::Env env_;
 
   std::atomic_bool running_;
+
+  // Synchronization: the background thread waits after uv_async_send until
+  // the main thread finishes ExecuteReadyHandles.  This prevents the
+  // background thread from re-entering rcl_wait (which holds a reference to
+  // subscriptions) while the main thread modifies subscription state (e.g.
+  // content filter changes).
+  std::mutex work_done_mutex_;
+  std::condition_variable work_done_cv_;
+  bool work_pending_;  // true while the main thread is processing handles
 };
 
 }  // namespace rclnodejs

src/rcl_action_client_bindings.cpp

@@ -250,6 +250,67 @@ Napi::Value ActionSendCancelRequest(const Napi::CallbackInfo& info) {
   return Napi::Number::New(env, static_cast<double>(sequence_number));
 }
 
+#if ROS_VERSION >= 5000  // ROS2 Rolling
+Napi::Value ActionConfigureFeedbackSubFilterAddGoalId(
+    const Napi::CallbackInfo& info) {
+  Napi::Env env = info.Env();
+
+  RclHandle* action_client_handle =
+      RclHandle::Unwrap(info[0].As<Napi::Object>());
+  rcl_action_client_t* action_client =
+      reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr());
+
+  auto goal_id_buffer = info[1].As<Napi::Buffer<uint8_t>>();
+  const uint8_t* goal_id_array = goal_id_buffer.Data();
+  size_t goal_id_size = goal_id_buffer.Length();
+
+  rcl_ret_t ret =
+      rcl_action_client_configure_feedback_subscription_filter_add_goal_id(
+          action_client, goal_id_array, goal_id_size);
+
+  if (RCL_RET_OK != ret) {
+    std::string error_text{
+        "Failed to add goal id to feedback subscription content filter: "};
+    error_text += rcl_get_error_string().str;
+    rcl_reset_error();
+    Napi::Error::New(env, error_text).ThrowAsJavaScriptException();
+    return Napi::Boolean::New(env, false);
+  }
+
+  return Napi::Boolean::New(env, true);
+}
+
+Napi::Value ActionConfigureFeedbackSubFilterRemoveGoalId(
+    const Napi::CallbackInfo& info) {
+  Napi::Env env = info.Env();
+
+  RclHandle* action_client_handle =
+      RclHandle::Unwrap(info[0].As<Napi::Object>());
+  rcl_action_client_t* action_client =
+      reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr());
+
+  auto goal_id_buffer = info[1].As<Napi::Buffer<uint8_t>>();
+  const uint8_t* goal_id_array = goal_id_buffer.Data();
+  size_t goal_id_size = goal_id_buffer.Length();
+
+  rcl_ret_t ret =
+      rcl_action_client_configure_feedback_subscription_filter_remove_goal_id(
+          action_client, goal_id_array, goal_id_size);
+
+  if (RCL_RET_OK != ret) {
+    std::string error_text{
+        "Failed to remove goal id from feedback subscription content "
+        "filter: "};
+    error_text += rcl_get_error_string().str;
+    rcl_reset_error();
+    Napi::Error::New(env, error_text).ThrowAsJavaScriptException();
+    return Napi::Boolean::New(env, false);
+  }
+
+  return Napi::Boolean::New(env, true);
+}
+#endif  // ROS_VERSION >= 5000
+
 #if ROS_VERSION >= 2505  // ROS2 >= Kilted
 Napi::Value ConfigureActionClientIntrospection(const Napi::CallbackInfo& info) {
   Napi::Env env = info.Env();
@@ -307,7 +368,15 @@ Napi::Object InitActionClientBindings(Napi::Env env, Napi::Object exports) {
 #if ROS_VERSION >= 2505  // ROS2 >= Kilted
   exports.Set("configureActionClientIntrospection",
               Napi::Function::New(env, ConfigureActionClientIntrospection));
-#endif  // ROS_VERSION >= 2505
+#endif                   // ROS_VERSION >= 2505
+#if ROS_VERSION >= 5000  // ROS2 Rolling
+  exports.Set(
+      "actionConfigureFeedbackSubFilterAddGoalId",
+      Napi::Function::New(env, ActionConfigureFeedbackSubFilterAddGoalId));
+  exports.Set(
+      "actionConfigureFeedbackSubFilterRemoveGoalId",
+      Napi::Function::New(env, ActionConfigureFeedbackSubFilterRemoveGoalId));
+#endif  // ROS_VERSION >= 5000
   return exports;
 }