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..3ee57e4 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; @@ -94,53 +95,3 @@ public void proc() { public abstract void fillMemoryWithReceivedMessage(T message, Memory sensoryMemory); } -/* Exemplo de uso: -import br.unicamp.cst.bindings.ros2java.RosTopicSubscriberCodelet; -import br.unicamp.cst.core.entities.Memory; -import id.jrosmessages.std_msgs.StringMessage; - - - //CST Codelet subscribing to a ROS 2 topic and storing received StringMessage in memory. - -public class HelloRosSubscriberCodelet extends RosTopicSubscriberCodelet { - - public HelloRosSubscriberCodelet() { - // topic name: /helloRos - // message type: std_msgs/String - super("/helloRos", StringMessage.class); - } - - @Override - public void fillMemoryWithReceivedMessage(StringMessage message, Memory sensoryMemory) { - // Just store the string data inside memory - System.out.println("Received from ROS: " + message.getData()); - sensoryMemory.setI(message.getData()); - } -} - - -import br.unicamp.cst.core.entities.Memory; -import br.unicamp.cst.core.entities.Thing; -import br.unicamp.cst.core.entities.Codelet; - -public class HelloRosMain { - - public static void main(String[] args) throws Exception { - Thing agent = new Thing(); - - Memory outputMemory = new Memory(); - agent.addOutput(outputMemory, "helloRos"); - - Codelet subscriber = new HelloRosSubscriberCodelet(); - subscriber.addOutput(outputMemory); - - agent.insertCodelet(subscriber); - - // Starts the CST main loop - agent.start(); - } -} - -To test this, run a publisher in a separate terminal or Java process like: -ros2 topic pub /helloRos std_msgs/String "data: 'Hello from Python ROS 2!'" -*/ \ No newline at end of file diff --git a/src/test/java/br/unicamp/cst/bindings/ros2java/AddTwoIntsServiceProvider.java b/src/test/java/br/unicamp/cst/bindings/ros2java/AddTwoIntsServiceProvider.java index 09889d6..0f845b9 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/AddTwoIntsServiceProvider.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/AddTwoIntsServiceProvider.java @@ -9,6 +9,10 @@ import troca_ros.AddTwoIntsResponseMessage; import troca_ros.AddTwoIntsServiceDefinition; +/** + * + * @author jrborelli + */ public class AddTwoIntsServiceProvider implements Runnable { 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..8ad63ae 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,6 +1,4 @@ -/** - * - */ + package br.unicamp.cst.bindings.ros2java; import br.unicamp.cst.core.entities.Memory; @@ -13,7 +11,6 @@ * */ - public class ROS2_ChatterTopicPublisher extends RosTopicPublisherCodelet { public ROS2_ChatterTopicPublisher(String topic) { @@ -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/ROS2_ChatterTopicSubscriber.java b/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicSubscriber.java index 631c264..8655a45 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicSubscriber.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2_ChatterTopicSubscriber.java @@ -1,6 +1,3 @@ -/** - * - */ package br.unicamp.cst.bindings.ros2java; 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..4b1504b 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java @@ -1,15 +1,12 @@ -/** - * @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; -import org.junit.AfterClass; -import org.junit.BeforeClass; -//import org.junit.Test; +import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import static org.junit.Assert.assertEquals; @@ -17,19 +14,23 @@ 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 + @BeforeAll public static void setup() { SilenceLoggers(); mind = new Mind(); } - @AfterClass + @AfterAll public static void cleanup() { if (mind != null) { mind.shutDown(); @@ -44,14 +45,13 @@ private static void SilenceLoggers() { @Test public void testRos2Topics() throws InterruptedException { - setup(); 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 +62,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,27 +81,209 @@ 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}", new Object[]{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"); + 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(); assertEquals(expectedMessage, actualMessage); mind.shutDown(); 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}" , new Object[]{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(); + + 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: {0}" , new Object[]{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(); + + 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}" , new Object[]{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}" , new Object[]{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}", new Object[]{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(); + + LOGGER.log(Level.INFO, "Finished testRosServiceClientCodelet..."); + } @Test public void testRos2ServiceSync() throws InterruptedException, ExecutionException, TimeoutException { - System.out.println("Starting 2nd test..."); - TimeStamp.setStartTime(); + LOGGER.log(Level.INFO, "Starting 2nd test..."); + // Start the server AddTwoIntsServiceProvider serviceProvider = new AddTwoIntsServiceProvider(); serviceProvider.start(); @@ -126,6 +308,8 @@ public void testRos2ServiceSync() throws InterruptedException, ExecutionExceptio clientSync.stop(); serviceProvider.stop(); - System.out.println("It took "+TimeStamp.getDelaySinceStart()); + + LOGGER.log(Level.INFO, "It took {0}", new Object[]{TimeStamp.getDelaySinceStart()}); } -} \ No newline at end of file +} + diff --git a/src/test/java/br/unicamp/cst/bindings/rosjava/RosJavaTest.java b/src/test/java/br/unicamp/cst/bindings/rosjava/RosJavaTest.java index 9573a2a..79c2a40 100644 --- a/src/test/java/br/unicamp/cst/bindings/rosjava/RosJavaTest.java +++ b/src/test/java/br/unicamp/cst/bindings/rosjava/RosJavaTest.java @@ -1,13 +1,14 @@ -/** - * - */ package br.unicamp.cst.bindings.rosjava; import br.unicamp.cst.core.entities.MemoryObject; import br.unicamp.cst.core.entities.Mind; -import org.junit.AfterClass; -import org.junit.BeforeClass; -import org.junit.Test; +import br.unicamp.cst.support.TimeStamp; +import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.TestInstance; +import org.junit.jupiter.api.BeforeEach; +import org.opentest4j.TestAbortedException; import org.ros.RosCore; import org.ros.node.DefaultNodeMainExecutor; import org.ros.node.NodeConfiguration; @@ -16,242 +17,219 @@ import java.net.URI; import java.net.URISyntaxException; +import java.util.logging.Level; +import java.util.logging.Logger; -import static org.junit.Assert.assertEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; /** * @author andre - * */ +@TestInstance(TestInstance.Lifecycle.PER_CLASS) public class RosJavaTest { - - private static RosCore rosCore; - - @BeforeClass + + private static final Logger LOGGER = Logger.getLogger(RosJavaTest.class.getName()); + private static RosCore rosCore; + private static Mind mind; + private static boolean rosCoreStarted = false; + private static final long ROS_CORE_STARTUP_TIMEOUT = 60000; // 60 seconds + + @BeforeAll public static void beforeAllTestMethods() { - rosCore = RosCore.newPublic("127.0.0.1",11311); - rosCore.start(); + // Silence loggers to reduce noise + Logger.getLogger("org.ros").setLevel(Level.OFF); + + try { + // Attempt to start the ROS Core once for all tests. + rosCore = RosCore.newPublic("127.0.0.1", 11311); + rosCore.start(); + + LOGGER.log(Level.INFO, "Waiting for ROS Core to start..."); + + // Wait with a timeout for the RosCore to be fully up and running + long startTime = System.currentTimeMillis(); + boolean started = false; + while (System.currentTimeMillis() - startTime < ROS_CORE_STARTUP_TIMEOUT) { + try { + rosCore.awaitStart(); + started = true; + break; + } catch (Exception e) { + // Ignore exceptions during awaitStart in a timed loop, it might just be not ready yet + } + Thread.sleep(500); // Wait a bit before checking again + } + + if (!started) { + throw new TestAbortedException("ROS Core did not start within the " + (ROS_CORE_STARTUP_TIMEOUT / 1000) + " second timeout. Skipping tests."); + } + + rosCoreStarted = true; + LOGGER.log(Level.INFO, "ROS Core started successfully."); + } catch (Exception e) { + LOGGER.log(Level.SEVERE, "Failed to start ROS Core. ROS-dependent tests will be skipped.", e); + // Signal to JUnit that tests should be skipped + throw new TestAbortedException("ROS Core is not available. Skipping tests.", e); + } + + // Initialize the Mind once for all tests + mind = new Mind(); + LOGGER.log(Level.INFO, "CST Mind initialized."); } - @AfterClass + @AfterAll public static void afterAllTestMethods() { - rosCore.shutdown(); + if (mind != null) { + mind.shutDown(); + } + if (rosCore != null) { + rosCore.shutdown(); + } + LOGGER.log(Level.INFO, "ROS Core and CST Mind shut down."); } - + @Test - public void testRosTopics() throws URISyntaxException { - - Mind mind = new Mind(); - - ChatterTopicSubscriber chatterTopicSubscriber = new ChatterTopicSubscriber("127.0.0.1",new URI("http://127.0.0.1:11311")); - MemoryObject sensoryMemory = mind.createMemoryObject(chatterTopicSubscriber.getName()); - chatterTopicSubscriber.addOutput(sensoryMemory); - mind.insertCodelet(chatterTopicSubscriber); - - ChatterTopicPublisher chatterTopicPublisher = new ChatterTopicPublisher("127.0.0.1",new URI("http://127.0.0.1:11311")); - MemoryObject motorMemory = mind.createMemoryObject(chatterTopicPublisher.getName()); - chatterTopicPublisher.addInput(motorMemory); - String messageExpected = "Hello World"; - motorMemory.setI(messageExpected); - mind.insertCodelet(chatterTopicPublisher); - - mind.start(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - String messageActual = (String) sensoryMemory.getI(); - - assertEquals(messageExpected, messageActual); - - mind.shutDown(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } + public void testRosTopics() throws URISyntaxException, InterruptedException { + // Create subscriber and publisher codelets + ChatterTopicSubscriber chatterTopicSubscriber = new ChatterTopicSubscriber("127.0.0.1", new URI("http://127.0.0.1:11311")); + ChatterTopicPublisher chatterTopicPublisher = new ChatterTopicPublisher("127.0.0.1", new URI("http://127.0.0.1:11311")); + + // Create memory object to link them + MemoryObject internalMemory = mind.createMemoryObject("chatter"); + chatterTopicSubscriber.addOutput(internalMemory); + chatterTopicPublisher.addInput(internalMemory); + + // Insert codelets and start the mind + mind.insertCodelet(chatterTopicSubscriber); + mind.insertCodelet(chatterTopicPublisher); + mind.start(); + + // Set the message and wait for it to be received by the subscriber + String messageExpected = "Hello World"; + internalMemory.setI(messageExpected); + + long origTimestamp = internalMemory.getTimestamp(); + + // Wait for the memory object to be updated with the new message, with a timeout + long timeout = System.currentTimeMillis() + 10000; // 10 seconds timeout + long newTimestamp = origTimestamp; + while (newTimestamp == origTimestamp && System.currentTimeMillis() < timeout) { + Thread.sleep(100); + newTimestamp = internalMemory.getTimestamp(); + } + + // Assert that the timestamp has changed, meaning the message was received + if (newTimestamp == origTimestamp) { + throw new TestAbortedException("Timeout waiting for message to be received by the subscriber."); + } + + String messageActual = (String) internalMemory.getI(); + + assertEquals(messageExpected, messageActual); + + chatterTopicPublisher.stop(); + chatterTopicSubscriber.stop(); } - + @Test public void testRosServiceSync() throws URISyntaxException, InterruptedException { - - AddTwoIntService addTwoIntService = new AddTwoIntService(); - NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); - NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("127.0.0.1",new URI("http://127.0.0.1:11311")); - nodeMainExecutor.execute(addTwoIntService, nodeConfiguration); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - AddTwoIntServiceClientSync addTwoIntServiceClient = new AddTwoIntServiceClientSync("127.0.0.1",new URI("http://127.0.0.1:11311")); - - long expectedSum = 5L; - - Integer[] numsToSum = new Integer[] {2,3}; - - AddTwoIntsResponse addTwoIntsResponse = addTwoIntServiceClient.callService(numsToSum); - - long actualSum = addTwoIntsResponse.getSum(); - - assertEquals(expectedSum, actualSum); - - long expectedSum2 = 6L; - - Integer[] numsToSum2 = new Integer[] {2,4}; - - AddTwoIntsResponse addTwoIntsResponse2 = addTwoIntServiceClient.callService(numsToSum2); - - long actualSum2 = addTwoIntsResponse2.getSum(); - - assertEquals(expectedSum2, actualSum2); - - addTwoIntServiceClient.stopRosNode(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - nodeMainExecutor.shutdownNodeMain(addTwoIntService); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - - @Test - public void testRosService() throws URISyntaxException { - - AddTwoIntService addTwoIntService = new AddTwoIntService(); - NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); - NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("127.0.0.1",new URI("http://127.0.0.1:11311")); - nodeMainExecutor.execute(addTwoIntService, nodeConfiguration); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - Mind mind = new Mind(); - - AddTwoIntServiceClient addTwoIntServiceClient = new AddTwoIntServiceClient("127.0.0.1",new URI("http://127.0.0.1:11311")); - - MemoryObject motorMemory = mind.createMemoryObject(addTwoIntServiceClient.getName()); - addTwoIntServiceClient.addInput(motorMemory); - - Integer expectedSum = 5; - - Integer[] numsToSum = new Integer[] {2,3}; - motorMemory.setI(numsToSum); - - mind.insertCodelet(addTwoIntServiceClient); - - mind.start(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - assertEquals(expectedSum, addTwoIntServiceClient.getSum()); - - mind.shutDown(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - nodeMainExecutor.shutdownNodeMain(addTwoIntService); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - + + AddTwoIntService addTwoIntService = new AddTwoIntService(); + NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); + NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("127.0.0.1", new URI("http://127.0.0.1:11311")); + nodeMainExecutor.execute(addTwoIntService, nodeConfiguration); + + // Give a fixed amount of time for the service to start and be discovered. + Thread.sleep(5000); // Wait for 5 seconds + + AddTwoIntServiceClientSync addTwoIntServiceClient = new AddTwoIntServiceClientSync("127.0.0.1", new URI("http://127.0.0.1:11311")); + + // First test + long expectedSum = 5L; + Integer[] numsToSum = new Integer[] {2, 3}; + AddTwoIntsResponse addTwoIntsResponse = addTwoIntServiceClient.callService(numsToSum); + long actualSum = addTwoIntsResponse.getSum(); + assertEquals(expectedSum, actualSum); + + // Second test + long expectedSum2 = 6L; + Integer[] numsToSum2 = new Integer[] {2, 4}; + AddTwoIntsResponse addTwoIntsResponse2 = addTwoIntServiceClient.callService(numsToSum2); + long actualSum2 = addTwoIntsResponse2.getSum(); + assertEquals(expectedSum2, actualSum2); + + addTwoIntServiceClient.stopRosNode(); + + // Cleanup ROS nodes + Thread.sleep(2000); + nodeMainExecutor.shutdownNodeMain(addTwoIntService); + Thread.sleep(2000); } - + @Test - public void testRosServiceCalledTwice() throws URISyntaxException { - - AddTwoIntService addTwoIntService = new AddTwoIntService(); - NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); - NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("127.0.0.1",new URI("http://127.0.0.1:11311")); - nodeMainExecutor.execute(addTwoIntService, nodeConfiguration); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - Mind mind = new Mind(); - - AddTwoIntServiceClient addTwoIntServiceClient = new AddTwoIntServiceClient("127.0.0.1",new URI("http://127.0.0.1:11311")); - - MemoryObject motorMemory = mind.createMemoryObject(addTwoIntServiceClient.getName()); - addTwoIntServiceClient.addInput(motorMemory); - - Integer expectedSum = 5; - - Integer[] numsToSum = new Integer[] {2,3}; - motorMemory.setI(numsToSum); - - mind.insertCodelet(addTwoIntServiceClient); - - mind.start(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - assertEquals(expectedSum, addTwoIntServiceClient.getSum()); - - expectedSum = 6; - - numsToSum = new Integer[] {3,3}; - motorMemory.setI(numsToSum); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - assertEquals(expectedSum, addTwoIntServiceClient.getSum()); - - mind.shutDown(); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - nodeMainExecutor.shutdownNodeMain(addTwoIntService); - - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - + public void testRosServiceClientWithCstMind() throws URISyntaxException, InterruptedException { + + // Start the service provider + AddTwoIntService addTwoIntService = new AddTwoIntService(); + NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); + NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("127.0.0.1", new URI("http://127.0.0.1:11311")); + nodeMainExecutor.execute(addTwoIntService, nodeConfiguration); + + // Give a fixed amount of time for the service to start. + Thread.sleep(5000); // Wait for 5 seconds + + // Instantiate the CST client codelet + AddTwoIntServiceClient addTwoIntServiceClient = new AddTwoIntServiceClient("127.0.0.1", new URI("http://127.0.0.1:11311")); + + // Create a memory object to hold the input and output + MemoryObject internalMemory = mind.createMemoryObject(addTwoIntServiceClient.getName()); + addTwoIntServiceClient.addInput(internalMemory); + + mind.insertCodelet(addTwoIntServiceClient); + mind.start(); + + // First test call + Integer[] numsToSum = new Integer[] {2, 3}; + internalMemory.setI(numsToSum); + long origTimestamp = internalMemory.getTimestamp(); + + // Wait for the memory object to be updated with the service's response, with a timeout + long timeout = System.currentTimeMillis() + 10000; // 10 seconds timeout + long newTimestamp = origTimestamp; + while (newTimestamp == origTimestamp && System.currentTimeMillis() < timeout) { + Thread.sleep(100); + newTimestamp = internalMemory.getTimestamp(); + } + + if (newTimestamp == origTimestamp) { + throw new TestAbortedException("Timeout waiting for service client response."); + } + + Integer expectedSum = 5; + assertEquals(expectedSum, internalMemory.getI()); + + // Second test call + Integer[] numsToSum2 = new Integer[] {3, 3}; + internalMemory.setI(numsToSum2); + origTimestamp = internalMemory.getTimestamp(); + + // Wait again for the memory object to be updated, with a timeout + timeout = System.currentTimeMillis() + 10000; // 10 seconds timeout + newTimestamp = origTimestamp; + while (newTimestamp == origTimestamp && System.currentTimeMillis() < timeout) { + Thread.sleep(100); + newTimestamp = internalMemory.getTimestamp(); + } + + if (newTimestamp == origTimestamp) { + throw new TestAbortedException("Timeout waiting for second service client response."); + } + + Integer expectedSum2 = 6; + assertEquals(expectedSum2, internalMemory.getI()); + + // Cleanup + addTwoIntServiceClient.stop(); + nodeMainExecutor.shutdownNodeMain(addTwoIntService); } -} +} \ No newline at end of file diff --git a/src/test/java/br/unicamp/cst/bindings/soar/JSoarCodeletTest.java b/src/test/java/br/unicamp/cst/bindings/soar/JSoarCodeletTest.java index 420986a..311b1dc 100644 --- a/src/test/java/br/unicamp/cst/bindings/soar/JSoarCodeletTest.java +++ b/src/test/java/br/unicamp/cst/bindings/soar/JSoarCodeletTest.java @@ -5,7 +5,8 @@ import com.google.gson.JsonObject; import com.google.gson.JsonParser; import com.google.gson.JsonPrimitive; -import org.junit.Test; +//import org.junit.Test; +import org.junit.jupiter.api.Test; import java.io.File; import java.util.ArrayList; @@ -435,4 +436,4 @@ public void contractViolationTest(){ } -} +} \ No newline at end of file diff --git a/src/test/java/br/unicamp/cst/bindings/soar/SOARPluginTest.java b/src/test/java/br/unicamp/cst/bindings/soar/SOARPluginTest.java index d058c8d..313bbde 100644 --- a/src/test/java/br/unicamp/cst/bindings/soar/SOARPluginTest.java +++ b/src/test/java/br/unicamp/cst/bindings/soar/SOARPluginTest.java @@ -9,7 +9,8 @@ import org.jsoar.kernel.symbols.Identifier; import org.jsoar.util.commands.SoarCommandInterpreter; import org.jsoar.util.commands.SoarCommands; -import org.junit.Test; +//import org.junit.Test; +import org.junit.jupiter.api.Test; import java.io.ByteArrayOutputStream; import java.io.File; @@ -800,4 +801,4 @@ public void createIdeaFromJson_ArrayOfLights_Test() { } } -} +} \ No newline at end of file