From 25cbf1c879e83b465441cf87a86ea5c423f90a82 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 19 Jan 2022 17:31:10 -0300 Subject: [PATCH 1/6] Improve executor performance Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/executors/BaseExecutor.java | 98 ++++++------------- 1 file changed, 31 insertions(+), 67 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index e500f9ac..02320c43 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -283,91 +283,55 @@ protected void waitForWork(long timeout) { nativeWait(waitSetHandle, timeout); - for (int i = 0; i < this.subscriptionHandles.size(); ++i) { + int i = 0; + while (i < this.subscriptionHandles.size()) { if (!nativeWaitSetSubscriptionIsReady(waitSetHandle, i)) { - this.subscriptionHandles.get(i).setValue(null); + this.subscriptionHandles.remove(i); + } else { + ++i; } } - - for (int i = 0; i < this.timerHandles.size(); ++i) { + i = 0; + while (i < this.timerHandles.size()) { if (!nativeWaitSetTimerIsReady(waitSetHandle, i)) { - this.timerHandles.get(i).setValue(null); + this.timerHandles.remove(i); + } else { + ++i; } } - - for (int i = 0; i < this.serviceHandles.size(); ++i) { + i = 0; + while (i < this.serviceHandles.size()) { if (!nativeWaitSetServiceIsReady(waitSetHandle, i)) { - this.serviceHandles.get(i).setValue(null); + this.serviceHandles.remove(i); + } else { + ++i; } } - - for (int i = 0; i < this.clientHandles.size(); ++i) { + i = 0; + while (i < this.clientHandles.size()) { if (!nativeWaitSetClientIsReady(waitSetHandle, i)) { - this.clientHandles.get(i).setValue(null); + this.clientHandles.remove(i); + } else { + ++i; } } - - for (int i = 0; i < this.eventHandles.size(); ++i) { + i = 0; + while (i < this.eventHandles.size()) { if (!nativeWaitSetEventIsReady(waitSetHandle, i)) { - this.eventHandles.get(i).setValue(null); + this.eventHandles.remove(i); + } else { + ++i; } } - - for (Map.Entry entry : this.actionServerHandles) { + i = 0; + while (i < this.actionServerHandles.size()) { + Map.Entry entry = this.actionServerHandles.get(i); if (!entry.getValue().isReady(waitSetHandle)) { - entry.setValue(null); - } - } - - Iterator> subscriptionIterator = - this.subscriptionHandles.iterator(); - while (subscriptionIterator.hasNext()) { - Map.Entry entry = subscriptionIterator.next(); - if (entry.getValue() == null) { - subscriptionIterator.remove(); - } - } - - Iterator> timerIterator = this.timerHandles.iterator(); - while (timerIterator.hasNext()) { - Map.Entry entry = timerIterator.next(); - if (entry.getValue() == null) { - timerIterator.remove(); - } - } - - Iterator> serviceIterator = this.serviceHandles.iterator(); - while (serviceIterator.hasNext()) { - Map.Entry entry = serviceIterator.next(); - if (entry.getValue() == null) { - serviceIterator.remove(); - } - } - - Iterator> clientIterator = this.clientHandles.iterator(); - while (clientIterator.hasNext()) { - Map.Entry entry = clientIterator.next(); - if (entry.getValue() == null) { - clientIterator.remove(); - } - } - - Iterator> eventIterator = this.eventHandles.iterator(); - while (eventIterator.hasNext()) { - Map.Entry entry = eventIterator.next(); - if (entry.getValue() == null) { - eventIterator.remove(); - } - } - - Iterator> actionServerIterator = this.actionServerHandles.iterator(); - while (actionServerIterator.hasNext()) { - Map.Entry entry = actionServerIterator.next(); - if (entry.getValue() == null) { - actionServerIterator.remove(); + this.actionServerHandles.remove(i); + } else { + ++i; } } - nativeDisposeWaitSet(waitSetHandle); } From 0610b4434f4fe9b727aa3960de2a8c716b6aebb3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 20 Jan 2022 12:10:01 -0300 Subject: [PATCH 2/6] Another minor improvement Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/executors/BaseExecutor.java | 27 ++++++++----------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 02320c43..8c5a2c98 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -28,6 +28,7 @@ import java.util.concurrent.Future; import java.util.concurrent.LinkedBlockingQueue; +import javax.swing.Action; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -225,24 +226,18 @@ protected void waitForWork(long timeout) { } } - int subscriptionsSize = 0; - int timersSize = 0; - int clientsSize = 0; - int servicesSize = 0; + int subscriptionsSize = this.subscriptionHandles.size(); + int timersSize = this.timerHandles.size(); + int clientsSize = this.clientHandles.size(); + int servicesSize = this.serviceHandles.size(); int eventsSize = this.eventHandles.size(); - for (ComposableNode node : this.nodes) { - subscriptionsSize += node.getNode().getSubscriptions().size(); - timersSize += node.getNode().getTimers().size(); - clientsSize += node.getNode().getClients().size(); - servicesSize += node.getNode().getServices().size(); - - for (ActionServer actionServer : node.getNode().getActionServers()) { - subscriptionsSize += actionServer.getNumberOfSubscriptions(); - timersSize += actionServer.getNumberOfTimers(); - clientsSize += actionServer.getNumberOfClients(); - servicesSize += actionServer.getNumberOfServices(); - } + for (Map.Entry entry : this.actionServerHandles) { + ActionServer actionServer = entry.getValue(); + subscriptionsSize += actionServer.getNumberOfSubscriptions(); + timersSize += actionServer.getNumberOfTimers(); + clientsSize += actionServer.getNumberOfClients(); + servicesSize += actionServer.getNumberOfServices(); } if (subscriptionsSize == 0 && timersSize == 0 && clientsSize == 0 && servicesSize == 0) { From 68af1aa4a82e9a49a07370ed0e9d6629440c68b9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 20 Jan 2022 15:40:35 -0300 Subject: [PATCH 3/6] Add native method to resize waitset Signed-off-by: Ivan Santiago Paunovic --- .../org_ros2_rcljava_executors_BaseExecutor.h | 9 +++++++++ ...org_ros2_rcljava_executors_BaseExecutor.cpp | 18 ++++++++++++++++++ .../ros2/rcljava/executors/BaseExecutor.java | 5 +++++ 3 files changed, 32 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index 08b95c08..e5f1d81c 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -47,6 +47,15 @@ JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetInit( JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeDisposeWaitSet(JNIEnv *, jclass, jlong); +/* + * Class: org_ros2_rcljava_executors_BaseExecutor + * Method: nativeWaitSetResize + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetResize( + JNIEnv *, jclass, jlong, jint, jint, jint, jint, jint, jint); + /* * Class: org_ros2_rcljava_executors_BaseExecutor * Method: nativeWaitSetClear diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 7cbdb6cc..ee6c1ef0 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -80,6 +80,24 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeDisposeWaitSet( } } +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetResize( + JNIEnv * env, jclass, jlong wait_set_handle, jint number_of_subscriptions, + jint number_of_guard_conditions, jint number_of_timers, jint number_of_clients, + jint number_of_services, jint number_of_events) +{ + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + + rcl_ret_t ret = rcl_wait_set_resize( + wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, + number_of_clients, number_of_services, number_of_events); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to resize wait set: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetClear( JNIEnv * env, jclass, jlong wait_set_handle) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 8c5a2c98..1fd235b3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -472,6 +472,11 @@ private static native void nativeWaitSetInit( long waitSetHandle, long contextHandle, int numberOfSubscriptions, int numberOfGuardConditions, int numberOfTimers, int numberOfClients, int numberOfServices, int numberOfEvents); + + private static native void nativeWaitSetResize( + long waitSetHandle, int numberOfSubscriptions, + int numberOfGuardConditions, int numberOfTimers, int numberOfClients, + int numberOfServices, int numberOfEvents); private static native void nativeWaitSetClear(long waitSetHandle); From 3de3325080108f72a0e031ec9f4cd3fade3e3cff Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 20 Jan 2022 16:13:20 -0300 Subject: [PATCH 4/6] Cache waitset through spin calls, to avoid allocations/deallocations Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/executors/BaseExecutor.java | 31 ++++++++++++++----- .../executors/MultiThreadedExecutor.java | 3 ++ .../executors/SingleThreadedExecutor.java | 3 ++ 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 1fd235b3..f5d2f90b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -77,6 +77,20 @@ public class BaseExecutor { private List> actionServerHandles = new ArrayList>(); + private long waitSetHandle = 0; + + public BaseExecutor() { + this.waitSetHandle = nativeGetZeroInitializedWaitSet(); + long contextHandle = RCLJava.getDefaultContext().getHandle(); + nativeWaitSetInit( + this.waitSetHandle, contextHandle, 0, 0, + 0, 0, 0, 0); + } + + public void dispose() { + nativeDisposeWaitSet(this.waitSetHandle); + } + protected void addNode(ComposableNode node) { this.nodes.add(node); } @@ -243,15 +257,11 @@ protected void waitForWork(long timeout) { if (subscriptionsSize == 0 && timersSize == 0 && clientsSize == 0 && servicesSize == 0) { return; } - - long waitSetHandle = nativeGetZeroInitializedWaitSet(); - long contextHandle = RCLJava.getDefaultContext().getHandle(); - nativeWaitSetInit( - waitSetHandle, contextHandle, subscriptionsSize, 0, + long waitSetHandle = this.waitSetHandle; + nativeWaitSetResize( + waitSetHandle, subscriptionsSize, 0, timersSize, clientsSize, servicesSize, eventsSize); - nativeWaitSetClear(waitSetHandle); - for (Map.Entry entry : this.subscriptionHandles) { nativeWaitSetAddSubscription(waitSetHandle, entry.getKey()); } @@ -327,7 +337,6 @@ protected void waitForWork(long timeout) { ++i; } } - nativeDisposeWaitSet(waitSetHandle); } protected AnyExecutable getNextExecutable() { @@ -422,6 +431,9 @@ public void spinUntilComplete(Future future, long maxDurationNs) { anyExecutable = getNextExecutable(); } } + if (!RCLJava.ok()) { + this.dispose(); + } } private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { @@ -442,6 +454,9 @@ private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { workAvailable = false; } } + if (!RCLJava.ok()) { + this.dispose(); + } } protected void spinSome(long maxDurationNs) { diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java index 5e813701..56c8ad92 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java @@ -95,5 +95,8 @@ private void run() { this.spinOnce(); } } + if (!RCLJava.ok()) { + this.baseExecutor.dispose(); + } } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java index a55259d4..f0251f9a 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java @@ -64,5 +64,8 @@ public void spin() { while (RCLJava.ok()) { this.spinOnce(); } + if (!RCLJava.ok()) { + this.baseExecutor.dispose(); + } } } From 80ff53dba799f239fef0a5f546fb0b5f8a24656a Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 21 Jan 2022 15:15:08 -0300 Subject: [PATCH 5/6] Fix indexing error Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/executors/BaseExecutor.java | 90 +++++++++++-------- 1 file changed, 52 insertions(+), 38 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index f5d2f90b..f1552b2f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -287,54 +287,68 @@ protected void waitForWork(long timeout) { } nativeWait(waitSetHandle, timeout); - - int i = 0; - while (i < this.subscriptionHandles.size()) { - if (!nativeWaitSetSubscriptionIsReady(waitSetHandle, i)) { - this.subscriptionHandles.remove(i); - } else { - ++i; + { + int waitSetIndex = 0; + Iterator> it = this.subscriptionHandles.iterator(); + while (it.hasNext()) { + it.next(); + if (!nativeWaitSetSubscriptionIsReady(waitSetHandle, waitSetIndex)) { + it.remove(); + } + ++waitSetIndex; } } - i = 0; - while (i < this.timerHandles.size()) { - if (!nativeWaitSetTimerIsReady(waitSetHandle, i)) { - this.timerHandles.remove(i); - } else { - ++i; + { + int waitSetIndex = 0; + Iterator> it = this.timerHandles.iterator(); + while (it.hasNext()) { + it.next(); + if (!nativeWaitSetTimerIsReady(waitSetHandle, waitSetIndex)) { + it.remove(); + } + ++waitSetIndex; } } - i = 0; - while (i < this.serviceHandles.size()) { - if (!nativeWaitSetServiceIsReady(waitSetHandle, i)) { - this.serviceHandles.remove(i); - } else { - ++i; + { + int waitSetIndex = 0; + Iterator> it = this.serviceHandles.iterator(); + while (it.hasNext()) { + it.next(); + if (!nativeWaitSetServiceIsReady(waitSetHandle, waitSetIndex)) { + it.remove(); + } + ++waitSetIndex; } } - i = 0; - while (i < this.clientHandles.size()) { - if (!nativeWaitSetClientIsReady(waitSetHandle, i)) { - this.clientHandles.remove(i); - } else { - ++i; + { + int waitSetIndex = 0; + Iterator> it = this.clientHandles.iterator(); + while (it.hasNext()) { + it.next(); + if (!nativeWaitSetClientIsReady(waitSetHandle, waitSetIndex)) { + it.remove(); + } + ++waitSetIndex; } } - i = 0; - while (i < this.eventHandles.size()) { - if (!nativeWaitSetEventIsReady(waitSetHandle, i)) { - this.eventHandles.remove(i); - } else { - ++i; + { + int waitSetIndex = 0; + Iterator> it = this.eventHandles.iterator(); + while (it.hasNext()) { + it.next(); + if (!nativeWaitSetEventIsReady(waitSetHandle, waitSetIndex)) { + it.remove(); + } + ++waitSetIndex; } } - i = 0; - while (i < this.actionServerHandles.size()) { - Map.Entry entry = this.actionServerHandles.get(i); - if (!entry.getValue().isReady(waitSetHandle)) { - this.actionServerHandles.remove(i); - } else { - ++i; + { + Iterator> it = this.actionServerHandles.iterator(); + while (it.hasNext()) { + Map.Entry entry = it.next(); + if (!entry.getValue().isReady(waitSetHandle)) { + it.remove(); + } } } } From 3dcc11cc9a6ce714bef66632d8bcb214dd1248e7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 26 Jan 2022 16:30:18 -0300 Subject: [PATCH 6/6] Update native method signature in docs Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index e5f1d81c..33b7ed67 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -50,7 +50,7 @@ JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeDisposeWaitSet(JNIEnv /* * Class: org_ros2_rcljava_executors_BaseExecutor * Method: nativeWaitSetResize - * Signature: (JJ)V + * Signature: (JIIIIII)V */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetResize(