Support feedback subscription content filter for action client

Author: minggangw

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,14 @@ 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.
+    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 +140,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 +220,9 @@ class ActionClient extends Entity {
             status === ActionInterfaces.GoalStatus.STATUS_ABORTED
           ) {
             this._goalHandles.delete(uuid);
+            this._feedbackSubFilterRemoveGoalId(
+              statusMessage.goal_info.goal_id
+            );
           }
         }
       } else {
@@ -393,6 +411,8 @@ class ActionClient extends Entity {
       this._removePendingCancelRequest(sequenceNumber)
     );
 
+    this._feedbackSubFilterRemoveGoalId(goalHandle.goalId);
+
     return deferred.promise;
   }
 
@@ -442,9 +462,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 +485,50 @@ 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}\nFeedback message optimization is automatically disabled.`
+        );
+    }
+  }
+
+  /**
+   * 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}\nFeedback message optimization is automatically disabled.`
+        );
+    }
+  }
+
   /**
    * Destroy the underlying action client handle.
    * @return {undefined}

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;
 }
 

test/test-action-client.js

@@ -310,4 +310,111 @@ describe('rclnodejs action client', function () {
       ServiceIntrospectionStates.CONTENTS
     );
   });
+
+  describe('enableFeedbackMsgOptimization', function () {
+    const nativeLoader = require('../lib/native_loader.js');
+    const isFeedbackFilterSupported = () =>
+      typeof nativeLoader.actionConfigureFeedbackSubFilterAddGoalId ===
+      'function';
+
+    it('Test option defaults to false', function () {
+      let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci');
+      assert.strictEqual(client._enableFeedbackMsgOptimization, false);
+      client.destroy();
+    });
+
+    it('Test option can be set to true', function () {
+      let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci', {
+        enableFeedbackMsgOptimization: true,
+      });
+      // If native API is available, it should be enabled; otherwise disabled
+      if (isFeedbackFilterSupported()) {
+        assert.strictEqual(client._enableFeedbackMsgOptimization, true);
+      } else {
+        assert.strictEqual(client._enableFeedbackMsgOptimization, false);
+      }
+      client.destroy();
+    });
+
+    it('Test send goal with optimization enabled', async function () {
+      let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci', {
+        enableFeedbackMsgOptimization: true,
+      });
+
+      let feedbackCallback = sinon.spy(function (feedback) {
+        assert.ok(feedback);
+      });
+
+      let goalUuid = ActionUuid.randomMessage();
+      publishFeedback = goalUuid;
+
+      let result = await client.waitForServer(2000);
+      assert.ok(result);
+
+      let goalHandle = await client.sendGoal(
+        new Fibonacci.Goal(),
+        feedbackCallback,
+        goalUuid
+      );
+      assert.ok(goalHandle.isAccepted());
+
+      await goalHandle.getResult();
+      assert.ok(goalHandle.isSucceeded());
+
+      client.destroy();
+    });
+
+    it('Test send multiple goals with optimization enabled', async function () {
+      let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci', {
+        enableFeedbackMsgOptimization: true,
+      });
+
+      let result = await client.waitForServer(2000);
+      assert.ok(result);
+
+      const [goal1Handle, goal2Handle, goal3Handle] = await Promise.all([
+        client.sendGoal(new Fibonacci.Goal()),
+        client.sendGoal(new Fibonacci.Goal()),
+        client.sendGoal(new Fibonacci.Goal()),
+      ]);
+
+      assert.ok(goal1Handle.accepted);
+      assert.ok(goal2Handle.accepted);
+      assert.ok(goal3Handle.accepted);
+
+      const [result1, result2, result3] = await Promise.all([
+        goal1Handle.getResult(),
+        goal2Handle.getResult(),
+        goal3Handle.getResult(),
+      ]);
+
+      assert.ok(result1);
+      assert.ok(result2);
+      assert.ok(result3);
+
+      client.destroy();
+    });
+
+    it('Test cancel goal with optimization enabled', async function () {
+      let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci', {
+        enableFeedbackMsgOptimization: true,
+      });
+
+      let result = await client.waitForServer(2000);
+      assert.ok(result);
+
+      let goalHandle = await client.sendGoal(new Fibonacci.Goal());
+      assert.ok(goalHandle.isAccepted());
+
+      result = await goalHandle.cancelGoal();
+      assert.ok(result);
+
+      assert.strictEqual(
+        ActionUuid.fromMessage(result.goals_canceling[0].goal_id).toString(),
+        ActionUuid.fromMessage(goalHandle.goalId).toString()
+      );
+
+      client.destroy();
+    });
+  });
 });

test/test-subscription-content-filter.js

@@ -480,3 +480,36 @@ describe('subscription content-filtering', function () {
     done();
   });
 });
+
+describe('subscription isContentFilterSupported', function () {
+  this.timeout(30 * 1000);
+
+  beforeEach(async function () {
+    await rclnodejs.init();
+    this.node = new Node('cft_support_test_node');
+  });
+
+  afterEach(function () {
+    this.node.destroy();
+    rclnodejs.shutdown();
+  });
+
+  it('isContentFilterSupported returns boolean matching RMW capability', function (done) {
+    const typeclass = 'std_msgs/msg/Int16';
+    const subscription = this.node.createSubscription(
+      typeclass,
+      TOPIC,
+      (msg) => {}
+    );
+
+    const supported = subscription.isContentFilterSupported();
+    assert.strictEqual(typeof supported, 'boolean');
+
+    // isContentFilterSupported requires rolling; on older distros it returns false
+    const isRolling = DistroUtils.getDistroId() >= DistroUtils.DistroId.ROLLING;
+    const expectedSupported = isRolling && isContentFilteringSupported();
+    assert.strictEqual(supported, expectedSupported);
+
+    done();
+  });
+});

types/action_client.d.ts

@@ -142,6 +142,14 @@ declare module 'rclnodejs' {
       options?: Options<ActionQoS> & {
         validateGoals?: boolean;
         validationOptions?: MessageValidationOptions;
+        /**
+         * 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, avoiding reception of
+         * irrelevant feedback messages. An action client can handle up to 6
+         * goals simultaneously with this optimization. Default: false.
+         */
+        enableFeedbackMsgOptimization?: boolean;
       }
     );