[Kilted] Support introspection for action client/server

Author: minggangw

lib/action/client.js

@@ -19,6 +19,7 @@ const ActionInterfaces = require('./interfaces.js');
 const ActionUuid = require('./uuid.js');
 const ClientGoalHandle = require('./client_goal_handle.js');
 const Deferred = require('./deferred.js');
+const DistroUtils = require('../distro.js');
 const Entity = require('../entity.js');
 const loader = require('../interface_loader.js');
 const QoS = require('../qos.js');
@@ -384,6 +385,32 @@ class ActionClient extends Entity {
   getNumEntities() {
     return rclnodejs.getNumEntities(this.handle);
   }
+
+  /**
+   * Configure introspection.
+   * @param {Clock} clock - Clock to use for service event timestamps
+   * @param {QoS} qos - QoSProfile for the service event publisher
+   * @param {ServiceIntrospectionState} introspectionState - State to set introspection to
+   */
+  configureIntrospection(clock, qos, introspectionState) {
+    if (DistroUtils.getDistroId() <= DistroUtils.getDistroId('jazzy')) {
+      console.warn(
+        'Service introspection is not supported by this versionof ROS 2'
+      );
+      return;
+    }
+
+    let type = this.typeClass.type();
+    rclnodejs.configureActionClientIntrospection(
+      this.handle,
+      this._node.handle,
+      clock.handle,
+      type.interfaceName,
+      type.pkgName,
+      qos,
+      introspectionState
+    );
+  }
 }
 
 module.exports = ActionClient;

lib/action/server.js

@@ -17,6 +17,7 @@
 const rclnodejs = require('bindings')('rclnodejs');
 const ActionInterfaces = require('./interfaces.js');
 const ActionUuid = require('./uuid.js');
+const DistroUtils = require('../distro.js');
 const Entity = require('../entity.js');
 const { CancelResponse, GoalEvent, GoalResponse } = require('./response.js');
 const loader = require('../interface_loader.js');
@@ -454,6 +455,26 @@ class ActionServer extends Entity {
 
     this._node._destroyEntity(this, this._node._actionServers);
   }
+
+  configureIntrospection(clock, qos, introspectionState) {
+    if (DistroUtils.getDistroId() <= DistroUtils.getDistroId('jazzy')) {
+      console.warn(
+        'Service introspection is not supported by this versionof ROS 2'
+      );
+      return;
+    }
+
+    let type = this.typeClass.type();
+    rclnodejs.configureActionServerIntrospection(
+      this.handle,
+      this._node.handle,
+      clock.handle,
+      type.interfaceName,
+      type.pkgName,
+      qos,
+      introspectionState
+    );
+  }
 }
 
 module.exports = ActionServer;

src/rcl_action_client_bindings.cpp

@@ -16,6 +16,7 @@
 
 #include <rcl/error_handling.h>
 #include <rcl/rcl.h>
+#include <rcl_action/action_client.h>
 #include <rcl_action/rcl_action.h>
 
 #include <string>
@@ -241,6 +242,45 @@ Napi::Value ActionSendCancelRequest(const Napi::CallbackInfo& info) {
   return Napi::Number::New(env, static_cast<int32_t>(sequence_number));
 }
 
+#if ROS_VERSION >= 2505  // ROS2 >= Kilted
+Napi::Value ConfigureActionClientIntrospection(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());
+  RclHandle* node_handle = RclHandle::Unwrap(info[1].As<Napi::Object>());
+  rcl_node_t* node = reinterpret_cast<rcl_node_t*>(node_handle->ptr());
+  rcl_clock_t* clock = reinterpret_cast<rcl_clock_t*>(
+      RclHandle::Unwrap(info[2].As<Napi::Object>())->ptr());
+
+  std::string action_name = info[3].As<Napi::String>().Utf8Value();
+  std::string package_name = info[4].As<Napi::String>().Utf8Value();
+  const rosidl_action_type_support_t* ts =
+      GetActionTypeSupport(package_name, action_name);
+  rcl_ret_t ret = RCL_RET_ERROR;
+  if (ts) {
+    rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options();
+    auto qos_profile = GetQoSProfile(info[5]);
+    if (qos_profile) {
+      publisher_ops.qos = *qos_profile;
+    }
+    rcl_service_introspection_state_t state =
+        static_cast<rcl_service_introspection_state_t>(
+            info[6].As<Napi::Number>().Uint32Value());
+    ret = rcl_action_client_configure_action_introspection(
+        action_client, node, clock, ts, publisher_ops, state);
+    if (ret == RCL_RET_OK) {
+      return env.Undefined();
+    }
+  }
+
+  Napi::Error::New(env, "failed to configure action client introspection")
+      .ThrowAsJavaScriptException();
+  return env.Undefined();
+}
+#endif  // ROS_VERSION >= 2505
+
 Napi::Object InitActionClientBindings(Napi::Env env, Napi::Object exports) {
   exports.Set("actionCreateClient",
               Napi::Function::New(env, ActionCreateClient));
@@ -256,6 +296,10 @@ Napi::Object InitActionClientBindings(Napi::Env env, Napi::Object exports) {
   exports.Set("getNumEntities", Napi::Function::New(env, GetNumEntities));
   exports.Set("actionSendCancelRequest",
               Napi::Function::New(env, ActionSendCancelRequest));
+#if ROS_VERSION >= 2505  // ROS2 >= Kilted
+  exports.Set("configureActionClientIntrospection",
+              Napi::Function::New(env, ConfigureActionClientIntrospection));
+#endif  // ROS_VERSION >= 2505
   return exports;
 }
 

src/rcl_action_server_bindings.cpp

@@ -16,6 +16,7 @@
 
 #include <rcl/error_handling.h>
 #include <rcl/rcl.h>
+#include <rcl_action/action_client.h>
 #include <rcl_action/rcl_action.h>
 
 #include <string>
@@ -437,6 +438,46 @@ Napi::Value ActionTakeCancelRequest(const Napi::CallbackInfo& info) {
   return env.Undefined();
 }
 
+#if ROS_VERSION >= 2505  // ROS2 >= Kilted
+Napi::Value ConfigureActionServerIntrospection(const Napi::CallbackInfo& info) {
+  Napi::Env env = info.Env();
+  RclHandle* action_server_handle =
+      RclHandle::Unwrap(info[0].As<Napi::Object>());
+  rcl_action_server_t* action_server =
+      reinterpret_cast<rcl_action_server_t*>(action_server_handle->ptr());
+  RclHandle* node_handle = RclHandle::Unwrap(info[1].As<Napi::Object>());
+  rcl_node_t* node = reinterpret_cast<rcl_node_t*>(node_handle->ptr());
+  rcl_clock_t* clock = reinterpret_cast<rcl_clock_t*>(
+      RclHandle::Unwrap(info[2].As<Napi::Object>())->ptr());
+
+  std::string action_name = info[3].As<Napi::String>().Utf8Value();
+  std::string package_name = info[4].As<Napi::String>().Utf8Value();
+  const rosidl_action_type_support_t* ts =
+      GetActionTypeSupport(package_name, action_name);
+
+  rcl_ret_t ret = RCL_RET_ERROR;
+  if (ts) {
+    rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options();
+    auto qos_profile = GetQoSProfile(info[5]);
+    if (qos_profile) {
+      publisher_ops.qos = *qos_profile;
+    }
+    rcl_service_introspection_state_t state =
+        static_cast<rcl_service_introspection_state_t>(
+            info[6].As<Napi::Number>().Uint32Value());
+    ret = rcl_action_server_configure_action_introspection(
+        action_server, node, clock, ts, publisher_ops, state);
+    if (ret == RCL_RET_OK) {
+      return env.Undefined();
+    }
+  }
+
+  Napi::Error::New(env, "failed to configure action server introspection")
+      .ThrowAsJavaScriptException();
+  return env.Undefined();
+}
+#endif  // ROS_VERSION >= 2505
+
 Napi::Object InitActionServerBindings(Napi::Env env, Napi::Object exports) {
   exports.Set("actionCreateServer",
               Napi::Function::New(env, ActionCreateServer));
@@ -469,6 +510,10 @@ Napi::Object InitActionServerBindings(Napi::Env env, Napi::Object exports) {
               Napi::Function::New(env, ActionServerGoalExists));
   exports.Set("actionTakeCancelRequest",
               Napi::Function::New(env, ActionTakeCancelRequest));
+#if ROS_VERSION >= 2505  // ROS2 >= Kilted
+  exports.Set("configureActionServerIntrospection",
+              Napi::Function::New(env, ConfigureActionServerIntrospection));
+#endif  // ROS_VERSION >= 2505
   return exports;
 }
 

test/test-action-client.js

@@ -19,6 +19,7 @@ const sinon = require('sinon');
 const assertUtils = require('./utils.js');
 const rclnodejs = require('../index.js');
 const { ActionUuid } = require('../index.js');
+const { isActionIntrospectionSupported } = require('./utils.js');
 
 describe('rclnodejs action client', function () {
   let node;
@@ -295,4 +296,18 @@ describe('rclnodejs action client', function () {
 
     client.destroy();
   });
+
+  it('Configure introspection', function () {
+    if (!isActionIntrospectionSupported()) {
+      this.skip();
+    }
+    let client = new rclnodejs.ActionClient(node, fibonacci, 'fibonacci');
+    const ServiceIntrospectionStates = rclnodejs.ServiceIntrospectionStates;
+    const QOS = rclnodejs.QoS.profileSystemDefault;
+    client.configureIntrospection(
+      node.getClock(),
+      QOS,
+      ServiceIntrospectionStates.CONTENTS
+    );
+  });
 });

test/test-action-server.js

@@ -17,6 +17,7 @@
 const assert = require('assert');
 const deepEqual = require('deep-equal');
 const assertUtils = require('./utils.js');
+const { isActionIntrospectionSupported } = require('./utils.js');
 const { randomUUID } = require('crypto');
 const rclnodejs = require('../index.js');
 
@@ -708,4 +709,23 @@ describe('rclnodejs action server', function () {
 
     server.destroy();
   });
+
+  it('Configure introspection', function () {
+    if (!isActionIntrospectionSupported()) {
+      this.skip();
+    }
+    let server = new rclnodejs.ActionServer(
+      node,
+      fibonacci,
+      'fibonacci',
+      () => {}
+    );
+    const ServiceIntrospectionStates = rclnodejs.ServiceIntrospectionStates;
+    const QOS = rclnodejs.QoS.profileSystemDefault;
+    server.configureIntrospection(
+      node.getClock(),
+      QOS,
+      ServiceIntrospectionStates.CONTENTS
+    );
+  });
 });

test/types/index.test-d.ts

@@ -346,6 +346,13 @@ goalHandlePromise.then((goalHandle) => {
   expectType<boolean>(goalHandle.isAborted());
 });
 expectType<object>(actionClient.getNumEntities());
+expectType<void>(
+  actionClient.configureIntrospection(
+    node.getClock(),
+    rclnodejs.Node.getDefaultOptions() as rclnodejs.QoS,
+    rclnodejs.ServiceIntrospectionStates.CONTENTS
+  )
+);
 
 // ---- ActionServer -----
 const actionServer = new rclnodejs.ActionServer(
@@ -383,6 +390,13 @@ function executeCallback(
 
   return new Fibonacci.Result();
 }
+expectType<void>(
+  actionServer.configureIntrospection(
+    node.getClock(),
+    rclnodejs.Node.getDefaultOptions() as rclnodejs.QoS,
+    rclnodejs.ServiceIntrospectionStates.CONTENTS
+  )
+);
 
 // ---- ActionUuid -----
 const actionUuid = new rclnodejs.ActionUuid();

test/utils.js

@@ -15,6 +15,7 @@
 'use strict';
 
 const assert = require('assert');
+const { DistroUtils } = require('../index.js');
 const fs = require('fs');
 const os = require('os');
 const path = require('path');
@@ -86,11 +87,16 @@ function isTypedArray(v) {
   return ArrayBuffer.isView(v) && !(v instanceof DataView);
 }
 
+function isActionIntrospectionSupported() {
+  return DistroUtils.getDistroId() > DistroUtils.getDistroId('jazzy');
+}
+
 module.exports = {
   assertMember: assertMember,
   assertThrowsError: assertThrowsError,
   createDelay: createDelay,
   getAvailablePath: getAvailablePath,
   launchPythonProcess: launchPythonProcess,
   isTypedArray: isTypedArray,
+  isActionIntrospectionSupported: isActionIntrospectionSupported,
 };

types/action_client.d.ts

@@ -179,5 +179,17 @@ declare module 'rclnodejs' {
      * @return - The number of subscriptions, guard_conditions, timers, and clients and services.
      */
     getNumEntities(): object;
+
+    /**
+     * Configure introspection.
+     * @param clock - Clock to use for service event timestamps
+     * @param QoSProfile - QOS profile for the service event publisher
+     * @param introspectionState - The state to set introspection to
+     */
+    configureIntrospection(
+      clock: Clock,
+      serviceEventPubQOS: QoS,
+      introspectionState: ServiceIntrospectionStates
+    ): void;
   }
 }

types/action_server.d.ts

@@ -181,5 +181,17 @@ declare module 'rclnodejs' {
      * Destroy the action server and all goals.
      */
     destroy(): void;
+
+    /**
+     * Configure introspection.
+     * @param clock - Clock to use for service event timestamps
+     * @param QoSProfile - QOS profile for the service event publisher
+     * @param introspectionState - The state to set introspection to
+     */
+    configureIntrospection(
+      clock: Clock,
+      serviceEventPubQOS: QoS,
+      introspectionState: ServiceIntrospectionStates
+    ): void;
   }
 }