fix(rtps): interoperate with CycloneDDS peers (ROS 2 rmw_cyclonedds_cpp)

components/rtps/interop/Dockerfile

@@ -1,14 +1,15 @@
 # FastDDS / ROS 2 interop harness image for the embeddedRTPS engine.
 # ros:jazzy-ros-base ships FastDDS + rmw_fastrtps + the ros2 CLI, giving both the
-# raw-DDS and ROS 2 sides of the interop matrix in one image. The espp repo is
+# raw-DDS and ROS 2 sides of the interop matrix in one image. rmw_cyclonedds_cpp is
+# added so the ROS 2 pairs also run against CycloneDDS. The espp repo is
 # bind-mounted read-only at /work, copied to a container-local tree, and built
 # there (the repo's lib/pc install dir carries the developer's host-platform
 # artifacts and must not be clobbered with linux ones). See run_interop.sh.
 FROM ros:jazzy-ros-base
 
 RUN apt-get update && apt-get install -y --no-install-recommends \
     build-essential cmake git rsync python3-dev python3-pip python3-venv pybind11-dev \
-    ros-jazzy-example-interfaces \
+    ros-jazzy-example-interfaces ros-jazzy-rmw-cyclonedds-cpp \
     && rm -rf /var/lib/apt/lists/*
 
 # A venv with the espp wheel build backend, so run_interop.sh can build the

components/rtps/interop/README.md

@@ -1,4 +1,4 @@
-# RTPS interop harness (FastDDS / ROS 2)
+# RTPS interop harness (FastDDS, CycloneDDS / ROS 2)
 
 The Phase 0 safety net from `../REFACTOR_PLAN.md`: every refactor phase must keep
 this matrix green.
@@ -11,7 +11,7 @@ cd components/rtps/interop
 ```
 
 Requires docker. One container (`ros:jazzy-ros-base` = FastDDS + rmw_fastrtps +
-`ros2` CLI) runs everything in a single network namespace, so RTPS multicast works
+`ros2` CLI, plus rmw_cyclonedds_cpp) runs everything in a single network namespace, so RTPS multicast works
 unconditionally. The repo is bind-mounted and copied to a container-local tree
 before building (espp is installed into a container-local staging prefix and the
 pc tests `find_package` it from there), so your host build artifacts are never
@@ -27,6 +27,7 @@ touched.
 | espp_pub->ros2_echo | espp reliable writer -> ROS 2 subscriber (`rt/chatter`, `std_msgs::msg::dds_::String_`) |
 | ros2_pub->espp_sub | ROS 2 publisher -> espp reliable reader |
 | espp_be_pub->ros2_be_echo | best-effort pairing |
+| cyclone: ... | the ROS 2 pub/sub pairs (reliable and best-effort, both directions) again on `rmw_cyclonedds_cpp`. Services are not covered: rmw_cyclonedds_cpp puts a `{guid, seq}` request header in the payload instead of `related_sample_identity` |
 
 ## Notes
 

components/rtps/interop/run_interop.sh

@@ -341,6 +341,62 @@ echo "--- espp action client ---"; tail -2 /tmp/actclient.log
 echo "--- rclpy server ---"; grep -iE "order=" /tmp/rosactsrv.log | tail -2
 result "espp_action_client->ros2_server" $actclient_rc
 
+# --- CycloneDDS: the ROS 2 pairs again on rmw_cyclonedds_cpp -----------------
+# CycloneDDS is stricter than FastDDS on the wire: it addresses HEARTBEAT/GAP to
+# ENTITYID_UNKNOWN, resolves ACKNACKs through INFO_DST, rejects a pre-emptive
+# ACKNACK with base 0 from vendors other than eProsima/RTI, and leaves the
+# per-endpoint unicast locator out of SEDP. Each of these alone stops all samples.
+# Services are not run here: rmw_cyclonedds_cpp correlates requests with a
+# {guid, seq} header in the payload, not the related_sample_identity inline QoS
+# that espp's services (and rmw_fastrtps_cpp) use.
+note "CycloneDDS (rmw_cyclonedds_cpp): ROS 2 pub/sub pairs"
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ros2 daemon stop > /dev/null 2>&1
+
+"$BIN"/rtps_interop_pub rt/chatter std_msgs::msg::dds_::String_ 1 60 200 > /tmp/cpub1.log 2>&1 &
+ESPP_PID=$!
+sleep 3
+timeout 30 ros2 topic echo --once /chatter std_msgs/msg/String > /tmp/cecho1.log 2>&1
+cecho1_rc=$?
+kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null
+cat /tmp/cecho1.log
+result "cyclone: espp_pub->ros2_echo" $cecho1_rc
+
+"$BIN"/rtps_interop_sub rt/chatter std_msgs::msg::dds_::String_ 1 3 30 > /tmp/csub1.log 2>&1 &
+SUB_PID=$!
+sleep 3
+timeout 35 ros2 topic pub -r 5 /chatter std_msgs/msg/String "data: 'cyclone to espp'" > /tmp/crospub1.log 2>&1 &
+ROS_PID=$!
+wait $SUB_PID
+csub1_rc=$?
+kill $ROS_PID 2>/dev/null; wait $ROS_PID 2>/dev/null
+tail -2 /tmp/csub1.log
+result "cyclone: ros2_pub->espp_sub" $csub1_rc
+
+"$BIN"/rtps_interop_pub rt/chatter std_msgs::msg::dds_::String_ 0 60 200 > /tmp/cpub2.log 2>&1 &
+ESPP_PID=$!
+sleep 3
+timeout 30 ros2 topic echo --once --qos-reliability best_effort /chatter std_msgs/msg/String > /tmp/cecho2.log 2>&1
+cecho2_rc=$?
+kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null
+result "cyclone: espp_be_pub->ros2_be_echo" $cecho2_rc
+
+"$BIN"/rtps_interop_sub rt/chatter std_msgs::msg::dds_::String_ 0 3 30 > /tmp/csub2.log 2>&1 &
+SUB_PID=$!
+sleep 3
+timeout 35 ros2 topic pub -r 5 --qos-reliability best_effort /chatter std_msgs/msg/String "data: 'cyclone be to espp'" > /tmp/crospub2.log 2>&1 &
+ROS_PID=$!
+wait $SUB_PID
+csub2_rc=$?
+kill $ROS_PID 2>/dev/null; wait $ROS_PID 2>/dev/null
+tail -2 /tmp/csub2.log
+result "cyclone: ros2_be_pub->espp_be_sub" $csub2_rc
+
+if grep -l "malformed packet received from vendor" /tmp/c*.log > /dev/null 2>&1; then
+  echo "CycloneDDS reported malformed packets from espp:"; grep -h "malformed packet" /tmp/c*.log | head -3
+fi
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+
 # --- Python bindings: functional round-trip of every RMI/AMI mechanism --------
 # Build the espp wheel in-container and run the demo (multicast discovery works
 # here via the shared network namespace), exercising the GIL-wrapped service /

components/rtps/src/discovery/SEDPAgent.cpp

@@ -31,6 +31,7 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "rtps/messages/MessageTypes.hpp"
 #include "rtps/utils/CdrBuffer.hpp"
 #include "rtps/utils/Log.hpp"
+#include <algorithm>
 #include <mutex>
 #include <span>
 
@@ -97,6 +98,30 @@ void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) {
   }
 }
 
+namespace {
+// The unicast locator a remote endpoint's traffic goes to. FastDDS puts
+// PID_UNICAST_LOCATOR in every SEDP sample; CycloneDDS omits it and relies on
+// the participant's default unicast locator from SPDP (RTPS 2.5 8.5.3.2 allows
+// both). Without this fallback a CycloneDDS endpoint gets an invalid locator
+// and nothing is ever sent to it.
+rtps::LocatorIPv4 endpointUnicastLocator(rtps::Participant *part, const rtps::TopicData &data) {
+  rtps::LocatorIPv4 loc(data.unicastLocator);
+  if (loc.isValid()) {
+    return loc;
+  }
+  const rtps::ParticipantProxyData *remote = part->findRemoteParticipant(data.endpointGuid.prefix);
+  if (remote != nullptr) {
+    const auto &defaults = remote->m_defaultUnicastLocatorList;
+    const auto found = std::find_if(defaults.begin(), defaults.end(),
+                                    [](const auto &candidate) { return candidate.isValid(); });
+    if (found != defaults.end()) {
+      return *found;
+    }
+  }
+  return loc;
+}
+} // namespace
+
 void SEDPAgent::addUnmatchedRemoteWriter(const TopicData &writerData) {
   addUnmatchedRemoteWriter(TopicDataCompressed(writerData));
 }
@@ -196,7 +221,7 @@ void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData,
   }
 #endif
   reader->addNewMatchedWriter(
-      WriterProxy{writerData.endpointGuid, LocatorIPv4(writerData.unicastLocator),
+      WriterProxy{writerData.endpointGuid, endpointUnicastLocator(m_part, writerData),
                   (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE)});
   if (mfp_onNewPublisherCallback != nullptr) {
     mfp_onNewPublisherCallback(m_onNewPublisherArgs);
@@ -276,12 +301,12 @@ void SEDPAgent::handleSubscriptionReaderMessage(const TopicData &readerData,
 #endif
   if (readerData.multicastLocator.kind == rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) {
     writer->addNewMatchedReader(
-        ReaderProxy{readerData.endpointGuid, LocatorIPv4(readerData.unicastLocator),
+        ReaderProxy{readerData.endpointGuid, endpointUnicastLocator(m_part, readerData),
                     LocatorIPv4(readerData.multicastLocator),
                     (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)});
   } else {
     writer->addNewMatchedReader(
-        ReaderProxy{readerData.endpointGuid, LocatorIPv4(readerData.unicastLocator),
+        ReaderProxy{readerData.endpointGuid, endpointUnicastLocator(m_part, readerData),
                     (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)});
   }
 

components/rtps/src/discovery/TopicData.cpp

@@ -53,6 +53,11 @@ bool TopicData::readFromBuffer(std::span<const uint8_t> data) {
   // Reset valid flags, as the respective parameters are optional
   statusInfoValid = false;
   entityIdFromKeyHashValid = false;
+  // Locators are optional too (RTPS 2.5 8.5.3.2): a peer that omits them, as
+  // CycloneDDS does, means "use my participant's default locators". Without
+  // this reset the constructor's placeholder address would pass for a real one.
+  unicastLocator.setInvalid();
+  multicastLocator.setInvalid();
 
   while (buffer.remaining() >= 4) {
     const auto pidRaw = buffer.read<uint16_t>();

components/rtps/src/entities/StatefulReader.cpp

@@ -159,11 +159,13 @@ bool StatefulReader::onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_
     info.destPort = writer->remoteLocator.port;
     PayloadBuffer payload;
     rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix);
+    GuidPrefix_t dst = writer->remoteWriterGuid.prefix;
+    rtps::MessageFactory::addSubMessageInfoDST(payload, dst);
     SequenceNumber_t last_valid = msg.gapStart;
     --last_valid;
     auto missing_sns = writer->getMissing(writer->expectedSN, last_valid);
-    rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns,
-                                     writer->getNextAckNackCount(), false);
+    rtps::MessageFactory::addAckNack(payload, msg.writerId, m_attributes.endpointGuid.entityId,
+                                     missing_sns, writer->getNextAckNackCount(), false);
     info.payload = std::move(payload.bytes);
     if (info.payload.empty()) {
       return false;
@@ -207,12 +209,14 @@ bool StatefulReader::onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_
         info.destPort = writer->remoteLocator.port;
         PayloadBuffer payload;
         rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix);
+        GuidPrefix_t dst = writer->remoteWriterGuid.prefix;
+        rtps::MessageFactory::addSubMessageInfoDST(payload, dst);
         SequenceNumberSet set;
         set.base = writer->expectedSN;
         set.numBits = 1;
         set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31;
-        rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, set,
-                                         writer->getNextAckNackCount(), false);
+        rtps::MessageFactory::addAckNack(payload, msg.writerId, m_attributes.endpointGuid.entityId,
+                                         set, writer->getNextAckNackCount(), false);
         info.payload = std::move(payload.bytes);
         if (info.payload.empty()) {
           return false;
@@ -259,10 +263,15 @@ bool StatefulReader::onNewHeartbeat(const SubmessageHeartbeat &msg,
   info.destAddr = writer->remoteLocator.getIp4AddressBytes();
   info.destPort = writer->remoteLocator.port;
   rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix);
+  // RTPS 2.5 8.3.7.1: an ACKNACK should be preceded by INFO_DST. CycloneDDS
+  // resolves the writer by (destination prefix, writerId); without INFO_DST the
+  // prefix is null and the ACKNACK never reaches the writer's reader table.
+  GuidPrefix_t dst = writer->remoteWriterGuid.prefix;
+  rtps::MessageFactory::addSubMessageInfoDST(payload, dst);
   auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN);
   bool final_flag = (missing_sns.numBits == 0);
-  rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns,
-                                   writer->getNextAckNackCount(), final_flag);
+  rtps::MessageFactory::addAckNack(payload, msg.writerId, m_attributes.endpointGuid.entityId,
+                                   missing_sns, writer->getNextAckNackCount(), final_flag);
 
   SFR_LOG("Sending acknack base {} bits {}.", (int)missing_sns.base.low, (int)missing_sns.numBits);
   info.payload = std::move(payload.bytes);
@@ -285,9 +294,15 @@ bool StatefulReader::sendPreemptiveAckNack(const WriterProxy &writer) {
   info.destPort = writer.remoteLocator.port;
   PayloadBuffer payload;
   rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix);
+  GuidPrefix_t dst = writer.remoteWriterGuid.prefix;
+  rtps::MessageFactory::addSubMessageInfoDST(payload, dst);
+  // "Nothing received yet, send from the start": base 1 is the lowest valid
+  // sequence number. CycloneDDS validates the base and discards a base of 0
+  // (it patches that only for eProsima and RTI vendor ids); FastDDS accepts
+  // either.
   SequenceNumberSet number_set;
   number_set.base.high = 0;
-  number_set.base.low = 0;
+  number_set.base.low = 1;
   number_set.numBits = 0;
   rtps::MessageFactory::addAckNack(payload, writer.remoteWriterGuid.entityId,
                                    m_attributes.endpointGuid.entityId, number_set, Count_t{1},

components/rtps/src/messages/MessageReceiver.cpp

@@ -348,7 +348,17 @@ bool MessageReceiver::processHeartbeatSubmessage(MessageProcessingInfo &msgInfo,
   }
 
   uint32_t readerGen = 0;
-  Reader *reader = mp_part->getReader(submsgHB.readerId, readerGen);
+  Reader *reader;
+  if (submsgHB.readerId == ENTITYID_UNKNOWN) {
+    // A HEARTBEAT addressed to the destination participant rather than to one
+    // reader (readerId ENTITYID_UNKNOWN, RTPS 2.5 8.3.7.5): CycloneDDS sends its
+    // SEDP and user-data heartbeats this way. Resolve the reader through the
+    // writer it is matched to, as the DATA path does.
+    reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, submsgHB.writerId},
+                                          readerGen);
+  } else {
+    reader = mp_part->getReader(submsgHB.readerId, readerGen);
+  }
   if (reader != nullptr) {
     reader->onNewHeartbeatIfCurrent(readerGen, submsgHB, sourceState.sourceGuidPrefix);
     mp_part->refreshRemoteParticipantLiveliness(sourceState.sourceGuidPrefix);
@@ -383,7 +393,13 @@ bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo,
   }
 
   uint32_t readerGen = 0;
-  Reader *reader = mp_part->getReader(submsgGap.readerId, readerGen);
+  Reader *reader;
+  if (submsgGap.readerId == ENTITYID_UNKNOWN) {
+    reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, submsgGap.writerId},
+                                          readerGen);
+  } else {
+    reader = mp_part->getReader(submsgGap.readerId, readerGen);
+  }
   if (reader != nullptr) {
     reader->onNewGapIfCurrent(readerGen, submsgGap, sourceState.sourceGuidPrefix);
     return true;