diff --git a/lib/subscription.js b/lib/subscription.js index 4060f983..ac776960 100644 --- a/lib/subscription.js +++ b/lib/subscription.js @@ -178,7 +178,7 @@ class Subscription extends Entity { */ setContentFilter(contentFilter) { return contentFilter?.expression - ? rclnodejs.setContentFilter(this.handle, contentFilter) + ? rclnodejs.setContentFilter(this.handle, contentFilter, this._node) : this.clearContentFilter(); } @@ -196,7 +196,7 @@ class Subscription extends Entity { */ clearContentFilter() { return this.hasContentFilter() - ? rclnodejs.clearContentFilter(this.handle) + ? rclnodejs.clearContentFilter(this.handle, this._node) : true; } diff --git a/src/executor.cpp b/src/executor.cpp index 19dd968d..11cac903 100644 --- a/src/executor.cpp +++ b/src/executor.cpp @@ -245,6 +245,17 @@ void Executor::SynchronizeHandles(const Napi::Object& node) { node, IsRunning() ? &interrupt_guard_condition_ : nullptr); } +rcl_ret_t Executor::SetSubscriptionContentFilter( + const rcl_subscription_t* subscription, + const rcl_subscription_content_filter_options_t* options) { + rcl_ret_t result = RCL_RET_ERROR; + handle_manager_->RunWithWaitSetWriteLock( + env_, IsRunning() ? &interrupt_guard_condition_ : nullptr, [&] { + result = rcl_subscription_set_content_filter(subscription, options); + }); + return result; +} + std::string Executor::ReleaseResources() { state_.store(ExecutorState::ReleasingResources); handle_manager_->TakeReadyHandles(); diff --git a/src/executor.h b/src/executor.h index bc0c34ba..b06881d7 100644 --- a/src/executor.h +++ b/src/executor.h @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -50,6 +51,9 @@ class Executor { void Stop(); void SpinOnce(rcl_context_t* context, int32_t time_out); void SynchronizeHandles(const Napi::Object& node); + rcl_ret_t SetSubscriptionContentFilter( + const rcl_subscription_t* subscription, + const rcl_subscription_content_filter_options_t* options); int32_t time_out() { return time_out_; } bool IsStopped() const; diff --git a/src/handle_manager.cpp b/src/handle_manager.cpp index cf3767ec..dfb6cd1c 100644 --- a/src/handle_manager.cpp +++ b/src/handle_manager.cpp @@ -40,21 +40,9 @@ HandleManager::~HandleManager() { uv_sem_destroy(&wait_handle_sem_); } -void HandleManager::SynchronizeHandles( - const Napi::Object& node, - rcl_guard_condition_t* interrupt_guard_condition) { - Napi::HandleScope scope(node.Env()); - Napi::Value timers = node.Get("_timers"); - Napi::Value subscriptions = node.Get("_subscriptions"); - Napi::Value clients = node.Get("_clients"); - Napi::Value services = node.Get("_services"); - Napi::Value guard_conditions = node.Get("_guards"); - Napi::Value action_clients = node.Get("_actionClients"); - Napi::Value action_servers = node.Get("_actionServers"); - Napi::Value events = node.Get("_events"); - - uint32_t sum = 0; - uint32_t sum_was = 0; +void HandleManager::RunWithWaitSetWriteLock( + Napi::Env env, rcl_guard_condition_t* interrupt_guard_condition, + const std::function& operation) { is_synchronizing_.store(true); struct SynchronizationGuard { HandleManager* manager; @@ -69,17 +57,34 @@ void HandleManager::SynchronizeHandles( rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); if (ret != RCL_RET_OK) { std::string error_message = - std::string( - "Failed to interrupt executor for handle synchronization: ") + + std::string("Failed to interrupt executor for a wait set update: ") + rcl_get_error_string().str; rcl_reset_error(); - throw Napi::Error::New(node.Env(), error_message); + throw Napi::Error::New(env, error_message); } } - { - ScopedReadWriteLock scoped_lock(&sync_handles_rwlock_, - ScopedReadWriteLock::LockType::kWrite); + ScopedReadWriteLock scoped_lock(&sync_handles_rwlock_, + ScopedReadWriteLock::LockType::kWrite); + operation(); +} + +void HandleManager::SynchronizeHandles( + const Napi::Object& node, + rcl_guard_condition_t* interrupt_guard_condition) { + Napi::HandleScope scope(node.Env()); + Napi::Value timers = node.Get("_timers"); + Napi::Value subscriptions = node.Get("_subscriptions"); + Napi::Value clients = node.Get("_clients"); + Napi::Value services = node.Get("_services"); + Napi::Value guard_conditions = node.Get("_guards"); + Napi::Value action_clients = node.Get("_actionClients"); + Napi::Value action_servers = node.Get("_actionServers"); + Napi::Value events = node.Get("_events"); + + uint32_t sum = 0; + uint32_t sum_was = 0; + RunWithWaitSetWriteLock(node.Env(), interrupt_guard_condition, [&] { ClearHandles(); sum += SynchronizeHandlesByType(timers.As(), &timers_); sum += SynchronizeHandlesByType(subscriptions.As(), @@ -95,7 +100,7 @@ void HandleManager::SynchronizeHandles( sum += SynchronizeHandlesByType(events.As(), &events_); PruneRemovedReadyHandles(sum); sum_was = sum_.exchange(sum); - } + }); // Wakeup the backgroud thread if the sum was zero, but now it is greater than // zero. diff --git a/src/handle_manager.h b/src/handle_manager.h index 87e5212d..9771da94 100644 --- a/src/handle_manager.h +++ b/src/handle_manager.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -59,6 +60,9 @@ class HandleManager { void SynchronizeHandles( const Napi::Object& node, rcl_guard_condition_t* interrupt_guard_condition = nullptr); + void RunWithWaitSetWriteLock(Napi::Env env, + rcl_guard_condition_t* interrupt_guard_condition, + const std::function& operation); void WaitForSynchronizing(); // Waits the handles to be attached from the background thread. diff --git a/src/rcl_subscription_bindings.cpp b/src/rcl_subscription_bindings.cpp index 7b6d645e..f2e4b46b 100644 --- a/src/rcl_subscription_bindings.cpp +++ b/src/rcl_subscription_bindings.cpp @@ -22,12 +22,14 @@ #include #include #include +#include #include #include "macros.h" #include "rcl_handle.h" #include "rcl_utilities.h" +#include "shadow_node.h" namespace rclnodejs { @@ -310,8 +312,40 @@ Napi::Value HasContentFilter(const Napi::CallbackInfo& info) { return Napi::Boolean::New(env, is_valid); } +namespace { + +Napi::Value ApplyContentFilter( + Napi::Env env, ShadowNode* node, rcl_subscription_t* subscription, + rcl_subscription_content_filter_options_t* options) { + auto cleanup = rcpputils::make_scope_exit([&] { + if (rcl_subscription_content_filter_options_fini(subscription, options) != + RCL_RET_OK) { + rcl_reset_error(); + } + }); + + if (node->executor()->SetSubscriptionContentFilter(subscription, options) != + RCL_RET_OK) { + std::string error_message = rcl_get_error_string().str; + rcl_reset_error(); + throw Napi::Error::New(env, error_message); + } + + cleanup.cancel(); + if (rcl_subscription_content_filter_options_fini(subscription, options) != + RCL_RET_OK) { + std::string error_message = rcl_get_error_string().str; + rcl_reset_error(); + throw Napi::Error::New(env, error_message); + } + return Napi::Boolean::New(env, true); +} + +} // namespace + Napi::Value SetContentFilter(const Napi::CallbackInfo& info) { Napi::Env env = info.Env(); + auto* node = ShadowNode::Unwrap(info[2].As()); RclHandle* subscription_handle = RclHandle::Unwrap(info[0].As()); @@ -323,79 +357,39 @@ Napi::Value SetContentFilter(const Napi::CallbackInfo& info) { std::string expression = contentFilter.Get("expression").As().Utf8Value(); - // parameters property (string[]) is optional - int argc = 0; - char** argv = nullptr; - + std::vector parameters; if (contentFilter.Has("parameters")) { - Napi::Array jsArgv = contentFilter.Get("parameters").As(); - argc = jsArgv.Length(); - if (argc > 0) { - argv = reinterpret_cast(malloc(argc * sizeof(char*))); - for (int i = 0; i < argc; i++) { - std::string arg = jsArgv.Get(i).As().Utf8Value(); - int len = arg.length() + 1; - argv[i] = reinterpret_cast(malloc(len * sizeof(char))); - snprintf(argv[i], len, "%s", arg.c_str()); - } + auto js_parameters = contentFilter.Get("parameters").As(); + parameters.reserve(js_parameters.Length()); + for (uint32_t index = 0; index < js_parameters.Length(); ++index) { + parameters.push_back( + js_parameters.Get(index).As().Utf8Value()); } } + std::vector argv; + argv.reserve(parameters.size()); + for (const auto& parameter : parameters) { + argv.push_back(parameter.c_str()); + } - // create ctf options rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); rcl_ret_t ret = rcl_subscription_content_filter_options_set( - subscription, expression.c_str(), argc, (const char**)argv, &options); + subscription, expression.c_str(), argv.size(), argv.data(), &options); if (ret != RCL_RET_OK) { - if (argc) { - for (int i = 0; i < argc; i++) { - free(argv[i]); - } - free(argv); - } std::string error_string = rcl_get_error_string().str; rcl_reset_error(); - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); + throw Napi::Error::New(env, error_string); } - ret = rcl_subscription_set_content_filter(subscription, &options); - - if (argc) { - for (int i = 0; i < argc; i++) { - free(argv[i]); - } - free(argv); - } - - std::string error_string = ""; - if (ret != RCL_RET_OK) { - error_string = rcl_get_error_string().str; - rcl_reset_error(); - } - - rcl_ret_t fini_ret = - rcl_subscription_content_filter_options_fini(subscription, &options); - - if (ret != RCL_RET_OK) { - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); - } - - if (fini_ret != RCL_RET_OK) { - error_string = rcl_get_error_string().str; - rcl_reset_error(); - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); - } - - return Napi::Boolean::New(env, true); + return ApplyContentFilter(env, node, subscription, &options); } Napi::Value ClearContentFilter(const Napi::CallbackInfo& info) { Napi::Env env = info.Env(); + auto* node = ShadowNode::Unwrap(info[1].As()); RclHandle* subscription_handle = RclHandle::Unwrap(info[0].As()); @@ -412,29 +406,10 @@ Napi::Value ClearContentFilter(const Napi::CallbackInfo& info) { if (ret != RCL_RET_OK) { std::string error_string = rcl_get_error_string().str; rcl_reset_error(); - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); - } - - ret = rcl_subscription_set_content_filter(subscription, &options); - rcl_ret_t fini_ret = - rcl_subscription_content_filter_options_fini(subscription, &options); - - if (ret != RCL_RET_OK) { - std::string error_string = rcl_get_error_string().str; - rcl_reset_error(); - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); + throw Napi::Error::New(env, error_string); } - if (fini_ret != RCL_RET_OK) { - std::string error_string = rcl_get_error_string().str; - rcl_reset_error(); - Napi::Error::New(env, error_string).ThrowAsJavaScriptException(); - return env.Undefined(); - } - - return Napi::Boolean::New(env, true); + return ApplyContentFilter(env, node, subscription, &options); } Napi::Value GetContentFilter(const Napi::CallbackInfo& info) { diff --git a/src/shadow_node.h b/src/shadow_node.h index 979a5653..d92d5b54 100644 --- a/src/shadow_node.h +++ b/src/shadow_node.h @@ -37,6 +37,7 @@ class ShadowNode : public Napi::ObjectWrap, void RunOnce(rcl_context_t* context, int32_t timeout); HandleManager* handle_manager() { return handle_manager_.get(); } + Executor* executor() { return executor_.get(); } // Executor::Delegate overrides: void Execute(const std::vector& handles) override; diff --git a/test/test-subscription-content-filter.js b/test/test-subscription-content-filter.js index dc63fd48..1d098efc 100644 --- a/test/test-subscription-content-filter.js +++ b/test/test-subscription-content-filter.js @@ -1,5 +1,6 @@ import childProcess from 'child_process'; import assert from 'assert'; +import { setTimeout as delay } from 'node:timers/promises'; import rclnodejs from '../index.js'; const DistroUtils = rclnodejs.DistroUtils; @@ -288,29 +289,14 @@ describe('subscription content-filtering', function () { this.subscriberNode.spin(); - const p1 = new Promise((resolve) => - setTimeout(() => { - let msgCnt = msgCnt0 + msgCnt5; - const contentFilter5 = { - expression: 'data = 5', - }; - // Stop spinning before changing content filter to avoid race condition - this.subscriberNode.stop(); - subscription.setContentFilter(contentFilter5); - this.subscriberNode.spin(); - resolve(msgCnt); - }, SUBSCRIBER_WAIT_TIME) - ); - let result = await p1; - assert.strictEqual(result, 0); + await delay(SUBSCRIBER_WAIT_TIME); + assert.strictEqual(msgCnt0 + msgCnt5, 0); + const contentFilter5 = { expression: 'data = 5' }; + assert.strictEqual(subscription.setContentFilter(contentFilter5), true); + assert.strictEqual(this.subscriberNode.spinning, true); - const p2 = new Promise((resolve) => - setTimeout(() => { - resolve(!fail && msgCnt5 && !msgCnt0); - }, SUBSCRIBER_WAIT_TIME) - ); - result = await p2; - assert.ok(result); + await delay(SUBSCRIBER_WAIT_TIME); + assert.ok(!fail && msgCnt5 && !msgCnt0); }); it('set undefined content filter', async function () { @@ -363,27 +349,79 @@ describe('subscription content-filtering', function () { this.subscriberNode.spin(); - const p1 = new Promise((resolve) => - setTimeout(() => { - const result = !msgCnt0 && msgCnt5 && !fail; - // Stop spinning before changing content filter to avoid race condition - this.subscriberNode.stop(); - subscription.setContentFilter(); - this.subscriberNode.spin(); - resolve(result); - }, SUBSCRIBER_WAIT_TIME) - ); - let result = await p1; - assert.ok(result); + await delay(SUBSCRIBER_WAIT_TIME); + assert.ok(!msgCnt0 && msgCnt5 && !fail); + assert.strictEqual(subscription.setContentFilter(), true); assert.ok(!subscription.hasContentFilter()); + assert.strictEqual(this.subscriberNode.spinning, true); - const p2 = new Promise((resolve) => - setTimeout(() => { - resolve(msgCnt0 && msgCnt5 && !fail); - }, SUBSCRIBER_WAIT_TIME) + await delay(SUBSCRIBER_WAIT_TIME); + assert.ok(msgCnt0 && msgCnt5 && !fail); + }); + + it('updates and clears a filter during an infinite spin', function (done) { + this.timeout(15000); + const moduleUrl = new URL('../index.js', import.meta.url).href; + const script = ` + import assert from 'node:assert/strict'; + import { setTimeout as delay } from 'node:timers/promises'; + import rclnodejs from ${JSON.stringify(moduleUrl)}; + + await rclnodejs.init(); + try { + const receiver = rclnodejs.createNode('filter_update_receiver'); + const sender = rclnodejs.createNode('filter_update_sender'); + const type = 'std_msgs/msg/Int32'; + const topic = 'filter_update_while_spinning'; + const filter = { expression: 'data = %0', parameters: ['30'] }; + const publisher = sender.createPublisher(type, topic); + let complete; + const delivered = new Promise((resolve) => { complete = resolve; }); + const subscription = receiver.createSubscription( + type, topic, { contentFilter: filter }, (message) => { + if (message.data === 42) complete(); + } + ); + + receiver.spin(-1); + await delay(100); + assert.equal(subscription.clearContentFilter(), true); + assert.equal(subscription.hasContentFilter(), false); + + for (let round = 0; round < 64; round++) { + await delay(2); + assert.equal(subscription.setContentFilter(filter), true); + assert.equal(subscription.hasContentFilter(), true); + publisher.publish({ data: 30 }); + await delay(2); + assert.equal(subscription.clearContentFilter(), true); + assert.equal(subscription.hasContentFilter(), false); + publisher.publish({ data: 5 }); + } + + assert.equal(receiver.spinning, true); + const deadline = Date.now() + 5000; + while ( + (!subscription.publisherCount || !publisher.subscriptionCount) && + Date.now() < deadline + ) { + await delay(20); + } + assert.ok(subscription.publisherCount > 0); + assert.ok(publisher.subscriptionCount > 0); + publisher.publish({ data: 42 }); + await delivered; + } finally { + rclnodejs.shutdown(); + } + `; + + childProcess.execFile( + process.execPath, + ['--input-type=module', '--eval', script], + { timeout: 10000, killSignal: 'SIGKILL' }, + done ); - result = await p2; - assert.ok(result); }); it('clearContentFilter', function (done) {