1515
1616package org .ros2 .rcljava .executors ;
1717
18+ import java .lang .SuppressWarnings ;
1819import java .util .AbstractMap ;
1920import java .util .ArrayList ;
2021import java .util .Iterator ;
2122import java .util .List ;
2223import java .util .Map ;
23-
2424import java .util .Collection ;
2525import java .util .concurrent .BlockingQueue ;
2626import java .util .concurrent .ConcurrentHashMap ;
2727import java .util .concurrent .LinkedBlockingQueue ;
2828
29+
2930import org .slf4j .Logger ;
3031import org .slf4j .LoggerFactory ;
3132
@@ -81,6 +82,23 @@ protected void removeNode(ComposableNode node) {
8182 this .nodes .remove (node );
8283 }
8384
85+ @ SuppressWarnings ("unchecked" )
86+ protected static void executeSubscriptionCallbackUnchecked (
87+ Subscription subscription ,
88+ MessageDefinition message )
89+ {
90+ subscription .executeCallback (message );
91+ }
92+
93+ @ SuppressWarnings ("unchecked" )
94+ protected static void clientHandleResponseUnchecked (
95+ Client client ,
96+ RMWRequestId rmwRequestId ,
97+ MessageDefinition response )
98+ {
99+ client .handleResponse (rmwRequestId , response );
100+ }
101+
84102 protected void executeAnyExecutable (AnyExecutable anyExecutable ) {
85103 if (anyExecutable .timer != null ) {
86104 anyExecutable .timer .callTimer ();
@@ -92,25 +110,25 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) {
92110 MessageDefinition message = nativeTake (
93111 anyExecutable .subscription .getHandle (), anyExecutable .subscription .getMessageType ());
94112 if (message != null ) {
95- anyExecutable .subscription .executeCallback (message );
113+ // Safety: nativeTake() will return the correct type here.
114+ // We can't do much better here, as subscriptions are type erased.
115+ executeSubscriptionCallbackUnchecked (anyExecutable .subscription , message );
96116 }
97117 subscriptionHandles .remove (anyExecutable .subscription .getHandle ());
98118 }
99119
100120 if (anyExecutable .service != null ) {
101- Class <MessageDefinition > requestType = anyExecutable .service .getRequestType ();
102- Class <MessageDefinition > responseType = anyExecutable .service .getResponseType ();
121+ Class <? extends MessageDefinition > requestType = anyExecutable .service .getRequestType ();
122+ Class <? extends MessageDefinition > responseType = anyExecutable .service .getResponseType ();
103123
104124 MessageDefinition requestMessage = null ;
105125 MessageDefinition responseMessage = null ;
106126
107127 try {
108- requestMessage = requestType .newInstance ();
109- responseMessage = responseType .newInstance ();
110- } catch (InstantiationException ie ) {
111- ie .printStackTrace ();
112- } catch (IllegalAccessException iae ) {
113- iae .printStackTrace ();
128+ requestMessage = requestType .getDeclaredConstructor ().newInstance ();
129+ responseMessage = responseType .getDeclaredConstructor ().newInstance ();
130+ } catch (ReflectiveOperationException e ) {
131+ throw new IllegalStateException ("Failed to instantiate service request/response" );
114132 }
115133
116134 if (requestMessage != null && responseMessage != null ) {
@@ -134,24 +152,18 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) {
134152 }
135153
136154 if (anyExecutable .client != null ) {
137- Class <MessageDefinition > requestType = anyExecutable .client .getRequestType ();
138- Class <MessageDefinition > responseType = anyExecutable .client .getResponseType ();
155+ Class <? extends MessageDefinition > requestType = anyExecutable .client .getRequestType ();
156+ Class <? extends MessageDefinition > responseType = anyExecutable .client .getResponseType ();
139157
140- MessageDefinition requestMessage = null ;
141158 MessageDefinition responseMessage = null ;
142159
143160 try {
144- requestMessage = requestType .newInstance ();
145- responseMessage = responseType .newInstance ();
146- } catch (InstantiationException ie ) {
147- ie .printStackTrace ();
148- } catch (IllegalAccessException iae ) {
149- iae .printStackTrace ();
161+ responseMessage = responseType .getDeclaredConstructor ().newInstance ();
162+ } catch (ReflectiveOperationException ie ) {
163+ throw new IllegalStateException ("Failed to instantiate service response" );
150164 }
151165
152- if (requestMessage != null && responseMessage != null ) {
153- long requestFromJavaConverterHandle = requestMessage .getFromJavaConverterInstance ();
154- long requestToJavaConverterHandle = requestMessage .getToJavaConverterInstance ();
166+ if (responseMessage != null ) {
155167 long responseFromJavaConverterHandle = responseMessage .getFromJavaConverterInstance ();
156168 long responseToJavaConverterHandle = responseMessage .getToJavaConverterInstance ();
157169 long responseDestructorHandle = responseMessage .getDestructorInstance ();
@@ -161,7 +173,9 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) {
161173 responseToJavaConverterHandle , responseDestructorHandle , responseMessage );
162174
163175 if (rmwRequestId != null ) {
164- anyExecutable .client .handleResponse (rmwRequestId , responseMessage );
176+ // Safety: nativeTakeResponse() will return the correct type here.
177+ // We can't do much better here, as subscriptions are type erased.
178+ clientHandleResponseUnchecked (anyExecutable .client , rmwRequestId , responseMessage );
165179 }
166180 }
167181 clientHandles .remove (anyExecutable .client .getHandle ());
@@ -187,7 +201,7 @@ protected void waitForWork(long timeout) {
187201 this .actionServerHandles .clear ();
188202
189203 for (ComposableNode node : this .nodes ) {
190- for (Subscription < MessageDefinition > subscription : node .getNode ().getSubscriptions ()) {
204+ for (Subscription subscription : node .getNode ().getSubscriptions ()) {
191205 this .subscriptionHandles .add (new AbstractMap .SimpleEntry <Long , Subscription >(
192206 subscription .getHandle (), subscription ));
193207 Collection <EventHandler > eventHandlers = subscription .getEventHandlers ();
@@ -209,17 +223,17 @@ protected void waitForWork(long timeout) {
209223 this .timerHandles .add (new AbstractMap .SimpleEntry <Long , Timer >(timer .getHandle (), timer ));
210224 }
211225
212- for (Service < ServiceDefinition > service : node .getNode ().getServices ()) {
226+ for (Service service : node .getNode ().getServices ()) {
213227 this .serviceHandles .add (
214228 new AbstractMap .SimpleEntry <Long , Service >(service .getHandle (), service ));
215229 }
216230
217- for (Client < ServiceDefinition > client : node .getNode ().getClients ()) {
231+ for (Client client : node .getNode ().getClients ()) {
218232 this .clientHandles .add (
219233 new AbstractMap .SimpleEntry <Long , Client >(client .getHandle (), client ));
220234 }
221235
222- for (ActionServer < ActionDefinition > actionServer : node .getNode ().getActionServers ()) {
236+ for (ActionServer actionServer : node .getNode ().getActionServers ()) {
223237 this .actionServerHandles .add (
224238 new AbstractMap .SimpleEntry <Long , ActionServer >(actionServer .getHandle (), actionServer ));
225239 }
@@ -498,7 +512,7 @@ private static native void nativeWaitSetAddSubscription(
498512 private static native void nativeWait (long waitSetHandle , long timeout );
499513
500514 private static native MessageDefinition nativeTake (
501- long subscriptionHandle , Class <MessageDefinition > messageType );
515+ long subscriptionHandle , Class <? extends MessageDefinition > messageType );
502516
503517 private static native void nativeWaitSetAddService (long waitSetHandle , long serviceHandle );
504518
0 commit comments