From f61521fa32e07bb2b4e76d8241479c0736b78177 Mon Sep 17 00:00:00 2001 From: ds58 <30220598+ds58@users.noreply.github.com> Date: Tue, 3 Dec 2024 15:43:26 -0600 Subject: [PATCH] Create ROS2NodeBuilder (#32) --- .../run-gradle-test-all-platforms.yml | 75 +-- .../run-gradle-test-linux-self-hosted.yml | 35 -- ...pointDiscoveryProtocolListenerExample.java | 3 +- .../pubsub/examples/PublisherExample.java | 1 + .../pubsub/examples/SubscriberExample.java | 1 + .../ihmc/pubsub/test/AggressivePublisher.java | 2 +- .../test/CreateSubscriptionUnderLoad.java | 2 +- .../us/ihmc/pubsub/test/HandshakeTest.java | 2 +- ...MultipleParticipantsInSameProcessTest.java | 37 +- .../PublishSubscribeUInt64AllocationTest.java | 2 +- ...ublishSubscribeUInt64SharedMemoryTest.java | 2 +- .../test/PublishSubscribeUInt64Test.java | 2 +- .../test/PublisherSubscriberMatchTest.java | 2 +- .../us/ihmc/pubsub/tools/PubSubTester.java | 1 + .../java/us/ihmc/pubsub/DomainFactory.java | 2 +- .../pubsub/attributes/ParticipantProfile.java | 183 ++++++-- .../attributes/PublisherAttributes.java | 17 +- .../attributes/SubscriberAttributes.java | 13 +- .../pubsub/impl/fastRTPS/FastRTPSDomain.java | 17 +- .../impl/fastRTPS/FastRTPSParticipant.java | 2 +- .../impl/fastRTPS/FastRTPSPublisher.java | 2 +- .../impl/fastRTPS/FastRTPSSubscriber.java | 2 +- .../ihmc/pubsub/participant/Participant.java | 5 +- .../participant/ParticipantDiscoveryInfo.java | 5 +- .../us/ihmc/pubsub/publisher/Publisher.java | 3 + .../us/ihmc/pubsub/subscriber/Subscriber.java | 4 + .../src/main/java/us/ihmc/ros2/ROS2Node.java | 251 +++------- .../java/us/ihmc/ros2/ROS2NodeBuilder.java | 433 ++++++++++++++++++ .../java/us/ihmc/ros2/RealtimeROS2Node.java | 151 +----- .../main/java/us/ihmc/ros2/SubnetUtils.java | 327 +++++++++++++ .../java/us/ihmc/ros2/CommunicationTest.java | 63 +-- .../us/ihmc/ros2/ROS2NodeBuilderTest.java | 32 ++ .../us/ihmc/ros2/example/DataTypesTest.java | 5 +- .../NonRealtimeROS2ListenerExample.java | 8 +- ...onRealtimeROS2PublishSubscribeExample.java | 13 +- .../example/NonRealtimeROS2TalkerExample.java | 28 +- .../RealtimeROS2IntraprocessCopyTest.java | 7 +- .../RealtimeROS2PublishSubscribeExample.java | 6 +- 38 files changed, 1186 insertions(+), 560 deletions(-) delete mode 100644 .github/workflows/run-gradle-test-linux-self-hosted.yml create mode 100644 ros2-library/src/main/java/us/ihmc/ros2/ROS2NodeBuilder.java create mode 100644 ros2-library/src/main/java/us/ihmc/ros2/SubnetUtils.java create mode 100644 ros2-library/src/test/java/us/ihmc/ros2/ROS2NodeBuilderTest.java diff --git a/.github/workflows/run-gradle-test-all-platforms.yml b/.github/workflows/run-gradle-test-all-platforms.yml index fe67aaca..2eb608d2 100644 --- a/.github/workflows/run-gradle-test-all-platforms.yml +++ b/.github/workflows/run-gradle-test-all-platforms.yml @@ -2,10 +2,18 @@ name: Run Gradle test (all platforms) on: workflow_dispatch: + push: + branches: + - develop + pull_request: jobs: - test-linux: - runs-on: [ubuntu-20.04] + test: + strategy: + matrix: + # os: [ubuntu-20.04, windows-2019, macos-12, macos-14] + os: [ubuntu-20.04, windows-2019] + runs-on: ${{ matrix.os }} steps: - uses: actions/checkout@v4 with: @@ -17,62 +25,13 @@ jobs: java-version: '17' - uses: gradle/actions/setup-gradle@v4 with: - gradle-version: 8.5 + gradle-version: 8.11.1 - name: Run tests run: | - patch ihmc-pub-sub/thirdparty/Fast-RTPS/resources/xsd/fastRTPS_profiles.xsd ihmc-pub-sub/patches/fastRTPS_profiles.patch - gradle compositeTask --stacktrace --info -PtaskName=test - test-windows: - runs-on: [windows-2019] - steps: - - uses: actions/checkout@v4 - with: - lfs: 'false' - submodules: 'recursive' - - uses: actions/setup-java@v4 - with: - distribution: 'temurin' - java-version: '17' - - uses: gradle/actions/setup-gradle@v4 - with: - gradle-version: 8.5 - - name: Run tests - run: | - patch ihmc-pub-sub/thirdparty/Fast-RTPS/resources/xsd/fastRTPS_profiles.xsd ihmc-pub-sub/patches/fastRTPS_profiles.patch - gradle compositeTask --stacktrace --info -PtaskName=test - test-macos-intel: - runs-on: [macos-12] # macos-12 is exclusively x86_64 intel - steps: - - uses: actions/checkout@v4 - with: - lfs: 'false' - submodules: 'recursive' - - uses: actions/setup-java@v4 + gradle compositeTask -PtaskName=test -PrunningOnCIServer=true + - name: Publish Test Report - ${{ inputs.test-category }} + uses: mikepenz/action-junit-report@v4 + if: success() || failure() # always run even if the previous step fails with: - distribution: 'temurin' - java-version: '17' - - uses: gradle/actions/setup-gradle@v4 - with: - gradle-version: 8.5 - - name: Run tests - run: | - patch ihmc-pub-sub/thirdparty/Fast-RTPS/resources/xsd/fastRTPS_profiles.xsd ihmc-pub-sub/patches/fastRTPS_profiles.patch - gradle compositeTask --stacktrace --info -PtaskName=test - test-macos-arm: - runs-on: [macos-14] # macos-14 runner is exclusively arm64 - steps: - - uses: actions/checkout@v4 - with: - lfs: 'false' - submodules: 'recursive' - - uses: actions/setup-java@v4 - with: - distribution: 'temurin' - java-version: '17' - - uses: gradle/actions/setup-gradle@v4 - with: - gradle-version: 8.5 - - name: Run tests - run: | - patch ihmc-pub-sub/thirdparty/Fast-RTPS/resources/xsd/fastRTPS_profiles.xsd ihmc-pub-sub/patches/fastRTPS_profiles.patch - gradle compositeTask --stacktrace --info -PtaskName=test + report_paths: '**/build/test-results/test/TEST-*.xml' + detailed_summary: true diff --git a/.github/workflows/run-gradle-test-linux-self-hosted.yml b/.github/workflows/run-gradle-test-linux-self-hosted.yml deleted file mode 100644 index 66a6a858..00000000 --- a/.github/workflows/run-gradle-test-linux-self-hosted.yml +++ /dev/null @@ -1,35 +0,0 @@ -name: Run Gradle test (Linux, self-hosted) - -on: - push: - branches: ["develop"] - pull_request: - branches: ["develop"] - workflow_dispatch: - -jobs: - test-linux: - runs-on: [self-hosted, Linux] - steps: - - uses: actions/checkout@v4 - with: - lfs: 'false' - submodules: 'recursive' - - uses: actions/setup-java@v4 - with: - distribution: 'temurin' - java-version: '17' - - uses: gradle/actions/setup-gradle@v4 - with: - gradle-version: 8.5 - - name: Run tests - run: | - gradle compositeTask -PtaskName=compileJava || true - gradle compositeTask -PtaskName=compileJava - gradle compositeTask -PtaskName=test --info --stacktrace --no-daemon -PrunningOnCIServer=true - - name: Publish Test Report - ${{ inputs.test-category }} - uses: mikepenz/action-junit-report@v4 - if: success() || failure() # always run even if the previous step fails - with: - report_paths: '**/build/test-results/test/TEST-*.xml' - detailed_summary: true diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/EndpointDiscoveryProtocolListenerExample.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/EndpointDiscoveryProtocolListenerExample.java index 9c617e6e..aa271d2a 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/EndpointDiscoveryProtocolListenerExample.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/EndpointDiscoveryProtocolListenerExample.java @@ -88,9 +88,10 @@ public void subscriberTopicChange(boolean isAlive, public EndpointDiscoveryProtocolListenerExample() throws IOException { Domain domain = DomainFactory.getDomain(); - + ParticipantProfile attributes = ParticipantProfile.create() .domainId(215) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) .name("EndpointDiscoveryProtocolListenerExample"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/PublisherExample.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/PublisherExample.java index faa82478..8ffee797 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/PublisherExample.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/PublisherExample.java @@ -70,6 +70,7 @@ public PublisherExample() throws IOException ParticipantProfile attributes2 = ParticipantProfile.create() .domainId(1) + .useOnlyIntraProcessDelivery() .name("PublisherExample2") .discoveryLeaseDuration(Time.Infinite); //.discoveryServer("127.0.0.1", 4); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/SubscriberExample.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/SubscriberExample.java index 6507578f..385976cc 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/SubscriberExample.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/examples/SubscriberExample.java @@ -82,6 +82,7 @@ public SubscriberExample() throws IOException ParticipantProfile attributes2 = ParticipantProfile.create() .domainId(1) + .useOnlyIntraProcessDelivery() .name("ParticipantExample") .discoveryLeaseDuration(Time.Infinite); //.discoveryServer("127.0.0.1", 4); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/AggressivePublisher.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/AggressivePublisher.java index 2d502b83..68ecad96 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/AggressivePublisher.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/AggressivePublisher.java @@ -32,8 +32,8 @@ public AggressivePublisher() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(215) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("AggressivePublisher"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/CreateSubscriptionUnderLoad.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/CreateSubscriptionUnderLoad.java index 792222cd..dfe7776c 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/CreateSubscriptionUnderLoad.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/CreateSubscriptionUnderLoad.java @@ -34,8 +34,8 @@ public CreateSubscriptionUnderLoad() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(215) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("CreateSubscriptionProcessDuringAggressivePublishTest"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/HandshakeTest.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/HandshakeTest.java index a59fb4db..e777e22c 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/HandshakeTest.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/HandshakeTest.java @@ -73,8 +73,8 @@ public void testPublishSubscribeFooHandshake() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(220) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("StatusTest"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/MultipleParticipantsInSameProcessTest.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/MultipleParticipantsInSameProcessTest.java index 9ede8012..c176b8ab 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/MultipleParticipantsInSameProcessTest.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/MultipleParticipantsInSameProcessTest.java @@ -54,23 +54,28 @@ public void onSubscriptionMatched(Subscriber subscriber, MatchingInfo info) } @Test - public void TestMulitpleParticipantsInSameProcess() throws IOException, InterruptedException + public void testMulitpleParticipantsInSameProcess() throws IOException, InterruptedException { - AtomicInteger counter = new AtomicInteger(0); + AtomicInteger counter = new AtomicInteger(); Domain domain = DomainFactory.getDomain(); try { - TopicDataType topicDataType = new ChatMessagePubSubType(); + TopicDataType topicDataType = new ChatMessagePubSubType(); - PublisherAttributes genericPublisherAttributes = PublisherAttributes.create().topicDataType(topicDataType).topicName("Status") + PublisherAttributes genericPublisherAttributes = PublisherAttributes.create() + .topicDataType(topicDataType) + .topicName("Status") .reliabilityKind(ReliabilityQosKindPolicyType.RELIABLE) .partitions(Collections.singletonList("us/ihmc")) .durabilityKind(DurabilityQosKindPolicyType.TRANSIENT_LOCAL) - .historyQosPolicyKind(HistoryQosKindPolicyType.KEEP_LAST).historyDepth(10); + .historyQosPolicyKind(HistoryQosKindPolicyType.KEEP_LAST) + .historyDepth(10); - SubscriberAttributes subscriberAttributes = SubscriberAttributes.create().topicDataType(topicDataType).topicName("Status") + SubscriberAttributes subscriberAttributes = SubscriberAttributes.create() + .topicDataType(topicDataType) + .topicName("Status") .reliabilityKind(ReliabilityQosKindPolicyType.RELIABLE) .partitions(Collections.singletonList("us/ihmc")) .durabilityKind(DurabilityQosKindPolicyType.TRANSIENT_LOCAL) @@ -81,25 +86,22 @@ public void TestMulitpleParticipantsInSameProcess() throws IOException, Interrup { ParticipantProfile participantProfile = ParticipantProfile.create() .domainId(217) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("StatusTest" + i); Participant participant = domain.createParticipant(participantProfile); - LogTools.info("Creating participant #" + i); participants.add(participant); } + LogTools.info("Created {} participants", participants.size()); List publishers = new ArrayList<>(); - - for (int i = 0; i < participants.size(); i++) - { - publishers.add(domain.createPublisher(participants.get(i), genericPublisherAttributes, null)); - LogTools.info("Creating publisher #" + (i + 1)); - } + for (Participant participant : participants) + publishers.add(domain.createPublisher(participant, genericPublisherAttributes, null)); + LogTools.info("Created {} publishers", publishers.size()); Subscriber subscriber = domain.createSubscriber(participants.get(0), subscriberAttributes, new SubscriberListenerImpl(counter)); - //publish one message from each publisher in each participant + // Publish one message from each publisher in each participant Thread thread = new Thread(() -> { AtomicInteger msgCounter = new AtomicInteger(); @@ -108,10 +110,9 @@ public void TestMulitpleParticipantsInSameProcess() throws IOException, Interrup try { ChatMessage msg = new ChatMessage(); - msg.setMsg(String.valueOf(msgCounter.get())); + msg.setMsg(String.valueOf(msgCounter.getAndIncrement())); publisher.write(msg); - Thread.sleep(5L); // Sleep a bit so FastDDS can deliver the message. - msgCounter.incrementAndGet(); + Thread.sleep(20L); // Sleep a bit so Fast-DDS can deliver the message. } catch (IOException | InterruptedException e) { diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64AllocationTest.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64AllocationTest.java index ad300d98..b3edec05 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64AllocationTest.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64AllocationTest.java @@ -66,8 +66,8 @@ public void runAllocationTest() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(218) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("StatusTest"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64SharedMemoryTest.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64SharedMemoryTest.java index def3272b..d5f15d8b 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64SharedMemoryTest.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64SharedMemoryTest.java @@ -39,8 +39,8 @@ public void testPublishSubscribeUInt32SharedMemory() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(219) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("StatusTest"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64Test.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64Test.java index 131e4657..93ccf02b 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64Test.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublishSubscribeUInt64Test.java @@ -39,8 +39,8 @@ public void testPublishSubscribeUInt32() throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(219) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) - .useOnlySharedMemoryTransport() .name("StatusTest"); Participant participant = domain.createParticipant(attributes, new ParticipantListenerImpl()); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublisherSubscriberMatchTest.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublisherSubscriberMatchTest.java index e94b5d07..33f25a22 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublisherSubscriberMatchTest.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/test/PublisherSubscriberMatchTest.java @@ -13,7 +13,7 @@ public class PublisherSubscriberMatchTest { @Test - public void TestMatchingAttributes() + public void testMatchingAttributes() { TopicDataType topicDataType = new ChatMessagePubSubType(); diff --git a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/tools/PubSubTester.java b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/tools/PubSubTester.java index bc0e63b3..82af8367 100644 --- a/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/tools/PubSubTester.java +++ b/ihmc-pub-sub-generator/src/test/java/us/ihmc/pubsub/tools/PubSubTester.java @@ -40,6 +40,7 @@ public PubSubTester(Supplier

msgTypeSupplier) throws IOException ParticipantProfile attributes = ParticipantProfile.create() .domainId(systemDomain()) + .useOnlyIntraProcessDelivery() .discoveryLeaseDuration(Time.Infinite) .name("PubSubTester"); diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/DomainFactory.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/DomainFactory.java index 80787298..09366567 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/DomainFactory.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/DomainFactory.java @@ -19,7 +19,7 @@ public class DomainFactory { - public static synchronized Domain getDomain() + public static synchronized FastRTPSDomain getDomain() { return FastRTPSDomain.getInstance(false); } diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/ParticipantProfile.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/ParticipantProfile.java index 4868196e..bf89fb9f 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/ParticipantProfile.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/ParticipantProfile.java @@ -1,10 +1,12 @@ package us.ihmc.pubsub.attributes; import com.eprosima.xmlschemas.fastrtps_profiles.BuiltinAttributesType; +import com.eprosima.xmlschemas.fastrtps_profiles.Dds; import com.eprosima.xmlschemas.fastrtps_profiles.DiscoveryProtocolType; import com.eprosima.xmlschemas.fastrtps_profiles.DiscoveryServersListType; import com.eprosima.xmlschemas.fastrtps_profiles.DiscoverySettingsType; import com.eprosima.xmlschemas.fastrtps_profiles.EDPType; +import com.eprosima.xmlschemas.fastrtps_profiles.LibrarySettingsType; import com.eprosima.xmlschemas.fastrtps_profiles.LocatorListType; import com.eprosima.xmlschemas.fastrtps_profiles.LocatorListType.Locator; import com.eprosima.xmlschemas.fastrtps_profiles.ParticipantProfileType; @@ -17,18 +19,21 @@ import com.eprosima.xmlschemas.fastrtps_profiles.TransportDescriptorType.InterfaceWhiteList; import com.eprosima.xmlschemas.fastrtps_profiles.Udpv4LocatorType; import jakarta.xml.bind.JAXBElement; +import us.ihmc.log.LogTools; import us.ihmc.pubsub.common.Time; import us.ihmc.pubsub.impl.fastRTPS.FastRTPSDomain; import javax.xml.namespace.QName; +import java.io.File; import java.io.IOException; import java.net.InetAddress; import java.util.UUID; public class ParticipantProfile { - private final ParticipantProfileType profileType = new ParticipantProfileType(); + private final ParticipantProfileType participantProfile = new ParticipantProfileType(); private final TransportDescriptorListType transportDescriptors = new TransportDescriptorListType(); + private final LibrarySettingsType librarySettings = new LibrarySettingsType(); public ParticipantProfile() { @@ -37,36 +42,48 @@ public ParticipantProfile() DiscoverySettingsType discoverySettingsType = new DiscoverySettingsType(); builtin.setDiscoveryConfig(discoverySettingsType); - profileType.setRtps(new Rtps()); - profileType.getRtps().setBuiltin(builtin); + participantProfile.setRtps(new Rtps()); + participantProfile.getRtps().setBuiltin(builtin); // Set default discovery duration discoveryLeaseDuration(Time.Infinite); + + useIntraProcessDelivery(true); } - + /** * Helper function to use a builder-like approach - * + * * @return new intance of ParticipantAttributes */ public static ParticipantProfile create() { return new ParticipantProfile(); } - + /** * Direct access to the participant profile. This allows the user to access all settings - * + * * @return Participant profile XML structure */ public ParticipantProfileType getProfile() { - return profileType; + return participantProfile; + } + + public TransportDescriptorListType getTransportDescriptors() + { + return transportDescriptors; + } + + public LibrarySettingsType getLibrarySettings() + { + return librarySettings; } public ParticipantProfile domainId(int id) { - profileType.setDomainId(id); + participantProfile.setDomainId(id); return this; } @@ -77,21 +94,21 @@ public int getDomainId() public ParticipantProfile name(String name) { - profileType.getRtps().setName(name); + participantProfile.getRtps().setName(name); return this; } public String getName() { - return profileType.getRtps().getName(); + return participantProfile.getRtps().getName(); } public ParticipantProfile discoveryLeaseDuration(Time discoveryLeaseDuration) { - profileType.getRtps().getBuiltin().getDiscoveryConfig().setLeaseDuration(DDSConversionTools.timeToDurationType(discoveryLeaseDuration)); + participantProfile.getRtps().getBuiltin().getDiscoveryConfig().setLeaseDuration(DDSConversionTools.timeToDurationType(discoveryLeaseDuration)); return this; } - + public ParticipantProfile discoveryServer(String discoveryServerAddress, int discoveryServerId) { return discoveryServer(discoveryServerAddress, discoveryServerId, FastRTPSDomain.DEFAULT_DISCOVERY_SERVER_PORT); @@ -108,8 +125,8 @@ public ParticipantProfile discoveryServer(String discoveryServerAddress, int dis { throw new RuntimeException("Invalid discovery server port"); } - - DiscoverySettingsType discoverySettingsType = profileType.getRtps().getBuiltin().getDiscoveryConfig(); + + DiscoverySettingsType discoverySettingsType = participantProfile.getRtps().getBuiltin().getDiscoveryConfig(); discoverySettingsType.setDiscoveryProtocol(DiscoveryProtocolType.CLIENT); LocatorListType locatorListType = new LocatorListType(); @@ -127,7 +144,7 @@ public ParticipantProfile discoveryServer(String discoveryServerAddress, int dis locatorListType)); remoteServerAttributes.setPrefix(String.format(FastRTPSDomain.FAST_DDS_DISCOVERY_CONFIGURABLE_PREFIX, discoveryServerId)); - DiscoveryServersListType discoveryServerList = profileType.getRtps().getBuiltin().getDiscoveryConfig().getDiscoveryServersList(); + DiscoveryServersListType discoveryServerList = participantProfile.getRtps().getBuiltin().getDiscoveryConfig().getDiscoveryServersList(); discoveryServerList.getRemoteServer().add(remoteServerAttributes); discoverySettingsType.setDiscoveryServersList(discoveryServerList); @@ -156,13 +173,13 @@ public ParticipantProfile addTransport(TransportDescriptorType transport) } // Create userTransports if it doesn't exist - if (profileType.getRtps().getUserTransports() == null) - profileType.getRtps().setUserTransports(new UserTransports()); + if (participantProfile.getRtps().getUserTransports() == null) + participantProfile.getRtps().setUserTransports(new UserTransports()); // Add to userTransports if it doesn't exist { boolean existsInUserTransports = false; - for (String transportId : profileType.getRtps().getUserTransports().getTransportId()) + for (String transportId : participantProfile.getRtps().getUserTransports().getTransportId()) { if (transportId.equals(transport.getTransportId())) { @@ -171,12 +188,12 @@ public ParticipantProfile addTransport(TransportDescriptorType transport) } } if (!existsInUserTransports) - profileType.getRtps().getUserTransports().getTransportId().add(transport.getTransportId()); + participantProfile.getRtps().getUserTransports().getTransportId().add(transport.getTransportId()); } return this; } - + /** * Add a shared memory transport to this participant. * By setting useBuiltinTransports to false, you can use only a shared memory transport @@ -206,7 +223,9 @@ public ParticipantProfile addUDPv4Transport(InetAddress... addressRestriction) for (InetAddress addr : addressRestriction) { - JAXBElement addressElement = new JAXBElement<>(new QName(FastRTPSDomain.FAST_DDS_XML_NAMESPACE, "address"), String.class, addr.getHostAddress()); + JAXBElement addressElement = new JAXBElement<>(new QName(FastRTPSDomain.FAST_DDS_XML_NAMESPACE, "address"), + String.class, + addr.getHostAddress()); addressWhitelist.getAddressOrInterface().add(addressElement); } @@ -226,11 +245,12 @@ public ParticipantProfile addUDPv4Transport(InetAddress... addressRestriction) public ParticipantProfile useOnlySharedMemoryTransport() { useBuiltinTransports(false); + useIntraProcessDelivery(false); - if (profileType.getRtps().getUserTransports() == null) - profileType.getRtps().setUserTransports(new UserTransports()); + if (participantProfile.getRtps().getUserTransports() == null) + participantProfile.getRtps().setUserTransports(new UserTransports()); - profileType.getRtps().getUserTransports().getTransportId().clear(); + participantProfile.getRtps().getUserTransports().getTransportId().clear(); // Find the SHM transport boolean shmTransportFound = false; @@ -250,46 +270,137 @@ public ParticipantProfile useOnlySharedMemoryTransport() return this; } + public ParticipantProfile useOnlyUDPv4Transport(InetAddress... addressRestriction) + { + useBuiltinTransports(false); + useIntraProcessDelivery(false); + + if (participantProfile.getRtps().getUserTransports() == null) + participantProfile.getRtps().setUserTransports(new UserTransports()); + + participantProfile.getRtps().getUserTransports().getTransportId().clear(); + + // Find the UDPv4 transport + boolean udpv4TransportFound = false; + for (TransportDescriptorType transportDescriptorType : transportDescriptors.getTransportDescriptor()) + { + if (transportDescriptorType.getType().equals("UDPv4")) + { + addTransport(transportDescriptorType); + udpv4TransportFound = true; + break; + } + } + + if (!udpv4TransportFound) + addUDPv4Transport(addressRestriction); + + return this; + } + + public ParticipantProfile useOnlyIntraProcessDelivery() + { + useBuiltinTransports(false); + useIntraProcessDelivery(true); + + if (participantProfile.getRtps().getUserTransports() == null) + participantProfile.getRtps().setUserTransports(new UserTransports()); + + participantProfile.getRtps().getUserTransports().getTransportId().clear(); + + // Intra-process delivery requires at least 1 transport. + // Use shared memory to not bind to any network interface or UDPv4 bound to the loopback address if that is not available + if (System.getProperty("os.name").toLowerCase().contains("win") && !fastrtpsSHMAvailableOnWindows()) + { + LogTools.error("Shared Memory Transport (SHM) is not available (Could not write to C:\\ProgramData\\eprosima\\fastrtps_interprocess)." + + " Falling back to UDPv4 transport on the loopback address."); + + addUDPv4Transport(InetAddress.getLoopbackAddress()); + } + else + { + addSharedMemoryTransport(); + } + + return this; + } + public ParticipantProfile useBuiltinTransports(boolean useBuiltinTransports) { - profileType.getRtps().setUseBuiltinTransports(useBuiltinTransports); + participantProfile.getRtps().setUseBuiltinTransports(useBuiltinTransports); return this; } - + public boolean isUseBuiltinTransports() { - return profileType.getRtps().isUseBuiltinTransports(); + return participantProfile.getRtps().isUseBuiltinTransports(); + } + + // https://fast-dds.docs.eprosima.com/en/v2.14.3/fastdds/xml_configuration/library_settings.html#intra-process-delivery-xml-profile + public ParticipantProfile useIntraProcessDelivery(boolean intraProcessDelivery) + { + librarySettings.setIntraprocessDelivery(intraProcessDelivery ? "FULL" : "OFF"); + return this; } public boolean isUseStaticDiscovery() { - return profileType.getRtps().getBuiltin().getDiscoveryConfig().getEDP() == EDPType.STATIC; + return participantProfile.getRtps().getBuiltin().getDiscoveryConfig().getEDP() == EDPType.STATIC; } public ParticipantProfile useStaticDiscovery(boolean useStaticDiscovery) { - profileType.getRtps().getBuiltin().getDiscoveryConfig().setEDP(useStaticDiscovery ? EDPType.STATIC : EDPType.SIMPLE); + participantProfile.getRtps().getBuiltin().getDiscoveryConfig().setEDP(useStaticDiscovery ? EDPType.STATIC : EDPType.SIMPLE); return this; } /** * Marshall this profile to a XML structure - * + * * @param profileName Unique name for this profile * @return XML representation of this profile - * @throws IOException + * @throws IOException */ public String marshall(String profileName) throws IOException { - profileType.setProfileName(profileName); + participantProfile.setProfileName(profileName); + + Dds dds = new Dds(); ProfilesType profilesType = new ProfilesType(); profilesType.getDomainparticipantFactoryOrParticipantOrDataWriter().add(transportDescriptors); - profilesType.getDomainparticipantFactoryOrParticipantOrDataWriter().add(profileType); + profilesType.getDomainparticipantFactoryOrParticipantOrDataWriter().add(participantProfile); - String profileXML =FastRTPSDomain.marshalProfile(profilesType); -// profileXML = Pattern.compile("(.*)<\\/id>").matcher(profileXML).replaceAll("$1<\\/transport_id>"); + dds.setProfiles(profilesType); + dds.setLibrarySettings(librarySettings); + + String profileXML = FastRTPSDomain.marshallProfile(dds); + // Tip: print profileXML for debugging + // profileXML = Pattern.compile("(.*)<\\/id>").matcher(profileXML).replaceAll("$1<\\/transport_id>"); return profileXML; } + + private static boolean fastrtpsSHMAvailableOnWindows() + { + File file = new File("C:\\ProgramData\\eprosima\\fastrtps_interprocess\\test"); + + try + { + if (file.getParentFile() != null) + { + file.getParentFile().mkdirs(); + } + + return file.createNewFile(); + } + catch (IOException e) + { + return false; + } + finally + { + file.delete(); + } + } } diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/PublisherAttributes.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/PublisherAttributes.java index ebdefc81..d1c1d26c 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/PublisherAttributes.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/PublisherAttributes.java @@ -1,6 +1,14 @@ package us.ihmc.pubsub.attributes; -import com.eprosima.xmlschemas.fastrtps_profiles.*; +import com.eprosima.xmlschemas.fastrtps_profiles.DataWriterQosPoliciesType; +import com.eprosima.xmlschemas.fastrtps_profiles.Dds; +import com.eprosima.xmlschemas.fastrtps_profiles.LifespanQosPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.PartitionQosPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.ProfilesType; +import com.eprosima.xmlschemas.fastrtps_profiles.PublishModeQosKindPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.PublishModeQosPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.PublisherProfileType; +import com.eprosima.xmlschemas.fastrtps_profiles.WriterTimesType; import us.ihmc.pubsub.common.Time; import us.ihmc.pubsub.impl.fastRTPS.FastRTPSDomain; @@ -67,15 +75,18 @@ public PublisherAttributes heartBeatPeriod(Time hearbeat) return this; } - public String marshall(String profileName) throws IOException { publisherProfile.setProfileName(profileName); + Dds dds = new Dds(); + ProfilesType profilesType = new ProfilesType(); profilesType.getDomainparticipantFactoryOrParticipantOrDataWriter().add(publisherProfile); - return FastRTPSDomain.marshalProfile(profilesType); + dds.setProfiles(profilesType); + + return FastRTPSDomain.marshallProfile(dds); } @Override diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/SubscriberAttributes.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/SubscriberAttributes.java index c3b4f26b..4e7af800 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/SubscriberAttributes.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/attributes/SubscriberAttributes.java @@ -1,6 +1,11 @@ package us.ihmc.pubsub.attributes; -import com.eprosima.xmlschemas.fastrtps_profiles.*; +import com.eprosima.xmlschemas.fastrtps_profiles.DataReaderQosPoliciesType; +import com.eprosima.xmlschemas.fastrtps_profiles.Dds; +import com.eprosima.xmlschemas.fastrtps_profiles.LifespanQosPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.PartitionQosPolicyType; +import com.eprosima.xmlschemas.fastrtps_profiles.ProfilesType; +import com.eprosima.xmlschemas.fastrtps_profiles.SubscriberProfileType; import us.ihmc.pubsub.impl.fastRTPS.FastRTPSDomain; import java.io.IOException; @@ -39,10 +44,14 @@ public String marshall(String profileName) throws IOException { subscriberProfile.setProfileName(profileName); + Dds dds = new Dds(); + ProfilesType profilesType = new ProfilesType(); profilesType.getDomainparticipantFactoryOrParticipantOrDataWriter().add(subscriberProfile); - return FastRTPSDomain.marshalProfile(profilesType); + dds.setProfiles(profilesType); + + return FastRTPSDomain.marshallProfile(dds); } @Override diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSDomain.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSDomain.java index 9149b70f..f45dcb5e 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSDomain.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSDomain.java @@ -10,7 +10,6 @@ package us.ihmc.pubsub.impl.fastRTPS; import com.eprosima.xmlschemas.fastrtps_profiles.Dds; -import com.eprosima.xmlschemas.fastrtps_profiles.ProfilesType; import jakarta.xml.bind.JAXBContext; import jakarta.xml.bind.JAXBException; import jakarta.xml.bind.Marshaller; @@ -275,22 +274,10 @@ public void setLogLevel(LogLevel level) { us.ihmc.rtps.impl.fastRTPS.LogLevel.setLogLevel(level.getLevel()); } - - /** - * Marshall a ProfilesType to a string - * - * Used internally to marshall data - * - * @param profile - * @return - * @throws IOException - */ - public static String marshalProfile(ProfilesType profile) throws IOException + + public static String marshallProfile(Dds dds) throws IOException { StringWriter writer = new StringWriter(); - - Dds dds = new Dds(); - dds.setProfiles(profile); try { diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSParticipant.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSParticipant.java index d6527673..e52c8aaa 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSParticipant.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSParticipant.java @@ -29,7 +29,7 @@ import java.util.ArrayList; import java.util.UUID; -class FastRTPSParticipant implements Participant +public class FastRTPSParticipant implements Participant { private final NativeParticipantImpl impl; diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSPublisher.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSPublisher.java index 8b36658a..d4ad7024 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSPublisher.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSPublisher.java @@ -30,7 +30,7 @@ import java.nio.ByteBuffer; import java.util.UUID; -class FastRTPSPublisher implements Publisher +public class FastRTPSPublisher implements Publisher { private final Object destructorLock = new Object(); diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSSubscriber.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSSubscriber.java index 2395bc48..42c31780 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSSubscriber.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/impl/fastRTPS/FastRTPSSubscriber.java @@ -30,7 +30,7 @@ import java.nio.ByteBuffer; import java.util.UUID; -class FastRTPSSubscriber implements Subscriber +public class FastRTPSSubscriber implements Subscriber { private final Object destructorLock = new Object(); diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/Participant.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/Participant.java index 7a05b9d9..c1abf3a1 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/Participant.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/Participant.java @@ -25,10 +25,13 @@ /** * Class Participant used to group Publishers and Subscribers into a single working unit - * + * + * Deprecated - use {@link us.ihmc.pubsub.impl.fastRTPS.FastRTPSParticipant} directly + * * @author Jesper Smith * */ +@Deprecated public interface Participant { /** diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/ParticipantDiscoveryInfo.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/ParticipantDiscoveryInfo.java index f8571863..0284c65f 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/ParticipantDiscoveryInfo.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/participant/ParticipantDiscoveryInfo.java @@ -20,10 +20,13 @@ /** * Class ParticipantDiscoveryInfo, provided to the user with information regarding a Discovered Participant. - * + * + * Deprecated - use {@link us.ihmc.pubsub.impl.fastRTPS.FastRTPSParticipantDiscoveryInfo} directly + * * @author Jesper Smith * */ +@Deprecated public class ParticipantDiscoveryInfo { protected final Guid guid; diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/publisher/Publisher.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/publisher/Publisher.java index e748a4a3..821003d4 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/publisher/Publisher.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/publisher/Publisher.java @@ -23,9 +23,12 @@ /** * Class Publisher, used to send data to associated subscribers. * + * Deprecated - use {@link us.ihmc.pubsub.impl.fastRTPS.FastRTPSPublisher} directly + * * @author Jesper Smith * */ +@Deprecated public interface Publisher { /** diff --git a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/subscriber/Subscriber.java b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/subscriber/Subscriber.java index af03df05..f941e6d1 100644 --- a/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/subscriber/Subscriber.java +++ b/ihmc-pub-sub/src/main/java/us/ihmc/pubsub/subscriber/Subscriber.java @@ -19,6 +19,10 @@ import us.ihmc.pubsub.common.Guid; import us.ihmc.pubsub.common.SampleInfo; +/** + * Deprecated - use {@link us.ihmc.pubsub.impl.fastRTPS.FastRTPSSubscriber} directly + */ +@Deprecated public interface Subscriber { /** diff --git a/ros2-library/src/main/java/us/ihmc/ros2/ROS2Node.java b/ros2-library/src/main/java/us/ihmc/ros2/ROS2Node.java index f194a164..78356bde 100644 --- a/ros2-library/src/main/java/us/ihmc/ros2/ROS2Node.java +++ b/ros2-library/src/main/java/us/ihmc/ros2/ROS2Node.java @@ -1,7 +1,6 @@ package us.ihmc.ros2; import us.ihmc.log.LogTools; -import us.ihmc.pubsub.Domain; import us.ihmc.pubsub.DomainFactory; import us.ihmc.pubsub.TopicDataType; import us.ihmc.pubsub.attributes.ParticipantProfile; @@ -9,12 +8,14 @@ import us.ihmc.pubsub.attributes.SubscriberAttributes; import us.ihmc.pubsub.common.MatchingInfo; import us.ihmc.pubsub.common.Time; -import us.ihmc.pubsub.participant.Participant; +import us.ihmc.pubsub.impl.fastRTPS.FastRTPSDomain; +import us.ihmc.pubsub.impl.fastRTPS.FastRTPSParticipant; import us.ihmc.pubsub.publisher.Publisher; import us.ihmc.pubsub.subscriber.Subscriber; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; +import javax.annotation.Nullable; import java.io.IOException; -import java.net.InetAddress; import java.util.function.Consumer; /** @@ -28,35 +29,37 @@ public class ROS2Node { static int DEFAULT_QUEUE_SIZE = 10; - public static final String DEFAULT_NAMESPACE = "/us/ihmc"; - - private Domain domain; - private Participant participant; + private FastRTPSDomain domain; + private FastRTPSParticipant participant; private final String nodeName; private final String namespace; + private final ParticipantProfile profile; + @Nullable + private final SpecialTransportMode specialTransportMode; /** - * Create a ROS2Node - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param attributes ParticipantAttributes for the domain + * Use {@link ROS2NodeBuilder} to construct + * 

{@code
+    *    ROS2Node node = new ROS2NodeBuilder().build("Node");
+    * }
*/ - public ROS2Node(Domain domain, String name, String namespace, ParticipantProfile attributes) + protected ROS2Node(String name, String namespace, ParticipantProfile profile, @Nullable SpecialTransportMode specialTransportMode) { - this.domain = domain; + this.domain = DomainFactory.getDomain(); ROS2TopicNameTools.checkNodename(name); ROS2TopicNameTools.checkNamespace(namespace); this.nodeName = name; this.namespace = namespace; + this.profile = profile; + this.specialTransportMode = specialTransportMode; - attributes.name(name); + profile.name(name); try { - participant = domain.createParticipant(attributes); + participant = (FastRTPSParticipant) domain.createParticipant(profile); } catch (IOException ioException) { @@ -64,69 +67,6 @@ public ROS2Node(Domain domain, String name, String namespace, ParticipantProfile } } - /** - * Create a ROS2Node with the default namespace - * @param name Name of the ROS 2 node - */ - public ROS2Node(String name) - { - this(DomainFactory.getDomain(), name); - } - - /** - * Create a ROS2Node with the default namespace - * @param name Name of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public ROS2Node(String name, int domainId, InetAddress... addressRestriction) - { - this(DomainFactory.getDomain(), name, DEFAULT_NAMESPACE, domainId, addressRestriction); - } - - /** - * Create a ROS2Node with the default namespace - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param name Name of the ROS 2 node - */ - public ROS2Node(Domain domain, String name) - { - this(domain, name, "", domainFromEnvironment(), useSHMFromEnvironment()); - } - - /** - * Create a ROS2Node - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public ROS2Node(Domain domain, String name, String namespace, int domainId, InetAddress... addressRestriction) - { - this(domain, name, namespace, domainId, useSHMFromEnvironment(), addressRestriction); - } - - /** - * Create a ROS2Node - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param useSharedMemory Enable shared memory transport if true - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public ROS2Node(Domain domain, String name, String namespace, int domainId, boolean useSharedMemory, InetAddress... addressRestriction) - { - this(domain, name, namespace, createParticipantAttributes(domainId, useSharedMemory, addressRestriction)); - } - /** * Create a new ROS 2 compatible publisher in this Node * @@ -182,8 +122,8 @@ public ROS2Publisher createPublisher(TopicDataType topicDataType, Stri /** * Create a new ROS 2 compatible publisher in this node. * - * @param messageType The type of the message - * @param topicName Name for the topic + * @param messageType The type of the message + * @param topicName Name for the topic * @return a ROS 2 publisher */ public ROS2Publisher createPublisher(Class messageType, String topicName) @@ -194,9 +134,9 @@ public ROS2Publisher createPublisher(Class messageType, String topicNa /** * Create a new ROS 2 compatible publisher in this node. * - * @param messageType The type of the message - * @param topicName Name for the topic - * @param qosProfile ROS 2 qos profile + * @param messageType The type of the message + * @param topicName Name for the topic + * @param qosProfile ROS 2 qos profile * @return a ROS 2 publisher */ public ROS2Publisher createPublisher(Class messageType, String topicName, ROS2QosProfile qosProfile) @@ -284,8 +224,8 @@ public QueuedROS2Subscription createQueuedSubscription(ROS2Topic topic * and can be polled by the realtime thread. The queueSize should weigh memory requirements of the * message vs the chance to lose incoming messages because the queue is full. * - * @param topic topic - * @param queueSize Depth of the subscription queue (10 would be a good size for small messages) + * @param topic topic + * @param queueSize Depth of the subscription queue (10 would be a good size for small messages) * @return a realtime-safe ROS 2 subscriber */ public QueuedROS2Subscription createQueuedSubscription(ROS2Topic topic, int queueSize) @@ -298,10 +238,10 @@ public QueuedROS2Subscription createQueuedSubscription(ROS2Topic topic * and can be polled by the realtime thread. The queueSize should weigh memory requirements of the * message vs the chance to lose incoming messages because the queue is full. * - * @param messageType The type of the message - * @param topicName Topic name - * @param qosProfile Desired ros qos profile - * @param queueSize Depth of the subscription queue (10 would be a good size for small messages) + * @param messageType The type of the message + * @param topicName Topic name + * @param qosProfile Desired ros qos profile + * @param queueSize Depth of the subscription queue (10 would be a good size for small messages) * @return a realtime-safe ROS 2 subscriber */ public QueuedROS2Subscription createQueuedSubscription(Class messageType, String topicName, ROS2QosProfile qosProfile, int queueSize) @@ -405,9 +345,9 @@ public ROS2Subscription createSubscription(TopicDataType topicDataType * @return a ROS 2 subscription */ public ROS2Subscription createSubscription(TopicDataType topicDataType, - NewMessageListener newMessageListener, - String topicName, - ROS2QosProfile qosProfile) + NewMessageListener newMessageListener, + String topicName, + ROS2QosProfile qosProfile) { return createSubscription(topicDataType, newMessageListener, createSubscriberAttributes(topicName, topicDataType, qosProfile)); } @@ -417,8 +357,8 @@ public ROS2Subscription createSubscription(TopicDataType topicDataType * default qos profile. * Note: This method generates garbage! * - * @param topic The topic - * @param messageCallback Message listener that gives the taken message directly + * @param topic The topic + * @param messageCallback Message listener that gives the taken message directly * @return a ROS 2 subscription */ public ROS2Subscription createSubscription2(ROS2Topic topic, Consumer messageCallback) @@ -452,12 +392,11 @@ public void onSubscriptionMatched(Subscriber subscriber, MatchingInfo info) * Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the * default qos profile. * - * @param topic The topic - * @param newMessageListener New message listener + * @param topic The topic + * @param newMessageListener New message listener * @return a ROS 2 subscription */ - public ROS2Subscription createSubscription(ROS2Topic topic, - NewMessageListener newMessageListener) + public ROS2Subscription createSubscription(ROS2Topic topic, NewMessageListener newMessageListener) { return createSubscription(topic.getType(), newMessageListener, topic.getName(), topic.getQoS()); } @@ -472,8 +411,8 @@ public ROS2Subscription createSubscription(ROS2Topic topic, * @return a ROS 2 subscription */ public ROS2Subscription createSubscription(ROS2Topic topic, - NewMessageListener newMessageListener, - SubscriptionMatchedListener subscriptionMatchedListener) + NewMessageListener newMessageListener, + SubscriptionMatchedListener subscriptionMatchedListener) { return createSubscription(topic.getType(), newMessageListener, subscriptionMatchedListener, topic.getName(), topic.getQoS()); } @@ -482,13 +421,11 @@ public ROS2Subscription createSubscription(ROS2Topic topic, * Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the * default qos profile. * - * @param messageType The type of the message - * @param newMessageListener New message listener + * @param messageType The type of the message + * @param newMessageListener New message listener * @return a ROS 2 subscription */ - public ROS2Subscription createSubscription(Class messageType, - NewMessageListener newMessageListener, - String topicName) + public ROS2Subscription createSubscription(Class messageType, NewMessageListener newMessageListener, String topicName) { return createSubscription(messageType, newMessageListener, topicName, ROS2QosProfile.DEFAULT()); } @@ -497,14 +434,14 @@ public ROS2Subscription createSubscription(Class messageType, * Create a new ROS 2 compatible subscription. This call can be used to make a ROS 2 topic with the * default qos profile. * - * @param messageType The type of the message - * @param newMessageListener New message listener + * @param messageType The type of the message + * @param newMessageListener New message listener * @return a ROS 2 subscription */ public ROS2Subscription createSubscription(Class messageType, - NewMessageListener newMessageListener, - String topicName, - ROS2QosProfile qosProfile) + NewMessageListener newMessageListener, + String topicName, + ROS2QosProfile qosProfile) { TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType); return createSubscription(topicDataType, newMessageListener, topicName, qosProfile); @@ -520,10 +457,10 @@ public ROS2Subscription createSubscription(Class messageType, * @return a ROS 2 subscription */ public ROS2Subscription createSubscription(Class messageType, - NewMessageListener newMessageListener, - SubscriptionMatchedListener subscriptionMatchedListener, - String topicName, - ROS2QosProfile qosProfile) + NewMessageListener newMessageListener, + SubscriptionMatchedListener subscriptionMatchedListener, + String topicName, + ROS2QosProfile qosProfile) { TopicDataType topicDataType = ROS2TopicNameTools.newMessageTopicDataTypeInstance(messageType); return createSubscription(topicDataType, newMessageListener, subscriptionMatchedListener, topicName, qosProfile); @@ -541,10 +478,10 @@ public ROS2Subscription createSubscription(Class messageType, * @return a ROS 2 subscription */ public ROS2Subscription createSubscription(TopicDataType topicDataType, - NewMessageListener newMessageListener, - SubscriptionMatchedListener subscriptionMatchedListener, - String topicName, - ROS2QosProfile qosProfile) + NewMessageListener newMessageListener, + SubscriptionMatchedListener subscriptionMatchedListener, + String topicName, + ROS2QosProfile qosProfile) { return createSubscription(topicDataType, new NewMessageListener() { @@ -572,6 +509,26 @@ public String getNamespace() return namespace; } + /** + * Get the underlying {@link ParticipantProfile} which was used to create the Fast-DDS Domain Participant for this ROS2Node. + * Should effectively be treated as read-only. Changing properties in the ParticipantProfile from here will have no effect. + */ + public ParticipantProfile getProfile() + { + return profile; + } + + /** + * Get the {@link SpecialTransportMode} used when building this ROS2Node. Will be null if no SpecialTransportMode was used. + * + * @return the SpecialTransportMode or null if unset + */ + @Nullable + public SpecialTransportMode getSpecialTransportMode() + { + return specialTransportMode; + } + /** * Destroys this node. This effectively removes this node's {@code Participant} from the domain and clears the internal * references to these two. After calling this method, this node becomes unusable, i.e. publisher or subscriber can no longer @@ -599,60 +556,4 @@ public void destroy() participant = null; } - - public static ParticipantProfile createParticipantAttributes(int domainId, boolean useSharedMemory, InetAddress... addressRestriction) - { - ParticipantProfile participantAttributes = ParticipantProfile.create().domainId(domainId); - - participantAttributes.useBuiltinTransports(false); - - if (useSharedMemory) - { - participantAttributes.addSharedMemoryTransport(); - } - - participantAttributes.addUDPv4Transport(addressRestriction); - - return participantAttributes; - } - - public static boolean useSHMFromEnvironment() - { - String disableSharedMemoryTransportEnv = System.getenv("ROS_DISABLE_SHARED_MEMORY_TRANSPORT"); - if (disableSharedMemoryTransportEnv != null && (disableSharedMemoryTransportEnv.equalsIgnoreCase("true") - || disableSharedMemoryTransportEnv.equals("1"))) - { - LogTools.info("Shared memory transport is disabled via environment variable ROS_DISABLE_SHARED_MEMORY_TRANSPORT"); - return false; - } - else - { - return true; - } - } - - public static int domainFromEnvironment() - { - String rosDomainId = System.getenv("ROS_DOMAIN_ID"); - - int rosDomainIdAsInteger = 0; // default to 0 - - if (rosDomainId != null) - { - rosDomainId = rosDomainId.trim(); - try - { - rosDomainIdAsInteger = Integer.parseInt(rosDomainId); - } - catch (NumberFormatException e) - { - LogTools.warn("Environment variable ROS_DOMAIN_ID cannot be parsed as an integer: {}", rosDomainId); - } - } - - LogTools.info("ROS_DOMAIN_ID environment variable is {}.", rosDomainIdAsInteger); - LogTools.info("Nodes created without a specified domain ID will use ROS_DOMAIN_ID."); - - return rosDomainIdAsInteger; - } } diff --git a/ros2-library/src/main/java/us/ihmc/ros2/ROS2NodeBuilder.java b/ros2-library/src/main/java/us/ihmc/ros2/ROS2NodeBuilder.java new file mode 100644 index 00000000..f91fc6e2 --- /dev/null +++ b/ros2-library/src/main/java/us/ihmc/ros2/ROS2NodeBuilder.java @@ -0,0 +1,433 @@ +package us.ihmc.ros2; + +import com.eprosima.xmlschemas.fastrtps_profiles.ParticipantProfileType.Rtps.UserTransports; +import com.eprosima.xmlschemas.fastrtps_profiles.TransportDescriptorType; +import us.ihmc.log.LogTools; +import us.ihmc.pubsub.attributes.ParticipantProfile; +import us.ihmc.ros2.SubnetUtils.SubnetInfo; +import us.ihmc.util.PeriodicNonRealtimeThreadSchedulerFactory; +import us.ihmc.util.PeriodicThreadSchedulerFactory; + +import javax.annotation.Nullable; +import java.io.File; +import java.io.FileInputStream; +import java.io.FileNotFoundException; +import java.io.IOException; +import java.net.Inet4Address; +import java.net.InetAddress; +import java.net.InterfaceAddress; +import java.net.NetworkInterface; +import java.net.SocketException; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; +import java.util.Properties; +import java.util.Set; +import java.util.Stack; +import java.util.StringJoiner; + +/** + * A builder to construct {@link ROS2Node}, {@link RealtimeROS2Node}. + *

+ * Basic usage: + * + * 

{@code
+ *    ROS2Node node = new ROS2Builder().build("Node");
+ *    RealtimeROS2Node realtimeSHMOnlyNode = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.SHARED_MEMORY_ONLY).buildRealtime("SHMNode");
+ * }
+ *

+ */ +public class ROS2NodeBuilder +{ + private static final int UNSET_DOMAIN_ID = -1; + private static final String DEFAULT_NAMESPACE = "/us/ihmc"; + + /** + * Used to denote that a ROS2Node should be set-up with a special mode of transport. + * These are custom use case specific modes. + */ + public enum SpecialTransportMode + { + /** + * SHARED_MEMORY_ONLY enables only the Shared Memory Transport. Participants communicate directly through memory on the host system. + * Temporary files are written to disk (on Linux in /dev/shm, on Windows in %APPDATA%\Local\Temp) to assist with facilitating this mode of transport. + * This mode is NOT compatible with mixed-transports, e.g. a participant with SHM and UDPv4 transports. + * Documentation: Shared Memory Transport + */ + SHARED_MEMORY_ONLY, + /** + * UDPV4_LOOPBACK_ADDRESS_ONLY enables only the UDP Transport on the loopback address. Useful for mixed-DDS implementations where you only want local + * communication, e.g. CycloneDDS and FastDDS communicating only locally. + * Documentation UDP Transport + */ + UDPV4_LOOPBACK_ADDRESS_ONLY, + /** + * UDPV4_ONLY enables only the UDP Transport with no special address restrictions. Useful for debugging or testing, not super practical in most + * applications. + * Documentation UDP Transport + */ + UDPV4_ONLY, + /** + * INTRAPROCESS_ONLY attempts to force Publishers to directly call reception functions of Subscribers. + * This mode still enables a shared memory (SHM) transport, as there has to be one fallback transport on a Participant. + * This mode is especially useful for unit testing and single-process applications. + * This mode does not bind to any network address, unless SHM transport was unable to initialize. + * This mode is NOT compatible with mixed-transports, e.g. a participant with SHM and UDPv4 transports. + * Documentation: Intra-process + * delivery + */ + INTRAPROCESS_ONLY + } + + private int domainId = UNSET_DOMAIN_ID; + private String namespace = DEFAULT_NAMESPACE; + private boolean useSharedMemory = true; + private InetAddress[] addressRestriction = null; + private boolean parseEnvironment = true; + private boolean parseProperties = true; + private boolean parseNetworkParametersConfig = true; + @Nullable + private SpecialTransportMode specialTransportMode; + + private final transient StringJoiner buildPrintout = new StringJoiner("\n\t\t"); + + public ROS2NodeBuilder domainId(int domainId) + { + this.domainId = domainId; + return this; + } + + public ROS2NodeBuilder namespace(String namespace) + { + this.namespace = namespace; + return this; + } + + public ROS2NodeBuilder useSharedMemory(boolean useSharedMemory) + { + this.useSharedMemory = useSharedMemory; + return this; + } + + public ROS2NodeBuilder addressRestriction(InetAddress... addressRestriction) + { + this.addressRestriction = addressRestriction; + return this; + } + + public ROS2NodeBuilder parseEnvironment(boolean parseEnvironment) + { + this.parseEnvironment = parseEnvironment; + return this; + } + + public ROS2NodeBuilder parseProperties(boolean parseProperties) + { + this.parseProperties = parseProperties; + return this; + } + + public ROS2NodeBuilder parseNetworkParametersConfig(boolean parseNetworkParametersConfig) + { + this.parseNetworkParametersConfig = parseNetworkParametersConfig; + return this; + } + + public ROS2NodeBuilder specialTransportMode(@Nullable SpecialTransportMode specialTransportMode) + { + this.specialTransportMode = specialTransportMode; + return this; + } + + public ROS2Node build(String name) + { + buildPrintout.add("Building ROS2Node: " + name); + return new ROS2Node(name, namespace, buildProfile(), specialTransportMode); + } + + public RealtimeROS2Node buildRealtime(String name) + { + return buildRealtime(name, new PeriodicNonRealtimeThreadSchedulerFactory()); + } + + public RealtimeROS2Node buildRealtime(String name, PeriodicThreadSchedulerFactory threadFactory) + { + buildPrintout.add("Building RealtimeROS2Node: " + name); + return new RealtimeROS2Node(name, namespace, buildProfile(), specialTransportMode, threadFactory); + } + + private ParticipantProfile buildProfile() + { + ParticipantProfile profile = ParticipantProfile.create(); + + // Set up ROS Domain ID + { + if (domainIDValid(domainId)) + { + buildPrintout.add("Using a programmatically set ROS Domain ID: " + domainId); + } + else + { + // Try to find a ROS Domain ID + domainId = findDomainID(); + + // If a valid domain ID was not found automatically + if (!domainIDValid(domainId)) + { + domainId = 100; + + buildPrintout.add("Unable to find any ROS Domain ID"); + buildPrintout.add( + "You can set a ROS Domain ID via: system property (ros.domain.id), environment variable (ROS_DOMAIN_ID), RTPSDomainID in IHMCNetworkParameters.ini"); + buildPrintout.add("Using a default ROS Domain ID: " + domainId); + } + } + + profile.domainId(domainId); + } + + // Check namespace and print if it was changed + { + if (!namespace.equals(DEFAULT_NAMESPACE)) + { + buildPrintout.add("Namespace: " + namespace); + } + } + + // Set up transports + { + profile.useBuiltinTransports(false); + + if (useSharedMemory) + profile.addSharedMemoryTransport(); + + if (addressRestriction != null) + buildPrintout.add("Using a programmatically set address restriction"); + else + addressRestriction = findAddressRestriction(); + + profile.addUDPv4Transport(addressRestriction); + + boolean runningInCI = (System.getenv("GITHUB_ACTIONS") != null) + || (System.getenv("RUNNING_ON_CONTINUOUS_INTEGRATION_SERVER") != null) + || (System.getProperty("runningOnCIServer") != null); + if (runningInCI && specialTransportMode == null) + { + buildPrintout.add("Detected running from CI, using INTRAPROCESS_ONLY SpecialTransportMode"); + + specialTransportMode = SpecialTransportMode.INTRAPROCESS_ONLY; + } + + if (specialTransportMode != null) + { + switch (specialTransportMode) + { + case SHARED_MEMORY_ONLY -> profile.useOnlySharedMemoryTransport(); + case UDPV4_LOOPBACK_ADDRESS_ONLY -> + { + InetAddress loopbackAddress = InetAddress.getLoopbackAddress(); + + addressRestriction = new InetAddress[] {loopbackAddress}; + } + case UDPV4_ONLY -> profile.useOnlyUDPv4Transport(addressRestriction); + case INTRAPROCESS_ONLY -> profile.useOnlyIntraProcessDelivery(); + } + } + + List addresses = new ArrayList<>(); + for (InetAddress inetAddress : addressRestriction) + addresses.add(inetAddress.getHostAddress()); + buildPrintout.add("Address restriction: " + String.join(", ", addresses)); + } + + // Print the current transports and delivery methods + { + if (specialTransportMode != null) + buildPrintout.add("Special transport mode: " + specialTransportMode.name()); + + StringBuilder printout = new StringBuilder(); + + StringJoiner transportsString = new StringJoiner(", "); + UserTransports transports = profile.getProfile().getRtps().getUserTransports(); + if (transports != null) + for (TransportDescriptorType transportDescriptorType : profile.getTransportDescriptors().getTransportDescriptor()) + if (transports.getTransportId().contains(transportDescriptorType.getTransportId())) + transportsString.add(transportDescriptorType.getType()); + + printout.append("Enabled transports: "); + printout.append(transportsString); + + buildPrintout.add(printout.toString()); + + if (profile.getLibrarySettings().getIntraprocessDelivery() != null) + { + String intraProcessMode = profile.getLibrarySettings().getIntraprocessDelivery(); + buildPrintout.add("Intra-process delivery mode: " + intraProcessMode); + } + } + + LogTools.info(buildPrintout.toString()); + + return profile; + } + + /** + * Find a value from the environment from several different places, where the places have some priority. + * Priority order: IHMCNetworkParameters.ini, Java System Property, system environment + * + * @param environmentKey Key from system environment + * @param propertiesKey Key from Java System Properties + * @param networkParametersKey Key from ~/.ihmc/IHMCNetworkParameters.ini properties file + * @return the value found for the most-prioritized key + */ + private String findValueForField(String environmentKey, String propertiesKey, String networkParametersKey) + { + Stack> possibleValues = new Stack<>(); + + if (parseEnvironment && !environmentKey.isEmpty()) + { + if (System.getenv(environmentKey) != null) + { + possibleValues.push(Map.entry(environmentKey, System.getenv(environmentKey))); + } + } + + if (parseProperties && !propertiesKey.isEmpty()) + { + if (System.getProperty(propertiesKey) != null) + { + possibleValues.push(Map.entry("-D" + propertiesKey, System.getProperty(propertiesKey))); + } + } + + if (parseNetworkParametersConfig && !networkParametersKey.isEmpty()) + { + File networkParametersFile = new File(System.getProperty("user.home"), ".ihmc/IHMCNetworkParameters.ini"); + Properties properties = new Properties(); + try (FileInputStream inputStream = new FileInputStream(networkParametersFile)) + { + properties.load(inputStream); + + if (properties.getProperty(networkParametersKey) != null) + { + possibleValues.push(Map.entry("(IHMCNetworkParameters.ini) " + networkParametersKey, properties.getProperty(networkParametersKey))); + } + } + catch (IOException e) + { + if (!(e instanceof FileNotFoundException)) + LogTools.error(e); + } + } + + if (!possibleValues.empty()) + { + StringJoiner printout = new StringJoiner(" <- "); + for (int i = possibleValues.size() - 1; i >= 0; i--) + { + Entry possibleValue = possibleValues.get(i); + printout.add(possibleValue.getKey() + "=" + possibleValue.getValue()); + } + + buildPrintout.add("Found ROS 2 property: " + printout); + } + + return !possibleValues.isEmpty() ? possibleValues.peek().getValue() : null; + } + + private int findDomainID() + { + int domainID = UNSET_DOMAIN_ID; + + String valueForField = findValueForField("ROS_DOMAIN_ID", "ros.domain.id", "RTPSDomainID"); + + if (valueForField != null) + { + try + { + domainID = Integer.parseInt(valueForField.trim()); + } + catch (NumberFormatException e) + { + LogTools.error("Unable to parse ROS Domain ID"); + } + } + + return domainID; + } + + private InetAddress[] findAddressRestriction() + { + String valueForField = findValueForField("ROS_ADDRESS_RESTRICTION", "ros.address.restriction", "RTPSSubnet"); + + return convertToInetAddressArray(valueForField != null ? valueForField : "127.0.0.1/8"); + } + + protected static boolean domainIDValid(int domainID) + { + return domainID >= 0 && domainID <= 232; + } + + /** + * Convert IP address CSV to InetAddress array + * + * @param restrictionHostString A list of IP addresses separated by comma in CIDR format. E.g. "127.0.0.1/8, 0.0.0.0/24" + * @return The array of InetAddresses representing the CSV list + */ + protected static InetAddress[] convertToInetAddressArray(String restrictionHostString) + { + Set foundAddressRestrictions = new HashSet<>(); + + String[] restrictionHostList = restrictionHostString.split("\\s*,\\s*"); + + List interfaceAddresses; + try + { + interfaceAddresses = NetworkInterface.networkInterfaces().flatMap(networkInterface -> networkInterface.getInterfaceAddresses().stream()).toList(); + } + catch (SocketException e) + { + throw new RuntimeException(e); + } + + for (String restrictionHost : restrictionHostList) + { + SubnetInfo restrictionSubnetInfo = new SubnetUtils(restrictionHost.trim()).getInfo(); + + for (InterfaceAddress interfaceAddress : interfaceAddresses) + { + InetAddress address = interfaceAddress.getAddress(); + + if (address instanceof Inet4Address) + { + short netmaskAsShort = interfaceAddress.getNetworkPrefixLength(); + + String interfaceHost = address.getHostAddress(); + SubnetInfo interfaceSubnetInfo = new SubnetUtils(interfaceHost + "/" + netmaskAsShort).getInfo(); + + boolean inRange; + if (System.getProperty("os.name").toLowerCase().contains("win")) + { + inRange = interfaceSubnetInfo.isInRange(restrictionSubnetInfo.getAddress()); // This worked on Windows, but not Linux: Doug + } + else // Linux and others + { + // This works on Linux. Does not work on Windows. Not tested on Mac. + inRange = restrictionSubnetInfo.isInRange(interfaceSubnetInfo.getAddress()); + } + + if (inRange) + { + foundAddressRestrictions.add(address); + } + } + } + } + + InetAddress[] addressRestrictions = new InetAddress[foundAddressRestrictions.size()]; + + return foundAddressRestrictions.toArray(addressRestrictions); + } +} diff --git a/ros2-library/src/main/java/us/ihmc/ros2/RealtimeROS2Node.java b/ros2-library/src/main/java/us/ihmc/ros2/RealtimeROS2Node.java index 6e7f2089..7c4e8d3e 100644 --- a/ros2-library/src/main/java/us/ihmc/ros2/RealtimeROS2Node.java +++ b/ros2-library/src/main/java/us/ihmc/ros2/RealtimeROS2Node.java @@ -1,15 +1,13 @@ package us.ihmc.ros2; -import us.ihmc.pubsub.Domain; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.pubsub.TopicDataType; import us.ihmc.pubsub.attributes.ParticipantProfile; import us.ihmc.pubsub.attributes.PublisherAttributes; -import us.ihmc.util.PeriodicNonRealtimeThreadSchedulerFactory; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import us.ihmc.util.PeriodicThreadScheduler; import us.ihmc.util.PeriodicThreadSchedulerFactory; -import java.net.InetAddress; +import javax.annotation.Nullable; import java.util.ArrayList; import java.util.concurrent.TimeUnit; import java.util.concurrent.locks.ReentrantLock; @@ -33,140 +31,21 @@ public class RealtimeROS2Node extends ROS2Node private long threadPeriod = DEFAULT_THREAD_PERIOD_MICROSECONDS; /** - * Create a new realtime ROS 2 node with non-realtime thread with the default namespace. - * - * @param name Name of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. + * Use {@link ROS2NodeBuilder} to construct + * 
{@code
+    *    RealtimeROS2Node realtimeNode = new ROS2NodeBuilder().buildRealtime("RealtimeNode");
+    * }
*/ - public RealtimeROS2Node(String name, int domainId, InetAddress... addressRestriction) + protected RealtimeROS2Node(String name, + String namespace, + ParticipantProfile attributes, + @Nullable SpecialTransportMode specialTransportMode, + PeriodicThreadSchedulerFactory threadFactory) { - this(DomainFactory.getDomain(), - new PeriodicNonRealtimeThreadSchedulerFactory(), - name, - DEFAULT_NAMESPACE, - domainId, - addressRestriction); - } - - /** - * Create a new realtime ROS 2 node with the default namespace. - * - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public RealtimeROS2Node(PeriodicThreadSchedulerFactory threadFactory, - String name, - int domainId, - InetAddress... addressRestriction) - { - this(DomainFactory.getDomain(), threadFactory, name, DEFAULT_NAMESPACE, domainId, addressRestriction); - } - - /** - * Create a new realtime ROS 2 node with the default namespace. - * - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - */ - public RealtimeROS2Node(Domain domain, PeriodicThreadSchedulerFactory threadFactory, String name) - { - this(domain, threadFactory, name, ROS2Node.DEFAULT_NAMESPACE); - } - - /** - * Create a new realtime ROS 2 node - * - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - */ - public RealtimeROS2Node(Domain domain, PeriodicThreadSchedulerFactory threadFactory, String name, String namespace) - { - this(domain, threadFactory, name, namespace, domainFromEnvironment(), useSHMFromEnvironment()); - } - - /** - * Create a new realtime ROS 2 node - * - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public RealtimeROS2Node(Domain domain, - PeriodicThreadSchedulerFactory threadFactory, - String name, - String namespace, - int domainId, - InetAddress... addressRestriction) - { - this(domain, threadFactory, name, namespace, domainId, useSHMFromEnvironment(), addressRestriction); - } - - /** - * Create a new realtime ROS 2 node - * - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param domainId Desired ROS domain ID - * @param useSharedMemory Enable shared memory transport if true - * @param addressRestriction Restrict network traffic to the given addresses. When provided, it - * should describe one of the addresses of the computer hosting this node. - * Optional. - */ - public RealtimeROS2Node(Domain domain, - PeriodicThreadSchedulerFactory threadFactory, - String name, - String namespace, - int domainId, - boolean useSharedMemory, - InetAddress... addressRestriction) - { - this(domain, threadFactory, name, namespace, createParticipantAttributes(domainId, useSharedMemory, addressRestriction)); - } - - /** - * Create a new realtime ROS 2 node - * - * @param domain DDS domain to use. Use DomainFactory.getDomain(implementation) - * @param threadFactory Thread factory for the publisher. Either - * PeriodicRealtimeThreadSchedulerFactory or - * PeriodicNonRealtimeThreadSchedulerFactory depending on the application - * @param name Name of the ROS 2 node - * @param namespace Namespace of the ROS 2 node - * @param attributes ParticipantAttributes for the domain - */ - public RealtimeROS2Node(Domain domain, PeriodicThreadSchedulerFactory threadFactory, String name, String namespace, ParticipantProfile attributes) - { - super(domain, name, namespace, attributes); + super(name, namespace, attributes, specialTransportMode); this.scheduler = threadFactory.createPeriodicThreadScheduler("RealtimeNode_" + namespace + "/" + name); } - + /** * Adjust the desired thread period from the default (1000 microseconds) * This could be useful if a faster response is desired, or to reduce load on the CPU. @@ -176,11 +55,11 @@ public void setThreadPeriod(long period, TimeUnit unit) startupLock.lock(); try { - if(spinning) + if (spinning) { throw new RuntimeException("Cannot set the thread period while the node is spinning."); } - + this.threadPeriod = period; this.threadPeriodUnit = unit; } diff --git a/ros2-library/src/main/java/us/ihmc/ros2/SubnetUtils.java b/ros2-library/src/main/java/us/ihmc/ros2/SubnetUtils.java new file mode 100644 index 00000000..f9bc0352 --- /dev/null +++ b/ros2-library/src/main/java/us/ihmc/ros2/SubnetUtils.java @@ -0,0 +1,327 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package us.ihmc.ros2; + +import java.util.regex.Matcher; +import java.util.regex.Pattern; + +/** + * A class that performs some subnet calculations given a network address and a subnet mask. + * @see "http://www.faqs.org/rfcs/rfc1519.html" + * @author + * @since 2.0 + */ +class SubnetUtils { + + private static final String IP_ADDRESS = "(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})"; + private static final String SLASH_FORMAT = IP_ADDRESS + "/(\\d{1,3})"; + private static final Pattern addressPattern = Pattern.compile(IP_ADDRESS); + private static final Pattern cidrPattern = Pattern.compile(SLASH_FORMAT); + private static final int NBITS = 32; + + private int netmask = 0; + private int address = 0; + private int network = 0; + private int broadcast = 0; + + /** Whether the broadcast/network address are included in host count */ + private boolean inclusiveHostCount = false; + + + /** + * Constructor that takes a CIDR-notation string, e.g. "192.168.0.1/16" + * @param cidrNotation A CIDR-notation string, e.g. "192.168.0.1/16" + * @throws IllegalArgumentException if the parameter is invalid, + * i.e. does not match n.n.n.n/m where n=1-3 decimal digits, m = 1-3 decimal digits in range 1-32 + */ + SubnetUtils(String cidrNotation) { + calculate(cidrNotation); + } + + /** + * Constructor that takes a dotted decimal address and a dotted decimal mask. + * @param address An IP address, e.g. "192.168.0.1" + * @param mask A dotted decimal netmask e.g. "255.255.0.0" + * @throws IllegalArgumentException if the address or mask is invalid, + * i.e. does not match n.n.n.n where n=1-3 decimal digits and the mask is not all zeros + */ + SubnetUtils(String address, String mask) { + calculate(toCidrNotation(address, mask)); + } + + + /** + * Returns true if the return value of {@link SubnetInfo#getAddressCount()} + * includes the network address and broadcast addresses. + * @since 2.2 + */ + public boolean isInclusiveHostCount() { + return inclusiveHostCount; + } + + /** + * Set to true if you want the return value of {@link SubnetInfo#getAddressCount()} + * to include the network and broadcast addresses. + * @param inclusiveHostCount + * @since 2.2 + */ + public void setInclusiveHostCount(boolean inclusiveHostCount) { + this.inclusiveHostCount = inclusiveHostCount; + } + + + + /** + * Convenience container for subnet summary information. + * + */ + public final class SubnetInfo { + private SubnetInfo() {} + + private int netmask() { return netmask; } + private int network() { return network; } + private int address() { return address; } + private int broadcast() { return broadcast; } + + private int low() { + return (isInclusiveHostCount() ? network() : + broadcast() - network() > 1 ? network() + 1 : 0); + } + + private int high() { + return (isInclusiveHostCount() ? broadcast() : + broadcast() - network() > 1 ? broadcast() -1 : 0); + } + + /** + * Returns true if the parameter address is in the + * range of usable endpoint addresses for this subnet. This excludes the + * network and broadcast adresses. + * @param address A dot-delimited IPv4 address, e.g. "192.168.0.1" + * @return True if in range, false otherwise + */ + public boolean isInRange(String address) { + return isInRange(toInteger(address)); + } + + private boolean isInRange(int address) { + int diff = address - low(); + return (diff >= 0 && (diff <= (high() - low()))); + } + + public String getBroadcastAddress() { + return format(toArray(broadcast())); + } + + public String getNetworkAddress() { + return format(toArray(network())); + } + + public String getNetmask() { + return format(toArray(netmask())); + } + + public String getAddress() { + return format(toArray(address())); + } + + /** + * Return the low address as a dotted IP address. + * Will be zero for CIDR/31 and CIDR/32 if the inclusive flag is false. + * + * @return the IP address in dotted format, may be "0.0.0.0" if there is no valid address + */ + public String getLowAddress() { + return format(toArray(low())); + } + + /** + * Return the high address as a dotted IP address. + * Will be zero for CIDR/31 and CIDR/32 if the inclusive flag is false. + * + * @return the IP address in dotted format, may be "0.0.0.0" if there is no valid address + */ + public String getHighAddress() { + return format(toArray(high())); + } + + /** + * Get the count of available addresses. + * Will be zero for CIDR/31 and CIDR/32 if the inclusive flag is false. + * @return the count of addresses, may be zero. + */ + public int getAddressCount() { + int count = broadcast() - network() + (isInclusiveHostCount() ? 1 : -1); + return count < 0 ? 0 : count; + } + + public int asInteger(String address) { + return toInteger(address); + } + + public String getCidrSignature() { + return toCidrNotation( + format(toArray(address())), + format(toArray(netmask())) + ); + } + + public String[] getAllAddresses() { + int ct = getAddressCount(); + String[] addresses = new String[ct]; + if (ct == 0) { + return addresses; + } + for (int add = low(), j=0; add <= high(); ++add, ++j) { + addresses[j] = format(toArray(add)); + } + return addresses; + } + + /** + * {@inheritDoc} + * @since 2.2 + */ + @Override + public String toString() { + final StringBuilder buf = new StringBuilder(); + buf.append("CIDR Signature:\t[").append(getCidrSignature()).append("]") + .append(" Netmask: [").append(getNetmask()).append("]\n") + .append("Network:\t[").append(getNetworkAddress()).append("]\n") + .append("Broadcast:\t[").append(getBroadcastAddress()).append("]\n") + .append("First Address:\t[").append(getLowAddress()).append("]\n") + .append("Last Address:\t[").append(getHighAddress()).append("]\n") + .append("# Addresses:\t[").append(getAddressCount()).append("]\n"); + return buf.toString(); + } + } + + /** + * Return a {@link SubnetInfo} instance that contains subnet-specific statistics + * @return new instance + */ + public final SubnetInfo getInfo() { return new SubnetInfo(); } + + /* + * Initialize the internal fields from the supplied CIDR mask + */ + private void calculate(String mask) { + Matcher matcher = cidrPattern.matcher(mask); + + if (matcher.matches()) { + address = matchAddress(matcher); + + /* Create a binary netmask from the number of bits specification /x */ + int cidrPart = rangeCheck(Integer.parseInt(matcher.group(5)), 0, NBITS); + for (int j = 0; j < cidrPart; ++j) { + netmask |= (1 << 31-j); + } + + /* Calculate base network address */ + network = (address & netmask); + + /* Calculate broadcast address */ + broadcast = network | ~(netmask); + } else { + throw new IllegalArgumentException("Could not parse [" + mask + "]"); + } + } + + /* + * Convert a dotted decimal format address to a packed integer format + */ + private int toInteger(String address) { + Matcher matcher = addressPattern.matcher(address); + if (matcher.matches()) { + return matchAddress(matcher); + } else { + throw new IllegalArgumentException("Could not parse [" + address + "]"); + } + } + + /* + * Convenience method to extract the components of a dotted decimal address and + * pack into an integer using a regex match + */ + private int matchAddress(Matcher matcher) { + int addr = 0; + for (int i = 1; i <= 4; ++i) { + int n = (rangeCheck(Integer.parseInt(matcher.group(i)), -1, 255)); + addr |= ((n & 0xff) << 8*(4-i)); + } + return addr; + } + + /* + * Convert a packed integer address into a 4-element array + */ + private int[] toArray(int val) { + int ret[] = new int[4]; + for (int j = 3; j >= 0; --j) { + ret[j] |= ((val >>> 8*(3-j)) & (0xff)); + } + return ret; + } + + /* + * Convert a 4-element array into dotted decimal format + */ + private String format(int[] octets) { + StringBuilder str = new StringBuilder(); + for (int i =0; i < octets.length; ++i){ + str.append(octets[i]); + if (i != octets.length - 1) { + str.append("."); + } + } + return str.toString(); + } + + /* + * Convenience function to check integer boundaries. + * Checks if a value x is in the range (begin,end]. + * Returns x if it is in range, throws an exception otherwise. + */ + private int rangeCheck(int value, int begin, int end) { + if (value > begin && value <= end) { // (begin,end] + return value; + } + + throw new IllegalArgumentException("Value [" + value + "] not in range ("+begin+","+end+"]"); + } + + /* + * Count the number of 1-bits in a 32-bit integer using a divide-and-conquer strategy + * see Hacker's Delight section 5.1 + */ + int pop(int x) { + x = x - ((x >>> 1) & 0x55555555); + x = (x & 0x33333333) + ((x >>> 2) & 0x33333333); + x = (x + (x >>> 4)) & 0x0F0F0F0F; + x = x + (x >>> 8); + x = x + (x >>> 16); + return x & 0x0000003F; + } + + /* Convert two dotted decimal addresses to a single xxx.xxx.xxx.xxx/yy format + * by counting the 1-bit population in the mask address. (It may be better to count + * NBITS-#trailing zeroes for this case) + */ + private String toCidrNotation(String addr, String mask) { + return addr + "/" + pop(toInteger(mask)); + } +} \ No newline at end of file diff --git a/ros2-library/src/test/java/us/ihmc/ros2/CommunicationTest.java b/ros2-library/src/test/java/us/ihmc/ros2/CommunicationTest.java index 384f1b81..234b025a 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/CommunicationTest.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/CommunicationTest.java @@ -7,48 +7,23 @@ import ros_msgs.msg.dds.TwoNum; import ros_msgs.msg.dds.TwoNumPubSubType; import us.ihmc.commons.thread.ThreadTools; -import us.ihmc.pubsub.Domain; -import us.ihmc.pubsub.DomainFactory; -import us.ihmc.util.PeriodicNonRealtimeThreadScheduler; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import java.time.Duration; public class CommunicationTest { - @Test// timeout = 5000 - public void testSimpleIntraProcessCommunication() - { - testSimpleCommunication(); - } - - @Test// timeout = 5000 - public void testSimpleRealRTPSCommunicationDefaultRosVersion() - { - testSimpleCommunication(); - } - - @Test// timeout = 5000 - public void testSimpleRealRTPSCommunicationArdent() - { - testSimpleCommunication(); - } - - @Test// timeout = 5000 - public void testSimpleRealRTPSCommunicationBouncy() - { - testSimpleCommunication(); - } - - private void testSimpleCommunication() + @Test // timeout = 5000 + public void testSimpleCommunication() { Assertions.assertTimeoutPreemptively(Duration.ofSeconds(5), () -> { + ROS2Node node = null; Pair messagesReceived = new MutablePair<>(); try { - Domain domain = DomainFactory.getDomain(); String name = "ROS2CommunicationTest"; - ROS2Node node = new ROS2Node(domain, name); + node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build(name); TwoNumPubSubType topicDataType = new TwoNumPubSubType(); ROS2Publisher publisher = node.createPublisher(topicDataType, "/chatter"); @@ -68,7 +43,7 @@ private void testSimpleCommunication() for (int i = 0; i < 11; i++) { TwoNum message = new TwoNum(); - message.getStr1().append("Hello world: " + i); + message.getStr1().append("Hello world: ").append(i); System.out.println("Publishing: " + message.getStr1()); publisher.publish(message); System.out.println("Published: " + message.getStr1()); @@ -81,6 +56,9 @@ private void testSimpleCommunication() while (messagesReceived.getValue() < 5) Thread.yield(); + + if (node != null) + node.destroy(); }); } @@ -90,10 +68,10 @@ public void testSimpleRealRTPSCommunicationAndDestroy() Assertions.assertTimeoutPreemptively(Duration.ofSeconds(5), () -> { Pair messagesReceived = new MutablePair<>(); + ROS2Node node = null; try { - Domain domain = DomainFactory.getDomain(); - ROS2Node node = new ROS2Node(domain, "ROS2CommunicationTest"); + node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build("ROS2CommunicationTest"); TwoNumPubSubType topicDataType = new TwoNumPubSubType(); ROS2Publisher publisher = node.createPublisher(topicDataType, "/chatter"); @@ -113,15 +91,13 @@ public void testSimpleRealRTPSCommunicationAndDestroy() for (int i = 0; i < 11; i++) { TwoNum message = new TwoNum(); - message.getStr1().append("Hello world: " + i); + message.getStr1().append("Hello world: ").append(i); System.out.println("Publishing: " + message.getStr1()); publisher.publish(message); System.out.println("Published: " + message.getStr1()); } ThreadTools.sleepSeconds(1.0); - - node.destroy(); } catch (Exception e) { @@ -130,6 +106,9 @@ public void testSimpleRealRTPSCommunicationAndDestroy() while (messagesReceived.getValue() < 5) Thread.yield(); + + if (node != null) + node.destroy(); }); } @@ -138,11 +117,14 @@ public void testSimpleIntraProcessCommunicationRealtime() { Assertions.assertTimeoutPreemptively(Duration.ofSeconds(5), () -> { + RealtimeROS2Node node = null; + Pair messagesReceived = new MutablePair<>(); try { - Domain domain = DomainFactory.getDomain(); - RealtimeROS2Node node = new RealtimeROS2Node(domain, PeriodicNonRealtimeThreadScheduler::new, "ROS2CommunicationTest", "/us/ihmc"); + node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY) + .namespace("/us/ihmc") + .buildRealtime("ROS2CommunicationTest"); TwoNumPubSubType topicDataType = new TwoNumPubSubType(); ROS2Publisher publisher = node.createPublisher(topicDataType, "/chatter"); @@ -155,7 +137,7 @@ public void testSimpleIntraProcessCommunicationRealtime() for (int i = 0; i < 11; i++) { TwoNum message = new TwoNum(); - message.getStr1().append("Hello world: " + i); + message.getStr1().append("Hello world: ").append(i); System.out.println("Publishing: " + message.getStr1()); boolean success = publisher.publish(message); System.out.println("Published: success: " + success + " content: " + message.getStr1()); @@ -185,6 +167,9 @@ public void testSimpleIntraProcessCommunicationRealtime() { e.printStackTrace(); } + + if (node != null) + node.destroy(); }); } } diff --git a/ros2-library/src/test/java/us/ihmc/ros2/ROS2NodeBuilderTest.java b/ros2-library/src/test/java/us/ihmc/ros2/ROS2NodeBuilderTest.java new file mode 100644 index 00000000..569904dd --- /dev/null +++ b/ros2-library/src/test/java/us/ihmc/ros2/ROS2NodeBuilderTest.java @@ -0,0 +1,32 @@ +package us.ihmc.ros2; + +import org.junit.jupiter.api.Test; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; + +import static org.junit.jupiter.api.Assertions.*; + +public class ROS2NodeBuilderTest +{ + @Test + public void createAndDestroyTest() + { + int domainId = 1; + SpecialTransportMode specialTransportMode = SpecialTransportMode.INTRAPROCESS_ONLY; + String namespace = "/test/test"; + boolean useSharedMemory = true; + String nodeName = "test_node"; + + ROS2Node node = new ROS2NodeBuilder().domainId(domainId) + .specialTransportMode(specialTransportMode) + .namespace(namespace) + .useSharedMemory(useSharedMemory) + .build(nodeName); + + assertEquals(domainId, node.getProfile().getDomainId()); + assertEquals(specialTransportMode, node.getSpecialTransportMode()); + assertEquals(namespace, node.getNamespace()); + assertEquals(nodeName, node.getName()); + + node.destroy(); + } +} diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/DataTypesTest.java b/ros2-library/src/test/java/us/ihmc/ros2/example/DataTypesTest.java index 081b4b79..2877128f 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/DataTypesTest.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/DataTypesTest.java @@ -5,9 +5,10 @@ import org.junit.jupiter.api.Test; import ros_msgs.msg.dds.Num; import ros_msgs.msg.dds.NumPubSubType; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.pubsub.common.SampleInfo; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import us.ihmc.ros2.ROS2Publisher; import static us.ihmc.robotics.Assert.assertEquals; @@ -22,7 +23,7 @@ public void testAllDoubleValuesGetAcross() int NUMBER_OF_MESSAGES_TO_SEND = 10; try { - ROS2Node node = new ROS2Node(DomainFactory.getDomain(), "ROS2CommunicationTest"); + ROS2Node node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build("ROS2CommunicationTest"); NumPubSubType topicDataType = new NumPubSubType(); ROS2Publisher publisher = node.createPublisher(topicDataType, "/chatter"); diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2ListenerExample.java b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2ListenerExample.java index e513fefc..b2917349 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2ListenerExample.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2ListenerExample.java @@ -15,8 +15,9 @@ */ package us.ihmc.ros2.example; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import java.io.IOException; @@ -34,7 +35,10 @@ public class NonRealtimeROS2ListenerExample { public static void main(String[] args) throws IOException, InterruptedException { - ROS2Node node = new ROS2Node(DomainFactory.getDomain(), "NonRealtimeROS2ChatterExample", "/us/ihmc", 112); + ROS2Node node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY) + .domainId(112) + .namespace("/us/ihmc") + .build("NonRealtimeROS2ChatterExample"); node.createSubscription(new std_msgs.msg.dds.StringPubSubType(), subscriber -> { std_msgs.msg.dds.String message = new std_msgs.msg.dds.String(); if (subscriber.takeNextData(message, null)) diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2PublishSubscribeExample.java b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2PublishSubscribeExample.java index 28738ef5..c8b0fb56 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2PublishSubscribeExample.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2PublishSubscribeExample.java @@ -1,22 +1,24 @@ /* * Copyright 2017 Florida Institute for Human and Machine Cognition (IHMC) - * + * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and - * limitations under the License. + * limitations under the License. */ package us.ihmc.ros2.example; import std_msgs.msg.dds.Int64; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.ROS2Topic; @@ -30,13 +32,12 @@ * ros2 run demo_nodes_cpp talker -- -t chatter * * @author Jesper Smith - * */ public class NonRealtimeROS2PublishSubscribeExample { public static void main(String[] args) throws IOException, InterruptedException { - ROS2Node node = new ROS2Node("NonRealtimeROS2PublishSubscribeExample"); + ROS2Node node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build("NonRealtimeROS2PublishSubscribeExample"); ROS2Topic topic = new ROS2Topic<>().withType(Int64.class).withSuffix("example"); node.createSubscription2(topic, message -> System.out.println(message.getData())); diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2TalkerExample.java b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2TalkerExample.java index 4f888424..4a66c457 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2TalkerExample.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/NonRealtimeROS2TalkerExample.java @@ -1,34 +1,34 @@ /* * Copyright 2017 Florida Institute for Human and Machine Cognition (IHMC) - * + * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and - * limitations under the License. + * limitations under the License. */ package us.ihmc.ros2.example; -import std_msgs.msg.dds.String; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import us.ihmc.ros2.ROS2Publisher; import java.io.IOException; /** * Java version of the ROS2 demo listener. - * - * To test, start a ROS2 talker using - * + * + * To test, start a ROS2 talker using + * * ros2 run demo_nodes_cpp talker -- -t chatter - * + * * @author Jesper Smith * */ @@ -36,9 +36,11 @@ public class NonRealtimeROS2TalkerExample { public static void main(String[] args) throws IOException, InterruptedException { - ROS2Node node = new ROS2Node(DomainFactory.getDomain(), "NonRealtimeROS2ChatterExample", "/us/ihmc", 112); - - ROS2Publisher publisher = node.createPublisher(new std_msgs.msg.dds.StringPubSubType(), "/chatter"); + ROS2Node node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY) + .domainId(112) + .namespace("/us/ihmc") + .build("NonRealtimeROS2ChatterExample"); + ROS2Publisher publisher = node.createPublisher(new std_msgs.msg.dds.StringPubSubType(), "/chatter"); std_msgs.msg.dds.String message = new std_msgs.msg.dds.String(); for (int i = 0; i < 1000000000; i++) { diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2IntraprocessCopyTest.java b/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2IntraprocessCopyTest.java index a9ad38ab..43ea506f 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2IntraprocessCopyTest.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2IntraprocessCopyTest.java @@ -19,8 +19,9 @@ import ros_msgs.msg.dds.BigNumSequence; import ros_msgs.msg.dds.BigNumSequencePubSubType; import ros_msgs.msg.dds.Num; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.ros2.QueuedROS2Subscription; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.util.PeriodicNonRealtimeThreadSchedulerFactory; @@ -54,7 +55,9 @@ public void testIntraprocessCopy() throws IOException, InterruptedException PeriodicThreadSchedulerFactory threadFactory = RUN_USING_REALTIME_THREAD ? // realtime threads only work on linux w/ RT kernel new PeriodicRealtimeThreadSchedulerFactory(20) : // see https://github.com/ihmcrobotics/ihmc-realtime new PeriodicNonRealtimeThreadSchedulerFactory(); // to setup realtime threads - RealtimeROS2Node node = new RealtimeROS2Node(DomainFactory.getDomain(), threadFactory, "RealtimeROS2IntraprocessCopyTest", "/us/ihmc"); + RealtimeROS2Node node = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY) + .namespace("/us/ihmc") + .buildRealtime("RealtimeROS2IntraprocessCopyTest", threadFactory); ROS2Publisher publisher = node.createPublisher(new BigNumSequencePubSubType(), "/example"); QueuedROS2Subscription subscription = node.createQueuedSubscription(new BigNumSequencePubSubType(), "/example"); diff --git a/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2PublishSubscribeExample.java b/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2PublishSubscribeExample.java index 157023d8..445107a1 100644 --- a/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2PublishSubscribeExample.java +++ b/ros2-library/src/test/java/us/ihmc/ros2/example/RealtimeROS2PublishSubscribeExample.java @@ -19,8 +19,8 @@ import std_msgs.msg.dds.Int64; import std_msgs.msg.dds.Int64PubSubType; import us.ihmc.log.LogTools; -import us.ihmc.pubsub.DomainFactory; import us.ihmc.ros2.QueuedROS2Subscription; +import us.ihmc.ros2.ROS2NodeBuilder; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.ROS2QosProfile; import us.ihmc.ros2.RealtimeROS2Node; @@ -99,9 +99,7 @@ private RealtimeROS2Node setupNode(boolean useRealtimeThread) new PeriodicRealtimeThreadSchedulerFactory(20) : new PeriodicNonRealtimeThreadSchedulerFactory(); - RealtimeROS2Node node = new RealtimeROS2Node(DomainFactory.getDomain(), - threadFactory, - "RealtimeROS2PublishSubscribeExample"); + RealtimeROS2Node node = new ROS2NodeBuilder().buildRealtime("RealtimeROS2PublishSubscribeExample", threadFactory); publisher = node.createPublisher(new Int64PubSubType(), "/example", ROS2QosProfile.KEEP_HISTORY(3), 10); subscription = node.createQueuedSubscription(new Int64PubSubType(), "/example", ROS2QosProfile.KEEP_HISTORY(3), 10);