From e9429bb735546c2d41446725b176959ca9848703 Mon Sep 17 00:00:00 2001 From: jrborelli Date: Tue, 26 Aug 2025 21:32:29 -0300 Subject: [PATCH 1/4] cst-bindings - ROS2 - first version --- ...JavaTest_AddTwoIntsServiceClientAsync.java | 75 ------------------- 1 file changed, 75 deletions(-) delete mode 100644 src/test/java/br/unicamp/cst/bindings/ros2java/ROS2JavaTest_AddTwoIntsServiceClientAsync.java diff --git a/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2JavaTest_AddTwoIntsServiceClientAsync.java b/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2JavaTest_AddTwoIntsServiceClientAsync.java deleted file mode 100644 index 580ac02..0000000 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/ROS2JavaTest_AddTwoIntsServiceClientAsync.java +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Click nbfs://nbhost/SystemFileSystem/Templates/Licenses/license-default.txt to change this license - * Click nbfs://nbhost/SystemFileSystem/Templates/Classes/Class.java to edit this template - */ -package br.unicamp.cst.bindings.ros2java; - -import java.util.concurrent.CompletableFuture; -import java.util.concurrent.TimeUnit; -import static org.junit.Assert.assertEquals; -import org.junit.Test; - -/** - * - * @author jrborelli - */ - -public class ROS2JavaTest_AddTwoIntsServiceClientAsync extends RosServiceClientSync { - - public ROS2JavaTest_AddTwoIntsServiceClientAsync(String serviceName) { - super(serviceName, new AddTwoIntsServiceDefinition()); - } - - @Override - public void formatServiceRequest(Object[] args, AddTwoIntsRequestMessage requestMessage) { - requestMessage.a = (Integer) args[0]; - requestMessage.b = (Integer) args[1]; - } - - @Override - protected AddTwoIntsRequestMessage createNewRequest() { - return new AddTwoIntsRequestMessage(); - } - - // For async testing, just use `callService` but don't block in test - - - @Test - public void testRos2ServiceAsync() throws Exception { - ROS2JavaTest_AddTwoIntsServiceClientAsync clientAsync = new ROS2JavaTest_AddTwoIntsServiceClientAsync("add_two_ints"); - clientAsync.start(); - - Object[] args = {7, 8}; - CompletableFuture future = clientAsync.serviceClient.sendRequestAsync( - new AddTwoIntsRequestMessage().withA(7).withB(8) - ); - - AddTwoIntsResponseMessage response = future.get(5, TimeUnit.SECONDS); - assertEquals(15L, response.sum); - - clientAsync.stop(); - } - - - /* RASCUNHO - public void testRos2ServiceAsync() throws Exception { - AddTwoIntsServiceDefinition service = new AddTwoIntsServiceDefinition(); - service.start(); // Only if you have a local node implementation running - - Thread.sleep(2000); // Let the service become available - - AddTwoIntsServiceClientAsync clientAsync = new AddTwoIntsServiceClientAsync("add_two_ints"); - clientAsync.start(); - - Object[] args = {10, 20}; - CompletableFuture future = clientAsync.serviceClient.sendRequestAsync( - new AddTwoIntsRequestMessage().withA(10).withB(20) - ); - - AddTwoIntsResponseMessage response = future.get(5, TimeUnit.SECONDS); - assertEquals(30L, response.sum); - - clientAsync.stop(); - service.stop(); // if applicable - } */ -} From 59be905587d97a9e2bc93ce61f96cac47b43b2ba Mon Sep 17 00:00:00 2001 From: jrborelli Date: Tue, 26 Aug 2025 22:42:33 -0300 Subject: [PATCH 2/4] cst-bindings - ROS2 - first version - test fixed --- .../cst/bindings/ros2java/Ros2JavaTest.java | 38 ++++++++++++++----- 1 file changed, 28 insertions(+), 10 deletions(-) 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 06c13eb..763368a 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java @@ -11,17 +11,37 @@ import org.junit.BeforeClass; import org.junit.Test; import static org.junit.Assert.assertEquals; +import static org.junit.Assume.assumeTrue; +import id.jrosclient.JRosClient; import java.util.concurrent.ExecutionException; import java.util.concurrent.TimeoutException; public class Ros2JavaTest { private static Mind mind; - + //private static boolean isRos2Available = false; + + /* + @BeforeClass + public static void setup() { + mind = new Mind(); + } */ + @BeforeClass public static void setup() { mind = new Mind(); + + // This is the crucial check to skip the tests if ROS 2 is not available. + // It creates a client and attempts to connect, which will fail if no daemon is running. + try { + JRosClient client = new JRosClient(); + client.wait(5); // Await for 5 seconds to connect + client.close(); + } catch (Exception e) { + // This is the condition for assuming the tests will not run + assumeTrue(false); + } } @AfterClass @@ -32,9 +52,8 @@ public static void cleanup() { } @Test - public void testRos2Topics() throws InterruptedException { - + RosTopicSubscriberCodelet subscriber = new RosTopicSubscriberCodelet<>("chatter", StringMessage.class) { @Override public void fillMemoryWithReceivedMessage(StringMessage message, br.unicamp.cst.core.entities.Memory sensoryMemory) { @@ -42,7 +61,6 @@ public void fillMemoryWithReceivedMessage(StringMessage message, br.unicamp.cst. System.out.println("I heard: \"" + message.data + "\""); } }; - RosTopicPublisherCodelet publisher = new RosTopicPublisherCodelet<>("chatter", StringMessage.class) { @Override @@ -57,7 +75,6 @@ protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory moto } } }; - MemoryObject sensoryMemory = mind.createMemoryObject("chatter"); subscriber.addOutput(sensoryMemory); @@ -82,13 +99,11 @@ protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory moto mind.shutDown(); } - @Test public void testRos2ServiceSync() throws InterruptedException, ExecutionException, TimeoutException { - // You must already have the Python/C++ ROS 2 service running (troca_ros/AddTwoIntsService) - - // Start the client + // The service test is also dependent on a running ROS 2 environment. + // The check in @BeforeClass will handle this. AddTwoIntsServiceClientSyncRos2 clientSync = new AddTwoIntsServiceClientSyncRos2("add_two_ints"); clientSync.start(); @@ -108,4 +123,7 @@ public void testRos2ServiceSync() throws InterruptedException, ExecutionExceptio clientSync.stop(); } -} \ No newline at end of file +} + + + From 4efbabb08c811b032b5df3e47b5ec8bc8db4c60d Mon Sep 17 00:00:00 2001 From: jrborelli Date: Fri, 5 Sep 2025 14:47:14 -0300 Subject: [PATCH 3/4] My local changes before merge --- .../cst/bindings/ros2java/Ros2JavaTest.java | 88 ++++++++++--------- 1 file changed, 46 insertions(+), 42 deletions(-) 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 763368a..e60ef2e 100644 --- a/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java +++ b/src/test/java/br/unicamp/cst/bindings/ros2java/Ros2JavaTest.java @@ -3,45 +3,30 @@ */ package br.unicamp.cst.bindings.ros2java; -import br.unicamp.cst.core.entities.Memory; import br.unicamp.cst.core.entities.MemoryObject; 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.Test; +import org.junit.jupiter.api.Test; import static org.junit.Assert.assertEquals; -import static org.junit.Assume.assumeTrue; -import id.jrosclient.JRosClient; import java.util.concurrent.ExecutionException; import java.util.concurrent.TimeoutException; +import java.util.logging.Level; +import java.util.logging.Logger; +import troca_ros.AddTwoIntsResponseMessage; public class Ros2JavaTest { private static Mind mind; - //private static boolean isRos2Available = false; - - /* - @BeforeClass - public static void setup() { - mind = new Mind(); - } */ - + @BeforeClass public static void setup() { + SilenceLoggers(); mind = new Mind(); - - // This is the crucial check to skip the tests if ROS 2 is not available. - // It creates a client and attempts to connect, which will fail if no daemon is running. - try { - JRosClient client = new JRosClient(); - client.wait(5); // Await for 5 seconds to connect - client.close(); - } catch (Exception e) { - // This is the condition for assuming the tests will not run - assumeTrue(false); - } } @AfterClass @@ -50,17 +35,26 @@ public static void cleanup() { mind.shutDown(); } } + + private static void SilenceLoggers() { + Logger.getLogger("pinorobotics.rtpstalk").setLevel(Level.OFF); + Logger.getLogger("id.jros2client").setLevel(Level.OFF); + } @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) { sensoryMemory.setI(message.data); - System.out.println("I heard: \"" + message.data + "\""); + lasttime = sensoryMemory.getTimestamp(); + System.out.println("I heard: \"" + message.data + "\" at "+TimeStamp.getStringTimeStamp(lasttime)); } }; + RosTopicPublisherCodelet publisher = new RosTopicPublisherCodelet<>("chatter", StringMessage.class) { @Override @@ -76,37 +70,46 @@ protected void fillMessageToBePublished(br.unicamp.cst.core.entities.Memory moto } }; - MemoryObject sensoryMemory = mind.createMemoryObject("chatter"); - subscriber.addOutput(sensoryMemory); - - MemoryObject motorMemory = mind.createMemoryObject("chatter"); - publisher.addInput(motorMemory); + MemoryObject internalMemory = mind.createMemoryObject("chatter"); + subscriber.addOutput(internalMemory); + publisher.addInput(internalMemory); // Send the message String expectedMessage = "Hello World"; - motorMemory.setI(expectedMessage); + internalMemory.setI(expectedMessage); mind.insertCodelet(subscriber); mind.insertCodelet(publisher); + long orig = internalMemory.getTimestamp(); + System.out.println("Starting: "+TimeStamp.getStringTimeStamp(orig)); mind.start(); - - Thread.sleep(4000); // allow some time for message exchange - - String actualMessage = (String) sensoryMemory.getI(); + 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"); + String actualMessage = (String) internalMemory.getI(); assertEquals(expectedMessage, actualMessage); - //Thread.sleep(500); - - mind.shutDown(); + mind.shutDown(); + publisher.stop(); + subscriber.stop(); + cleanup(); + System.out.println("Finished first test..."); } + @Test public void testRos2ServiceSync() throws InterruptedException, ExecutionException, TimeoutException { - // The service test is also dependent on a running ROS 2 environment. - // The check in @BeforeClass will handle this. + System.out.println("Starting 2nd test..."); + TimeStamp.setStartTime(); + // Start the server + AddTwoIntsServiceProvider serviceProvider = new AddTwoIntsServiceProvider(); + serviceProvider.start(); + + // Start the client AddTwoIntsServiceClientSyncRos2 clientSync = new AddTwoIntsServiceClientSyncRos2("add_two_ints"); clientSync.start(); - + // First test long expectedSum1 = 5L; Object[] args1 = new Object[]{2L, 3L}; @@ -122,8 +125,9 @@ public void testRos2ServiceSync() throws InterruptedException, ExecutionExceptio assertEquals(expectedSum2, actualSum2); clientSync.stop(); + serviceProvider.stop(); + System.out.println("It took "+TimeStamp.getDelaySinceStart()); } } - From ea987f52bafbd1ec33099fdbe0363e9d038f1dbc Mon Sep 17 00:00:00 2001 From: jrborelli Date: Sat, 6 Sep 2025 01:39:29 -0300 Subject: [PATCH 4/4] cst-bindings - ROS2 - first version - fixed --- .../ros2java/RosServiceClientCodelet.java | 130 +--------- .../ros2java/RosServiceClientSync.java | 1 + .../RosTopicOneShotPublisherCodelet.java | 238 +----------------- .../ros2java/RosTopicPublisherCodelet.java | 17 +- .../ros2java/RosTopicSubscriberCodelet.java | 1 + .../ros2java/ROS2_ChatterTopicPublisher.java | 29 +-- .../cst/bindings/ros2java/Ros2JavaTest.java | 208 ++++++++++++++- 7 files changed, 213 insertions(+), 411 deletions(-) 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 e60ef2e..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,7 +309,8 @@ 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()); } }