From e2fcaada2fb20fb67a9b68ba67ec7f01b9211633 Mon Sep 17 00:00:00 2001 From: alexchunlin Date: Thu, 29 Oct 2026 18:17:43 -0400 Subject: [PATCH 1/3] test(rtps): run the ROS 2 interop pairs on CycloneDDS too The interop matrix only ran the ROS 2 pairs on rmw_fastrtps_cpp. Add rmw_cyclonedds_cpp to the harness image and run the pub/sub pairs (reliable and best-effort, both directions) again on CycloneDDS. Services are left out: rmw_cyclonedds_cpp correlates requests with a {guid, seq} header in the payload, not the related_sample_identity inline QoS espp uses. Co-Authored-By: Claude Opus 5.5 --- components/rtps/interop/Dockerfile | 5 ++- components/rtps/interop/README.md | 5 ++- components/rtps/interop/run_interop.sh | 56 ++++++++++++++++++++++++++ 3 files changed, 62 insertions(+), 4 deletions(-) diff --git a/components/rtps/interop/Dockerfile b/components/rtps/interop/Dockerfile index 338438efb8..06b952fea4 100644 --- a/components/rtps/interop/Dockerfile +++ b/components/rtps/interop/Dockerfile @@ -1,6 +1,7 @@ # 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. @@ -8,7 +9,7 @@ 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 diff --git a/components/rtps/interop/README.md b/components/rtps/interop/README.md index 0ae835e912..52b6e01bb4 100644 --- a/components/rtps/interop/README.md +++ b/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 diff --git a/components/rtps/interop/run_interop.sh b/components/rtps/interop/run_interop.sh index adb7486a64..0017a45e34 100755 --- a/components/rtps/interop/run_interop.sh +++ b/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 / From 10a76248b67086d1058dd6b7e473d1df4656f588 Mon Sep 17 00:00:00 2001 From: alexchunlin Date: Thu, 29 Oct 2026 18:17:43 -0400 Subject: [PATCH 2/3] fix(rtps): interoperate with CycloneDDS peers Four defects stopped samples between espp and a CycloneDDS participant (ROS 2 rmw_cyclonedds_cpp). FastDDS tolerates all four. - HEARTBEAT and GAP addressed to readerId ENTITYID_UNKNOWN were dropped. CycloneDDS addresses its SEDP and user-data heartbeats that way, so espp never NACKed and never received SEDP data. Resolve the reader through the matched writer, as the DATA path already does. - ACKNACKs had no INFO_DST and echoed the incoming readerId. CycloneDDS resolves the writer by (destination prefix, writerId), so it ignored them. Add INFO_DST and use the reader's own entity id. - The pre-emptive ACKNACK used sequence base 0. RTPS 8.3.5.5 requires >= 1; CycloneDDS patches base 0 only for eProsima and RTI vendor ids and logs "malformed packet received from vendor 13.37" for espp. - SEDP samples without PID_UNICAST_LOCATOR (CycloneDDS omits it) kept the TopicData constructor's placeholder address. Reset the locators before parsing and fall back to the participant's default unicast locator. Co-authored-by: ewang360 <61071212+ewang360@users.noreply.github.com> Co-Authored-By: Claude Opus 5.5 --- components/rtps/src/discovery/SEDPAgent.cpp | 29 +++++++++++++++++-- components/rtps/src/discovery/TopicData.cpp | 5 ++++ .../rtps/src/entities/StatefulReader.cpp | 29 ++++++++++++++----- .../rtps/src/messages/MessageReceiver.cpp | 20 +++++++++++-- 4 files changed, 71 insertions(+), 12 deletions(-) diff --git a/components/rtps/src/discovery/SEDPAgent.cpp b/components/rtps/src/discovery/SEDPAgent.cpp index 28ff79d867..edc3c14f2d 100644 --- a/components/rtps/src/discovery/SEDPAgent.cpp +++ b/components/rtps/src/discovery/SEDPAgent.cpp @@ -97,6 +97,29 @@ 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) { + for (const auto &candidate : remote->m_defaultUnicastLocatorList) { + if (candidate.isValid()) { + return candidate; + } + } + } + return loc; +} +} // namespace + void SEDPAgent::addUnmatchedRemoteWriter(const TopicData &writerData) { addUnmatchedRemoteWriter(TopicDataCompressed(writerData)); } @@ -196,7 +219,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 +299,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)}); } diff --git a/components/rtps/src/discovery/TopicData.cpp b/components/rtps/src/discovery/TopicData.cpp index f4d6de3e8b..83c7ffa2e8 100644 --- a/components/rtps/src/discovery/TopicData.cpp +++ b/components/rtps/src/discovery/TopicData.cpp @@ -53,6 +53,11 @@ bool TopicData::readFromBuffer(std::span 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(); diff --git a/components/rtps/src/entities/StatefulReader.cpp b/components/rtps/src/entities/StatefulReader.cpp index 2f5824a025..30332fcd67 100644 --- a/components/rtps/src/entities/StatefulReader.cpp +++ b/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}, diff --git a/components/rtps/src/messages/MessageReceiver.cpp b/components/rtps/src/messages/MessageReceiver.cpp index d51da9ace6..2a031074c4 100644 --- a/components/rtps/src/messages/MessageReceiver.cpp +++ b/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; From fab52cb0c34e558c82c16ca401f6966d446618b4 Mon Sep 17 00:00:00 2001 From: alexchunlin Date: Thu, 29 Oct 2026 18:38:26 -0400 Subject: [PATCH 3/3] refactor(rtps): std::find_if for the default-locator fallback cppcheck useStlAlgorithm on SEDPAgent.cpp:114. Co-Authored-By: Claude Opus 5.5 --- components/rtps/src/discovery/SEDPAgent.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/components/rtps/src/discovery/SEDPAgent.cpp b/components/rtps/src/discovery/SEDPAgent.cpp index edc3c14f2d..dcadc10c69 100644 --- a/components/rtps/src/discovery/SEDPAgent.cpp +++ b/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 #include #include @@ -110,10 +111,11 @@ rtps::LocatorIPv4 endpointUnicastLocator(rtps::Participant *part, const rtps::To } const rtps::ParticipantProxyData *remote = part->findRemoteParticipant(data.endpointGuid.prefix); if (remote != nullptr) { - for (const auto &candidate : remote->m_defaultUnicastLocatorList) { - if (candidate.isValid()) { - return candidate; - } + 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;