diff --git a/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientCodelet.java b/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientCodelet.java index 914e8d9..6d15a12 100644 --- a/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientCodelet.java +++ b/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientCodelet.java @@ -1,4 +1,14 @@ - +/*********************************************************************************************** + * Copyright (c) 2012 DCA-FEEC-UNICAMP + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the GNU Lesser Public License v3 + * which accompanies this distribution, and is available at + * http://www.gnu.org/licenses/lgpl.html + *

+ * Contributors: + * K. Raizer, A. L. O. Paraense, E. M. Froes, R. R. Gudwin - initial API and implementation + * jrborelli - ROS2 + ***********************************************************************************************/ package br.unicamp.cst.bindings.ros2java; @@ -145,121 +155,3 @@ protected void processServiceResponse(AddTwoIntsResponseMessage response) { } */ } - - - - -/* Fisrt verstion... - -package br.unicamp.cst.bindings.ros2java; - -import br.unicamp.cst.core.entities.Codelet; -import br.unicamp.cst.core.entities.Memory; -import br.unicamp.cst.core.exceptions.CodeletActivationBoundsException; -import id.jrosmessages.Message; -//import pinorobotics.jros2client.JRos2Client; -import id.jros2client.JRos2Client; -import id.jros2client.JRos2ClientFactory; -import pinorobotics.jros2services.JRos2ServiceClient; -//import pinorobotics.jrosservices.JRosServiceClient; -import pinorobotics.jros2services.JRos2ServicesFactory; - -import java.util.concurrent.Semaphore; - - -public abstract class RosServiceClientCodelet extends Codelet { - - protected String serviceName; - protected Class requestType = (Class) AddTwoIntsRequestMessage.class;; - protected Class responseType = (Class) AddTwoIntsResponseMessage.class; - - protected Memory inputMemory; - - protected S requestMessage; - protected JRos2ServiceClient serviceClient; - protected JRos2Client ros2Client; - - private final Semaphore callInProgressSemaphore = new Semaphore(1); - - public RosServiceClientCodelet(String serviceName, Class requestType, Class responseType) { - this.serviceName = serviceName; - this.requestType = requestType; - this.responseType = responseType; - this.setName("Ros2Client:" + serviceName); - } - - @Override - public void start() { - try { - - ros2Client = new JRos2ClientFactory().createClient(); - - // Assuming you have a ServiceDefinition class for your service, e.g. AddTwoIntsServiceDefinition - serviceClient = new JRos2ServicesFactory().createClient(ros2Client, new AddTwoIntsServiceDefinition(), serviceName); - - } catch (Exception e) { - throw new RuntimeException("Failed to initialize ROS 2 client for service: " + serviceName, e); - } - - super.start(); - } - - @Override - public void stop() { - try { - if (serviceClient != null) serviceClient.close(); - if (ros2Client != null) ros2Client.close(); - } catch (Exception e) { - e.printStackTrace(); - } - - super.stop(); - } - - @Override - public void accessMemoryObjects() { - if (inputMemory == null) { - inputMemory = this.getInput(serviceName, 0); - } - } - - @Override - public void calculateActivation() { - try { - setActivation(1.0); // Always run if needed - } catch (CodeletActivationBoundsException e) { - e.printStackTrace(); - } - } - - @Override - public void proc() { - if (inputMemory == null || inputMemory.getI() == null) return; - - try { - requestMessage = createNewRequest(); - if (formatServiceRequest(inputMemory, requestMessage)) { - callInProgressSemaphore.acquire(); - - T response = serviceClient.sendRequestAsync(requestMessage).get(); - - if (response != null) { - processServiceResponse(response); - } - } - } catch (Exception e) { - System.err.println("Error in ROS 2 service call: " + e.getMessage()); - } finally { - callInProgressSemaphore.release(); - } - } - - protected abstract S createNewRequest(); - - - protected abstract boolean formatServiceRequest(Memory memory, S request); - - protected abstract void processServiceResponse(T response); -} - -*/ \ No newline at end of file diff --git a/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientSync.java b/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientSync.java index 401a8b2..dbcde85 100644 --- a/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientSync.java +++ b/src/main/java/br/unicamp/cst/bindings/ros2java/RosServiceClientSync.java @@ -7,6 +7,7 @@ *

* Contributors: * K. Raizer, A. L. O. Paraense, E. M. Froes, R. R. Gudwin - initial API and implementation + * jrborelli - ROS2 ***********************************************************************************************/ package br.unicamp.cst.bindings.ros2java; diff --git a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicOneShotPublisherCodelet.java b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicOneShotPublisherCodelet.java index 1ceb990..8890656 100644 --- a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicOneShotPublisherCodelet.java +++ b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicOneShotPublisherCodelet.java @@ -7,6 +7,7 @@ *

* Contributors: * K. Raizer, A. L. O. Paraense, E. M. Froes, R. R. Gudwin - initial API and implementation + * jrborelli - ROS2 ***********************************************************************************************/ @@ -109,240 +110,3 @@ public void setEnabled(boolean enabled) { } -/* // Second Version, almost there, see reference. -package br.unicamp.cst.bindings.ros2java; - -import br.unicamp.cst.core.entities.Codelet; -import br.unicamp.cst.core.entities.Memory; -import br.unicamp.cst.core.exceptions.CodeletActivationBoundsException; -import id.jrosmessages.Message; -import id.jros2client.JRos2Client; -import id.jros2client.JRos2ClientFactory; -import id.jros2client.qos.PublisherQos; -import id.jros2client.qos.SubscriberQos; -//import id.jros2client.publisher.JRos2Publisher; -import id.jrosclient.TopicSubmissionPublisher; - -public abstract class RosTopicOneShotPublisherCodelet extends Codelet { - - protected String topic; - protected Class messageType; - protected Memory motorMemory; - - protected JRos2Client ros2Client; - //protected JRos2Publisher publisher; - protected TopicSubmissionPublisher publisher; - protected T message; - - private volatile boolean enabled = false; - - public RosTopicOneShotPublisherCodelet(String topic, Class messageType) { - this.topic = topic; - this.messageType = messageType; - setName("Ros2Publisher:" + topic); - } - - @Override - public synchronized void start() { - - // Exemplo: - //https://github.com/lambdaprime/jros2client/blob/main/jros2client.examples/generic/src/PublisherApp.java - // var configBuilder = new JRos2ClientConfiguration.Builder(); - // use configBuilder to override default parameters (network interface, RTPS settings etc) - // var client = new JRos2ClientFactory().createClient(configBuilder.build()); - // String topicName = "/helloRos"; - // var publisher = new TopicSubmissionPublisher<>(StringMessage.class, topicName); - // register a new publisher for a new topic with ROS - // client.publish(publisher); - // while (true) { - // publisher.submit(new StringMessage().withData("Hello ROS")); - // System.out.println("Published"); - // Thread.sleep(1000); - //} - - - ros2Client = new JRos2ClientFactory().createClient(); - publisher = new TopicSubmissionPublisher<>(messageType, topic); - - //publisher = ros2Client.createPublisher(topic, messageType); - //message = publisher.newMessage(); - super.start(); - } - - @Override - public synchronized void stop() { - try { - if (publisher != null) publisher.close(); - if (ros2Client != null) ros2Client.close(); - } catch (Exception e) { - e.printStackTrace(); - } - super.stop(); - } - - @Override - public void accessMemoryObjects() { - if (motorMemory == null) { - motorMemory = getInput(topic, 0); - } - } - - @Override - public void calculateActivation() { - try { - setActivation(enabled ? 1.0 : 0.0); - } catch (CodeletActivationBoundsException e) { - e.printStackTrace(); - } - } - - @Override - public void proc() { - if (!enabled) return; - if (motorMemory == null) return; - - fillMessageToBePublished(motorMemory, message); - //publisher.publish(message); - enabled = false; // publish only once per enable - } - - - public abstract void fillMessageToBePublished(Memory motorMemory, T message); - - public boolean getEnabled() { - return enabled; - } - - public void setEnabled(boolean enabled) { - this.enabled = enabled; - } - -} - -*/ - -/* //First Version: -package br.unicamp.cst.bindings.ros2java; - -import br.unicamp.cst.core.entities.Codelet; -import br.unicamp.cst.core.entities.Memory; -import br.unicamp.cst.core.exceptions.CodeletActivationBoundsException; - -import id.jrosmessages.Message; -import id.jrosmessages.MessageDescriptor; -import id.jros2client.JRos2Client; -import id.jros2client.JRos2ClientFactory; -import id.jros2client.publisher.JRos2Publisher; // your publisher interface/class - -import java.util.concurrent.atomic.AtomicBoolean; - - - //ROS 2 Topic One-Shot Publisher Codelet that creates and holds a publisher instance. - - //@param ROS 2 Message type extending Message - -public abstract class RosTopicOneShotPublisherCodelet extends Codelet { - - protected String topic; - protected Class messageTypeClass; // the class of message type (for createPublisher) - protected MessageDescriptor messageDescriptor; // optional, if your client uses descriptors - - protected Memory motorMemory; - protected T message; - - protected JRos2Client ros2Client; - protected JRos2Publisher publisher; - - private final AtomicBoolean enabled = new AtomicBoolean(false); - - public RosTopicOneShotPublisherCodelet(String topic, Class messageTypeClass, MessageDescriptor messageDescriptor) { - super(); - this.topic = topic; - this.messageTypeClass = messageTypeClass; - this.messageDescriptor = messageDescriptor; - setName("Ros2Publisher:" + topic); - } - - @Override - public synchronized void start() { - try { - ros2Client = new JRos2ClientFactory().createClient(); - if (messageDescriptor != null) { - publisher = ros2Client.createPublisher(topic, messageDescriptor); - } else { - publisher = ros2Client.createPublisher(topic, messageTypeClass); - } - } catch (Exception e) { - throw new RuntimeException("Failed to initialize ROS 2 client or create publisher", e); - } - super.start(); - } - - @Override - public synchronized void stop() { - try { - if (ros2Client != null) ros2Client.close(); - } catch (Exception e) { - e.printStackTrace(); - } - super.stop(); - } - - @Override - public void accessMemoryObjects() { - if (motorMemory == null) { - motorMemory = this.getInput(topic, 0); - } - } - - @Override - public void calculateActivation() { - try { - setActivation(0.0d); - } catch (CodeletActivationBoundsException e) { - e.printStackTrace(); - } - } - - @Override - public void proc() { - if (!enabled.get()) return; - if (motorMemory == null || motorMemory.getI() == null) return; - - if (message == null) { - message = createNewMessage(); - } - - fillMessageToBePublished(motorMemory, message); - - try { - if (messageDescriptor != null) { - ros2Client.publish(topic, messageDescriptor, publisher); - } else { - ros2Client.publish(topic, messageTypeClass, publisher); - } - } catch (Exception e) { - System.err.println("Failed to publish message on topic " + topic + ": " + e.getMessage()); - } - - enabled.set(false); // one-shot disable after publish - } - - - protected abstract T createNewMessage(); - - - protected abstract void fillMessageToBePublished(Memory motorMemory, T message); - - public boolean isEnabled() { - return enabled.get(); - } - - public void setEnabled(boolean enabled) { - this.enabled.set(enabled); - } -} - -*/ - - diff --git a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicPublisherCodelet.java b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicPublisherCodelet.java index ff8bed9..abb2989 100644 --- a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicPublisherCodelet.java +++ b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicPublisherCodelet.java @@ -7,6 +7,7 @@ *

* Contributors: * K. Raizer, A. L. O. Paraense, E. M. Froes, R. R. Gudwin - initial API and implementation + * jrborelli - ROS2 ***********************************************************************************************/ package br.unicamp.cst.bindings.ros2java; @@ -88,19 +89,3 @@ public void proc() { /** Fill the message using memory content */ protected abstract void fillMessageToBePublished(Memory motorMemory, T message); } - -/* // Exemplo de uso: - -RosTopicPublisherCodelet pub = new RosTopicPublisherCodelet<>("/chatter", StringMessage.class) { - @Override - protected StringMessage createNewMessage() { - return new StringMessage(); - } - - @Override - protected void fillMessageToBePublished(Memory motorMemory, StringMessage message) { - message.data = "Hello " + motorMemory.getI(); - } -}; - -*/ diff --git a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicSubscriberCodelet.java b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicSubscriberCodelet.java index 2e8d071..a2b5d03 100644 --- a/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicSubscriberCodelet.java +++ b/src/main/java/br/unicamp/cst/bindings/ros2java/RosTopicSubscriberCodelet.java @@ -7,6 +7,7 @@ *

* Contributors: * K. Raizer, A. L. O. Paraense, E. M. Froes, R. R. Gudwin - initial API and implementation + * jrborelli - ROS2 ***********************************************************************************************/ package br.unicamp.cst.bindings.ros2java; diff --git a/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicPublisher.java b/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicPublisher.java index fa8d710..35781b2 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicPublisher.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicPublisher.java @@ -1,5 +1,6 @@ /** - * + * @author jrborelli + * */ package br.unicamp.cst.bindings.ros2java; @@ -8,10 +9,6 @@ import java.net.URI; -/** - * @author jrborelli - * - */ public class ROS2_ChatterTopicPublisher extends RosTopicPublisherCodelet { @@ -36,25 +33,3 @@ protected void fillMessageToBePublished(Memory motorMemory, StringMessage messag } } -/* -// Setup CST Mind and Memory: -Mind mind = new Mind(); - -ChatterTopicPublisher publisher = new ChatterTopicPublisher("chatter"); -Memory motorMemory = mind.createMemoryObject("chatter"); -publisher.addInput(motorMemory); - -mind.insertCodelet(publisher); - -// Set message to publish -motorMemory.setI("Hello ROS2 from CST!"); - -// Start mind, let publisher run for a few cycles -mind.start(); - -// Sleep a bit to allow publishing -Thread.sleep(2000); - -// Then shutdown mind -mind.shutDown(); -*/ \ No newline at end of file diff --git a/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java b/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java index 8d7a743..3734ab8 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java @@ -1,9 +1,8 @@ -/** - * @author jrborelli - */ + package br.unicamp.cst.bindings.ros2java; import br.unicamp.cst.core.entities.MemoryObject; +import br.unicamp.cst.core.entities.Memory; import br.unicamp.cst.core.entities.Mind; import br.unicamp.cst.support.TimeStamp; import id.jrosmessages.std_msgs.StringMessage; @@ -17,10 +16,15 @@ import java.util.concurrent.TimeoutException; import java.util.logging.Level; import java.util.logging.Logger; -import troca_ros.AddTwoIntsResponseMessage; +import troca_ros.*; +/** + * @author jrborelli + */ + public class Ros2JavaTest { + private static final Logger logger = Logger.getLogger(Ros2JavaTest.class.getName()); private static Mind mind; @BeforeClass @@ -48,10 +52,10 @@ public void testRos2Topics() throws InterruptedException { RosTopicSubscriberCodelet subscriber = new RosTopicSubscriberCodelet<>("chatter", StringMessage.class) { public long lasttime = 0; @Override - public void fillMemoryWithReceivedMessage(StringMessage message, br.unicamp.cst.core.entities.Memory sensoryMemory) { + public void fillMemoryWithReceivedMessage(StringMessage message, Memory sensoryMemory) { sensoryMemory.setI(message.data); lasttime = sensoryMemory.getTimestamp(); - System.out.println("I heard: \"" + message.data + "\" at "+TimeStamp.getStringTimeStamp(lasttime)); + logger.log(Level.INFO,"I heard: {0} at {1}", new Object[]{message.data , TimeStamp.getStringTimeStamp(lasttime)}); } }; @@ -62,7 +66,7 @@ protected StringMessage createNewMessage() { return new StringMessage(); } @Override - protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory motorMemory, StringMessage message) { + protected void fillMessageToBePublished(Memory motorMemory, StringMessage message) { String data = (String) motorMemory.getI(); if (data != null) { message.data = data; @@ -81,12 +85,12 @@ protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory moto mind.insertCodelet(subscriber); mind.insertCodelet(publisher); long orig = internalMemory.getTimestamp(); - System.out.println("Starting: "+TimeStamp.getStringTimeStamp(orig)); + logger.log(Level.INFO,"Starting: {0}",TimeStamp.getStringTimeStamp(orig)); mind.start(); long novo = internalMemory.getTimestamp(); // Wait until a new info is actualized by the subscriber codelet (the Timestamp is changed) while(novo == orig) novo = internalMemory.getTimestamp(); - System.out.println("First message received by: "+TimeStamp.getStringTimeStamp(novo)+" ... it took "+TimeStamp.getStringTimeStamp(novo-orig,"ss.SSS")+" seconds"); + logger.log(Level.INFO, "First message received by: {0} ... it took {1} seconds", new Object[]{TimeStamp.getStringTimeStamp(novo), TimeStamp.getStringTimeStamp(novo - orig, "ss.SSS")}); String actualMessage = (String) internalMemory.getI(); assertEquals(expectedMessage, actualMessage); @@ -94,13 +98,192 @@ protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory moto publisher.stop(); subscriber.stop(); cleanup(); - System.out.println("Finished first test..."); + logger.log(Level.INFO,"Finished first test..."); } + @Test + public void testChatterTopicSpecializedCodelets() throws InterruptedException { + + // Instantiate both specialized classes + ROS2_ChatterTopicPublisher publisher = new ROS2_ChatterTopicPublisher("chatter"); + ROS2_ChatterTopicSubscriber subscriber = new ROS2_ChatterTopicSubscriber("chatter"); + + // Create a memory object to link them + MemoryObject internalMemory = mind.createMemoryObject("chatter"); + subscriber.addOutput(internalMemory); + publisher.addInput(internalMemory); + + // Send a message + String expectedMessage = "Hello from the CST Mind, this is a Specialized Topic Publisher Codelet!"; + internalMemory.setI(expectedMessage); + + mind.insertCodelet(subscriber); + mind.insertCodelet(publisher); + long orig = internalMemory.getTimestamp(); + logger.log(Level.INFO,"Starting: {0}" , TimeStamp.getStringTimeStamp(orig)); + mind.start(); + + long novo = internalMemory.getTimestamp(); + // Wait until a new info is actualized by the subscriber codelet (the Timestamp is changed) + while(novo == orig) { + Thread.sleep(100); + novo = internalMemory.getTimestamp(); + } + + logger.log(Level.INFO, "First message received by: {0} ... it took {1} seconds", new Object[]{TimeStamp.getStringTimeStamp(novo), TimeStamp.getStringTimeStamp(novo - orig, "ss.SSS")}); + String actualMessage = (String) internalMemory.getI(); + + // Assert that the received message is the same as the sent one + assertEquals(expectedMessage, actualMessage); + + // Cleanup + mind.shutDown(); + publisher.stop(); + subscriber.stop(); + cleanup(); + logger.log(Level.INFO,"Finished testChatterTopicIntegration..."); + } + + + @Test + public void testOneShotPublisher() throws InterruptedException { + + // Instantiate the one-shot publisher + RosTopicOneShotPublisherCodelet oneShotPublisher = new RosTopicOneShotPublisherCodelet<>("one_shot_chatter", StringMessage.class) { + @Override + public void fillMessageToBePublished(Memory motorMemory, StringMessage message) { + Object data = motorMemory.getI(); + if (data instanceof String) { + message.withData((String) data); + } else { + message.withData(""); + } + } + + @Override + public StringMessage createMessage() { + return new StringMessage(); + } + }; + + // Instantiate the subscriber + ROS2_ChatterTopicSubscriber subscriber = new ROS2_ChatterTopicSubscriber("one_shot_chatter"); + + // Create a memory object to link them + MemoryObject internalMemory = mind.createMemoryObject("one_shot_chatter"); + subscriber.addOutput(internalMemory); + oneShotPublisher.addInput(internalMemory); + + mind.insertCodelet(subscriber); + mind.insertCodelet(oneShotPublisher); + + mind.start(); + + // Wait for mind to stabilize + Thread.sleep(500); + + // Set the message and enable the publisher + String expectedMessage = "This message should be sent only once!"; + internalMemory.setI(expectedMessage); + oneShotPublisher.setEnabled(true); + + long orig = internalMemory.getTimestamp(); + logger.log(Level.INFO,"Starting one-shot test: " , TimeStamp.getStringTimeStamp(orig)); + + // Wait until a new info is actualized by the subscriber codelet + long novo = internalMemory.getTimestamp(); + while(novo == orig) { + Thread.sleep(100); + novo = internalMemory.getTimestamp(); + } + + logger.log(Level.INFO, "One-shot message received at: {0} ... it took {1} seconds", new Object[]{TimeStamp.getStringTimeStamp(novo), TimeStamp.getStringTimeStamp(novo - orig, "ss.SSS")}); + String actualMessage = (String) internalMemory.getI(); + + // Assert that the received message is the same as the sent one + assertEquals(expectedMessage, actualMessage); + + // Cleanup + mind.shutDown(); + oneShotPublisher.stop(); + subscriber.stop(); + cleanup(); + logger.log(Level.INFO,"Finished testOneShotPublisher..."); + } + + @Test + public void testRosServiceClientCodeletAsync() throws InterruptedException { + logger.log(Level.INFO,"Starting ROS service client test..."); + + // Create a mock service provider for the test + AddTwoIntsServiceProvider serviceProvider = new AddTwoIntsServiceProvider(); + serviceProvider.start(); + Thread.sleep(500); // Allow time for service to be discovered + + // Instantiate the specialized client codelet + RosServiceClientCodelet clientCodelet = new RosServiceClientCodelet<>("add_two_ints", new AddTwoIntsServiceDefinition()) { + @Override + protected AddTwoIntsRequestMessage createNewRequest() { + return new AddTwoIntsRequestMessage(); + } + + @Override + protected boolean formatServiceRequest(Memory inputMemoryObject, AddTwoIntsRequestMessage request) { + Long[] inputs = (Long[]) inputMemoryObject.getI(); + if (inputs == null || inputs.length < 2) return false; + request.withA(inputs[0]); + request.withB(inputs[1]); + return true; + } + + @Override + protected void processServiceResponse(AddTwoIntsResponseMessage response) { + if (response != null) { + logger.log(Level.INFO,"Sum received from service: {0}" , response.sum); + // Update the input memory with the response for verification + this.inputMemory.setI(response.sum); + } + } + }; + + // Create a memory object to hold the input and output + MemoryObject internalMemory = mind.createMemoryObject("add_two_ints"); + clientCodelet.addInput(internalMemory); + + // Set the input data and run the codelet + Long[] inputs = new Long[]{10L, 20L}; + internalMemory.setI(inputs); + + mind.insertCodelet(clientCodelet); + mind.start(); + + long orig = internalMemory.getTimestamp(); + logger.log(Level.INFO,"Starting service request at: {0}" , TimeStamp.getStringTimeStamp(orig)); + + long novo = internalMemory.getTimestamp(); + // Wait until the memory object is updated with the service response + while(novo == orig) { + Thread.sleep(100); + novo = internalMemory.getTimestamp(); + } + + logger.log(Level.INFO, "Service response received at: {0}", TimeStamp.getStringTimeStamp(novo)); + Long actualSum = (Long) internalMemory.getI(); + + // Assert that the sum is correct + assertEquals(Long.valueOf(30L), actualSum); + + // Cleanup + mind.shutDown(); + clientCodelet.stop(); + serviceProvider.stop(); + cleanup(); + logger.log(Level.INFO, "Finished testRosServiceClientCodelet..."); + } @Test public void testRos2ServiceSync() throws InterruptedException, ExecutionException, TimeoutException { - System.out.println("Starting 2nd test..."); + logger.log(Level.INFO, "Starting 2nd test..."); TimeStamp.setStartTime(); // Start the server AddTwoIntsServiceProvider serviceProvider = new AddTwoIntsServiceProvider(); @@ -126,6 +309,9 @@ public void testRos2ServiceSync() throws InterruptedException, ExecutionExceptio clientSync.stop(); serviceProvider.stop(); - System.out.println("It took "+TimeStamp.getDelaySinceStart()); + //logger.info("It took "+TimeStamp.getDelaySinceStart()); + logger.log(Level.INFO, "It took {0}", TimeStamp.getDelaySinceStart()); } -} \ No newline at end of file +} + +