diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index e19ccef43..257545428 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -213,6 +213,8 @@ jobs: target: esp32s3 - path: 'components/rtps/example' target: esp32 + - path: 'components/rtps_embedded/example' + target: esp32 - path: 'components/rtsp/example' target: esp32 - path: 'components/runqueue/example' diff --git a/.github/workflows/rtps_interop.yml b/.github/workflows/rtps_interop.yml index bf9487002..50795cc3e 100644 --- a/.github/workflows/rtps_interop.yml +++ b/.github/workflows/rtps_interop.yml @@ -16,6 +16,16 @@ on: - ".github/workflows/rtps_interop.yml" workflow_dispatch: +# Supersede in-progress runs: a new commit on the same PR (or the same branch for +# a manual dispatch) cancels the earlier, now-stale interop run. Keyed by the +# workflow (so it never cross-cancels other workflows) + the PR number, which is +# globally unique - unlike the head branch name, two fork PRs can't collide on it. +# Falls back to github.ref for workflow_dispatch (no PR number there). This +# workflow never runs on push to main, so cancel-in-progress is always safe here. +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + jobs: interop: runs-on: ubuntu-latest diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index 0465b1aa0..c09058553 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -54,3 +54,12 @@ if(CONFIG_RTPS_ENABLE_FRAGMENTATION) RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=262144) endif() +# The RPC layer (services + actions) is compiled in by default; the facade +# header defines RTPS_WITH_RPC unless RTPS_NO_RPC is set. When the Kconfig option +# is turned OFF, define RTPS_NO_RPC so the whole services/actions surface (and its +# std::thread/std::future use) is excluded, saving flash. ESP-only (this file is +# the ESP-IDF component build); host builds always keep RPC on. +if(NOT CONFIG_RTPS_ENABLE_RPC) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_NO_RPC) +endif() + diff --git a/components/rtps_embedded/Kconfig b/components/rtps_embedded/Kconfig index aeb1f1f18..b5ed85b36 100644 --- a/components/rtps_embedded/Kconfig +++ b/components/rtps_embedded/Kconfig @@ -57,4 +57,15 @@ menu "RTPS (rtps_embedded)" bounded by RTPS_MAX_SAMPLE_SIZE (256 KB on the embedded profile) and the participant's max payload size rises accordingly. + config RTPS_ENABLE_RPC + bool "Enable RPC: services + actions (RMI/AMI)" + default y + help + Compiles in the request/reply (services) and goal (actions) layers of + the RtpsParticipant facade - both the ROS 2-interoperable + (add_service_*/add_action_*) and native (add_native_*) variants. + Disable to drop all of that code (and its std::thread/std::future use) + when the device only needs pub/sub, saving flash. Pure pub/sub is + unaffected either way. Default ON. + endmenu diff --git a/components/rtps_embedded/README.md b/components/rtps_embedded/README.md index df92f3280..4d8fa567d 100644 --- a/components/rtps_embedded/README.md +++ b/components/rtps_embedded/README.md @@ -1,18 +1,77 @@ # rtps_embedded ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) -RTPS/DDS stack into the ESPP ecosystem. -Any platform that can build ESPP — including ESP32, Linux, and desktop PCs — -can use this component to discover and exchange typed messages with ROS 2 nodes -or any other DDS participant on the same network using the standard RTPS wire -protocol. - -The original embeddedRTPS library has hard dependencies on FreeRTOS and lwIP. -`rtps_embedded` removes those dependencies by replacing all socket, task, and -synchronisation calls with ESPP's platform-agnostic `UdpSocket`, `Task`, and -`ThreadPool` primitives. When built for ESP32, ESPP uses FreeRTOS and lwIP -under the hood; on other platforms it uses the host OS equivalents — the RTPS -code itself is unchanged in either case. +RTPS/DDS stack into the ESPP ecosystem, behind an idiomatic `espp::RtpsParticipant` +facade. Any platform that can build ESPP — ESP32, Linux, macOS, Windows — can use +it to interoperate with **ROS 2** nodes (rmw_fastrtps) or any DDS participant on +the network over the standard RTPS wire protocol. + +It provides three messaging patterns, all validated against live ROS 2: + +- **Pub/sub** — topic-based, best-effort or reliable (HEARTBEAT/ACKNACK). +- **Services (RMI)** — request/reply with correlated responses. +- **Actions (AMI)** — long-running goals with feedback, result, and cancellation. + +Each has a **typed** layer (reflectable structs, no manual bytes) and a +**byte-level** layer. Services and actions come in a ROS 2-interoperable flavour +and a lean **native** (espp ↔ espp) flavour. + +The upstream embeddedRTPS library hard-depends on FreeRTOS and lwIP; +`rtps_embedded` removes those by routing all socket, task, and synchronisation +through ESPP's platform-agnostic `UdpSocket`, `Task`, `ThreadPool`, and +`SocketReactor`. On ESP32 those map to lwIP + FreeRTOS; elsewhere to the host OS. +Micro-CDR is gone — (de)serialization uses ESPP's reflection-driven `cdr`. + +--- + +## Quick-start (typed facade) + +```cpp +#include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" // typed Publisher / Subscriber +#include "rtps_service.hpp" // typed ServiceServer / ServiceClient +#include "rtps_action.hpp" // typed ActionServer / ActionClient + +// Any reflectable struct is a message - fields map straight to CDR. +struct StringMsg { std::string data; }; +struct AddReq { int64_t a, b; }; +struct AddResp { int64_t sum; }; + +espp::RtpsParticipant participant({.interface_address = "192.168.1.10"}); +participant.start(); + +// Pub/sub +espp::Publisher pub(participant, {.topic = "rt/chatter", + .type_name = "std_msgs::msg::dds_::String_", + .reliability = espp::RtpsParticipant::Reliability::RELIABLE}); +espp::Subscriber sub(participant, {.topic = "rt/chatter", + .type_name = "std_msgs::msg::dds_::String_", + .on_message = [](const StringMsg &m) { /* use m.data */ }}); +pub.publish(StringMsg{"hello"}); + +// Service (RMI) - ros2 service call /add_two_ints ... hits this server +espp::ServiceServer server(participant, { + .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); +espp::ServiceClient client(participant, { + .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); +if (auto resp = client.call(AddReq{7, 35}, std::chrono::seconds(1))) { /* resp->sum == 42 */ } +``` + +For ROS 2 interop use ROS 2 naming: topic `rt/`, type `::msg::dds_::_`. +The full request/reply + goal APIs (including the three client call styles and the +native protocol) are documented in +[`doc/en/protocols/rtps_rmi_ami.rst`](../../doc/en/protocols/rtps_rmi_ami.rst). + +### Byte-level API + +The typed wrappers are thin layers over `espp::RtpsParticipant`'s byte-level +methods (`add_writer`/`add_reader`/`publish`, `add_service_server`/`_client`, +`add_action_server`/`_client`, and the `add_native_*` variants), which take/return +CDR-encapsulated `std::span`. Use those for dynamic types. + +Python bindings expose the same surface via the `espp` module (see +[`python/rtps_rpc_demo.py`](../../python/rtps_rpc_demo.py)). --- @@ -22,101 +81,47 @@ code itself is unchanged in either case. user code │ ▼ -rtps::Domain — routes packets to participants; owns discovery threads - │ - ├── rtps::Participant — groups writers and readers - │ ├── rtps::Writer — publishes CacheChange samples - │ └── rtps::Reader — delivers samples to a user callback +espp::RtpsParticipant — the facade: start()/stop(), add_writer/reader, + │ publish, add_service_*/add_action_* + ▼ +rtps::Domain — routes packets to participants; owns discovery │ - ├── rtps::ThreadPool — espp::ThreadPool workers that drain the four - │ incoming/outgoing meta/user traffic queues + ├── rtps::Participant — groups writers and readers + │ ├── rtps::Writer — publishes CacheChange samples + │ └── rtps::Reader — delivers samples to a user callback │ - └── rtps::EsppTransport — one espp::UdpSocket per open UDP port, - each with its own receive task + └── rtps::EsppTransport — the sole platform-specific adapter: one + espp::UdpSocket per UDP port, dispatched by an + espp::SocketReactor onto a shared espp::ThreadPool + (also used for async writer work) ``` -`EsppTransport` is the sole platform-specific adapter. It wraps ESPP's -`UdpSocket` and `Task`, which in turn map to: - | Build target | Socket backend | Task backend | |---|---|---| | ESP32 | lwIP (via ESP-IDF) | FreeRTOS | -| Linux / PC | POSIX sockets | `std::thread` | - ---- - -## Quick-start - -```cpp -#include "rtps/entities/Domain.h" - -// 1. Construct the domain with the local interface IP. -rtps::Domain domain(local_ip); - -// 2. Create a participant *before* completeInit(). -rtps::Participant *part = domain.createParticipant(); - -// 3. Add user-defined writer and reader endpoints. -rtps::Writer *writer = domain.createWriter(*part, "my/topic", - "std_msgs::msg::String", false); -rtps::Reader *reader = domain.createReader(*part, "my/topic", - "std_msgs::msg::String", false); - -// 4. Register a receive callback on the reader. -reader->registerCallback( - [](void *, const rtps::ReaderCacheChange &change) { - // process change.getData() / change.copyInto(...) - }, nullptr); - -// 5. Start discovery (SPDP/SEDP) and worker threads. -domain.completeInit(); - -// 6. Publish a sample. -const char *payload = "hello"; -writer->newChange(rtps::ChangeKind_t::ALIVE, - reinterpret_cast(payload), - static_cast(strlen(payload) + 1)); -``` +| Linux / macOS / PC | POSIX sockets | `std::thread` | -> **Note**: `createParticipant()` **must** be called before `completeInit()`. -> No new participants can be added after init is complete. +Services/actions are pure library code over pub/sub — the only wire addition is +a `related_sample_identity` inline QoS on service replies (for ROS 2 correlation). --- ## Configuration -Two built-in config headers are provided. Select one by defining -`RTPS_CONFIG_HEADER`, or let `include/rtps/config.h` pick automatically based -on the build target. +Capacity limits are chosen at build time by a **limits profile** header; storage +policy, fragmentation, and the RPC layer are separate, independent knobs. On +ESP32 these are ESP-IDF menuconfig options (`RTPS (rtps_embedded)`); on host they +default via `include/rtps/config.hpp`. -| Header | Target | -|---|---| -| [`include/rtps/config_esp32.h`](include/rtps/config_esp32.h) | ESP32 (ESP-IDF) | -| [`include/rtps/config_desktop.h`](include/rtps/config_desktop.h) | Linux / PC | - -All tunable constants follow the same layout in both files: - -| Constant | Default | Description | +| Knob | Options / default | Effect | |---|---|---| -| `DOMAIN_ID` | 0 | RTPS domain number (0–230 with UDP) | -| `MAX_NUM_PARTICIPANTS` | 1 | Participant pool size | -| `NUM_STATEFUL_WRITERS` | 5 | User writer endpoint pool | -| `NUM_STATEFUL_READERS` | 5 | User reader endpoint pool | -| `NUM_STATELESS_WRITERS` | 5 | Discovery writer endpoint pool | -| `NUM_STATELESS_READERS` | 5 | Discovery reader endpoint pool | -| `NUM_WRITERS_PER_PARTICIPANT` | 10 | Max writers per participant | -| `NUM_READERS_PER_PARTICIPANT` | 10 | Max readers per participant | -| `HISTORY_SIZE_STATEFUL` | 10 | Per-endpoint history depth | -| `THREAD_POOL_NUM_WRITERS` | 2 | Writer worker threads | -| `THREAD_POOL_NUM_READERS` | 2 | Reader worker threads | -| `THREAD_POOL_WRITER_STACKSIZE` | 4096 B | Writer task stack | -| `THREAD_POOL_READER_STACKSIZE` | 6144 B | Reader / UDP-receive task stack | -| `MAX_NUM_UDP_CONNECTIONS` | 10 | UDP socket pool size | -| `SPDP_RESEND_PERIOD_MS` | 2000 | Discovery announce period | -| `SF_WRITER_HB_PERIOD_MS` | 4000 | Reliable-writer heartbeat period | - -The `OVERALL_HEAP_SIZE` constant at the bottom of that file estimates the -total stack RAM consumed by all internal tasks. +| `RTPS_LIMITS_PROFILE` | `embedded` (default) / `host` / `host_large` | Compile-time endpoint/history capacity caps (`config_esp32.hpp` / `config_desktop.hpp` / `config_host_large.hpp`). Wire-neutral. | +| `RTPS_STORAGE_DYNAMIC` | off on ESP32 / on host | Static `std::array` history (zero-heap, drop-oldest) vs heap-backed `std::deque` (grows). Orthogonal to the profile. | +| `RTPS_ENABLE_FRAGMENTATION` | off on ESP32 / on host | DATA_FRAG for samples > ~64 KB (interoperates with FastDDS/ROS 2). | +| `RTPS_ENABLE_RPC` | on (default) | Compile in services + actions (RMI/AMI). Disable to drop that code + its threads on a pure-pub/sub device. | + +The domain id, announcement/heartbeat periods, and pool sizes live in the profile +headers. --- @@ -125,35 +130,27 @@ total stack RAM consumed by all internal tasks. | Component | Purpose | |---|---| | `base_component` | ESPP base class with integrated `espp::Logger` | -| `socket` | ESPP `UdpSocket` used by `EsppTransport` | -| `task` | ESPP `Task` for per-port UDP receive loops | -| `thread_pool` | ESPP `ThreadPool` for writer/reader workers | -| `cdr` | CDR serialization helpers | - -These components abstract away all OS and network-stack details, so -`rtps_embedded` itself has no direct dependency on FreeRTOS, lwIP, or any -other platform library. Discovery (SPDP/SEDP) parameter-list serialization is -built on the espp `cdr` component's stream primitives (see -`include/rtps/utils/CdrBuffer.hpp`); the engine carries no vendored -third-party code. +| `socket` | `UdpSocket` + `SocketReactor` used by `EsppTransport` | +| `task` | `espp::Task` / `espp::Timer` | +| `thread_pool` | shared worker pool for receive dispatch + async writer work | +| `cdr` | reflection-driven CDR/XCDR (de)serialization | + +The engine carries no vendored third-party code and has no direct dependency on +FreeRTOS, lwIP, or any platform library. --- ## Example -See [`example/`](example/) for a two-node **initiator / responder** demo. - -The same logic runs on any ESPP-supported platform. For ESP32, flash one board -as *Initiator* and a second as *Responder* via menuconfig -(`idf.py menuconfig → RTPS Example Configuration`). The initiator periodically -publishes numbered request messages; the responder echoes each message back on -the response topic. +See [`example/`](example/) — an ESP32 (esp32-ethernet-kit) node that brings up a +participant over Ethernet and exercises the typed APIs: a `Publisher`/`Subscriber` +pair, a `ServiceServer` (`/add_two_ints`) + `ActionServer` (`/fibonacci`) a ROS 2 +client can drive, and a `ServiceClient` + `ActionClient`. A `menuconfig` option +adds a second, self-testing participant. See [`example/README.md`](example/README.md). -Key menuconfig options (ESP32 example): +## Interop & tests -| Option | Description | -|---|---| -| `RTPS_EXAMPLE_ROLE` | `Initiator` or `Responder` | -| `RTPS_EXAMPLE_TOPIC_PREFIX` | Shared topic prefix (e.g. `espp/rtps_example`) | -| `RTPS_EXAMPLE_PUBLISH_PERIOD_MS` | Initiator publish interval | -| `ESP_WIFI_SSID` / `ESP_WIFI_PASSWORD` | Wi-Fi credentials | +[`interop/`](interop/) runs a dockerised FastDDS / ROS 2 (Jazzy) matrix — golden +byte-for-byte wire tests, in-process loopbacks (pub/sub, services, actions, +native, typed), and live `ros2 service call` / `ros2 action send_goal` both +directions. It is gated in CI (`.github/workflows/rtps_interop.yml`). diff --git a/components/rtps_embedded/RMI_AMI_DESIGN.md b/components/rtps_embedded/RMI_AMI_DESIGN.md new file mode 100644 index 000000000..a6d99b056 --- /dev/null +++ b/components/rtps_embedded/RMI_AMI_DESIGN.md @@ -0,0 +1,430 @@ +# RMI / AMI design: services (request/reply) and actions (goal server) + +Status: **DRAFT for review** (design-first, no code yet). +Scope: add ROS-style **services** (RMI, request/reply) and **actions** (AMI, goal +server) to the `rtps_embedded` engine, in two tracks: + +- **Track A — ROS 2 interoperable.** Byte-compatible with FastDDS / `rmw_fastrtps` + (the stack already validated by the pub/sub interop gate). Other DDS RMWs + (cyclonedds, connext) are explicitly **later**. +- **Track B — native minimal protocol.** A separate, deliberately lightweight + request/reply + action protocol for espp↔espp, tuned for embedded entity/memory + budgets. Does **not** interoperate with ROS and is not meant to. + +Guiding invariant (unchanged from the refactor): **never break the existing +FastDDS/ROS 2 pub/sub wire format.** Everything here is *additive* — plain topics +carry no new bytes. New frames get their own golden coverage and their own +interop legs. + +--- + +## 0. The one structural fact that shapes everything + +In ROS 2, **services and actions are built entirely on DDS pub/sub**: + +- A **service** = two topics (Request + Reply) + a way to correlate each reply to + its request. +- An **action** = **3 services + 2 topics**, nothing more: + - services: `send_goal`, `cancel_goal`, `get_result` + - topics: `feedback`, `status` + +So actions add **no new wire primitive**. The entire effort reduces to adding one +capability to the engine — **reliable request/reply with correlation** — and then +writing library code (client/server + an action state machine) on top. + +--- + +## 1. What the engine already provides (verified) + +| Capability | Where | Note | +|---|---|---| +| DATA / DATA_FRAG transport | L0 | request/reply payloads ride this unchanged | +| BEST_EFFORT + RELIABLE (HEARTBEAT/ACKNACK) | #660 | services use RELIABLE | +| Reader walks the inline-QoS TLV list | `MessageReceiver.cpp:258` | today it *skips* params; extracting one PID is a bounded add | +| Sender identity per sample | `ReaderCacheChange{writerGuid, sn}` (`Reader.hpp:52`) | the request's **SampleIdentity is already available server-side** — no wire change needed to read it | +| Per-build limit profiles | `RTPS_LIMITS_PROFILE` + Kconfig | services/actions get sizing knobs here | + +Gaps to fill: + +1. **Reply-side inline-QoS emission** (Track A): the writer has a + `containsInlineQos` bool but no general parameter emission. We must emit + `related_sample_identity` on reply frames. +2. **Inline-QoS parse** (Track A): extend the existing TLV skip-loop to capture + PID `0x8002`. +3. **Correlation/state layer** (both tracks): pending-request table with timeouts; + action goal state machine. +4. **Name/type mangling helpers** (Track A): `rq/`,`rr/`,`rt/` + `dds_` + + `_Request_`/`_Response_`. Today mangling is caller-side (tests pass + pre-mangled names); we add helpers, not a policy change. +5. **Facade surface** to expose sender identity to a service handler (data already + present internally; just not plumbed through `on_sample`). + +--- + +## 2. Layering + +``` +L0 RTPS pub/sub (DATA/FRAG, BEST_EFFORT + RELIABLE) [exists] + │ +L1 Request/Reply primitive + correlation [NEW — only wire work] + ├─ A: sample_identity inline QoS (rmw_fastrtps) + └─ B: compact in-band correlation header (native) + │ +L2a Service client/server API [NEW — no wire work] + │ +L2b Action client/server (state machine over L2a + topics) [NEW — no wire work] +``` + +Tracks A and B share L2 *shapes* (same C++ API ergonomics where possible) but have +distinct L1 wire encodings selected at construction / build time. + +--- + +## 3. Track A — ROS 2 interoperable (FastDDS / rmw_fastrtps) + +### 3.1 Naming & type mangling + +For service `/add_two_ints`, type `example_interfaces/srv/AddTwoInts`: + +| Endpoint | DDS topic | DDS type | +|---|---|---| +| request | `rq/add_two_intsRequest` | `example_interfaces::srv::dds_::AddTwoInts_Request_` | +| reply | `rr/add_two_intsReply` | `example_interfaces::srv::dds_::AddTwoInts_Response_` | + +Rules: strip leading `/`, prefix `rq`/`rr`, suffix `Request`/`Reply` on the topic; +`::dds_::` infix + `_Request_`/`_Response_` suffix on the type. Namespaced names +keep internal slashes (`/ns/svc` → `rq/ns/svcRequest`). Plain pub/sub topics use +`rt/` + `_` type suffix — the same rule the existing interop already relies on. + +### 3.2 Correlation (the crux) — CONFIRMED against a live capture + +Verified against a real `rmw_fastrtps` (ROS 2 **Jazzy**, Fast-RTPS vendorId 01.15) +`AddTwoInts` exchange, UDP-only, dissected with tshark. The captured bytes below +supersede the earlier `0x8002` guess. + +**The `related_sample_identity` is a 24-byte SampleIdentity carried as inline QoS +under TWO parameter IDs, both present with the identical value:** + +``` +PID 0x0083 (PID_RELATED_SAMPLE_IDENTITY, OMG DDS-RPC standard) len 24 +PID 0x800f (eProsima legacy PID_CUSTOM_RELATED_SAMPLE_IDENTITY) len 24 +PID_SENTINEL (0x0001, len 0) + +value (24 B, CDR_LE) = GUID (16) + SequenceNumber (8) + GUID = guidPrefix (12) + entityId (4) + SequenceNumber= high (int32) + low (uint32) // UNKNOWN = high=-1(0xffffffff), low=0 +``` + +Emit BOTH PIDs (for server->ROS-client compat); accept EITHER on receive. + +**Both the request AND the reply carry inline QoS** (the earlier "request has no +correlation QoS" note was wrong). Captured pair (client guidPrefix +`010feb7d6c00b8fd0000...`, reply-reader entityId `0x00001304`): + +``` +REQUEST rq/add_two_intsRequest writerSeqNumber=1 payload a=1,b=100 + 0x0083/0x800f value = 010feb7d6c00b8fd00000000 00001304 | ffffffff 00000000 + └─ client reply-reader GUID ─────┘ └ SN = UNKNOWN ┘ +REPLY rr/add_two_intsReply writerSeqNumber=1 payload sum=101 + 0x0083/0x800f value = 010feb7d6c00b8fd00000000 00001304 | 00000000 01000000 + └─ SAME client GUID ────────────┘ └ SN = 1 ───────┘ (= request's writerSeqNumber) +``` + +**Correlation algorithm (this is what espp implements):** +- espp as **SERVER**: on a request, capture (a) `related_sample_identity.guid` from + the request's inline QoS = `client_id_guid`, and (b) the request's RTPS + `writerSeqNumber` = `req_sn`. Write the reply to `rr/` with inline QoS + 0x0083 + 0x800f = `{ guid = client_id_guid, seq = req_sn }`. +- espp as **CLIENT**: write the request to `rq/` with inline QoS + `{ guid = , seq = UNKNOWN }`, and remember the RTPS + `writerSeqNumber` used. On each reply, match + `related_sample_identity.guid == my reply-reader GUID` **and** + `related_sample_identity.seq == pending writerSeqNumber`. + +Reply is **broadcast** on the shared `rr/` topic; every client's reply reader +receives it and filters as above — no directed addressing, pure pub/sub with +client-side filtering, which fits the engine natively. Payload encapsulation is +CDR_LE (0x0001); services are RELIABLE (HEARTBEAT/ACKNACK observed). + +Engine impact: the reader already exposes the request's `{writerGuid, sn}` via +`ReaderCacheChange`, but correlation needs the inline-QoS **guid** too, so the +reader must additionally capture the 0x0083/0x800f parameter (extend the existing +inline-QoS TLV loop) and surface it on `ReaderCacheChange`. + +> Still to verify before M1 lands (non-blocking for the wire format): whether +> SEDP needs a type hash / `TypeInformation` for `ros2 service list` and matching, +> or plain topic+type-name match suffices. Test against a live node. + +### 3.3 Service API (L2a) + +```cpp +// Server +participant.add_service_server({ + .service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts", // base; _Request_/_Response_ derived + .on_request = [](RequestId id, std::span req_cdr) -> std::vector { + return make_response_cdr(...); // returned bytes → reply + }, +}); + +// Client +auto client = participant.add_service_client({ .service = "/add_two_ints", + .type_name = "...AddTwoInts" }); +client.call_async(req_cdr, [](CallResult r, std::span resp_cdr){ ... }); +auto resp = client.call(req_cdr, 1s); // optional>, nullopt on timeout +``` + +`RequestId` wraps `{Guid_t writerGuid, SequenceNumber_t sn}` (the sample identity). +Reliability defaults to RELIABLE for services. + +### 3.4 Actions (L2b) — composition, no wire work + +Action `/fibonacci`, type `.../Fibonacci` expands to the standard 5 endpoints: + +| Kind | DDS name (mangled) | Payload | +|---|---|---| +| service send_goal | `rq/fibonacci/_action/send_goalRequest` … | `{goal_id: UUID(16), goal}` → `{accepted: bool, stamp}` | +| service cancel_goal | `…/cancel_goal…` | `action_msgs/srv/CancelGoal` | +| service get_result | `…/get_result…` | `{goal_id}` → `{status: int8, result}` | +| topic feedback | `rt/fibonacci/_action/feedback` | `{goal_id, feedback}` | +| topic status | `rt/fibonacci/_action/status` | `action_msgs/msg/GoalStatusArray` | + +Goal states (`action_msgs/msg/GoalStatus`): `UNKNOWN=0, ACCEPTED=1, EXECUTING=2, +CANCELING=3, SUCCEEDED=4, CANCELED=5, ABORTED=6`. The action server is a state +machine driving these across the 3 services + status topic; the action client +mirrors it. All of this is library code over L2a + L0. + +**Wire format CONFIRMED** against a live `example_interfaces/action/Fibonacci` +capture (ROS 2 Jazzy, order=5, tshark; goal UUID `93beb052…d15de68e`). All CDR_LE, +after the 4-byte encapsulation header. The 3 services correlate exactly like §3.2 +(related_sample_identity); actions add NO new wire primitive. Endpoint mangling: +services `rq|rr//_action/{send_goal,cancel_goal,get_result}{Request,Reply}`, +topics `rt//_action/{feedback,status}`; types +`::action::dds_::_{SendGoal,GetResult}_{Request,Response}_`, +`_FeedbackMessage_`, and `action_msgs::{srv::dds_::CancelGoal_*, msg::dds_::GoalStatusArray_}`. + +| Message | CDR layout (post-encap) | captured bytes | +|---|---|---| +| SendGoal_Request | `goal_id:UUID(16)` + goal | `…UUID… 05000000` (order=5) | +| SendGoal_Response | `accepted:bool(1)`+pad(3) + `stamp{sec:i32,nsec:u32}` | `01000000 a3927e6a dc014325` | +| GetResult_Request | `goal_id:UUID(16)` | `…UUID…` | +| GetResult_Response | `status:i8(1)`+pad(3) + result | `04000000 06000000 00.. 01.. 01.. 02.. 03.. 05..` (SUCCEEDED, [0,1,1,2,3,5]) | +| FeedbackMessage | `goal_id:UUID(16)` + feedback | `…UUID… 03000000 00.. 01.. 01..` (seq len 3) | +| GoalStatusArray | `status_list[]{goal_id:UUID(16), stamp{sec,nsec}, status:i8+pad}` | `01000000 …UUID… a3927e6a 298f4425 01000000` (1 entry, ACCEPTED→…) | + +UUID = 16 raw bytes (`unique_identifier_msgs/UUID`), identical across all messages +for one goal - the correlation key for feedback/status/get_result. Arrays are +length-prefixed (uint32) then elements; a leading array count of 1 in +GoalStatusArray is the sequence length. + +```cpp +participant.add_action_server({ + .action = "/fibonacci", .type_name = ".../Fibonacci", + .on_goal = [](GoalId, std::span goal) -> GoalResponse { return ACCEPT; }, + .on_cancel = [](GoalId) -> CancelResponse { return ACCEPT; }, + .execute = [](GoalHandle h) { + h.publish_feedback(fb_cdr); + h.succeed(result_cdr); // → get_result reply + terminal status + }, +}); +``` + +### 3.5 Engine wire additions (Track A total) + +1. `MessageFactory`: emit an inline-QoS ParameterList (`0x8002` + sentinel) on a + nominated DATA/reply. Additive; gated so plain topics are byte-identical. +2. `MessageReceiver`: in the existing TLV loop, capture `0x8002` into the + `ReaderCacheChange` (extend struct with an optional `relatedSampleIdentity`). +3. Facade: expose sender `RequestId` to service handlers. + +That is the **entire** new wire surface. Everything else is L2 library code. + +--- + +## 4. Track B — native minimal protocol (espp↔espp) + +Deliberately a **separate** protocol (per decision): no ROS mangling, no inline-QoS +machinery, no UUIDs, minimal entity count. It still rides the **same L0 RTPS +transport** (DATA + RELIABLE) — "separate" means the correlation/semantics layer, +not a new transport. + +### 4.1 Principles + +- Correlation in a **compact in-band header** prepended to the CDR payload — no + inline-QoS emit path needed (just prepend bytes), the simplest possible impl. +- `uint32` ids instead of 16-byte UUIDs / 24-byte sample identities. +- Collapse action's 5 endpoints toward ~3. +- Everything sizeable via `RTPS_LIMITS_PROFILE` (static alloc on esp32). + +### 4.2 Native request/reply wire (in-band header) + +Reply and request DATA payloads begin with a fixed 20-byte header, then the CDR +body: + +``` +offset 0 target_prefix : 12 bytes // RTPS GuidPrefix of the intended peer +offset 12 request_id : uint32 // client-monotonic +offset 16 op : uint8 // REQUEST=0, REPLY=1, ERROR=2, CANCEL=3, ... +offset 17 flags : uint8 +offset 18 reserved : uint16 +offset 20 +``` + +- Request: `target_prefix` = server prefix (from discovery), `op=REQUEST`. +- Reply: `target_prefix` = the requesting client's prefix (learned for free from + the request's RTPS source), `op=REPLY`. +- A peer accepts a frame iff `target_prefix == myGuidPrefix`; the client then + matches `request_id` against its pending table. Shared reply topic, client-side + filter — same pattern as Track A but with a 16-byte in-band key instead of a + 24-byte inline-QoS sample identity, and no PID machinery. + +Two topics per service: `es_rq/` and `es_rr/` (prefix distinguishes them +from ROS `rq/`/`rr/`, so the two protocols never alias on a shared bus). + +### 4.3 Native action wire — collapse the endpoint explosion + +| ROS (Track A) | Native (Track B) | +|---|---| +| send_goal service (2 topics) | goal service `es_rq/` + `es_rr/` (2) | +| cancel_goal service (2 topics) | cancel folded into the goal service via `op=CANCEL` (0) | +| get_result service (2 topics) | result delivered as a terminal feedback msg (0) | +| feedback topic (1) | feedback topic `es_fb/` (1) | +| status topic (1) | status folded into feedback `status` field (0) | +| **≈10 endpoints / pair** | **≈3 endpoints one-way, ~4–6 / pair** | + +Native feedback message: + +``` +{ goal_handle: uint32, status: uint8, seq: uint32, payload: } +``` + +`status` reuses the ROS state enum values for conceptual parity. A terminal status +(`SUCCEEDED/ABORTED/CANCELED`) carries the result in `payload`; no separate +get_result round-trip. + +```cpp +auto h = participant.add_native_action_client({ .action = "grip" }); +h.send_goal(goal_cdr, + on_feedback = [](uint8_t status, std::span fb){...}, + on_result = [](uint8_t status, std::span res){...}); +h.cancel(); +``` + +### 4.4 Budget vs ROS (the whole point) + +- A ROS action client+server pair ≈ **~10 DDS endpoints**, each with its own + history cache + proxy set under static allocation. +- Native pair ≈ **~4–6**, no UUID/GoalStatusArray types, 20-byte header vs + inline-QoS + wrapper messages. Concrete esp32 RAM/entity savings, selectable per + build. + +--- + +## 5. How the two tracks coexist in the codebase + +- One request/reply **core** (pending table, timeout, ret/ack) parameterized by an + **encoding strategy**: `RosSampleIdentity` (A) vs `NativeInbandHeader` (B). +- L2 service/action classes templated/injected on the strategy so the client/server + logic and the action state machine are written **once**. +- Public API: `add_service_server` / `add_service_client` / + `add_action_server` / `add_action_client` with a `Wire::Ros | Wire::Native` + selector (default `Ros` on host, `Native` where interop isn't compiled). esp32 + can compile out Track A entirely (Kconfig), like fragmentation. +- Topic-prefix disjointness (`rq/`/`rr/` vs `es_rq/`/`es_rr/`) means both can run on + one bus without aliasing. + +--- + +## 6. esp32 considerations + +- New Kconfig: `RTPS_ENABLE_SERVICES`, `RTPS_ENABLE_ACTIONS`, + `RTPS_ENABLE_ROS_RPC` (Track A), each opt-out-able; native-only build drops all + ROS mangling/inline-QoS code. +- Limit knobs in the profiles: max concurrent services, max in-flight requests per + client, pending-table depth, per-goal state slots. Embedded profile static; + host/host_large dynamic (reuse the `StorageArray` policy). +- Timeouts + pending-table eviction must be bounded/static on embedded (no + unbounded growth from lost replies). + +--- + +## 7. Testing & gates (extends the existing discipline) + +1. **Golden** — new byte-for-byte captures for: a Track-A reply frame with the + `0x8002` inline QoS; a Track-B request/reply header. Never regenerate existing + golden. +2. **Docker interop matrix** — new legs: + - espp service **server** ↔ `ros2 service call` client + - espp service **client** ↔ rclpy service server + - espp action **server** ↔ `ros2 action send_goal` (feedback + result + cancel) + - espp action **client** ↔ rclpy action server +3. **Host loopback** — Track-B service + action request/reply/cancel/feedback, + in-process, byte-exact + no-skip under concurrency (mirrors `rtps_facade_backlog`). +4. **esp32 build** — services on/off, actions on/off, ROS-RPC on/off, frag on/off. + +--- + +## 8. Milestones (stacked PRs) + +1. **M1 — Track A request/reply core. ✅ DONE** (branch `feat/rtps-services`). + - M1.1 `rpc/service_naming.hpp` mangling + host test (7/7). + - M1.2 `rpc/sample_identity.hpp` + `addSubMessageDataWithRelatedSampleIdentity` + emit + `MessageReceiver` parse into `ReaderCacheChange`; golden section + `data_related_sample_identity` (all prior golden bytes unchanged). + - M1.3 send-path plumbing: `CacheChange` carries the identity; both writers + branch to the RSI emit; plain pub/sub byte-identical. + - M1.4 facade `add_service_server` / `add_service_client` + `ServiceClient` + (sync `call` + `call_async`), pending-request correlation; in-process + `rtps_service_loopback`. + - M1.5 live ROS 2 interop **both directions** (`ros2 service call` -> espp, + and espp client -> rclpy server). Final gate: interop **16/16**. + The actual correlation is inline QoS PIDs **0x0083 + 0x800f** (not 0x8002); + both request and reply carry it. See §3.2. +2. **M2 — Track A actions. ✅ DONE** (branch `feat/rtps-services`). + - M2.1 `rpc/action_naming.hpp` mangling (host test 12/12). + - M2.2 `rpc/action_types.hpp` envelope codec, byte-exact vs Fibonacci capture (11/11). + - M2.3 facade: deferred-reply service extension (`ServiceResponder` + + `add_service_server_deferred`, needed because get_result holds the request + until the goal finishes) + `add_action_server`/`add_action_client`, + `ActionGoalHandle` (publish_feedback/succeed/abort/canceled), goal + correlation by UUID. + - M2.4 in-process `rtps_action_loopback` (Fibonacci: feedback + deferred result). + - M2.5 live ROS 2 interop **both directions** (`ros2 action send_goal` -> espp; + espp client -> rclpy server). Final gate: interop **22/22**. No type-hash + needed for actions either (`ros2 action list` shows the espp action). + The client offers all three call styles on services (sync/callback/future). +3. **M3 — Track B native RMI/AMI. ✅ DONE.** In-band 20-byte header over pub/sub + (`rpc/native_protocol.hpp`); `add_native_service_*` (sync/async/future) + + `add_native_action_*` (lean: 1 goal service + 1 feedback topic carrying the + result, ~3 endpoints vs ROS's ~10). Host loopbacks + `rtps_native_service_loopback` / `rtps_native_action_loopback`. +4. **M4 — Consolidation. ✅ DONE (examples/docs/compile-out).** + - Python bindings for every RMI/AMI API (`lib/python_bindings/rtps_bindings.cpp`) + + `python/rtps_rpc_demo.py` (all four mechanisms, 5/5). + - Docs: `doc/en/protocols/rtps_rmi_ami.rst` (function, use, rationale). + - Kconfig compile-out: `RTPS_ENABLE_RPC` (default y) → `RTPS_NO_RPC` excludes + the whole services/actions layer; esp32 builds both ways, host keeps it on. + - Not done (deliberately deferred): the "shared L2 over both strategies" + refactor — the ROS and native paths already share the pub/sub + service + primitives; a further template-unification is cosmetic and higher-risk, left + as a follow-up. + +All milestones M1–M4 are implemented and gated (interop 24/24, esp32 on/off, +Python demo 5/5) on branch `feat/rtps-services`. + +--- + +## 9. Open verification items + +- [x] Live `rmw_fastrtps` service capture → **done** (§3.2). Correlation is inline + QoS PIDs **0x0083 + 0x800f** (24-byte SampleIdentity, CDR_LE), on BOTH + request and reply. Corrected the earlier 0x8002 / no-request-QoS guesses. +- [x] Target ROS distro → **Jazzy** (Fast-RTPS vendorId 01.15). +- [x] Does SEDP need service-specific discovery attributes (type hash, + `TypeInformation`) for `ros2 service list` / matching? **Answered: NO** (M1.5). + Plain rq/rr topic + `_Request_`/`_Response_` type-name matching suffices; + the espp service even appears in `ros2 service list` and a live + `ros2 service call` succeeds against it, with no type-hash exchange. +- [ ] Confirm `action_msgs` / `unique_identifier_msgs` CDR layouts (UUID = 16 raw + bytes; GoalStatus/GoalStatusArray) for Track A actions (M2). +- [ ] Decide default `Wire` per platform and whether Track A is default-off on esp32. diff --git a/components/rtps_embedded/example/README.md b/components/rtps_embedded/example/README.md new file mode 100644 index 000000000..3323e6134 --- /dev/null +++ b/components/rtps_embedded/example/README.md @@ -0,0 +1,67 @@ +# RTPS (embedded) Example + +This example brings up an `espp::RtpsParticipant` on an **ESP32-Ethernet-Kit** and +demonstrates every typed API the `rtps_embedded` component offers, interoperable +with FastDDS / ROS 2 over the standard RTPS wire protocol. + +It demonstrates: + +- Ethernet bring-up (DHCP **server** on `192.168.4.1/24`, so a directly-attached + PC gets an address) and starting a participant on the interface's IPv4 address +- a typed `Publisher` / `Subscriber` pair (reliable pub/sub) + that pairs with the FastDDS host peer in [`pc/host_pubsub.cpp`](pc/host_pubsub.cpp) +- a typed **service server** (`/add_two_ints`) and **action server** + (`/fibonacci`) the device hosts — a ROS 2 client can drive them directly with + `ros2 service call` / `ros2 action send_goal` (no manual CDR; reflectable + `AddReq`/`AddResp`, `FibGoal`/`FibSeq` structs) +- a typed **service client** + **action client** that call a peer's + `/peer_add_two_ints` / `/peer_fib` (run a ROS 2 / rclpy server for those names + to see a full round-trip; otherwise the calls simply time out, still exercising + the client API) + +All of the RMI/AMI code is compiled out when `RTPS_ENABLE_RPC` is disabled. + +## How to use example + +### Configure + +```bash +idf.py menuconfig +``` + +Under **RTPS Example Configuration**: + +| Option | Description | +|---|---| +| `RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS` | Period of the outgoing publisher (default 1500 ms). | +| `RTPS_EXAMPLE_SECOND_PARTICIPANT` | Additively bring up a second, self-testing participant that calls the device's own `/add_two_ints` + `/fibonacci` for a full on-device round-trip (default off; roughly doubles the RTPS engine RAM). | + +Under **RTPS (rtps_embedded)** you can also toggle the limits profile, dynamic +storage, DATA_FRAG fragmentation, and the RPC (services + actions) layer. + +### Build and Flash + +```bash +idf.py -p PORT flash monitor +``` + +Replace `PORT` with the serial port. Connect the board's Ethernet port to a PC +(or a switch) so the participant has a network. + +### Talk to it + +- **Pub/sub host peer** (FastDDS): build and run [`pc/host_pubsub.cpp`](pc/) + against the board's topics. +- **ROS 2**: with `example_interfaces` installed and on the same network/domain: + ```bash + ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}" + ros2 action send_goal -f /fibonacci example_interfaces/action/Fibonacci "{order: 5}" + ``` + +## Expected Output + +The monitor logs Ethernet link-up + the assigned IP, then `tx`/`rx` lines for the +publisher/subscriber and `service '/add_two_ints' + action '/fibonacci' ready`. +Each ROS 2 call logs the handled request/goal (e.g. `service add_two_ints: 7 + 35 += 42`, `action fibonacci(5) done`). With the second participant enabled, `[self-test]` +lines report `PASS`/`FAIL` for the local round-trips. diff --git a/components/rtps_embedded/example/main/Kconfig.projbuild b/components/rtps_embedded/example/main/Kconfig.projbuild index ea5538359..1843ad271 100644 --- a/components/rtps_embedded/example/main/Kconfig.projbuild +++ b/components/rtps_embedded/example/main/Kconfig.projbuild @@ -7,4 +7,19 @@ menu "RTPS Example Configuration" help Period between outgoing messages published by the MCU. + config RTPS_EXAMPLE_SECOND_PARTICIPANT + bool "Add a second (self-test) participant that calls the local servers" + default n + depends on RTPS_ENABLE_RPC + help + Additively bring up a SECOND RtpsParticipant on the device, with its + own typed ServiceClient + ActionClient, that call THIS device's own + /add_two_ints service and /fibonacci action. A participant filters out + its own messages, so this is the only way to fully round-trip the + client APIs on one device with no external peer - a self-contained + self-test. Off by default: a second participant roughly doubles the + RTPS engine's RAM (two discovery stacks, socket sets, and pools), which + may not fit on a plain ESP32 without PSRAM. Independent of the + peer-facing client demo, which always runs. + endmenu diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp deleted file mode 100644 index a72f588dd..000000000 --- a/components/rtps_embedded/example/main/main.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include -#include - -#include "esp32-ethernet-kit.hpp" - -#include "logger.hpp" -#include "rtps_participant.hpp" -#include "rtps_pubsub.hpp" -#include "timer.hpp" - -using namespace std::chrono_literals; - -// std_msgs/msg/String as a plain reflectable struct. The typed Publisher / -// Subscriber serialize any such struct to the DDS wire format (ROS 2 / classic -// CDR) with no manual (de)serialization in application code. -struct StringMsg { - std::string data; -}; - -extern "C" void app_main(void) { - espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); - - //! [rtps example] - // Bring up Ethernet (DHCP server on 192.168.4.1/24 so a directly-attached PC - // gets an address); any espp network interface works - the RTPS participant - // only needs the interface's IPv4 address. - auto &board = espp::Esp32EthernetKit::get(); - bool eth_ok = board.initialize_ethernet({ - .mode = espp::Esp32EthernetKit::DhcpMode::SERVER, - .on_link_up = [&]() { logger.info("Ethernet link up"); }, - .on_link_down = [&]() { logger.warn("Ethernet link down"); }, - }); - if (!eth_ok) { - logger.error("Ethernet initialization failed"); - return; - } - logger.info("Waiting for Ethernet link..."); - while (!board.is_ethernet_connected()) { - std::this_thread::sleep_for(100ms); - } - auto eth_ip = board.ethernet_ip(); - const std::string interface_address = - fmt::format("{}.{}.{}.{}", esp_ip4_addr1_16(ð_ip), esp_ip4_addr2_16(ð_ip), - esp_ip4_addr3_16(ð_ip), esp_ip4_addr4_16(ð_ip)); - logger.info("Ethernet up, IP {}", interface_address); - - // RTPS/DDS participant (embeddedRTPS engine behind the espp facade). The - // topics pair with the FastDDS host peer in example/pc/host_pubsub.cpp; for - // ROS 2 instead, use topic "rt/" with type "::msg::dds_::_" - // (e.g. "rt/chatter" + "std_msgs::msg::dds_::String_"). - constexpr const char *pub_topic = "mcu_to_pc"; - constexpr const char *sub_topic = "pc_to_mcu"; - constexpr const char *type_name = "std_msgs::msg::String"; - - // Automatic locals: they RAII-clean up in reverse order on any early return - // (subscriber/publisher stop referencing the participant before it is - // destroyed), and the trailing while(true) keeps them alive in normal use. - espp::RtpsParticipant participant({ - .interface_address = interface_address, - .on_publisher_matched = [&]() { logger.info("publisher matched a remote reader"); }, - .on_subscriber_matched = [&]() { logger.info("subscriber matched a remote writer"); }, - .log_level = espp::Logger::Verbosity::INFO, - }); - if (!participant.start()) { - logger.error("Failed to start the RTPS participant"); - return; - } - - // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ - // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. - using Reliability = espp::RtpsParticipant::Reliability; - espp::Publisher publisher(participant, { - .topic = pub_topic, - .type_name = type_name, - .reliability = Reliability::RELIABLE, - }); - // Typed subscriber: receive StringMsg structs directly. - espp::Subscriber subscriber( - participant, { - .topic = sub_topic, - .type_name = type_name, - .on_message = [&](const StringMsg &msg) { logger.info("rx: {}", msg.data); }, - }); - if (!publisher.is_valid() || !subscriber.is_valid()) { - logger.error("Failed to create the typed publisher/subscriber"); - return; - } - - // Publish a counter periodically via the typed publisher. - uint32_t counter = 0; - espp::Timer publish_timer({ - .name = "rtps_pub", - .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), - .callback = - [&]() { - if (publisher.publish(StringMsg{fmt::format("msg {}", counter++)})) { - logger.info("tx: msg {}", counter - 1); - } else { - logger.warn("tx dropped (history full)"); - } - return false; // keep the timer running - }, - .log_level = espp::Logger::Verbosity::WARN, - }); - logger.info("started: pub='{}' sub='{}' type='{}'", pub_topic, sub_topic, type_name); - //! [rtps example] - - while (true) { - std::this_thread::sleep_for(1s); - } -} diff --git a/components/rtps_embedded/example/main/rtps_embedded_example.cpp b/components/rtps_embedded/example/main/rtps_embedded_example.cpp new file mode 100644 index 000000000..d39fed5e3 --- /dev/null +++ b/components/rtps_embedded/example/main/rtps_embedded_example.cpp @@ -0,0 +1,283 @@ +// rtps_embedded component example (ESP32 / esp32-ethernet-kit). +// +// Brings up an espp::RtpsParticipant over Ethernet and demonstrates the typed +// APIs: a Publisher/Subscriber pair (pub/sub), typed ServiceServer + +// ActionServer the device hosts, and typed ServiceClient + ActionClient the +// device runs. See components/rtps_embedded/example/README.md and the RMI/AMI +// docs (doc/en/protocols/rtps_rmi_ami.rst). + +#include +#include +#include + +#include "esp32-ethernet-kit.hpp" + +#include "logger.hpp" +#include "rtps_action.hpp" +#include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" +#include "rtps_service.hpp" +#include "timer.hpp" + +using namespace std::chrono_literals; + +// std_msgs/msg/String as a plain reflectable struct. The typed Publisher / +// Subscriber serialize any such struct to the DDS wire format (ROS 2 / classic +// CDR) with no manual (de)serialization in application code. +struct StringMsg { + std::string data; +}; + +// Reflectable request/reply + goal/result structs for the typed service + action +// servers below. Their fields map straight to CDR, matching example_interfaces +// so a ROS 2 client (ros2 service call / ros2 action send_goal) can drive them. +struct AddReq { + int64_t a; + int64_t b; +}; +struct AddResp { + int64_t sum; +}; +struct FibGoal { + int32_t order; +}; +struct FibSeq { + std::vector sequence; +}; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + + //! [rtps example] + // Bring up Ethernet (DHCP server on 192.168.4.1/24 so a directly-attached PC + // gets an address); any espp network interface works - the RTPS participant + // only needs the interface's IPv4 address. + auto &board = espp::Esp32EthernetKit::get(); + bool eth_ok = board.initialize_ethernet({ + .mode = espp::Esp32EthernetKit::DhcpMode::SERVER, + .on_link_up = [&]() { logger.info("Ethernet link up"); }, + .on_link_down = [&]() { logger.warn("Ethernet link down"); }, + }); + if (!eth_ok) { + logger.error("Ethernet initialization failed"); + return; + } + logger.info("Waiting for Ethernet link..."); + while (!board.is_ethernet_connected()) { + std::this_thread::sleep_for(100ms); + } + auto eth_ip = board.ethernet_ip(); + const std::string interface_address = + fmt::format("{}.{}.{}.{}", esp_ip4_addr1_16(ð_ip), esp_ip4_addr2_16(ð_ip), + esp_ip4_addr3_16(ð_ip), esp_ip4_addr4_16(ð_ip)); + logger.info("Ethernet up, IP {}", interface_address); + + // RTPS/DDS participant (embeddedRTPS engine behind the espp facade). The + // topics pair with the FastDDS host peer in example/pc/host_pubsub.cpp; for + // ROS 2 instead, use topic "rt/" with type "::msg::dds_::_" + // (e.g. "rt/chatter" + "std_msgs::msg::dds_::String_"). + constexpr const char *pub_topic = "mcu_to_pc"; + constexpr const char *sub_topic = "pc_to_mcu"; + constexpr const char *type_name = "std_msgs::msg::String"; + + // Automatic locals: they RAII-clean up in reverse order on any early return + // (subscriber/publisher stop referencing the participant before it is + // destroyed), and the trailing while(true) keeps them alive in normal use. + espp::RtpsParticipant participant({ + .interface_address = interface_address, + .on_publisher_matched = [&]() { logger.info("publisher matched a remote reader"); }, + .on_subscriber_matched = [&]() { logger.info("subscriber matched a remote writer"); }, + .log_level = espp::Logger::Verbosity::INFO, + }); + if (!participant.start()) { + logger.error("Failed to start the RTPS participant"); + return; + } + + // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ + // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. + using Reliability = espp::RtpsParticipant::Reliability; + espp::Publisher publisher(participant, { + .topic = pub_topic, + .type_name = type_name, + .reliability = Reliability::RELIABLE, + }); + // Typed subscriber: receive StringMsg structs directly. + espp::Subscriber subscriber( + participant, { + .topic = sub_topic, + .type_name = type_name, + .on_message = [&](const StringMsg &msg) { logger.info("rx: {}", msg.data); }, + }); + if (!publisher.is_valid() || !subscriber.is_valid()) { + logger.error("Failed to create the typed publisher/subscriber"); + return; + } + + // Publish a counter periodically via the typed publisher. + uint32_t counter = 0; + espp::Timer publish_timer({ + .name = "rtps_pub", + .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), + .callback = + [&]() { + if (publisher.publish(StringMsg{fmt::format("msg {}", counter++)})) { + logger.info("tx: msg {}", counter - 1); + } else { + logger.warn("tx dropped (history full)"); + } + return false; // keep the timer running + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + logger.info("started: pub='{}' sub='{}' type='{}'", pub_topic, sub_topic, type_name); + +#ifdef RTPS_WITH_RPC + // Typed service (RMI) server: a ROS 2 client can `ros2 service call + // /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}"` and get 42. + // No manual CDR - the reflectable AddReq/AddResp structs are (de)serialized for + // us. (Compiled out when CONFIG_RTPS_ENABLE_RPC is disabled.) + espp::ServiceServer add_service( + participant, { + .service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = + [&](const AddReq &r) { + logger.info("service add_two_ints: {} + {} = {}", r.a, r.b, r.a + r.b); + return AddResp{r.a + r.b}; + }, + }); + + // Typed action (AMI) server: a ROS 2 client can `ros2 action send_goal + // /fibonacci example_interfaces/action/Fibonacci "{order: 5}"` and receive + // feedback + the [0,1,1,2,3,5] result. execute() runs on its own thread. + espp::ActionServer fib_action( + participant, { + .action = "/fibonacci", + .type_name = "example_interfaces::action::dds_::Fibonacci", + .on_goal = [&](const FibGoal &g) { return g.order > 0; }, + .execute = + [&](auto &h) { + const int32_t order = h.goal().order; + std::vector seq{0, 1}; + for (int32_t i = 1; i < order; ++i) { + seq.push_back(seq[i] + seq[i - 1]); + h.publish_feedback(FibSeq{seq}); + std::this_thread::sleep_for(200ms); + } + h.succeed(FibSeq{seq}); + logger.info("action fibonacci({}) done", order); + }, + }); + if (!add_service.is_valid() || !fib_action.is_valid()) { + logger.error("Failed to create the typed service/action servers"); + return; + } + logger.info("service '/add_two_ints' + action '/fibonacci' ready"); + + // Also demonstrate the CLIENT side on-device: a typed service client + action + // client that call services a peer hosts ("/peer_add_two_ints", "/peer_fib"). + // Run a ROS 2 / rclpy server (or another espp device) for those names to see a + // full round-trip; until then the calls simply time out (logged), which still + // exercises the client API on-target. (Calling this device's OWN services is + // not possible - a participant filters out its own messages.) + espp::ServiceClient add_client( + participant, + {.service = "/peer_add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + espp::ActionClient fib_client( + participant, + {.action = "/peer_fib", .type_name = "example_interfaces::action::dds_::Fibonacci"}); + + // Only one action goal in flight at a time: without a peer the goal never + // completes, so re-sending on every tick would leak a pending goal each time. + // The service call() below self-cleans on its 1s timeout, so it can run freely. + std::atomic fib_in_flight{false}; + espp::Timer rpc_client_timer({ + .name = "rtps_rpc_client", + .period = 5s, + .callback = + [&]() { + // Typed blocking service call (RMI). + if (auto resp = add_client.call(AddReq{20, 22}, 1s)) { + logger.info("[client] /peer_add_two_ints(20,22) = {}", resp->sum); + } else { + logger.info("[client] /peer_add_two_ints: no reply (peer serving it?)"); + } + // Typed action goal (AMI) with typed feedback + result. Skip if the + // previous goal has not finished (e.g. no peer is serving it). + if (!fib_in_flight.exchange(true)) { + fib_client.send_goal( + FibGoal{5}, [&](const FibSeq &) { /* per-feedback */ }, + [&](espp::GoalStatus status, const FibSeq &res) { + logger.info("[client] /peer_fib result: status={} len={}", + static_cast(status), res.sequence.size()); + fib_in_flight.store(false); + }); + } + return false; // keep the timer running + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + if (!add_client.is_valid() || !fib_client.is_valid()) { + logger.error("Failed to create the typed service/action clients"); + return; + } + logger.info("client for '/peer_add_two_ints' + '/peer_fib' running"); + +#if CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT + // Purely additive on-device SELF-TEST (Kconfig, default off): a SECOND + // participant with its own service + action clients that call THIS device's own + // /add_two_ints and /fibonacci servers, for a full local round-trip (a + // participant filters out its own messages, so the loopback needs a distinct + // participant). This roughly doubles the RTPS engine RAM - only enable on a + // target with headroom (e.g. PSRAM). + espp::RtpsParticipant selftest_participant({ + .interface_address = interface_address, + .log_level = espp::Logger::Verbosity::WARN, + }); + if (!selftest_participant.start()) { + logger.error("Failed to start the self-test participant"); + return; + } + espp::ServiceClient selftest_add_client( + selftest_participant, + {.service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + espp::ActionClient selftest_fib_client( + selftest_participant, + {.action = "/fibonacci", .type_name = "example_interfaces::action::dds_::Fibonacci"}); + espp::Timer selftest_timer({ + .name = "rtps_selftest", + .period = 5s, + .callback = + [&]() { + if (auto resp = selftest_add_client.call(AddReq{20, 22}, 2s)) { + logger.info("[self-test] /add_two_ints(20,22) = {} ({})", resp->sum, + resp->sum == 42 ? "PASS" : "FAIL"); + } else { + logger.warn("[self-test] /add_two_ints: no reply"); + } + selftest_fib_client.send_goal( + FibGoal{5}, [&](const FibSeq &) {}, + [&](espp::GoalStatus status, const FibSeq &res) { + const std::vector expected{0, 1, 1, 2, 3, 5}; + const bool ok = status == espp::GoalStatus::SUCCEEDED && res.sequence == expected; + logger.info("[self-test] /fibonacci(5) len={} ({})", res.sequence.size(), + ok ? "PASS" : "FAIL"); + }); + return false; // keep the timer running + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + if (!selftest_add_client.is_valid() || !selftest_fib_client.is_valid()) { + logger.error("Failed to create the self-test clients"); + return; + } + logger.info("self-test participant round-tripping the local service + action"); +#endif // CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT +#endif // RTPS_WITH_RPC + //! [rtps example] + + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/rtps_embedded/include/rtps/entities/Reader.hpp b/components/rtps_embedded/include/rtps/entities/Reader.hpp index 8e5e169f0..52a977eac 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.hpp +++ b/components/rtps_embedded/include/rtps/entities/Reader.hpp @@ -31,6 +31,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.hpp" #include "rtps/discovery/TopicData.hpp" #include "rtps/entities/WriterProxy.hpp" +#include "rtps/rpc/sample_identity.hpp" #include "rtps/storages/MemoryPool.hpp" #include #include @@ -52,14 +53,23 @@ class ReaderCacheChange { const DataSize_t size; const Guid_t writerGuid; const SequenceNumber_t sn; + // ROS 2 request/reply correlation: when the DATA carried a + // related_sample_identity inline QoS (PID 0x0083 / 0x800f), it is surfaced here + // so a service endpoint can correlate. Zero-cost for plain pub/sub (the flag + // stays false). See rpc/sample_identity.hpp. + const bool hasRelatedSampleIdentity; + const rpc::SampleIdentity relatedSampleIdentity; ReaderCacheChange(ChangeKind_t kind, Guid_t &writerGuid, SequenceNumber_t sn, const uint8_t *data, - DataSize_t size) + DataSize_t size, bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) : data(data) , kind(kind) , size(size) , writerGuid(writerGuid) - , sn(sn){}; + , sn(sn) + , hasRelatedSampleIdentity(hasRelatedSampleIdentity) + , relatedSampleIdentity(relatedSampleIdentity){}; ~ReaderCacheChange() = default; // No need to free data. It's not owned by this object // Not allowed because this class doesn't own the ptr and the user isn't diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp index 743b0325f..0cfb66987 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp @@ -53,8 +53,9 @@ class StatefulWriter final : public Writer { //! worker threads void progress() override; const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, - bool inLineQoS = false, - bool markDisposedAfterWrite = false) override; + bool inLineQoS = false, bool markDisposedAfterWrite = false, + bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) override; bool removeFromHistory(const SequenceNumber_t &s); diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp index eb2f4d88f..2ad8f58c1 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp @@ -46,8 +46,9 @@ class StatelessWriter : public Writer { void progress() override; const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, - bool inLineQoS = false, - bool markDisposedAfterWrite = false) override; + bool inLineQoS = false, bool markDisposedAfterWrite = false, + bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) override; bool removeFromHistory(const SequenceNumber_t &s); void setAllChangesToUnsent() override; diff --git a/components/rtps_embedded/include/rtps/entities/Writer.hpp b/components/rtps_embedded/include/rtps/entities/Writer.hpp index 966a5bf97..25983a43e 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.hpp +++ b/components/rtps_embedded/include/rtps/entities/Writer.hpp @@ -62,6 +62,16 @@ class Writer : public espp::BaseComponent { virtual void reset() = 0; virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size); + //! Add a change that will be sent as a DATA carrying relatedSampleIdentity as + //! inline QoS (ROS 2 service request/reply correlation). Used by the service + //! request/reply writers; plain pub/sub uses the forms above and is unaffected. + const CacheChange * + newChangeWithRelatedSampleIdentity(ChangeKind_t kind, const uint8_t *data, DataSize_t size, + const rpc::SampleIdentity &relatedSampleIdentity) { + return newChange(kind, data, size, /*inLineQoS=*/false, /*markDisposedAfterWrite=*/false, + /*hasRelatedSampleIdentity=*/true, relatedSampleIdentity); + } + //! Executes required steps like sending packets. Intended to be called by //! worker threads virtual void progress() = 0; @@ -107,7 +117,9 @@ class Writer : public espp::BaseComponent { friend class SEDPAgent; virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, - bool inLineQoS, bool markDisposedAfterWrite) = 0; + bool inLineQoS, bool markDisposedAfterWrite, + bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) = 0; friend class SizeInspector; bool m_is_initialized_ = false; diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp index ccdec7b8f..8bc590027 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp @@ -32,6 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/types.hpp" #include "rtps/config.hpp" #include "rtps/messages/MessageTypes.hpp" +#include "rtps/rpc/sample_identity.hpp" #include "rtps/utils/sysFunctions.hpp" #include @@ -138,6 +139,75 @@ void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool } } +// Append a DATA submessage that carries a related_sample_identity inline QoS, as +// rmw_fastrtps uses for ROS 2 service request/reply correlation (see +// rpc/sample_identity.hpp and RMI_AMI_DESIGN.md 3.2). Distinct from +// addSubMessageData so the plain pub/sub path stays byte-identical; only the +// service request/reply writers call this. +// +// Layout after the SubmessageData header (readerId..writerSN): an inline QoS +// ParameterList of two 24-byte related_sample_identity parameters (PID 0x0083 +// and 0x800f, both emitted with the identical value) terminated by PID_SENTINEL, +// followed by the serialized payload. +template +void addSubMessageDataWithRelatedSampleIdentity(Buffer &buffer, const PayloadBuffer &filledPayload, + const rpc::SampleIdentity &relatedSampleIdentity, + const SequenceNumber_t &SN, + const EntityId_t &writerID, + const EntityId_t &readerID) { + // Inline QoS parameter list byte count: two (PID + length + 24-byte value) + // parameters plus a (PID + length) sentinel. + constexpr uint16_t kParamHeader = 2 * sizeof(uint16_t); // parameterId + length + constexpr uint16_t kInlineQosBytes = + 2 * (kParamHeader + rpc::SAMPLE_IDENTITY_CDR_SIZE) + kParamHeader; // + PID_SENTINEL + + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + msg.header.flags |= FLAG_INLINE_QOS; + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + // octetsToNextHeader spans the fixed DATA body + inline QoS + payload (the + // narrowing to uint16 is safe: a service request/reply always fits one + // submessage). See addSubMessageData for the base calculation. + msg.header.octetsToNextHeader = + static_cast(SubmessageData::getRawSize() + kInlineQosBytes + + filledPayload.spaceUsed() - numBytesUntilEndOfLength); + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + constexpr uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + // Inline QoS ParameterList: same 24-byte value under both PIDs, then sentinel. + const auto appendParam = [&](uint16_t pid) { + uint16_t length = rpc::SAMPLE_IDENTITY_CDR_SIZE; + buffer.append(reinterpret_cast(&pid), sizeof(pid)); + buffer.append(reinterpret_cast(&length), sizeof(length)); + rpc::serializeSampleIdentity(buffer, relatedSampleIdentity); + }; + appendParam(rpc::PID_RELATED_SAMPLE_IDENTITY); + appendParam(rpc::PID_CUSTOM_RELATED_SAMPLE_IDENTITY); + uint16_t sentinel = SMElement::PID_SENTINEL; + uint16_t sentinelLen = 0; + buffer.append(reinterpret_cast(&sentinel), sizeof(sentinel)); + buffer.append(reinterpret_cast(&sentinelLen), sizeof(sentinelLen)); + + if (filledPayload.isValid()) { + buffer.append(filledPayload); + } +} + #ifdef RTPS_ENABLE_FRAGMENTATION // Append one DATA_FRAG submessage carrying [fragData, fragData+fragLen) as the // serializedData for fragment(s) starting at fragStartNum (1-based). fragLen must diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp index fd76c7072..f08eb56a8 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp @@ -248,6 +248,19 @@ static constexpr DataSize_t MAX_UNFRAGMENTED_PAYLOAD = 65507 - Header::getRawSize() - (SubmessageHeader::getRawSize() + sizeof(Time_t)) - SubmessageData::getRawSize(); +// Largest RPC (service/action) payload that still fits one unfragmented DATA +// carrying a related_sample_identity. Such a DATA appends an inline QoS +// ParameterList - two (PID + length + 24-byte SampleIdentity) parameters plus a +// (PID + length) sentinel = 60 bytes - which eats into the single-DATA budget. +// A related-identity sample must NOT be fragmented (DATA_FRAG carries no inline +// QoS, so the correlation would be lost and the caller would time out), so the +// RPC facade rejects requests/replies above this bound instead. Keep the 60 in +// sync with MessageFactory::addSubMessageDataWithRelatedSampleIdentity's +// kInlineQosBytes. +static constexpr DataSize_t RELATED_SAMPLE_IDENTITY_INLINE_QOS_BYTES = 60; +static constexpr DataSize_t MAX_UNFRAGMENTED_RPC_PAYLOAD = + MAX_UNFRAGMENTED_PAYLOAD - RELATED_SAMPLE_IDENTITY_INLINE_QOS_BYTES; + // Largest per-fragment payload that keeps a single-fragment DATA_FRAG submessage // within one UDP datagram (max UDP payload 65507 - RTPS header 20 - INFO_TS 12 - // DATA_FRAG submessage header 36). DATA_FRAG's fixed header is 12 bytes larger diff --git a/components/rtps_embedded/include/rtps/rpc/action_naming.hpp b/components/rtps_embedded/include/rtps/rpc/action_naming.hpp new file mode 100644 index 000000000..215082449 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rpc/action_naming.hpp @@ -0,0 +1,94 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of the espp embeddedRTPS port. +*/ + +#ifndef RTPS_RPC_ACTION_NAMING_H +#define RTPS_RPC_ACTION_NAMING_H + +// --------------------------------------------------------------------------- +// ROS 2 (rmw_fastrtps) action name/type mangling. An action maps onto 3 services +// (send_goal, cancel_goal, get_result) + 2 topics (feedback, status). Verified +// against a live rmw_fastrtps (ROS 2 Jazzy) Fibonacci action capture (see +// RMI_AMI_DESIGN.md 3.4): +// +// action "/fibonacci", base type "example_interfaces::action::dds_::Fibonacci" +// send_goal svc base "fibonacci/_action/send_goal" type "...Fibonacci_SendGoal" +// cancel_goal svc base "fibonacci/_action/cancel_goal" type +// "action_msgs::srv::dds_::CancelGoal" get_result svc base "fibonacci/_action/get_result" type +// "...Fibonacci_GetResult" feedback topic "rt/fibonacci/_action/feedback" type +// "...Fibonacci_FeedbackMessage_" status topic "rt/fibonacci/_action/status" type +// "action_msgs::msg::dds_::GoalStatusArray_" +// +// The service base names feed rpc::service_request_topic/service_request_type +// (etc.) to get the final rq/rr topics and _Request_/_Response_ types, reusing +// the service mangling exactly. Header-only, unit-testable. +// --------------------------------------------------------------------------- + +#include "rtps/rpc/service_naming.hpp" + +#include +#include + +namespace rtps { +namespace rpc { + +// --- service base names (feed to service_{request,reply}_topic / _type) --- + +inline std::string action_send_goal_service(std::string_view action) { + return strip_leading_slash(action) + "/_action/send_goal"; +} +inline std::string action_cancel_goal_service(std::string_view action) { + return strip_leading_slash(action) + "/_action/cancel_goal"; +} +inline std::string action_get_result_service(std::string_view action) { + return strip_leading_slash(action) + "/_action/get_result"; +} + +// --- topics --- + +inline std::string action_feedback_topic(std::string_view action) { + return "rt/" + strip_leading_slash(action) + "/_action/feedback"; +} +inline std::string action_status_topic(std::string_view action) { + return "rt/" + strip_leading_slash(action) + "/_action/status"; +} + +// --- service base types (feed to service_request_type / service_response_type) --- +// base = "::action::dds_::", e.g. "..::action::dds_::Fibonacci". + +inline std::string action_send_goal_type(std::string_view base) { + return std::string(base) + "_SendGoal"; +} +inline std::string action_get_result_type(std::string_view base) { + return std::string(base) + "_GetResult"; +} +inline std::string action_feedback_type(std::string_view base) { + return std::string(base) + "_FeedbackMessage_"; +} + +// Fixed action_msgs types (not derived from the action's own type). +inline std::string action_cancel_goal_type() { return "action_msgs::srv::dds_::CancelGoal"; } +inline std::string action_status_type() { return "action_msgs::msg::dds_::GoalStatusArray_"; } + +} // namespace rpc +} // namespace rtps + +#endif // RTPS_RPC_ACTION_NAMING_H diff --git a/components/rtps_embedded/include/rtps/rpc/action_types.hpp b/components/rtps_embedded/include/rtps/rpc/action_types.hpp new file mode 100644 index 000000000..5a7cef0a1 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rpc/action_types.hpp @@ -0,0 +1,235 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of the espp embeddedRTPS port. +*/ + +#ifndef RTPS_RPC_ACTION_TYPES_H +#define RTPS_RPC_ACTION_TYPES_H + +// --------------------------------------------------------------------------- +// ROS 2 action envelope codec: wraps/unwraps the user's opaque goal/result/ +// feedback CDR payloads in the action message envelopes (goal_id UUID, accepted +// flag, stamp, status). All layouts CDR_LE, confirmed against a live Fibonacci +// capture (RMI_AMI_DESIGN.md 3.4). Every payload here (in and out) is a full +// CDR message: a 4-byte encapsulation header {0x00,0x01,0x00,0x00} + body, +// exactly like the service request/reply payloads. +// +// The nested user payload (goal / result / feedback) is spliced in field-wise: +// we strip its 4-byte encapsulation header and place its body after the envelope +// prefix, under the envelope's single encapsulation header. This is byte-exact +// as long as the nested body starts at a CDR offset with matching alignment: +// - goal follows UUID(16) -> offset 16 (8-aligned): correct for any field. +// - feedback follows UUID(16) -> offset 16 (8-aligned): correct for any field. +// - result follows status(1)+pad(3) -> offset 4 (4-aligned): correct when the +// result's first field is <= 4-byte aligned (int32/uint32/arrays/strings/ +// nested of those). An 8-byte-aligned first field (int64/float64/uint64) +// would need one more pad; that case needs the typed (cdr-reflection) path +// and is out of scope for this byte-level v1. +// --------------------------------------------------------------------------- + +#include +#include +#include +#include +#include + +namespace rtps { +namespace rpc { + +// unique_identifier_msgs/UUID: 16 raw bytes. +using GoalUuid = std::array; + +// GoalStatus.status values (action_msgs/msg/GoalStatus). +enum class GoalStatus : int8_t { + UNKNOWN = 0, + ACCEPTED = 1, + EXECUTING = 2, + CANCELING = 3, + SUCCEEDED = 4, + CANCELED = 5, + ABORTED = 6, +}; + +namespace detail { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +inline bool has_encap(std::span b) { return b.size() >= 4; } +inline void put_u32(std::vector &v, uint32_t x) { + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); + } +} +inline uint32_t get_u32(std::span b, size_t off) { + return static_cast(b[off]) | (static_cast(b[off + 1]) << 8) | + (static_cast(b[off + 2]) << 16) | (static_cast(b[off + 3]) << 24); +} +} // namespace detail + +// --- SendGoal_Request = { goal_id: UUID(16), goal } --- +inline std::vector wrap_send_goal_request(const GoalUuid &id, + std::span goal_cdr) { + std::vector v(detail::kEncap, detail::kEncap + 4); + v.insert(v.end(), id.begin(), id.end()); + if (detail::has_encap(goal_cdr)) { + v.insert(v.end(), goal_cdr.begin() + 4, goal_cdr.end()); // strip nested encap + } + return v; +} +// Returns false if too short. goal_cdr_out gets a re-encapsulated nested payload. +inline bool unwrap_send_goal_request(std::span msg, GoalUuid &id_out, + std::vector &goal_cdr_out) { + if (msg.size() < 4 + 16) { + return false; + } + std::memcpy(id_out.data(), msg.data() + 4, 16); + goal_cdr_out.assign(detail::kEncap, detail::kEncap + 4); + goal_cdr_out.insert(goal_cdr_out.end(), msg.begin() + 4 + 16, msg.end()); + return true; +} + +// --- SendGoal_Response = { accepted: bool(1)+pad(3), stamp{sec:i32,nsec:u32} } --- +inline std::vector make_send_goal_response(bool accepted, int32_t sec = 0, + uint32_t nsec = 0) { + std::vector v(detail::kEncap, detail::kEncap + 4); + v.push_back(accepted ? 1 : 0); + v.push_back(0); + v.push_back(0); + v.push_back(0); + detail::put_u32(v, static_cast(sec)); + detail::put_u32(v, nsec); + return v; +} +inline bool parse_send_goal_response(std::span msg, bool &accepted_out) { + if (msg.size() < 4 + 4) { + return false; + } + accepted_out = (msg[4] != 0); + return true; +} + +// --- GetResult_Request = { goal_id: UUID(16) } --- +inline std::vector make_get_result_request(const GoalUuid &id) { + std::vector v(detail::kEncap, detail::kEncap + 4); + v.insert(v.end(), id.begin(), id.end()); + return v; +} +inline bool parse_get_result_request(std::span msg, GoalUuid &id_out) { + if (msg.size() < 4 + 16) { + return false; + } + std::memcpy(id_out.data(), msg.data() + 4, 16); + return true; +} + +// --- GetResult_Response = { status: i8(1)+pad(3), result } --- +inline std::vector wrap_get_result_response(GoalStatus status, + std::span result_cdr) { + std::vector v(detail::kEncap, detail::kEncap + 4); + v.push_back(static_cast(status)); + v.push_back(0); + v.push_back(0); + v.push_back(0); + if (detail::has_encap(result_cdr)) { + v.insert(v.end(), result_cdr.begin() + 4, result_cdr.end()); + } + return v; +} +inline bool unwrap_get_result_response(std::span msg, GoalStatus &status_out, + std::vector &result_cdr_out) { + if (msg.size() < 4 + 4) { + return false; + } + status_out = static_cast(static_cast(msg[4])); + result_cdr_out.assign(detail::kEncap, detail::kEncap + 4); + result_cdr_out.insert(result_cdr_out.end(), msg.begin() + 4 + 4, msg.end()); + return true; +} + +// --- FeedbackMessage = { goal_id: UUID(16), feedback } --- +inline std::vector wrap_feedback(const GoalUuid &id, + std::span feedback_cdr) { + std::vector v(detail::kEncap, detail::kEncap + 4); + v.insert(v.end(), id.begin(), id.end()); + if (detail::has_encap(feedback_cdr)) { + v.insert(v.end(), feedback_cdr.begin() + 4, feedback_cdr.end()); + } + return v; +} +inline bool unwrap_feedback(std::span msg, GoalUuid &id_out, + std::vector &feedback_cdr_out) { + if (msg.size() < 4 + 16) { + return false; + } + std::memcpy(id_out.data(), msg.data() + 4, 16); + feedback_cdr_out.assign(detail::kEncap, detail::kEncap + 4); + feedback_cdr_out.insert(feedback_cdr_out.end(), msg.begin() + 4 + 16, msg.end()); + return true; +} + +// --- GoalStatusArray = status_list[]{ goal_id:UUID(16), stamp{sec,nsec}, status:i8+pad } --- +struct GoalStatusEntry { + GoalUuid goal_id{}; + int32_t sec{0}; + uint32_t nsec{0}; + GoalStatus status{GoalStatus::UNKNOWN}; +}; +inline std::vector make_goal_status_array(std::span entries) { + std::vector v(detail::kEncap, detail::kEncap + 4); + detail::put_u32(v, static_cast(entries.size())); + for (const auto &e : entries) { + v.insert(v.end(), e.goal_id.begin(), e.goal_id.end()); + detail::put_u32(v, static_cast(e.sec)); + detail::put_u32(v, e.nsec); + v.push_back(static_cast(e.status)); + v.push_back(0); + v.push_back(0); + v.push_back(0); + } + return v; +} +inline bool parse_goal_status_array(std::span msg, + std::vector &out) { + if (msg.size() < 4 + 4) { + return false; + } + const uint32_t n = detail::get_u32(msg, 4); + size_t off = 8; + out.clear(); + for (uint32_t i = 0; i < n; ++i) { + if (off + 16 + 4 + 4 + 4 > msg.size()) { + return false; + } + GoalStatusEntry e; + std::memcpy(e.goal_id.data(), msg.data() + off, 16); + off += 16; + e.sec = static_cast(detail::get_u32(msg, off)); + off += 4; + e.nsec = detail::get_u32(msg, off); + off += 4; + e.status = static_cast(static_cast(msg[off])); + off += 4; // status(1) + pad(3) + out.push_back(e); + } + return true; +} + +} // namespace rpc +} // namespace rtps + +#endif // RTPS_RPC_ACTION_TYPES_H diff --git a/components/rtps_embedded/include/rtps/rpc/native_protocol.hpp b/components/rtps_embedded/include/rtps/rpc/native_protocol.hpp new file mode 100644 index 000000000..d452d34a0 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rpc/native_protocol.hpp @@ -0,0 +1,232 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of the espp embeddedRTPS port. +*/ + +#ifndef RTPS_RPC_NATIVE_PROTOCOL_H +#define RTPS_RPC_NATIVE_PROTOCOL_H + +// --------------------------------------------------------------------------- +// Native (espp<->espp) request/reply protocol - Track B in RMI_AMI_DESIGN.md. +// Deliberately NOT ROS 2-compatible: it trades interop for simplicity + a small +// footprint. Correlation is an in-band 20-byte header prepended to the payload, +// so it needs no inline-QoS engine support and rides plain reliable pub/sub. +// +// offset 0 client_prefix : 12 bytes (the requesting participant's GUID +// prefix - the correlation key; the +// server echoes it on the reply) +// offset 12 request_id : uint32 LE (client-monotonic) +// offset 16 op : uint8 (REQUEST=0, REPLY=1) +// offset 17 flags : uint8 (reserved; 0) +// offset 18 reserved : uint16 (0) +// offset 20 (the CDR-encapsulated user message) +// +// A request is broadcast on es_rq/; the server echoes {client_prefix, +// request_id} on the reply, broadcast on es_rr/. Each client accepts +// only replies whose client_prefix == its own prefix and matches request_id +// against its pending table (client-side filtering, same shape as the ROS path +// but with the key in-band instead of as related_sample_identity inline QoS). +// +// Single-server assumption (v1): a broadcast request reaches every matched +// server, so N servers on one topic would each reply. Run one server per service. +// --------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include + +namespace rtps { +namespace rpc { + +constexpr std::size_t NATIVE_HEADER_SIZE = 20; + +enum class NativeOp : uint8_t { + REQUEST = 0, + REPLY = 1, +}; + +struct NativeHeader { + std::array client_prefix{}; + uint32_t request_id{0}; + NativeOp op{NativeOp::REQUEST}; + uint8_t flags{0}; +}; + +// Prepend the native header to a payload, producing the transported frame. +inline std::vector native_encode(const NativeHeader &h, std::span payload) { + std::vector v; + v.reserve(NATIVE_HEADER_SIZE + payload.size()); + v.insert(v.end(), h.client_prefix.begin(), h.client_prefix.end()); + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast((h.request_id >> (8 * i)) & 0xFF)); + } + v.push_back(static_cast(h.op)); + v.push_back(h.flags); + v.push_back(0); // reserved + v.push_back(0); + v.insert(v.end(), payload.begin(), payload.end()); + return v; +} + +// Parse a frame: fill the header and return the payload span (into `frame`). +// Returns false if the frame is too short to hold the header. +inline bool native_decode(std::span frame, NativeHeader &h_out, + std::span &payload_out) { + if (frame.size() < NATIVE_HEADER_SIZE) { + return false; + } + std::memcpy(h_out.client_prefix.data(), frame.data(), 12); + h_out.request_id = static_cast(frame[12]) | (static_cast(frame[13]) << 8) | + (static_cast(frame[14]) << 16) | + (static_cast(frame[15]) << 24); + h_out.op = static_cast(frame[16]); + h_out.flags = frame[17]; + payload_out = frame.subspan(NATIVE_HEADER_SIZE); + return true; +} + +// Native topics - a distinct prefix so they never alias the ROS rq/rr topics. +inline std::string native_strip_slash(std::string_view s) { + std::string r(s); + if (!r.empty() && r.front() == '/') { + r.erase(0, 1); + } + return r; +} +inline std::string native_request_topic(std::string_view service) { + return "es_rq/" + native_strip_slash(service); +} +inline std::string native_reply_topic(std::string_view service) { + return "es_rr/" + native_strip_slash(service); +} + +// --------------------------------------------------------------------------- +// Native action (Track B, lean AMI): collapses ROS's 3 services + 2 topics to a +// send_goal native request/reply (-> {accepted, goal_handle}), a small cancel +// native request/reply (-> {accepted}), and ONE feedback topic. No UUIDs (a +// uint32 goal_handle), no separate get_result / status - the terminal result +// rides the feedback stream as a SUCCEEDED/ABORTED/CANCELED message. ~4 +// endpoints/pair vs ROS's ~10. See RMI_AMI_DESIGN.md 4.3. +// --------------------------------------------------------------------------- + +// Reuse the ROS GoalStatus values for conceptual parity (see action_types.hpp, +// but kept independent here so the native path has no ROS-envelope dependency). +enum class NativeGoalStatus : uint8_t { + ACCEPTED = 1, + EXECUTING = 2, + SUCCEEDED = 4, + CANCELED = 5, + ABORTED = 6, +}; + +inline std::string native_goal_service(std::string_view action) { + return native_strip_slash(action) + "/goal"; +} +inline std::string native_cancel_service(std::string_view action) { + return native_strip_slash(action) + "/cancel"; +} +inline std::string native_feedback_topic(std::string_view action) { + return "es_fb/" + native_strip_slash(action); +} + +// cancel request = { goal_handle:uint32 } (after encap). +inline std::vector native_make_cancel_request(uint32_t goal_handle) { + std::vector v{0x00, 0x01, 0x00, 0x00}; + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast((goal_handle >> (8 * i)) & 0xFF)); + } + return v; +} +inline bool native_parse_cancel_request(std::span msg, uint32_t &goal_handle_out) { + if (msg.size() < 4 + 4) { + return false; + } + goal_handle_out = static_cast(msg[4]) | (static_cast(msg[5]) << 8) | + (static_cast(msg[6]) << 16) | (static_cast(msg[7]) << 24); + return true; +} +// cancel reply = { accepted:uint8 + pad(3) } (after encap). +inline std::vector native_make_cancel_reply(bool accepted) { + return {0x00, 0x01, 0x00, 0x00, static_cast(accepted ? 1 : 0), 0, 0, 0}; +} +inline bool native_parse_cancel_reply(std::span msg, bool &accepted_out) { + if (msg.size() < 4 + 4) { + return false; + } + accepted_out = (msg[4] != 0); + return true; +} + +// send_goal reply = { accepted:uint8 + pad(3), goal_handle:uint32 } (after encap). +inline std::vector native_make_goal_reply(bool accepted, uint32_t goal_handle) { + std::vector v{0x00, 0x01, 0x00, 0x00, static_cast(accepted ? 1 : 0), 0, 0, 0}; + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast((goal_handle >> (8 * i)) & 0xFF)); + } + return v; +} +inline bool native_parse_goal_reply(std::span msg, bool &accepted_out, + uint32_t &goal_handle_out) { + if (msg.size() < 4 + 8) { + return false; + } + accepted_out = (msg[4] != 0); + goal_handle_out = static_cast(msg[8]) | (static_cast(msg[9]) << 8) | + (static_cast(msg[10]) << 16) | (static_cast(msg[11]) << 24); + return true; +} + +// feedback/result msg = { goal_handle:uint32, status:uint8 + pad(3), payload } (after encap). +inline std::vector native_make_feedback(uint32_t goal_handle, NativeGoalStatus status, + std::span payload) { + std::vector v{0x00, 0x01, 0x00, 0x00}; + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast((goal_handle >> (8 * i)) & 0xFF)); + } + v.push_back(static_cast(status)); + v.push_back(0); + v.push_back(0); + v.push_back(0); + if (payload.size() >= 4) { + v.insert(v.end(), payload.begin() + 4, payload.end()); // splice past the payload's encap + } + return v; +} +inline bool native_parse_feedback(std::span msg, uint32_t &goal_handle_out, + NativeGoalStatus &status_out, std::vector &payload_out) { + if (msg.size() < 4 + 8) { + return false; + } + goal_handle_out = static_cast(msg[4]) | (static_cast(msg[5]) << 8) | + (static_cast(msg[6]) << 16) | (static_cast(msg[7]) << 24); + status_out = static_cast(msg[8]); + payload_out.assign({0x00, 0x01, 0x00, 0x00}); + payload_out.insert(payload_out.end(), msg.begin() + 12, msg.end()); + return true; +} + +} // namespace rpc +} // namespace rtps + +#endif // RTPS_RPC_NATIVE_PROTOCOL_H diff --git a/components/rtps_embedded/include/rtps/rpc/sample_identity.hpp b/components/rtps_embedded/include/rtps/rpc/sample_identity.hpp new file mode 100644 index 000000000..126d1b069 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rpc/sample_identity.hpp @@ -0,0 +1,92 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of the espp embeddedRTPS port. +*/ + +#ifndef RTPS_RPC_SAMPLE_IDENTITY_H +#define RTPS_RPC_SAMPLE_IDENTITY_H + +// --------------------------------------------------------------------------- +// SampleIdentity + related_sample_identity inline-QoS wire constants for ROS 2 +// (rmw_fastrtps) request/reply correlation. +// +// A SampleIdentity is a {GUID, SequenceNumber}. rmw_fastrtps correlates a reply +// to its request by carrying the request's identity on BOTH the request and the +// reply as inline QoS, under two parameter IDs (see RMI_AMI_DESIGN.md 3.2, +// confirmed against a live ROS 2 Jazzy AddTwoInts capture): +// +// PID 0x0083 PID_RELATED_SAMPLE_IDENTITY (OMG DDS-RPC standard) +// PID 0x800f PID_CUSTOM_RELATED_SAMPLE_IDENTITY (eProsima legacy) +// +// Both carry the identical 24-byte value and both are emitted; a receiver +// accepts either. The 24-byte CDR_LE serialization is: +// +// guidPrefix (12) | entityKey (3) | entityKind (1) | seq.high (int32) | seq.low (uint32) +// +// i.e. the raw Guid_t layout (16) followed by SequenceNumber_t (8), all little- +// endian. UNKNOWN sequence number = {high=-1, low=0} (client's request value; +// the server echoes the request's RTPS writerSeqNumber instead). +// --------------------------------------------------------------------------- + +#include "rtps/common/types.hpp" + +#include + +namespace rtps { +namespace rpc { + +constexpr uint16_t PID_RELATED_SAMPLE_IDENTITY = 0x0083; +constexpr uint16_t PID_CUSTOM_RELATED_SAMPLE_IDENTITY = 0x800f; + +// On-wire size of a serialized SampleIdentity: Guid_t (16) + SequenceNumber_t (8). +constexpr uint16_t SAMPLE_IDENTITY_CDR_SIZE = 24; + +struct SampleIdentity { + Guid_t writer_guid; + SequenceNumber_t sequence_number; + + bool operator==(const SampleIdentity &o) const { + return writer_guid == o.writer_guid && sequence_number == o.sequence_number; + } +}; + +// The SequenceNumber value rmw uses for an as-yet-unassigned identity (the value +// a client stamps on its outgoing request). +inline SampleIdentity unknown_sample_identity(const Guid_t &reply_reader_guid) { + return SampleIdentity{reply_reader_guid, SequenceNumber_t{-1, 0}}; +} + +// Append the 24-byte CDR_LE SampleIdentity value to a MessageFactory-style +// Buffer (append(const uint8_t*, len)). Little-endian integers match the wire. +template void serializeSampleIdentity(Buffer &buffer, const SampleIdentity &id) { + buffer.append(id.writer_guid.prefix.id.data(), id.writer_guid.prefix.id.size()); + buffer.append(id.writer_guid.entityId.entityKey.data(), id.writer_guid.entityId.entityKey.size()); + buffer.append(reinterpret_cast(&id.writer_guid.entityId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&id.sequence_number.high), + sizeof(id.sequence_number.high)); + buffer.append(reinterpret_cast(&id.sequence_number.low), + sizeof(id.sequence_number.low)); +} + +} // namespace rpc +} // namespace rtps + +#endif // RTPS_RPC_SAMPLE_IDENTITY_H diff --git a/components/rtps_embedded/include/rtps/rpc/service_naming.hpp b/components/rtps_embedded/include/rtps/rpc/service_naming.hpp new file mode 100644 index 000000000..7b7d2c02f --- /dev/null +++ b/components/rtps_embedded/include/rtps/rpc/service_naming.hpp @@ -0,0 +1,82 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of the espp embeddedRTPS port. +*/ + +#ifndef RTPS_RPC_SERVICE_NAMING_H +#define RTPS_RPC_SERVICE_NAMING_H + +// --------------------------------------------------------------------------- +// ROS 2 (rmw_fastrtps) service name/type mangling. +// +// A ROS 2 service maps onto a pair of DDS topics + types. Verified against a +// live rmw_fastrtps (ROS 2 Jazzy) AddTwoInts capture (see RMI_AMI_DESIGN.md 3.1): +// +// service "/add_two_ints", base type "example_interfaces::srv::dds_::AddTwoInts" +// request topic "rq/add_two_intsRequest" type "...AddTwoInts_Request_" +// reply topic "rr/add_two_intsReply" type "...AddTwoInts_Response_" +// +// Topic rule: strip a single leading '/', prefix "rq"/"rr", suffix +// "Request"/"Reply". Internal slashes (namespaces) are preserved +// ("/ns/svc" -> "rq/ns/svcRequest"). Type rule: append "_Request_"/"_Response_" +// to the base DDS type name (already in "pkg::srv::dds_::Name" form, matching how +// the pub/sub facade takes "std_msgs::msg::dds_::UInt32_"). +// +// Header-only, no engine dependency, so it is unit-testable on the host. +// --------------------------------------------------------------------------- + +#include +#include + +namespace rtps { +namespace rpc { + +// Strip a single leading '/', if present. "/a/b" -> "a/b", "a" -> "a". +inline std::string strip_leading_slash(std::string_view service) { + if (!service.empty() && service.front() == '/') { + service.remove_prefix(1); + } + return std::string(service); +} + +// "/add_two_ints" -> "rq/add_two_intsRequest" +inline std::string service_request_topic(std::string_view service) { + return "rq/" + strip_leading_slash(service) + "Request"; +} + +// "/add_two_ints" -> "rr/add_two_intsReply" +inline std::string service_reply_topic(std::string_view service) { + return "rr/" + strip_leading_slash(service) + "Reply"; +} + +// "example_interfaces::srv::dds_::AddTwoInts" -> "..._Request_" +inline std::string service_request_type(std::string_view base_type) { + return std::string(base_type) + "_Request_"; +} + +// "example_interfaces::srv::dds_::AddTwoInts" -> "..._Response_" +inline std::string service_response_type(std::string_view base_type) { + return std::string(base_type) + "_Response_"; +} + +} // namespace rpc +} // namespace rtps + +#endif // RTPS_RPC_SERVICE_NAMING_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.hpp b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp index 46bf8c64e..751ba5b83 100644 --- a/components/rtps_embedded/include/rtps/storages/CacheChange.hpp +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp @@ -27,6 +27,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #define PROJECT_CACHECHANGE_H #include "rtps/common/types.hpp" +#include "rtps/rpc/sample_identity.hpp" #include "rtps/storages/PayloadBuffer.hpp" #include @@ -38,6 +39,12 @@ struct CacheChange { ChangeKind_t kind = ChangeKind_t::INVALID; bool inLineQoS = false; bool disposeAfterWrite = false; + // ROS 2 service request/reply correlation: when true, this change is sent as a + // DATA carrying relatedSampleIdentity as inline QoS (PID 0x0083/0x800f) instead + // of the plain DATA path. False for all plain pub/sub, so the pub/sub wire + // format is unchanged. See rpc/sample_identity.hpp. + bool hasRelatedSampleIdentity = false; + rpc::SampleIdentity relatedSampleIdentity{}; TimePoint sentTime{}; SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; PayloadBuffer data; @@ -48,6 +55,8 @@ struct CacheChange { kind = other.kind; inLineQoS = other.inLineQoS; disposeAfterWrite = other.disposeAfterWrite; + hasRelatedSampleIdentity = other.hasRelatedSampleIdentity; + relatedSampleIdentity = other.relatedSampleIdentity; sentTime = other.sentTime; sequenceNumber = other.sequenceNumber; data = std::move(other.data); @@ -64,6 +73,8 @@ struct CacheChange { sequenceNumber = SEQUENCENUMBER_UNKNOWN; inLineQoS = false; disposeAfterWrite = false; + hasRelatedSampleIdentity = false; + relatedSampleIdentity = rpc::SampleIdentity{}; sentTime = TimePoint{}; } diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp index 72545d6ec..697008ac0 100644 --- a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp @@ -55,11 +55,14 @@ template class HistoryCacheWithDeletion { } const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, - bool disposeAfterWrite) { + bool disposeAfterWrite, bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) { CacheChange change; change.kind = ChangeKind_t::ALIVE; change.inLineQoS = inLineQoS; change.disposeAfterWrite = disposeAfterWrite; + change.hasRelatedSampleIdentity = hasRelatedSampleIdentity; + change.relatedSampleIdentity = relatedSampleIdentity; change.data.reserve(size); change.data.append(data, size); change.sequenceNumber = ++m_lastUsedSequenceNumber; diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp index 06ed0371b..b1fc926b9 100644 --- a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp @@ -51,11 +51,14 @@ template class SimpleHistoryCache { } const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, - bool disposeAfterWrite) { + bool disposeAfterWrite, bool hasRelatedSampleIdentity = false, + const rpc::SampleIdentity &relatedSampleIdentity = {}) { CacheChange change; change.kind = ChangeKind_t::ALIVE; change.inLineQoS = inLineQoS; change.disposeAfterWrite = disposeAfterWrite; + change.hasRelatedSampleIdentity = hasRelatedSampleIdentity; + change.relatedSampleIdentity = relatedSampleIdentity; change.data.reserve(size); change.data.append(data, size); change.sequenceNumber = ++m_lastUsedSequenceNumber; diff --git a/components/rtps_embedded/include/rtps_action.hpp b/components/rtps_embedded/include/rtps_action.hpp new file mode 100644 index 000000000..d4506c71b --- /dev/null +++ b/components/rtps_embedded/include/rtps_action.hpp @@ -0,0 +1,302 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_message.hpp" // RtpsMessage, RtpsProtocol, detail::rtps_(de)serialize +#include "rtps_participant.hpp" +#include "rtps_service.hpp" // typed ServiceClient/Server (shares the CDR helpers) + +namespace espp { + +#ifdef RTPS_WITH_RPC + +/// @brief Terminal goal status (mirrors action_msgs/msg/GoalStatus). +enum class GoalStatus : int8_t { + SUCCEEDED = 4, + CANCELED = 5, + ABORTED = 6, +}; + +/// @brief Server-side handle to a running typed goal, passed to the execute +/// callback (which runs on its own thread). Publish feedback and terminate the +/// goal through it, all with typed messages (no manual CDR). Exactly one +/// terminator (succeed / abort) should be called per goal. +/// +/// @tparam Goal Reflectable goal message type. +/// @tparam Result Reflectable result message type. +/// @tparam Feedback Reflectable feedback message type. +template class ActionGoalHandle { +public: + /// \return The typed goal being executed. + const Goal &goal() const { return goal_; } + /// \return True if the client has requested cancellation of this goal (both + /// the ROS 2 and native protocols). A long-running execute callback + /// should poll this and wind the goal down - calling canceled() - when + /// it becomes true. + bool is_canceling() const { return is_canceling_ ? is_canceling_() : false; } + /// Publish a typed feedback message for this goal. + /// \param feedback The feedback to send to the client. + void publish_feedback(const Feedback &feedback) const { + if (publish_feedback_) { + publish_feedback_(detail::rtps_serialize(feedback)); + } + } + /// Terminate the goal as SUCCEEDED and deliver the result to the client. + /// \param result The final result. + void succeed(const Result &result) const { + if (succeed_) { + succeed_(detail::rtps_serialize(result)); + } + } + /// Terminate the goal as ABORTED and deliver the result to the client. + /// \param result The (partial/error) result. + void abort(const Result &result) const { + if (abort_) { + abort_(detail::rtps_serialize(result)); + } + } + /// Terminate the goal as CANCELED (in response to is_canceling()) and deliver + /// the (partial) result to the client. + /// \param result The result gathered before cancellation. + void canceled(const Result &result) const { + if (canceled_) { + canceled_(detail::rtps_serialize(result)); + } + } + + /// @cond INTERNAL + // Populated by ActionServer from a byte-level goal handle; not user-facing. + Goal goal_{}; + std::function)> publish_feedback_; + std::function)> succeed_; + std::function)> abort_; + std::function)> canceled_; + std::function is_canceling_; + /// @endcond +}; + +/// @brief Typed action server (AMI): runs long goals with typed Goal / Result / +/// Feedback messages, no manual CDR handling. +/// +/// @code +/// struct Goal { int32_t order; }; +/// struct Seq { std::vector sequence; }; // Result + Feedback +/// espp::ActionServer server(participant, { +/// .action = "/fibonacci", +/// .type_name = "example_interfaces::action::dds_::Fibonacci", +/// .on_goal = [](const Goal &g) { return g.order > 0; }, +/// .execute = [](auto &h) { +/// h.publish_feedback(...); h.succeed(...); }}); +/// @endcode +/// +/// @tparam Goal Reflectable goal message type. +/// @tparam Result Reflectable result message type. +/// @tparam Feedback Reflectable feedback message type. +template class ActionServer { +public: + /// The typed goal handle passed to the execute callback. + using Handle = ActionGoalHandle; + /// Called when a goal arrives; return true to accept it, false to reject. + /// Runs on an engine worker thread - return promptly. + using goal_callback_t = std::function; + /// Called (on its own thread) to run an accepted goal to completion via the + /// handle (publish_feedback / succeed / abort). + using execute_callback_t = std::function; + + /// Configuration for a typed action server. + struct Config { + std::string action; ///< Action name, e.g. "/fibonacci". + std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). + goal_callback_t on_goal; ///< Accept/reject each incoming goal. + execute_callback_t execute; ///< Run each accepted goal (own thread). + RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + }; + + /// Construct and register the action server on a started participant, which + /// must outlive this object. Check is_valid(). + /// \param participant The started participant to serve through. + /// \param config The server configuration. + ActionServer(RtpsParticipant &participant, const Config &config) { + auto on_goal = config.on_goal; + auto execute = config.execute; + if (config.protocol == RtpsProtocol::NATIVE) { + valid_ = participant.add_native_action_server( + {config.action, config.type_name}, + [on_goal](std::span goal_bytes) -> bool { + auto g = detail::rtps_deserialize(goal_bytes); + return g && (!on_goal || on_goal(*g)); + }, + [execute](RtpsParticipant::NativeGoalHandle bh) { + auto g = detail::rtps_deserialize(bh.goal()); + if (!g) { + return; + } + Handle h; + h.goal_ = std::move(*g); + h.publish_feedback_ = [bh](std::span b) { bh.publish_feedback(b); }; + h.succeed_ = [bh](std::span b) { bh.succeed(b); }; + h.abort_ = [bh](std::span b) { bh.abort(b); }; + h.canceled_ = [bh](std::span b) { bh.canceled(b); }; + h.is_canceling_ = [bh]() { return bh.is_canceling(); }; + if (execute) { + execute(h); + } + }); + } else { + valid_ = participant.add_action_server( + {config.action, config.type_name}, + [on_goal](const RtpsParticipant::GoalId &, std::span goal_bytes) -> bool { + auto g = detail::rtps_deserialize(goal_bytes); + return g && (!on_goal || on_goal(*g)); + }, + [execute](RtpsParticipant::ActionGoalHandle bh) { + auto g = detail::rtps_deserialize(bh.goal()); + if (!g) { + return; + } + Handle h; + h.goal_ = std::move(*g); + h.publish_feedback_ = [bh](std::span b) { bh.publish_feedback(b); }; + h.succeed_ = [bh](std::span b) { bh.succeed(b); }; + h.abort_ = [bh](std::span b) { bh.abort(b); }; + h.canceled_ = [bh](std::span b) { bh.canceled(b); }; + h.is_canceling_ = [bh]() { return bh.is_canceling(); }; + if (execute) { + execute(h); + } + }); + } + } + + /// \return True if the action server registered successfully. + [[nodiscard]] bool is_valid() const { return valid_; } + +private: + bool valid_{false}; +}; + +/// @brief Typed action client (AMI): send typed goals and receive typed feedback +/// + result, no manual CDR handling. +/// +/// @code +/// espp::ActionClient client(participant, { +/// .action = "/fibonacci", +/// .type_name = "example_interfaces::action::dds_::Fibonacci"}); +/// client.send_goal(Goal{5}, +/// [](const Seq &fb) { ... }, +/// [](espp::GoalStatus st, const Seq &res) { ... }); +/// @endcode +/// +/// @tparam Goal Reflectable goal message type. +/// @tparam Result Reflectable result message type. +/// @tparam Feedback Reflectable feedback message type. +template class ActionClient { +public: + /// Callback delivering one typed feedback message (on an engine worker thread). + using feedback_callback_t = std::function; + /// Callback delivering the terminal status + typed result, once per goal. + using result_callback_t = std::function; + + /// Configuration for a typed action client. + struct Config { + std::string action; ///< Action name, e.g. "/fibonacci". + std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). + RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + }; + + /// Construct and register the action client on a started participant, which + /// must outlive this object. Check is_valid(). + /// \param participant The started participant to drive the action through. + /// \param config The client configuration. + ActionClient(RtpsParticipant &participant, const Config &config) { + if (config.protocol == RtpsProtocol::NATIVE) { + native_ = participant.add_native_action_client({config.action, config.type_name}); + } else { + ros_ = participant.add_action_client({config.action, config.type_name}); + } + } + + /// \return True if the action client registered successfully. + [[nodiscard]] bool is_valid() const { return ros_ != nullptr || native_ != nullptr; } + + /// Send a typed goal to the server. + /// \param goal The typed goal. + /// \param on_feedback Invoked for each feedback message during execution. + /// \param on_result Invoked once with the terminal status + result (an empty + /// Result and non-SUCCEEDED status if the goal was rejected). + /// \return True if the goal was queued. + bool send_goal(const Goal &goal, feedback_callback_t on_feedback, result_callback_t on_result) { + const auto goal_bytes = detail::rtps_serialize(goal); + auto fb_cb = [on_feedback](std::span b) { + auto fb = detail::rtps_deserialize(b); + if (fb && on_feedback) { + on_feedback(*fb); + } + }; + auto res_cb = [on_result](int8_t status, std::span b) { + auto res = detail::rtps_deserialize(b); + if (on_result) { + on_result(static_cast(status), res ? *res : Result{}); + } + }; + if (ros_) { + auto id = ros_->send_goal(goal_bytes, std::move(fb_cb), std::move(res_cb)); + if (id) { + std::lock_guard lock(latest_->m); + latest_->ros_id = *id; + } + return id.has_value(); + } + if (native_) { + auto lat = latest_; + return native_->send_goal( + goal_bytes, std::move(fb_cb), + [res_cb](uint8_t status, std::span b) { + res_cb(static_cast(status), b); + }, + [lat](uint32_t handle) { + std::lock_guard lock(lat->m); + lat->native_handle = handle; + }); + } + return false; + } + + /// Request cancellation of the most recently accepted goal (works on both the + /// ROS 2 and native protocols). The server observes the cancel via its goal + /// handle's is_canceling() and should wind the goal down cooperatively. + /// \return True if the cancel request was queued. + bool cancel_goal() { + std::lock_guard lock(latest_->m); + if (native_ && latest_->native_handle) { + return native_->cancel_goal(*latest_->native_handle); + } + if (ros_ && latest_->ros_id) { + return ros_->cancel_goal(*latest_->ros_id); + } + return false; + } + +private: + // Tracks the most recently accepted goal so cancel_goal() can target it. A + // shared_ptr so the native on_accepted callback (which runs later, on an engine + // thread) can record the server-assigned handle here. + struct Latest { + std::mutex m; + std::optional native_handle; + std::optional ros_id; + }; + std::shared_ptr latest_ = std::make_shared(); + std::shared_ptr ros_; + std::shared_ptr native_; +}; + +#endif // RTPS_WITH_RPC + +} // namespace espp diff --git a/components/rtps_embedded/include/rtps_message.hpp b/components/rtps_embedded/include/rtps_message.hpp new file mode 100644 index 000000000..3a5167b09 --- /dev/null +++ b/components/rtps_embedded/include/rtps_message.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" + +namespace espp { + +/// @brief A type usable with the typed RTPS layer (pub/sub, services, actions). +/// +/// Any reflectable struct the `cdr` component can serialize and deserialize +/// qualifies - no base class, macros, or member functions required. This mirrors +/// the ROS 2 / DDS message model: a plain data struct whose fields map to CDR. +template +concept RtpsMessage = requires(const T &value, std::span bytes) { + { cdr::serialized_size(value) } -> std::convertible_to; + {cdr::deserialize(bytes)}; +}; + +/// @brief Which request/reply protocol a typed service or action endpoint uses. +enum class RtpsProtocol { + ROS2, ///< ROS 2-interoperable (rq/rr topics + related_sample_identity). + NATIVE, ///< Lean espp<->espp protocol (in-band header, es_rq/es_rr topics). +}; + +namespace detail { +/// Serialize a reflectable message to CDR (ROS 2 / classic XCDR1) bytes. Returns +/// an empty vector on failure. +template std::vector rtps_serialize(const T &value) { + std::vector buf(cdr::serialized_size(value)); + const auto written = + cdr::serialize_into(value, std::as_writable_bytes(std::span(buf))); + if (!written) { + return {}; + } + buf.resize(*written); + return buf; +} + +/// Deserialize CDR bytes to a reflectable message. cdr::deserialize returns a +/// std::expected; this collapses it to std::optional (nullopt on failure). +template std::optional rtps_deserialize(std::span bytes) { + auto result = cdr::deserialize(std::as_bytes(bytes)); + if (!result) { + return std::nullopt; + } + return std::move(*result); +} +} // namespace detail + +} // namespace espp diff --git a/components/rtps_embedded/include/rtps_participant.hpp b/components/rtps_embedded/include/rtps_participant.hpp index cd9e4fbc9..20aadeaf7 100644 --- a/components/rtps_embedded/include/rtps_participant.hpp +++ b/components/rtps_embedded/include/rtps_participant.hpp @@ -2,10 +2,13 @@ #include #include +#include #include #include +#include #include #include +#include #include #include #include @@ -25,6 +28,14 @@ class Reader; class ReaderCacheChange; } // namespace rtps +// The RPC layer (services + actions, ROS-interoperable and native) is compiled +// in by default. Define RTPS_NO_RPC (the ESP Kconfig option RTPS_ENABLE_RPC=n +// does this via CMake) to exclude it and its std::thread/std::future use, saving +// flash on devices that only need pub/sub. Pure pub/sub is unaffected. +#if !defined(RTPS_NO_RPC) +#define RTPS_WITH_RPC 1 +#endif + namespace espp { /// @brief RTPS/DDS participant for pub/sub interop with FastDDS and ROS 2. @@ -169,6 +180,314 @@ class RtpsParticipant : public BaseComponent { /// the payload exceeds max_payload_size (see that constant). bool publish(std::string_view topic, std::span cdr_payload); + // --------------------------------------------------------------------------- + // Services (RMI: request/reply), ROS 2 (rmw_fastrtps) interoperable. + // + // A service maps onto a request topic (rq/Request) + a reply topic + // (rr/Reply), with replies correlated to requests via the + // related_sample_identity inline QoS (see RMI_AMI_DESIGN.md). Payloads are + // CDR-encapsulated, exactly like publish()/on_sample: for ROS 2 the request is + // a _Request and the reply a _Response. + // --------------------------------------------------------------------------- + +#ifdef RTPS_WITH_RPC + /// Handler for a service server: given a CDR-encapsulated request, return the + /// CDR-encapsulated reply. Runs on an engine worker thread - return promptly. + using service_handler_t = std::function(std::span request)>; + + /// Configuration for a service server or client. + struct ServiceConfig { + std::string service; ///< ROS 2 service name, e.g. "/add_two_ints". + /// Base DDS service type, e.g. "example_interfaces::srv::dds_::AddTwoInts". + /// The _Request_/_Response_ suffixes are derived internally. + std::string type_name; + }; + + /// Handle to reply to a service request later (deferred reply). Copyable and + /// movable; safe to store and fulfill from any thread. reply() sends the + /// correlated response exactly once (subsequent calls are ignored). Used when + /// the response is not ready when the request arrives - e.g. an action's + /// get_result, which must wait for the goal to finish. See + /// add_service_server_deferred(). + class ServiceResponder { + public: + ServiceResponder() = default; ///< Empty/invalid responder. + /// Send the CDR-encapsulated response, correlated to the original request. + /// No-op if invalid or already replied. + void reply(std::span response) const; + bool valid() const { return static_cast(state_); } + + private: + friend class RtpsParticipant; + struct State; + explicit ServiceResponder(std::shared_ptr state) + : state_(std::move(state)) {} + std::shared_ptr state_; + }; + + /// Deferred service handler: invoked with the request and a responder. The + /// handler may call responder.reply() immediately or store the responder and + /// reply later (from any thread). Unlike service_handler_t this never blocks a + /// worker waiting for a slow response. + using service_deferred_handler_t = + std::function request, ServiceResponder responder)>; + + /// Client handle for calling a service. Obtain one from add_service_client(); + /// it stays valid until the participant is stopped/destroyed. + class ServiceClient { + public: + /// Callback delivering a CDR-encapsulated reply for a call_async() request. + using reply_callback_t = std::function reply)>; + + ~ServiceClient(); + ServiceClient(const ServiceClient &) = delete; + ServiceClient &operator=(const ServiceClient &) = delete; + + /// Send a request and invoke on_reply when the correlated reply arrives. + /// \return False if the participant is not started or the request could not + /// be queued. The callback runs on an engine worker thread. + bool call_async(std::span request, reply_callback_t on_reply); + + /// Send a request and block until the correlated reply arrives or timeout. + /// \return The CDR-encapsulated reply, or std::nullopt on timeout/failure. + /// Do not call from within an engine callback (it would deadlock). + std::optional> call(std::span request, + std::chrono::milliseconds timeout); + + /// Send a request and return a future that becomes ready with the correlated + /// reply (or std::nullopt if the request could not be queued). The future + /// never blocks a worker thread; wait on it (or wait_for a timeout) from the + /// caller. A pending request without a reply leaves the future unfulfilled + /// until the participant stops. + std::future>> call_future(std::span request); + + private: + friend class RtpsParticipant; + struct Impl; + explicit ServiceClient(std::unique_ptr impl); + std::unique_ptr impl_; + }; + + /// Add a service server. The handler is invoked for each request; its return + /// value is sent back as the reply, correlated to the requesting client. + /// \return True on success (false when not started or endpoint creation fails). + bool add_service_server(const ServiceConfig &config, service_handler_t handler); + + /// Add a service server that replies asynchronously via a ServiceResponder. + /// Use this when the response may not be ready when the request arrives (e.g. + /// an action's get_result). \return True on success. + bool add_service_server_deferred(const ServiceConfig &config, service_deferred_handler_t handler); + + /// Add a service client for calling a service. + /// \return A client handle, or nullptr on failure (not started / endpoint + /// creation failed). Owned by the participant; valid until stop(). + std::shared_ptr add_service_client(const ServiceConfig &config); + + // --------------------------------------------------------------------------- + // Actions (AMI: goal server), ROS 2 (rmw_fastrtps) interoperable. + // + // An action maps onto 3 services (send_goal, cancel_goal, get_result) + 2 + // topics (feedback, status) - no new wire primitive. Goal/result/feedback + // payloads are CDR-encapsulated exactly like publish()/services (for ROS 2, the + // action's Goal/Result/Feedback messages). See RMI_AMI_DESIGN.md. + // --------------------------------------------------------------------------- + + /// 16-byte action goal id (unique_identifier_msgs/UUID). + using GoalId = std::array; + + /// Configuration for an action server or client. + struct ActionConfig { + std::string action; ///< ROS 2 action name, e.g. "/fibonacci". + /// Base DDS action type, e.g. "example_interfaces::action::dds_::Fibonacci". + std::string type_name; + }; + + /// Server-side handle to a running goal, passed to the execute callback (which + /// runs on its own thread). Publish feedback and terminate the goal through it. + class ActionGoalHandle { + public: + const GoalId &goal_id() const; + /// The CDR-encapsulated goal payload. + std::span goal() const; + /// Publish a CDR-encapsulated feedback message for this goal. + void publish_feedback(std::span feedback) const; + /// Terminate the goal SUCCEEDED/ABORTED/CANCELED with a CDR result payload. + void succeed(std::span result) const; + void abort(std::span result) const; + void canceled(std::span result) const; + /// True if a cancel has been requested for this goal. + bool is_canceling() const; + + private: + friend class RtpsParticipant; + struct State; + explicit ActionGoalHandle(std::shared_ptr state) + : state_(std::move(state)) {} + void terminate(int status_value, std::span result) const; + std::shared_ptr state_; + }; + + /// Called when a goal arrives; return true to accept, false to reject. + using action_goal_callback_t = + std::function goal)>; + /// Called (on its own thread) to run an accepted goal to completion. + using action_execute_callback_t = std::function; + /// Called when a cancel is requested for a goal; return true to accept. + using action_cancel_callback_t = std::function; + + /// Add an action server. on_goal decides acceptance; execute runs each accepted + /// goal on its own thread; on_cancel (optional) accepts/rejects cancellations. + /// \return True on success (false when not started or endpoint creation fails). + bool add_action_server(const ActionConfig &config, action_goal_callback_t on_goal, + action_execute_callback_t execute, + action_cancel_callback_t on_cancel = nullptr); + + /// Client handle for calling an action. Obtain from add_action_client(). + class ActionClient { + public: + /// CDR-encapsulated feedback for an in-progress goal. + using feedback_callback_t = std::function feedback)>; + /// Terminal result: the GoalStatus value + the CDR-encapsulated result. + using result_callback_t = std::function result)>; + + ~ActionClient(); + ActionClient(const ActionClient &) = delete; + ActionClient &operator=(const ActionClient &) = delete; + + /// Send a goal. on_feedback is invoked for each feedback message; on_result + /// once when the goal terminates (or is rejected, with an empty result). + /// \return The generated goal id, or std::nullopt on failure. + std::optional send_goal(std::span goal, feedback_callback_t on_feedback, + result_callback_t on_result); + /// Request cancellation of a previously sent goal. + bool cancel_goal(const GoalId &goal_id); + + private: + friend class RtpsParticipant; + struct Impl; + explicit ActionClient(std::unique_ptr impl); + std::unique_ptr impl_; + }; + + /// Add an action client. \return A handle, or nullptr on failure. + std::shared_ptr add_action_client(const ActionConfig &config); + + // --------------------------------------------------------------------------- + // Native services (espp<->espp): a lean request/reply that trades ROS 2 + // interop for simplicity. Correlation is a 20-byte in-band header (no inline + // QoS, no rq/rr mangling), riding plain reliable pub/sub on es_rq/es_rr topics. + // Same client ergonomics as the ROS services (sync / callback / future). NOT + // interoperable with ROS 2 - use add_service_* for that. See RMI_AMI_DESIGN.md. + // --------------------------------------------------------------------------- + + /// Client handle for a native service (see add_native_service_client()). + class NativeServiceClient { + public: + using reply_callback_t = std::function reply)>; + ~NativeServiceClient(); + NativeServiceClient(const NativeServiceClient &) = delete; + NativeServiceClient &operator=(const NativeServiceClient &) = delete; + + /// Send a request; invoke on_reply when the correlated reply arrives. + bool call_async(std::span request, reply_callback_t on_reply); + /// Send a request and block for the reply (std::nullopt on timeout/failure). + std::optional> call(std::span request, + std::chrono::milliseconds timeout); + /// Send a request and return a future for the reply. + std::future>> call_future(std::span request); + + private: + friend class RtpsParticipant; + struct Impl; + explicit NativeServiceClient(std::unique_ptr impl); + std::unique_ptr impl_; + }; + + /// Add a native (espp<->espp) service server. \return True on success. + bool add_native_service_server(const ServiceConfig &config, service_handler_t handler); + /// Add a native (espp<->espp) service client. \return A handle, or nullptr. + std::shared_ptr add_native_service_client(const ServiceConfig &config); + + // --------------------------------------------------------------------------- + // Native actions (espp<->espp): a lean AMI - one native request/reply + // (send_goal -> {accepted, goal_handle}) + one feedback topic carrying the + // terminal result. ~3 endpoints vs the ROS action's ~10. NOT ROS-interoperable. + // --------------------------------------------------------------------------- + + /// Server-side handle to a running native goal (passed to the execute callback, + /// which runs on its own thread). + class NativeGoalHandle { + public: + uint32_t goal_handle() const; + std::span goal() const; + void publish_feedback(std::span feedback) const; + void succeed(std::span result) const; + void abort(std::span result) const; + /// Terminate the goal CANCELED (in response to a cancel request). + void canceled(std::span result) const; + /// True if the client has requested cancellation of this goal (and the + /// server's on_cancel, if any, accepted it). A long-running execute callback + /// should poll this and wind down - calling canceled()/abort() - when set. + bool is_canceling() const; + + private: + friend class RtpsParticipant; + struct State; + explicit NativeGoalHandle(std::shared_ptr state) + : state_(std::move(state)) {} + void terminate(uint8_t status, std::span result) const; + std::shared_ptr state_; + }; + + using native_goal_callback_t = std::function goal)>; + using native_execute_callback_t = std::function; + /// Cancel policy: return true to accept a cancel request for the goal_handle + /// (the execute callback then observes is_canceling()). Default (nullptr) + /// accepts every cancel. + using native_cancel_callback_t = std::function; + + /// Add a native action server. on_goal accepts/rejects; execute runs each + /// accepted goal on its own thread; on_cancel (optional) gates cancel requests. + /// \return True on success. + bool add_native_action_server(const ActionConfig &config, native_goal_callback_t on_goal, + native_execute_callback_t execute, + native_cancel_callback_t on_cancel = nullptr); + + /// Client handle for a native action. + class NativeActionClient { + public: + using feedback_callback_t = std::function feedback)>; + /// Terminal result: the NativeGoalStatus value + the CDR result payload. + using result_callback_t = std::function result)>; + /// Invoked once when the server accepts the goal, with the server-assigned + /// goal_handle - keep it to cancel_goal() the goal later. + using accepted_callback_t = std::function; + + ~NativeActionClient(); + NativeActionClient(const NativeActionClient &) = delete; + NativeActionClient &operator=(const NativeActionClient &) = delete; + + /// Send a goal; on_feedback per feedback message, on_result once at the end, + /// on_accepted (optional) with the assigned goal_handle when accepted. + /// \return True if the goal was queued. + bool send_goal(std::span goal, feedback_callback_t on_feedback, + result_callback_t on_result, accepted_callback_t on_accepted = nullptr); + + /// Request cancellation of a previously accepted goal by its goal_handle + /// (from on_accepted). \return True if the cancel request was queued. + bool cancel_goal(uint32_t goal_handle); + + private: + friend class RtpsParticipant; + struct Impl; + explicit NativeActionClient(std::unique_ptr impl); + std::unique_ptr impl_; + }; + + /// Add a native action client. \return A handle, or nullptr. + std::shared_ptr add_native_action_client(const ActionConfig &config); +#endif // RTPS_WITH_RPC + protected: /// Per-reader context bridging the engine's C function-pointer callback to /// the std::function callback; heap-allocated so its address stays stable @@ -184,6 +503,15 @@ class RtpsParticipant : public BaseComponent { static void publisher_matched_trampoline(void *arg); static void subscriber_matched_trampoline(void *arg); +#ifdef RTPS_WITH_RPC + /// Per-server bridge from the engine request-reader callback to the user + /// handler + reply writer. Heap-allocated for a stable address; defined in the + /// .cpp so engine types stay out of this header. + struct ServiceServerContext; + static void service_request_trampoline(void *arg, const rtps::ReaderCacheChange &change); + static void service_reply_trampoline(void *arg, const rtps::ReaderCacheChange &change); +#endif // RTPS_WITH_RPC + bool resolve_interface_address(std::array &ip_bytes) const; Config config_; @@ -193,6 +521,40 @@ class RtpsParticipant : public BaseComponent { rtps::Participant *participant_{nullptr}; std::unordered_map writers_; std::vector> reader_contexts_; + + // Shared liveness token for async RPC reply paths. A deferred service responder + // (which user code may hold and fulfill arbitrarily long after the request) + // checks `alive` under this mutex before writing through its engine reply + // writer; stop() flips it false under the same mutex before destroying the + // domain, so a reply that races shutdown safely no-ops instead of using a + // freed writer. A shared_ptr so it outlives the participant - a late reply + // then sees `alive == false` and never dereferences freed state. + struct Liveness { + std::mutex m; + bool alive{true}; + }; + std::shared_ptr live_; +#ifdef RTPS_WITH_RPC + // shared_ptr (not unique_ptr) so an incomplete ServiceServerContext can be held + // here: shared_ptr's destructor is type-erased, so the member vector needs no + // complete type at the participant's construction/destruction point (the + // context is defined in the .cpp). All the other RPC context containers below + // follow the same rule. + std::vector> service_servers_; + std::vector> service_clients_; + + struct ActionServerContext; + std::vector> action_servers_; + std::vector> action_clients_; + + struct NativeServiceServerContext; + std::vector> native_service_servers_; + std::vector> native_service_clients_; + + struct NativeActionServerContext; + std::vector> native_action_servers_; + std::vector> native_action_clients_; +#endif // RTPS_WITH_RPC }; } // namespace espp diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps_embedded/include/rtps_pubsub.hpp index 6d75da1c9..84315546a 100644 --- a/components/rtps_embedded/include/rtps_pubsub.hpp +++ b/components/rtps_embedded/include/rtps_pubsub.hpp @@ -12,21 +12,11 @@ #include #include "cdr.hpp" +#include "rtps_message.hpp" // RtpsMessage concept (shared with services/actions) #include "rtps_participant.hpp" namespace espp { -/// @brief A type usable with the typed RTPS pub/sub layer. -/// -/// Any reflectable struct the `cdr` component can serialize and deserialize -/// qualifies - no base class, macros, or member functions required. This mirrors -/// the ROS 2 / DDS message model: a plain data struct whose fields map to CDR. -template -concept RtpsMessage = requires(const T &value, std::span bytes) { - { cdr::serialized_size(value) } -> std::convertible_to; - {cdr::deserialize(bytes)}; -}; - /// @brief Typed publisher: publish reflectable message structs on a topic. /// /// A thin, header-only wrapper over espp::RtpsParticipant that removes the manual diff --git a/components/rtps_embedded/include/rtps_service.hpp b/components/rtps_embedded/include/rtps_service.hpp new file mode 100644 index 000000000..1893056dc --- /dev/null +++ b/components/rtps_embedded/include/rtps_service.hpp @@ -0,0 +1,180 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_message.hpp" // RtpsMessage, RtpsProtocol, detail::rtps_(de)serialize +#include "rtps_participant.hpp" + +namespace espp { + +#ifdef RTPS_WITH_RPC + +/// @brief Typed service server (RMI): answers Request messages with Response +/// messages, with no manual CDR handling. +/// +/// A thin, header-only wrapper over espp::RtpsParticipant that (de)serializes the +/// reflectable Request/Response structs around the byte-level service API. Works +/// for both the ROS 2-interoperable and the native protocol (see Config::protocol). +/// +/// @code +/// struct AddReq { int64_t a, b; }; +/// struct AddResp { int64_t sum; }; +/// espp::ServiceServer server(participant, { +/// .service = "/add_two_ints", +/// .type_name = "example_interfaces::srv::dds_::AddTwoInts", +/// .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); +/// @endcode +/// +/// @tparam Request Reflectable request message type (the service Request). +/// @tparam Response Reflectable response message type (the service Response). +template class ServiceServer { +public: + /// Handler: given a typed request, return the typed response. Runs on an + /// engine worker thread - return promptly. + using handler_t = std::function; + + /// Configuration for a typed service server. + struct Config { + std::string service; ///< Service name, e.g. "/add_two_ints". + std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). + handler_t handler; ///< Request -> Response. + RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + }; + + /// Construct and register the server on a started participant, which must + /// outlive this object. Check is_valid(). + /// \param participant The started participant to serve through. + /// \param config The server configuration (service name, type, handler). + ServiceServer(RtpsParticipant &participant, const Config &config) { + auto handler = config.handler; + auto byte_handler = [handler](std::span req_bytes) -> std::vector { + auto req = detail::rtps_deserialize(req_bytes); + if (!req || !handler) { + return {}; + } + return detail::rtps_serialize(handler(*req)); + }; + if (config.protocol == RtpsProtocol::NATIVE) { + valid_ = participant.add_native_service_server({config.service, config.type_name}, + std::move(byte_handler)); + } else { + valid_ = participant.add_service_server({config.service, config.type_name}, + std::move(byte_handler)); + } + } + + /// \return True if the server registered successfully. + [[nodiscard]] bool is_valid() const { return valid_; } + +private: + bool valid_{false}; +}; + +/// @brief Typed service client (RMI): call a service with a Request and get a +/// Response, with no manual CDR handling. Blocking, callback, and future styles. +/// +/// @code +/// espp::ServiceClient client(participant, { +/// .service = "/add_two_ints", +/// .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); +/// if (auto resp = client.call(AddReq{7, 35}, 1s)) use(resp->sum); +/// @endcode +/// +/// @tparam Request Reflectable request message type (the service Request). +/// @tparam Response Reflectable response message type (the service Response). +template class ServiceClient { +public: + /// Callback delivering the typed response for a call_async() request. Runs on + /// an engine worker thread - return promptly. + using response_callback_t = std::function; + + /// Configuration for a typed service client. + struct Config { + std::string service; ///< Service name, e.g. "/add_two_ints". + std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). + RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + }; + + /// Construct and register the client on a started participant, which must + /// outlive this object. Check is_valid(). + /// \param participant The started participant to call through. + /// \param config The client configuration (service name, type, protocol). + ServiceClient(RtpsParticipant &participant, const Config &config) { + if (config.protocol == RtpsProtocol::NATIVE) { + native_ = participant.add_native_service_client({config.service, config.type_name}); + } else { + ros_ = participant.add_service_client({config.service, config.type_name}); + } + } + + /// \return True if the client registered successfully. + [[nodiscard]] bool is_valid() const { return ros_ != nullptr || native_ != nullptr; } + + /// Blocking call (RMI): send the request and wait for the correlated reply. + /// \param request The typed request. + /// \param timeout How long to wait for the reply. + /// \return The Response, or std::nullopt on timeout / failure. Do not call + /// from within an engine callback (it would deadlock). + std::optional call(const Request &request, std::chrono::milliseconds timeout) { + const auto req = detail::rtps_serialize(request); + std::optional> reply; + if (ros_) { + reply = ros_->call(req, timeout); + } else if (native_) { + reply = native_->call(req, timeout); + } + if (!reply) { + return std::nullopt; + } + return detail::rtps_deserialize(*reply); + } + + /// Async call (AMI): on_response(Response) is invoked when the correlated reply + /// arrives (on an engine worker thread). + /// \param request The typed request. + /// \param on_response Called once with the typed response. + /// \return False if the request could not be queued. + bool call_async(const Request &request, const response_callback_t &on_response) { + const auto req = detail::rtps_serialize(request); + auto cb = [on_response](std::span reply_bytes) { + auto resp = detail::rtps_deserialize(reply_bytes); + if (resp && on_response) { + on_response(*resp); + } + }; + if (ros_) { + return ros_->call_async(req, std::move(cb)); + } + if (native_) { + return native_->call_async(req, std::move(cb)); + } + return false; + } + + /// Future-based call (AMI): the future becomes ready with the Response + /// (std::nullopt if the request could not be queued). Works for both the ROS 2 + /// and native protocols (built on call_async). Wait on the future - or + /// wait_for a timeout - from the caller; do not block a worker thread. + std::future> call_future(const Request &request) { + auto promise = std::make_shared>>(); + auto future = promise->get_future(); + if (!call_async(request, [promise](const Response &r) { promise->set_value(r); })) { + promise->set_value(std::nullopt); + } + return future; + } + +private: + std::shared_ptr ros_; + std::shared_ptr native_; +}; + +#endif // RTPS_WITH_RPC + +} // namespace espp diff --git a/components/rtps_embedded/interop/Dockerfile b/components/rtps_embedded/interop/Dockerfile index 459fcbc43..338438efb 100644 --- a/components/rtps_embedded/interop/Dockerfile +++ b/components/rtps_embedded/interop/Dockerfile @@ -7,7 +7,17 @@ FROM ros:jazzy-ros-base RUN apt-get update && apt-get install -y --no-install-recommends \ - build-essential cmake git rsync python3-dev pybind11-dev \ + build-essential cmake git rsync python3-dev python3-pip python3-venv pybind11-dev \ + ros-jazzy-example-interfaces \ && rm -rf /var/lib/apt/lists/* +# A venv with the espp wheel build backend, so run_interop.sh can build the +# bindings in-container and functionally test them (python/rtps_rpc_demo.py). A +# venv (not --break-system-packages) avoids clashing with Ubuntu's debian-managed +# packaging. --system-site-packages so the built espp module can still see any +# system deps if needed; the demo itself is pure espp (no rclpy). +RUN python3 -m venv --system-site-packages /opt/espp-venv \ + && /opt/espp-venv/bin/pip install --no-cache-dir \ + scikit-build-core pybind11 setuptools-scm pycdr2 + WORKDIR /work diff --git a/components/rtps_embedded/interop/ros2_add_two_ints_server.py b/components/rtps_embedded/interop/ros2_add_two_ints_server.py new file mode 100644 index 000000000..e99c4a14b --- /dev/null +++ b/components/rtps_embedded/interop/ros2_add_two_ints_server.py @@ -0,0 +1,24 @@ +# rclpy add_two_ints service server for the espp interop matrix: the espp +# service CLIENT (rtps_service_interop_client) calls this and checks the sum. +import rclpy +from rclpy.node import Node +from example_interfaces.srv import AddTwoInts + + +class AddTwoIntsServer(Node): + def __init__(self): + super().__init__("add_two_ints_server") + self.create_service(AddTwoInts, "add_two_ints", self.cb) + + def cb(self, request, response): + response.sum = request.a + request.b + self.get_logger().info(f"{request.a} + {request.b} = {response.sum}") + return response + + +def main(): + rclpy.init() + rclpy.spin(AddTwoIntsServer()) + + +main() diff --git a/components/rtps_embedded/interop/ros2_fibonacci_server.py b/components/rtps_embedded/interop/ros2_fibonacci_server.py new file mode 100644 index 000000000..fe29cdce9 --- /dev/null +++ b/components/rtps_embedded/interop/ros2_fibonacci_server.py @@ -0,0 +1,37 @@ +# rclpy Fibonacci action server for the espp interop matrix: the espp action +# CLIENT (rtps_action_interop_client) drives this and checks the result. +import time + +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node +from example_interfaces.action import Fibonacci + + +class FibonacciServer(Node): + def __init__(self): + super().__init__("fibonacci_server") + self._server = ActionServer(self, Fibonacci, "fibonacci", self.execute) + + def execute(self, goal_handle): + order = goal_handle.request.order + seq = [0, 1] + for i in range(1, order): + seq.append(seq[i] + seq[i - 1]) + fb = Fibonacci.Feedback() + fb.sequence = seq + goal_handle.publish_feedback(fb) + time.sleep(0.2) + goal_handle.succeed() + result = Fibonacci.Result() + result.sequence = seq + self.get_logger().info(f"goal order={order} -> {seq}") + return result + + +def main(): + rclpy.init() + rclpy.spin(FibonacciServer()) + + +main() diff --git a/components/rtps_embedded/interop/run_interop.sh b/components/rtps_embedded/interop/run_interop.sh index fa4e597f2..8e333619a 100755 --- a/components/rtps_embedded/interop/run_interop.sh +++ b/components/rtps_embedded/interop/run_interop.sh @@ -28,7 +28,12 @@ cmake -S lib -B lib/build -DCMAKE_BUILD_TYPE=Release > /tmp/cmake_lib.log 2>&1 \ && cmake -S pc -B pc/build -DCMAKE_BUILD_TYPE=Release > /tmp/cmake.log 2>&1 \ && cmake --build pc/build -j"$(nproc)" --target \ rtps_embedded_pubsub rtps_embedded_golden rtps_facade_pubsub rtps_typed_pubsub \ - rtps_facade_frag rtps_facade_backlog rtps_facade_frag_sizes \ + rtps_facade_frag rtps_facade_backlog rtps_facade_frag_sizes rtps_service_loopback \ + rtps_service_naming rtps_action_naming rtps_action_types rtps_native_protocol \ + rtps_action_loopback \ + rtps_native_service_loopback rtps_native_action_loopback rtps_typed_rpc_loopback \ + rtps_service_interop_server rtps_service_interop_client \ + rtps_action_interop_server rtps_action_interop_client \ rtps_embedded_interop_pub rtps_embedded_interop_sub > /tmp/build.log 2>&1 build_rc=$? result "build" $build_rc @@ -42,6 +47,16 @@ export RMW_IMPLEMENTATION=rmw_fastrtps_cpp note "Golden wire-format tests" "$BIN"/rtps_embedded_golden; result "golden" $? +note "ROS 2 service + action name/type mangling (host)" +"$BIN"/rtps_service_naming; result "service_naming" $? +"$BIN"/rtps_action_naming; result "action_naming" $? + +note "ROS 2 action envelope codec vs captured Fibonacci bytes (host)" +"$BIN"/rtps_action_types; result "action_types" $? + +note "native (espp<->espp) protocol codec: header + action framing (host)" +"$BIN"/rtps_native_protocol; result "native_protocol" $? + note "espp <-> espp in-process loopback" "$BIN"/rtps_embedded_pubsub; result "loopback" $? @@ -57,6 +72,30 @@ note "typed pub/sub in-process" note "reliable backlog: no sample skipped (dynamic history growth)" "$BIN"/rtps_facade_backlog; result "backlog_no_skip" $? +# Service (RMI) request/reply in-process: exercises name mangling + the +# related_sample_identity inline-QoS emit/parse + pending-request correlation. +# Non-fragmented small payloads, so robust in the shared-netns container. +note "service (RMI) request/reply loopback (related_sample_identity correlation)" +"$BIN"/rtps_service_loopback; result "service_loopback" $? + +# Action (AMI) in-process: send_goal + feedback + get_result (deferred) over the +# 3 services + 2 topics, correlated by goal UUID. Non-fragmented, container-robust. +note "action (AMI) goal server loopback (Fibonacci: feedback + deferred result)" +"$BIN"/rtps_action_loopback; result "action_loopback" $? + +# Native (espp<->espp) lean request/reply: 20-byte in-band header over pub/sub, +# all three client styles (sync/async/future). Not ROS-interoperable by design. +note "native (espp<->espp) service loopback (in-band correlation header)" +"$BIN"/rtps_native_service_loopback; result "native_service_loopback" $? + +note "native (espp<->espp) action loopback (lean AMI: goal + feedback + result)" +"$BIN"/rtps_native_action_loopback; result "native_action_loopback" $? + +# Typed espp-idiomatic wrappers (ServiceServer/Client, ActionServer/Client) over +# reflectable structs - the RMI/AMI analogue of Publisher/Subscriber. +note "typed RMI/AMI wrappers loopback (ServiceServer/Client + ActionServer/Client)" +"$BIN"/rtps_typed_rpc_loopback; result "typed_rpc_loopback" $? + # NOTE: the in-process fragmented loopbacks (rtps_facade_frag, rtps_facade_frag_sizes) # are BUILT above (compile guard) but run as standalone host gates (docker-free), # not here: two participants sharing one @@ -160,6 +199,90 @@ echo "--- subbig tail ---"; tail -20 /tmp/subbig.log echo "--- rospubbig tail ---"; tail -5 /tmp/rospubbig.log result "ros2_pub->espp_sub_200k" $big2_rc +# --- Services (RMI): live ROS 2 <-> espp request/reply ----------------------- +# The espp service uses the same rq/rr topics + _Request_/_Response_ types + +# related_sample_identity inline QoS that rmw_fastrtps uses, so a real ROS 2 +# client/server interoperates with no type-hash exchange (verified: the service +# even appears in `ros2 service list`). + +note "espp service server <- ROS 2 client (ros2 service call add_two_ints)" +"$BIN"/rtps_service_interop_server /add_two_ints example_interfaces::srv::dds_::AddTwoInts 40 \ + > /tmp/svcsrv.log 2>&1 & +SVCSRV=$! +sleep 4 +timeout 25 ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}" \ + > /tmp/svccall.log 2>&1 +# Pass iff ROS 2 got the correct response (sum=42). +grep -q "sum=42" /tmp/svccall.log; svccall_rc=$? +sleep 1 +kill $SVCSRV 2>/dev/null; wait $SVCSRV 2>/dev/null +echo "--- ros2 service call ---"; tail -3 /tmp/svccall.log +echo "--- espp server ---"; grep "server:" /tmp/svcsrv.log | tail -3 +result "espp_service_server<-ros2_client" $svccall_rc + +note "espp service client -> ROS 2 server (rclpy add_two_ints)" +python3 /work/components/rtps_embedded/interop/ros2_add_two_ints_server.py > /tmp/rossvcsrv.log 2>&1 & +ROSSVC=$! +sleep 4 +timeout 35 "$BIN"/rtps_service_interop_client /add_two_ints example_interfaces::srv::dds_::AddTwoInts \ + 20 22 30 > /tmp/svcclient.log 2>&1 +svcclient_rc=$? +kill $ROSSVC 2>/dev/null; wait $ROSSVC 2>/dev/null +echo "--- espp client ---"; tail -2 /tmp/svcclient.log +echo "--- rclpy server ---"; grep -iE "= [0-9]" /tmp/rossvcsrv.log | tail -2 +result "espp_service_client->ros2_server" $svcclient_rc + +# --- Actions (AMI): live ROS 2 <-> espp Fibonacci --------------------------- + +note "espp action server <- ROS 2 client (ros2 action send_goal -f fibonacci)" +"$BIN"/rtps_action_interop_server /fibonacci example_interfaces::action::dds_::Fibonacci 45 \ + > /tmp/actsrv.log 2>&1 & +ACTSRV=$! +sleep 5 +timeout 30 ros2 action send_goal -f /fibonacci example_interfaces/action/Fibonacci "{order: 5}" \ + > /tmp/actsend.log 2>&1 +# Pass iff the ROS 2 client's goal completed on the espp server (full round-trip: +# send_goal accepted -> feedback -> deferred get_result -> SUCCEEDED). The result +# sequence itself is byte-validated separately by the action_types codec test. +grep -q "status: SUCCEEDED" /tmp/actsend.log +actsend_rc=$? +sleep 1 +kill $ACTSRV 2>/dev/null; wait $ACTSRV 2>/dev/null +echo "--- ros2 action send_goal ---"; grep -iE "Result:|sequence=|status|Goal finished" /tmp/actsend.log | tail -4 +echo "--- espp action server ---"; grep "server:" /tmp/actsrv.log | tail -3 +result "espp_action_server<-ros2_client" $actsend_rc + +note "espp action client -> ROS 2 server (rclpy Fibonacci)" +python3 /work/components/rtps_embedded/interop/ros2_fibonacci_server.py > /tmp/rosactsrv.log 2>&1 & +ROSACT=$! +sleep 5 +timeout 40 "$BIN"/rtps_action_interop_client /fibonacci example_interfaces::action::dds_::Fibonacci \ + 5 30 > /tmp/actclient.log 2>&1 +actclient_rc=$? +kill $ROSACT 2>/dev/null; wait $ROSACT 2>/dev/null +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 + +# --- 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 / +# action / native bindings that the C++ tests cannot reach. +note "Python bindings functional demo (services + actions + native, 5 mechanisms)" +# The container-local copy has no .git (rsync excludes it), so setuptools-scm +# cannot derive a version - pin one via SETUPTOOLS_SCM_PRETEND_VERSION. +SETUPTOOLS_SCM_PRETEND_VERSION=0.0.0+interop \ + /opt/espp-venv/bin/pip install --no-build-isolation --no-deps -q . > /tmp/pywheel.log 2>&1 +pywheel_rc=$? +if [ $pywheel_rc -ne 0 ]; then + echo "python wheel build failed"; tail -20 /tmp/pywheel.log; result "python_rpc_demo" 1 +else + /opt/espp-venv/bin/python python/rtps_rpc_demo.py > /tmp/pydemo.log 2>&1 + pydemo_rc=$? + grep -E "PASS|FAIL|ALL PASS|FAILURES" /tmp/pydemo.log | tail -8 + result "python_rpc_demo" $pydemo_rc +fi + echo "" echo "==================== SUMMARY ====================" echo "PASS=$PASS FAIL=$FAIL" diff --git a/components/rtps_embedded/src/entities/StatefulWriter.cpp b/components/rtps_embedded/src/entities/StatefulWriter.cpp index c39e5a436..a899960ad 100644 --- a/components/rtps_embedded/src/entities/StatefulWriter.cpp +++ b/components/rtps_embedded/src/entities/StatefulWriter.cpp @@ -85,9 +85,10 @@ void StatefulWriter::reset() { // TODO } -const rtps::CacheChange *StatefulWriter::newChange(ChangeKind_t kind, const uint8_t *data, - DataSize_t size, bool inLineQoS, - bool markDisposedAfterWrite) { +const rtps::CacheChange * +StatefulWriter::newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, bool inLineQoS, + bool markDisposedAfterWrite, bool hasRelatedSampleIdentity, + const rpc::SampleIdentity &relatedSampleIdentity) { INIT_GUARD() if (isIrrelevant(kind)) { return nullptr; @@ -111,7 +112,8 @@ const rtps::CacheChange *StatefulWriter::newChange(ChangeKind_t kind, const uint const bool wasFull = m_history.isFull(); const SequenceNumber_t minBefore = m_history.getCurrentSeqNumMin(); - auto *result = m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite); + auto *result = m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite, + hasRelatedSampleIdentity, relatedSampleIdentity); if (wasFull) { const SequenceNumber_t minAfter = m_history.getCurrentSeqNumMin(); @@ -331,16 +333,29 @@ bool StatefulWriter::sendData(const ReaderProxy &reader, const CacheChange *next info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; + if (next->hasRelatedSampleIdentity) { + // ROS 2 service request/reply: carry related_sample_identity as inline QoS. + // Such a sample must fit one DATA submessage - DATA_FRAG carries no inline + // QoS, so fragmenting it would drop the correlation and the caller would + // time out. The RPC facade rejects payloads above MAX_UNFRAGMENTED_RPC_PAYLOAD; + // guard here too rather than emit an uncorrelated fragment. + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_RPC_PAYLOAD) { + return false; + } + MessageFactory::addSubMessageDataWithRelatedSampleIdentity( + payload, next->data, next->relatedSampleIdentity, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); + } else { #ifdef RTPS_ENABLE_FRAGMENTATION - if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { - return sendSampleFragmented(info.destAddr, info.destPort, reader.remoteReaderGuid.entityId, - next); - } + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { + return sendSampleFragmented(info.destAddr, info.destPort, reader.remoteReaderGuid.entityId, + next); + } #endif - - MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, - m_attributes.endpointGuid.entityId, - reader.remoteReaderGuid.entityId); + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId); + } info.payload = std::move(payload.bytes); if (info.payload.empty()) { return false; @@ -450,8 +465,15 @@ bool StatefulWriter::sendDataWRMulticast(const ReaderProxy &reader, const CacheC } #endif - MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, - m_attributes.endpointGuid.entityId, reid); + if (next->hasRelatedSampleIdentity) { + // ROS 2 service request/reply: carry related_sample_identity as inline QoS. + MessageFactory::addSubMessageDataWithRelatedSampleIdentity( + payload, next->data, next->relatedSampleIdentity, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + } else { + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + } info.payload = std::move(payload.bytes); if (info.payload.empty()) { diff --git a/components/rtps_embedded/src/entities/StatelessWriter.cpp b/components/rtps_embedded/src/entities/StatelessWriter.cpp index 83274a125..546434f0d 100644 --- a/components/rtps_embedded/src/entities/StatelessWriter.cpp +++ b/components/rtps_embedded/src/entities/StatelessWriter.cpp @@ -81,7 +81,9 @@ void StatelessWriter::reset() { m_is_initialized_ = false; } const CacheChange *StatelessWriter::newChange(rtps::ChangeKind_t kind, const uint8_t *data, DataSize_t size, bool inLineQoS, - bool markDisposedAfterWrite) { + bool markDisposedAfterWrite, + bool hasRelatedSampleIdentity, + const rpc::SampleIdentity &relatedSampleIdentity) { INIT_GUARD(); if (isIrrelevant(kind)) { return nullptr; @@ -104,7 +106,8 @@ const CacheChange *StatelessWriter::newChange(rtps::ChangeKind_t kind, const uin const bool wasFull = m_history.isFull(); const SequenceNumber_t minBefore = m_history.getSeqNumMin(); - auto *result = m_history.addChange(data, size); + auto *result = m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite, + hasRelatedSampleIdentity, relatedSampleIdentity); if (wasFull) { const SequenceNumber_t minAfter = m_history.getSeqNumMin(); @@ -200,18 +203,33 @@ void StatelessWriter::progress() { reid = proxy.remoteReaderGuid.entityId; } + if (next->hasRelatedSampleIdentity) { + // ROS 2 service request/reply: carry the related_sample_identity as + // inline QoS. Such a sample must fit one DATA submessage - DATA_FRAG + // carries no inline QoS, so fragmenting it would drop the correlation + // and the caller would time out. The RPC facade rejects payloads above + // MAX_UNFRAGMENTED_RPC_PAYLOAD; guard here too rather than emit an + // uncorrelated fragment. + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_RPC_PAYLOAD) { + continue; + } + MessageFactory::addSubMessageDataWithRelatedSampleIdentity( + payload, next->data, next->relatedSampleIdentity, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + } else { #ifdef RTPS_ENABLE_FRAGMENTATION - if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { - // Oversized sample: emit DATA_FRAG submessages (built + sent under the - // lock so next->data stays valid across fragments) and skip the single - // DATA path for this proxy. - sendSampleFragmented(info.destAddr, info.destPort, reid, next); - continue; - } + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { + // Oversized plain sample: emit DATA_FRAG submessages (built + sent + // under the lock so next->data stays valid across fragments) and + // skip the single DATA path for this proxy. + sendSampleFragmented(info.destAddr, info.destPort, reid, next); + continue; + } #endif - MessageFactory::addSubMessageData(payload, next->data, false, next->sequenceNumber, - m_attributes.endpointGuid.entityId, - reid); // TODO + MessageFactory::addSubMessageData(payload, next->data, false, next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reid); // TODO + } } info.payload = std::move(payload.bytes); diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp index 42979eef3..f9619f0d1 100644 --- a/components/rtps_embedded/src/messages/MessageReceiver.cpp +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -157,6 +157,12 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, return false; } + // ROS 2 service correlation: capture a related_sample_identity inline QoS if + // present (PID 0x0083 or the eProsima legacy 0x800f, both a 24-byte + // SampleIdentity). Plain pub/sub leaves this unset. + bool hasRelatedSampleIdentity = false; + rpc::SampleIdentity relatedSampleIdentity{}; + if ((submsgHeader.flags & FLAG_INLINE_QOS) != 0) { const uint8_t *cursor = serializedData; bool foundSentinel = false; @@ -178,6 +184,23 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, if (cursor + length > submessageEnd) { return false; } + + if ((pid == rpc::PID_RELATED_SAMPLE_IDENTITY || + pid == rpc::PID_CUSTOM_RELATED_SAMPLE_IDENTITY) && + length >= rpc::SAMPLE_IDENTITY_CDR_SIZE && !hasRelatedSampleIdentity) { + // 24-byte value: Guid_t (prefix 12 + entityKey 3 + entityKind 1) then + // SequenceNumber_t (high int32, low uint32), all little-endian. + Guid_t g{}; + memcpy(g.prefix.id.data(), cursor, g.prefix.id.size()); + memcpy(g.entityId.entityKey.data(), cursor + 12, g.entityId.entityKey.size()); + memcpy(&g.entityId.entityKind, cursor + 15, sizeof(EntityKind_t)); + SequenceNumber_t seq{}; + memcpy(&seq.high, cursor + 16, sizeof(seq.high)); + memcpy(&seq.low, cursor + 20, sizeof(seq.low)); + relatedSampleIdentity = rpc::SampleIdentity{g, seq}; + hasRelatedSampleIdentity = true; + } + cursor += length; const std::size_t consumed = static_cast(cursor - serializedData); @@ -221,8 +244,9 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, } if (reader != nullptr) { Guid_t writerGuid{sourceState.sourceGuidPrefix, dataSubmsg.writerId}; - ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, dataSubmsg.writerSN, serializedData, - size}; + ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, dataSubmsg.writerSN, + serializedData, size, hasRelatedSampleIdentity, + relatedSampleIdentity}; reader->newChange(change); } else { #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE diff --git a/components/rtps_embedded/src/rtps_participant.cpp b/components/rtps_embedded/src/rtps_participant.cpp index ebdaa21c3..1439da857 100644 --- a/components/rtps_embedded/src/rtps_participant.cpp +++ b/components/rtps_embedded/src/rtps_participant.cpp @@ -1,9 +1,24 @@ #include "rtps_participant.hpp" +#include +#include +#include #include #include +#include +#include +#include +#include +#include +#include +#include #include "rtps/entities/Domain.hpp" +#include "rtps/rpc/action_naming.hpp" +#include "rtps/rpc/action_types.hpp" +#include "rtps/rpc/native_protocol.hpp" +#include "rtps/rpc/sample_identity.hpp" +#include "rtps/rpc/service_naming.hpp" // Host-side interface auto-detection uses platform-specific enumeration APIs. #if defined(ESP_PLATFORM) @@ -27,7 +42,11 @@ RtpsParticipant::RtpsParticipant(const Config &config) : BaseComponent("RtpsParticipant", config.log_level) , config_(config) {} -RtpsParticipant::~RtpsParticipant() { stop(); } +// NOTE: ~RtpsParticipant is defined at the END of this file, after the RPC +// context structs (ServiceServerContext, ActionServerContext, Native*Context, +// *Client::Impl) are complete - the destructor destroys vectors of +// unique_ptr/shared_ptr to those types, and libc++ requires the pointee to be +// complete at the point of destruction (libstdc++ is more lenient). bool RtpsParticipant::resolve_interface_address(std::array &ip_bytes) const { std::string addr = config_.interface_address; @@ -133,6 +152,8 @@ bool RtpsParticipant::start() { } domain_ = std::make_unique(ip_bytes); + // Fresh liveness token for this run (a prior stop() left the old one flipped). + live_ = std::make_shared(); // Engine lifecycle: participants must be created before completeInit() // starts the discovery machinery; endpoints are added after. @@ -163,23 +184,6 @@ bool RtpsParticipant::start() { return true; } -void RtpsParticipant::stop() { - std::lock_guard lock(mutex_); - if (!started_) { - return; - } - started_ = false; - domain_->stop(); - // The engine owns the endpoint objects; drop our references before the - // domain (and with it every writer/reader and their callback registrations) - // goes away. - writers_.clear(); - participant_ = nullptr; - domain_.reset(); - reader_contexts_.clear(); - logger_.info("Stopped"); -} - bool RtpsParticipant::add_writer(const WriterConfig &config) { std::lock_guard lock(mutex_); if (!started_) { @@ -318,4 +322,1309 @@ void RtpsParticipant::subscriber_matched_trampoline(void *arg) { } } +#ifdef RTPS_WITH_RPC +// =========================================================================== +// Services (RMI) - request/reply with related_sample_identity correlation. +// =========================================================================== + +namespace { +// Pack a SequenceNumber_t into a single key for the pending-request map. +uint64_t seq_key(const rtps::SequenceNumber_t &sn) { + return (static_cast(static_cast(sn.high)) << 32) | sn.low; +} +} // namespace + +// Per-server bridge: engine request-reader callback -> user handler -> reply. +struct RtpsParticipant::ServiceServerContext { + RtpsParticipant *self{nullptr}; + service_deferred_handler_t handler{nullptr}; // sync handlers are wrapped as deferred + rtps::Writer *reply_writer{nullptr}; + rtps::Reader *request_reader{nullptr}; +}; + +// Deferred-reply state: the reply writer + the identity to echo, so a response +// can be sent once, later, from any thread. +struct RtpsParticipant::ServiceResponder::State { + rtps::Writer *reply_writer{nullptr}; + rtps::rpc::SampleIdentity related{}; + std::atomic replied{false}; + // Held so a deferred reply that races participant shutdown no-ops instead of + // writing through a freed engine writer (see RtpsParticipant::Liveness). + std::shared_ptr live; +}; + +void RtpsParticipant::ServiceResponder::reply(std::span response) const { + if (!state_ || state_->reply_writer == nullptr || !state_->live) { + return; + } + // A reply carries its related_sample_identity as inline QoS, so it must fit a + // single DATA submessage (a fragmented reply loses the correlation - see + // MAX_UNFRAGMENTED_RPC_PAYLOAD). Reject rather than send an uncorrelated reply. + if (response.size() > rtps::MAX_UNFRAGMENTED_RPC_PAYLOAD) { + return; + } + bool expected = false; + if (!state_->replied.compare_exchange_strong(expected, true)) { + return; // reply exactly once + } + // Hold the liveness lock across the write: stop() flips `alive` false under + // the same lock before destroying the domain, so we either complete the write + // against a still-valid writer or observe !alive and drop. + std::lock_guard live_lock(state_->live->m); + if (!state_->live->alive) { + return; + } + state_->reply_writer->newChangeWithRelatedSampleIdentity( + rtps::ChangeKind_t::ALIVE, response.data(), static_cast(response.size()), + state_->related); +} + +// Client state: request writer + pending-request table keyed by the request's +// RTPS writerSeqNumber (which the server echoes in the reply's +// related_sample_identity), matched on our own reply-reader GUID. +struct RtpsParticipant::ServiceClient::Impl { + struct SyncSlot { + std::mutex m; + std::condition_variable cv; + bool done{false}; + std::vector reply; + }; + struct Pending { + reply_callback_t on_reply{nullptr}; // set for call_async + std::shared_ptr sync{nullptr}; // set for call + }; + + RtpsParticipant *self{nullptr}; + rtps::Writer *request_writer{nullptr}; + rtps::Guid_t reply_reader_guid{}; + std::mutex mutex; + std::unordered_map pending; + + // Send a request carrying our reply-reader GUID as related_sample_identity + // (with an UNKNOWN sequence number, per rmw), register the pending entry keyed + // by the assigned writerSeqNumber, and return that key. nullopt on failure. + std::optional send(std::span request, reply_callback_t on_reply, + std::shared_ptr sync) { + // A request carries its related_sample_identity as inline QoS, so it must + // fit a single DATA submessage (see MAX_UNFRAGMENTED_RPC_PAYLOAD). + if (request.size() > rtps::MAX_UNFRAGMENTED_RPC_PAYLOAD) { + return std::nullopt; + } + std::lock_guard lock(mutex); + // Hold the lock across newChange + insert so a reply can never look up the + // key before it is registered (the send itself is async). + const rtps::rpc::SampleIdentity related{reply_reader_guid, rtps::SequenceNumber_t{-1, 0}}; + const auto *change = request_writer->newChangeWithRelatedSampleIdentity( + rtps::ChangeKind_t::ALIVE, request.data(), static_cast(request.size()), + related); + if (change == nullptr) { + return std::nullopt; + } + const uint64_t key = seq_key(change->sequenceNumber); + pending[key] = Pending{std::move(on_reply), std::move(sync)}; + return key; + } +}; + +void RtpsParticipant::service_request_trampoline(void *arg, const rtps::ReaderCacheChange &change) { + auto *ctx = static_cast(arg); + if (ctx == nullptr || !ctx->handler || ctx->reply_writer == nullptr) { + return; + } + // Copy the request payload (valid only during this callback). + std::vector request(change.getDataSize()); + if (!request.empty() && !change.copyInto(request.data(), change.getDataSize())) { + return; + } + + // Build a responder correlated to this request: echo {client reply-reader GUID + // (from the request's related sample identity), request writerSeqNumber}. A + // sync handler replies immediately; a deferred one may hold the responder. + auto state = std::make_shared(); + state->reply_writer = ctx->reply_writer; + state->live = ctx->self->live_; + state->related.writer_guid = change.hasRelatedSampleIdentity + ? change.relatedSampleIdentity.writer_guid + : change.writerGuid; + state->related.sequence_number = change.sn; + ctx->handler(request, ServiceResponder(state)); +} + +void RtpsParticipant::service_reply_trampoline(void *arg, const rtps::ReaderCacheChange &change) { + auto *impl = static_cast(arg); + if (impl == nullptr || !change.hasRelatedSampleIdentity) { + return; + } + // Only replies addressed to this client (our reply-reader GUID). + if (!(change.relatedSampleIdentity.writer_guid == impl->reply_reader_guid)) { + return; + } + const uint64_t key = seq_key(change.relatedSampleIdentity.sequence_number); + + std::vector reply(change.getDataSize()); + if (!reply.empty() && !change.copyInto(reply.data(), change.getDataSize())) { + return; + } + + ServiceClient::Impl::Pending pending; + { + std::lock_guard lock(impl->mutex); + auto it = impl->pending.find(key); + if (it == impl->pending.end()) { + return; // unknown/duplicate/late reply + } + pending = std::move(it->second); + impl->pending.erase(it); + } + if (pending.sync) { + std::lock_guard lock(pending.sync->m); + pending.sync->reply = std::move(reply); + pending.sync->done = true; + pending.sync->cv.notify_one(); + } else if (pending.on_reply) { + pending.on_reply(reply); + } +} + +RtpsParticipant::ServiceClient::ServiceClient(std::unique_ptr impl) + : impl_(std::move(impl)) {} +RtpsParticipant::ServiceClient::~ServiceClient() = default; + +bool RtpsParticipant::ServiceClient::call_async(std::span request, + reply_callback_t on_reply) { + return impl_->send(request, std::move(on_reply), nullptr).has_value(); +} + +std::optional> +RtpsParticipant::ServiceClient::call(std::span request, + std::chrono::milliseconds timeout) { + auto slot = std::make_shared(); + auto key = impl_->send(request, nullptr, slot); + if (!key.has_value()) { + return std::nullopt; + } + std::unique_lock lock(slot->m); + if (!slot->cv.wait_for(lock, timeout, [&] { return slot->done; })) { + // Timed out: drop the pending entry so a late reply is ignored cleanly. + std::lock_guard plock(impl_->mutex); + impl_->pending.erase(*key); + return std::nullopt; + } + return std::move(slot->reply); +} + +std::future>> +RtpsParticipant::ServiceClient::call_future(std::span request) { + // Promise fulfilled from the async reply callback (shared_ptr so it outlives + // this call and is owned by the pending entry until the reply arrives). + auto promise = std::make_shared>>>(); + auto future = promise->get_future(); + const bool queued = call_async(request, [promise](std::span reply) { + promise->set_value(std::vector(reply.begin(), reply.end())); + }); + if (!queued) { + promise->set_value(std::nullopt); + } + return future; +} + +bool RtpsParticipant::add_service_server(const ServiceConfig &config, service_handler_t handler) { + // A synchronous handler is a deferred handler that replies immediately. + return add_service_server_deferred( + config, [h = std::move(handler)](std::span request, ServiceResponder resp) { + resp.reply(h(request)); + }); +} + +bool RtpsParticipant::add_service_server_deferred(const ServiceConfig &config, + service_deferred_handler_t handler) { + std::lock_guard lock(mutex_); + if (!started_) { + logger_.error("Cannot add service server '{}': not started", config.service); + return false; + } + const std::string req_topic = rtps::rpc::service_request_topic(config.service); + const std::string rep_topic = rtps::rpc::service_reply_topic(config.service); + const std::string req_type = rtps::rpc::service_request_type(config.type_name); + const std::string rep_type = rtps::rpc::service_response_type(config.type_name); + + rtps::Writer *reply_writer = + domain_->createWriter(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true); + rtps::Reader *request_reader = + domain_->createReader(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true); + if (reply_writer == nullptr || request_reader == nullptr) { + logger_.error("Service server '{}': endpoint creation failed", config.service); + return false; + } + auto ctx = std::make_shared(); + ctx->self = this; + ctx->handler = std::move(handler); + ctx->reply_writer = reply_writer; + ctx->request_reader = request_reader; + if (request_reader->registerCallback(&service_request_trampoline, ctx.get()) == 0) { + logger_.error("Service server '{}': could not register request callback", config.service); + return false; + } + service_servers_.push_back(std::move(ctx)); + logger_.info("Added service server: '{}' ({})", config.service, config.type_name); + return true; +} + +// =========================================================================== +// Actions (AMI) - 3 services (send_goal/cancel_goal/get_result) + 2 topics +// (feedback/status), composed over the service + pub/sub facade above. +// =========================================================================== + +namespace { +namespace ract = rtps::rpc; + +// An owned action-execute worker: the std::thread plus a flag it sets true just +// before returning. Threads are stored (not detached) so shutdown can join them +// before the participant is torn down; finished ones are reaped when new goals +// arrive so the list stays bounded over a long-lived server. See +// reap_and_store / the join loop in stop(). +struct ActionExecThread { + std::thread thread; + std::shared_ptr> finished; +}; + +// Append a freshly-spawned worker after reaping any that have finished (joining +// them releases their captured goal state). Bounds the vector without blocking. +void reap_and_store(std::mutex &m, std::vector &threads, std::thread th, + std::shared_ptr> finished) { + std::lock_guard lock(m); + for (auto it = threads.begin(); it != threads.end();) { + if (it->finished->load()) { + if (it->thread.joinable()) { + it->thread.join(); + } + it = threads.erase(it); + } else { + ++it; + } + } + threads.push_back(ActionExecThread{std::move(th), std::move(finished)}); +} + +// Generate a unique 16-byte goal id: random_device bytes mixed with a process +// counter so uniqueness holds even if random_device is weak (e.g. on an MCU). +ract::GoalUuid generate_goal_id() { + static std::atomic counter{0}; + ract::GoalUuid id{}; + std::random_device rd; + std::generate(id.begin(), id.end(), [&rd]() { return static_cast(rd() & 0xFF); }); + const uint32_t c = counter.fetch_add(1); + id[0] = static_cast(c & 0xFF); + id[1] = static_cast((c >> 8) & 0xFF); + return id; +} +} // namespace + +// --- Action server ------------------------------------------------------- + +// One accepted goal's state + the terminal-status plumbing. +struct RtpsParticipant::ActionGoalHandle::State { + ract::GoalUuid goal_id{}; + std::vector goal; // CDR goal payload + RtpsParticipant *self{nullptr}; // for feedback/status publish (via publish()) + // weak (not shared) back-reference: the server's goals map owns the State, so + // a shared_ptr here would form a cycle and leak both. Only needed to retire + // the goal from that map; publishing uses the copied topics below. + std::weak_ptr server; + std::string feedback_topic; // copied so feedback/status publishing needs no + std::string status_topic; // server lock and survives server teardown. + std::mutex mutex; + ract::GoalStatus status{ract::GoalStatus::ACCEPTED}; + bool done{false}; // terminate() has run + bool result_delivered{false}; // get_result answered -> goal may be retired + std::vector result; // set on terminate + ServiceResponder result_responder; // pending get_result (if any) + std::atomic cancel_requested{false}; +}; + +struct RtpsParticipant::ActionServerContext { + RtpsParticipant *self{nullptr}; + std::string feedback_topic; + std::string status_topic; + action_execute_callback_t execute{nullptr}; + std::mutex goals_mutex; + std::map> goals; + // Owned execute workers (not detached): joined in stop() before the domain is + // torn down, reaped as they finish. See ActionExecThread / reap_and_store. + std::mutex threads_mutex; + std::vector exec_threads; +}; + +const RtpsParticipant::GoalId &RtpsParticipant::ActionGoalHandle::goal_id() const { + return state_->goal_id; +} +std::span RtpsParticipant::ActionGoalHandle::goal() const { + return {state_->goal.data(), state_->goal.size()}; +} +bool RtpsParticipant::ActionGoalHandle::is_canceling() const { + return state_->cancel_requested.load(); +} + +void RtpsParticipant::ActionGoalHandle::publish_feedback(std::span feedback) const { + auto msg = ract::wrap_feedback(state_->goal_id, feedback); + state_->self->publish(state_->feedback_topic, {msg.data(), msg.size()}); +} + +// Publish a single-goal GoalStatusArray (the common case; a full multi-goal list +// is not needed for correlation - clients match on the goal id). +static void publish_goal_status(RtpsParticipant *self, const std::string &status_topic, + const ract::GoalUuid &id, ract::GoalStatus status) { + ract::GoalStatusEntry e; + e.goal_id = id; + e.status = status; + std::array arr{e}; + auto msg = ract::make_goal_status_array(arr); + self->publish(status_topic, {msg.data(), msg.size()}); +} + +void RtpsParticipant::ActionGoalHandle::terminate(int status_value, + std::span result) const { + const auto status = static_cast(static_cast(status_value)); + ServiceResponder responder; + bool retire = false; + { + std::lock_guard lock(state_->mutex); + if (state_->done) { + return; + } + state_->done = true; + state_->status = status; + state_->result.assign(result.begin(), result.end()); + if (state_->result_responder.valid()) { + responder = state_->result_responder; // get_result already waiting + state_->result_delivered = true; + retire = true; + } + } + publish_goal_status(state_->self, state_->status_topic, state_->goal_id, status); + if (responder.valid()) { + responder.reply( + ract::wrap_get_result_response(status, {state_->result.data(), state_->result.size()})); + } + if (retire) { + // Terminated with the client's get_result already waiting: it now has the + // result, so drop the goal from the server map (a still-running worker keeps + // the State alive via its own reference). + if (auto server = state_->server.lock()) { + std::lock_guard lock(server->goals_mutex); + server->goals.erase(state_->goal_id); + } + } +} +void RtpsParticipant::ActionGoalHandle::succeed(std::span result) const { + terminate(static_cast(ract::GoalStatus::SUCCEEDED), result); +} +void RtpsParticipant::ActionGoalHandle::abort(std::span result) const { + terminate(static_cast(ract::GoalStatus::ABORTED), result); +} +void RtpsParticipant::ActionGoalHandle::canceled(std::span result) const { + terminate(static_cast(ract::GoalStatus::CANCELED), result); +} + +bool RtpsParticipant::add_action_server(const ActionConfig &config, action_goal_callback_t on_goal, + action_execute_callback_t execute, + action_cancel_callback_t on_cancel) { + if (!started_) { + logger_.error("Cannot add action server '{}': not started", config.action); + return false; + } + auto ctx = std::make_shared(); + ctx->self = this; + ctx->feedback_topic = rtps::rpc::action_feedback_topic(config.action); + ctx->status_topic = rtps::rpc::action_status_topic(config.action); + ctx->execute = std::move(execute); + + // Feedback + status publishers (plain reliable topics). + if (!add_writer({ctx->feedback_topic, rtps::rpc::action_feedback_type(config.type_name), + Reliability::RELIABLE}) || + !add_writer({ctx->status_topic, rtps::rpc::action_status_type(), Reliability::RELIABLE})) { + logger_.error("Action server '{}': feedback/status writer creation failed", config.action); + return false; + } + + auto weak = std::weak_ptr(ctx); + + // send_goal service: accept/reject, then spawn the execute thread. + const ServiceConfig send_goal_cfg{rtps::rpc::action_send_goal_service(config.action), + rtps::rpc::action_send_goal_type(config.type_name)}; + bool ok = add_service_server( + send_goal_cfg, [this, weak, on_goal](std::span req) -> std::vector { + auto server = weak.lock(); + ract::GoalUuid id{}; + std::vector goal; + if (server == nullptr || !ract::unwrap_send_goal_request(req, id, goal)) { + return ract::make_send_goal_response(false); + } + const bool accept = !on_goal || on_goal(id, {goal.data(), goal.size()}); + if (!accept) { + return ract::make_send_goal_response(false); + } + auto gstate = std::make_shared(); + gstate->goal_id = id; + gstate->goal = std::move(goal); + gstate->self = this; + gstate->server = server; // weak back-reference (see State) + gstate->feedback_topic = server->feedback_topic; + gstate->status_topic = server->status_topic; + { + std::lock_guard lock(server->goals_mutex); + server->goals[id] = gstate; + } + publish_goal_status(this, server->status_topic, id, ract::GoalStatus::EXECUTING); + if (server->execute) { + // Own the worker (not detached) so stop() joins it before the domain + // is torn down. It captures a weak server ref (locked only while + // running), so a finished worker forms no goals<->server cycle. + auto finished = std::make_shared>(false); + std::weak_ptr weak_server = server; + std::thread worker([gstate, weak_server, finished]() { + if (auto s = weak_server.lock()) { + s->execute(ActionGoalHandle(gstate)); + } + finished->store(true); + }); + reap_and_store(server->threads_mutex, server->exec_threads, std::move(worker), finished); + } + return ract::make_send_goal_response(true); + }); + + // get_result service (DEFERRED): reply now if done, else hold the responder. + const ServiceConfig get_result_cfg{rtps::rpc::action_get_result_service(config.action), + rtps::rpc::action_get_result_type(config.type_name)}; + ok = ok && add_service_server_deferred( + get_result_cfg, [weak](std::span req, ServiceResponder responder) { + auto server = weak.lock(); + ract::GoalUuid id{}; + if (server == nullptr || !ract::parse_get_result_request(req, id)) { + return; + } + std::shared_ptr gstate; + { + std::lock_guard lock(server->goals_mutex); + auto it = server->goals.find(id); + if (it != server->goals.end()) { + gstate = it->second; + } + } + if (gstate == nullptr) { + responder.reply(ract::wrap_get_result_response(ract::GoalStatus::UNKNOWN, {})); + return; + } + bool retire = false; + { + std::lock_guard lock(gstate->mutex); + if (gstate->done) { + responder.reply(ract::wrap_get_result_response( + gstate->status, {gstate->result.data(), gstate->result.size()})); + gstate->result_delivered = true; + retire = true; + } else { + gstate->result_responder = responder; // fulfilled on terminate() + } + } + if (retire) { + // Client has its result: drop the goal from the server map. + std::lock_guard glock(server->goals_mutex); + server->goals.erase(id); + } + }); + + // cancel_goal service: mark the goal canceling; the execute callback observes + // is_canceling(). Minimal CancelGoal_Response (return_code=0, empty list). + const ServiceConfig cancel_cfg{rtps::rpc::action_cancel_goal_service(config.action), + rtps::rpc::action_cancel_goal_type()}; + ok = ok && + add_service_server( + cancel_cfg, [weak, on_cancel](std::span req) -> std::vector { + auto server = weak.lock(); + // CancelGoal_Request: goal_info{ goal_id: UUID(16), stamp }. + if (server != nullptr && req.size() >= 4 + 16) { + ract::GoalUuid id{}; + std::memcpy(id.data(), req.data() + 4, 16); + std::shared_ptr gstate; + { + std::lock_guard lock(server->goals_mutex); + auto it = server->goals.find(id); + if (it != server->goals.end()) { + gstate = it->second; + } + } + if (gstate && (!on_cancel || on_cancel(id))) { + gstate->cancel_requested.store(true); + } + } + // CancelGoal_Response: return_code:int8 + pad(3) + goals[]=0. + std::vector resp{0x00, 0x01, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0}; + return resp; + }); + + if (!ok) { + logger_.error("Action server '{}': service endpoint creation failed", config.action); + return false; + } + action_servers_.push_back(std::move(ctx)); + logger_.info("Added action server: '{}' ({})", config.action, config.type_name); + return true; +} + +// --- Action client ------------------------------------------------------- + +struct RtpsParticipant::ActionClient::Impl { + struct Goal { + feedback_callback_t on_feedback{nullptr}; + result_callback_t on_result{nullptr}; + }; + RtpsParticipant *self{nullptr}; + std::shared_ptr send_goal_client; + std::shared_ptr get_result_client; + std::shared_ptr cancel_client; + std::string action; + std::mutex mutex; + std::map goals; +}; + +RtpsParticipant::ActionClient::ActionClient(std::unique_ptr impl) + : impl_(std::move(impl)) {} +RtpsParticipant::ActionClient::~ActionClient() = default; + +std::optional RtpsParticipant::ActionClient::send_goal( + std::span goal, feedback_callback_t on_feedback, result_callback_t on_result) { + const ract::GoalUuid id = generate_goal_id(); + { + std::lock_guard lock(impl_->mutex); + impl_->goals[id] = Impl::Goal{std::move(on_feedback), std::move(on_result)}; + } + auto *impl = impl_.get(); + const bool queued = impl->send_goal_client->call_async( + ract::wrap_send_goal_request(id, goal), [impl, id](std::span reply) { + bool accepted = false; + if (!ract::parse_send_goal_response(reply, accepted) || !accepted) { + Impl::Goal g; + { + std::lock_guard lock(impl->mutex); + auto it = impl->goals.find(id); + if (it == impl->goals.end()) { + return; + } + g = std::move(it->second); + impl->goals.erase(it); + } + if (g.on_result) { + g.on_result(static_cast(ract::GoalStatus::ABORTED), {}); + } + return; + } + // Accepted: request the result (completes when the goal finishes). + impl->get_result_client->call_async( + ract::make_get_result_request(id), [impl, id](std::span res) { + ract::GoalStatus status{}; + std::vector result; + ract::unwrap_get_result_response(res, status, result); + Impl::Goal g; + { + std::lock_guard lock(impl->mutex); + auto it = impl->goals.find(id); + if (it == impl->goals.end()) { + return; + } + g = std::move(it->second); + impl->goals.erase(it); + } + if (g.on_result) { + g.on_result(static_cast(status), {result.data(), result.size()}); + } + }); + }); + if (!queued) { + std::lock_guard lock(impl_->mutex); + impl_->goals.erase(id); + return std::nullopt; + } + return id; +} + +bool RtpsParticipant::ActionClient::cancel_goal(const GoalId &goal_id) { + // CancelGoal_Request: goal_info{ goal_id: UUID(16), stamp{sec,nsec} }. + std::vector req{0x00, 0x01, 0x00, 0x00}; + req.insert(req.end(), goal_id.begin(), goal_id.end()); + for (int i = 0; i < 8; ++i) { + req.push_back(0); // stamp + } + return impl_->cancel_client->call_async(req, [](std::span) {}); +} + +std::shared_ptr +RtpsParticipant::add_action_client(const ActionConfig &config) { + if (!started_) { + logger_.error("Cannot add action client '{}': not started", config.action); + return nullptr; + } + auto impl = std::make_unique(); + impl->self = this; + impl->action = config.action; + impl->send_goal_client = add_service_client({rtps::rpc::action_send_goal_service(config.action), + rtps::rpc::action_send_goal_type(config.type_name)}); + impl->get_result_client = + add_service_client({rtps::rpc::action_get_result_service(config.action), + rtps::rpc::action_get_result_type(config.type_name)}); + impl->cancel_client = add_service_client( + {rtps::rpc::action_cancel_goal_service(config.action), rtps::rpc::action_cancel_goal_type()}); + if (!impl->send_goal_client || !impl->get_result_client || !impl->cancel_client) { + logger_.error("Action client '{}': service client creation failed", config.action); + return nullptr; + } + + ActionClient::Impl *raw = impl.get(); + // Feedback subscriber routes by goal id to the goal's on_feedback. + if (!add_reader({rtps::rpc::action_feedback_topic(config.action), + rtps::rpc::action_feedback_type(config.type_name), Reliability::RELIABLE, + [raw](std::span msg) { + ract::GoalUuid id{}; + std::vector fb; + if (!ract::unwrap_feedback(msg, id, fb)) { + return; + } + ActionClient::feedback_callback_t cb; + { + std::lock_guard lock(raw->mutex); + auto it = raw->goals.find(id); + if (it != raw->goals.end()) { + cb = it->second.on_feedback; + } + } + if (cb) { + cb({fb.data(), fb.size()}); + } + }})) { + logger_.error("Action client '{}': feedback reader creation failed", config.action); + return nullptr; + } + + auto client = std::shared_ptr(new ActionClient(std::move(impl))); + action_clients_.push_back(client); + logger_.info("Added action client: '{}' ({})", config.action, config.type_name); + return client; +} + +std::shared_ptr +RtpsParticipant::add_service_client(const ServiceConfig &config) { + std::lock_guard lock(mutex_); + if (!started_) { + logger_.error("Cannot add service client '{}': not started", config.service); + return nullptr; + } + const std::string req_topic = rtps::rpc::service_request_topic(config.service); + const std::string rep_topic = rtps::rpc::service_reply_topic(config.service); + const std::string req_type = rtps::rpc::service_request_type(config.type_name); + const std::string rep_type = rtps::rpc::service_response_type(config.type_name); + + rtps::Reader *reply_reader = + domain_->createReader(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true); + rtps::Writer *request_writer = + domain_->createWriter(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true); + if (reply_reader == nullptr || request_writer == nullptr) { + logger_.error("Service client '{}': endpoint creation failed", config.service); + return nullptr; + } + auto impl = std::make_unique(); + impl->self = this; + impl->request_writer = request_writer; + impl->reply_reader_guid = reply_reader->m_attributes.endpointGuid; + if (reply_reader->registerCallback(&service_reply_trampoline, impl.get()) == 0) { + logger_.error("Service client '{}': could not register reply callback", config.service); + return nullptr; + } + auto client = std::shared_ptr(new ServiceClient(std::move(impl))); + service_clients_.push_back(client); + logger_.info("Added service client: '{}' ({})", config.service, config.type_name); + return client; +} + +// =========================================================================== +// Native services (espp<->espp): lean request/reply over plain pub/sub with a +// 20-byte in-band correlation header. No engine wire support needed. +// =========================================================================== + +struct RtpsParticipant::NativeServiceServerContext { + RtpsParticipant *self{nullptr}; + std::string reply_topic; + service_handler_t handler{nullptr}; +}; + +struct RtpsParticipant::NativeServiceClient::Impl { + struct SyncSlot { + std::mutex m; + std::condition_variable cv; + bool done{false}; + std::vector reply; + }; + struct Pending { + reply_callback_t on_reply{nullptr}; + std::shared_ptr sync{nullptr}; + }; + RtpsParticipant *self{nullptr}; + std::string request_topic; + std::array my_prefix{}; + std::atomic next_id{1}; + std::mutex mutex; + std::unordered_map pending; + + std::optional send(std::span request, reply_callback_t on_reply, + std::shared_ptr sync) { + rtps::rpc::NativeHeader h; + h.client_prefix = my_prefix; + h.op = rtps::rpc::NativeOp::REQUEST; + const uint32_t id = next_id.fetch_add(1); + h.request_id = id; + auto frame = rtps::rpc::native_encode(h, request); + { + std::lock_guard lock(mutex); + pending[id] = Pending{std::move(on_reply), std::move(sync)}; + } + if (!self->publish(request_topic, {frame.data(), frame.size()})) { + std::lock_guard lock(mutex); + pending.erase(id); + return std::nullopt; + } + return id; + } +}; + +RtpsParticipant::NativeServiceClient::NativeServiceClient(std::unique_ptr impl) + : impl_(std::move(impl)) {} +RtpsParticipant::NativeServiceClient::~NativeServiceClient() = default; + +bool RtpsParticipant::NativeServiceClient::call_async(std::span request, + reply_callback_t on_reply) { + return impl_->send(request, std::move(on_reply), nullptr).has_value(); +} + +std::optional> +RtpsParticipant::NativeServiceClient::call(std::span request, + std::chrono::milliseconds timeout) { + auto slot = std::make_shared(); + auto id = impl_->send(request, nullptr, slot); + if (!id.has_value()) { + return std::nullopt; + } + std::unique_lock lock(slot->m); + if (!slot->cv.wait_for(lock, timeout, [&] { return slot->done; })) { + std::lock_guard plock(impl_->mutex); + impl_->pending.erase(*id); + return std::nullopt; + } + return std::move(slot->reply); +} + +std::future>> +RtpsParticipant::NativeServiceClient::call_future(std::span request) { + auto promise = std::make_shared>>>(); + auto future = promise->get_future(); + const bool queued = call_async(request, [promise](std::span reply) { + promise->set_value(std::vector(reply.begin(), reply.end())); + }); + if (!queued) { + promise->set_value(std::nullopt); + } + return future; +} + +bool RtpsParticipant::add_native_service_server(const ServiceConfig &config, + service_handler_t handler) { + if (!started_) { + logger_.error("Cannot add native service server '{}': not started", config.service); + return false; + } + auto ctx = std::make_shared(); + ctx->self = this; + ctx->reply_topic = rtps::rpc::native_reply_topic(config.service); + ctx->handler = std::move(handler); + const std::string req_topic = rtps::rpc::native_request_topic(config.service); + + if (!add_writer({ctx->reply_topic, config.type_name, Reliability::RELIABLE})) { + logger_.error("Native service server '{}': reply writer failed", config.service); + return false; + } + NativeServiceServerContext *raw = ctx.get(); + if (!add_reader({req_topic, config.type_name, Reliability::RELIABLE, + [raw](std::span frame) { + rtps::rpc::NativeHeader h; + std::span payload; + if (!rtps::rpc::native_decode(frame, h, payload) || + h.op != rtps::rpc::NativeOp::REQUEST) { + return; + } + std::vector reply = + raw->handler ? raw->handler(payload) : std::vector{}; + // Echo {client_prefix, request_id} back as a REPLY. + rtps::rpc::NativeHeader rh; + rh.client_prefix = h.client_prefix; + rh.request_id = h.request_id; + rh.op = rtps::rpc::NativeOp::REPLY; + auto out = rtps::rpc::native_encode(rh, reply); + raw->self->publish(raw->reply_topic, {out.data(), out.size()}); + }})) { + logger_.error("Native service server '{}': request reader failed", config.service); + return false; + } + native_service_servers_.push_back(std::move(ctx)); + logger_.info("Added native service server: '{}'", config.service); + return true; +} + +std::shared_ptr +RtpsParticipant::add_native_service_client(const ServiceConfig &config) { + if (!started_ || participant_ == nullptr) { + logger_.error("Cannot add native service client '{}': not started", config.service); + return nullptr; + } + auto impl = std::make_unique(); + impl->self = this; + impl->request_topic = rtps::rpc::native_request_topic(config.service); + impl->my_prefix = participant_->m_guidPrefix.id; + const std::string rep_topic = rtps::rpc::native_reply_topic(config.service); + + if (!add_writer({impl->request_topic, config.type_name, Reliability::RELIABLE})) { + logger_.error("Native service client '{}': request writer failed", config.service); + return nullptr; + } + NativeServiceClient::Impl *raw = impl.get(); + if (!add_reader({rep_topic, config.type_name, Reliability::RELIABLE, + [raw](std::span frame) { + rtps::rpc::NativeHeader h; + std::span payload; + if (!rtps::rpc::native_decode(frame, h, payload) || + h.op != rtps::rpc::NativeOp::REPLY || h.client_prefix != raw->my_prefix) { + return; + } + NativeServiceClient::Impl::Pending p; + { + std::lock_guard lock(raw->mutex); + auto it = raw->pending.find(h.request_id); + if (it == raw->pending.end()) { + return; + } + p = std::move(it->second); + raw->pending.erase(it); + } + if (p.sync) { + std::lock_guard lock(p.sync->m); + p.sync->reply.assign(payload.begin(), payload.end()); + p.sync->done = true; + p.sync->cv.notify_one(); + } else if (p.on_reply) { + p.on_reply(payload); + } + }})) { + logger_.error("Native service client '{}': reply reader failed", config.service); + return nullptr; + } + auto client = std::shared_ptr(new NativeServiceClient(std::move(impl))); + native_service_clients_.push_back(client); + logger_.info("Added native service client: '{}'", config.service); + return client; +} + +// =========================================================================== +// Native actions (espp<->espp): lean AMI - a native send_goal service + a +// feedback topic carrying the terminal result. +// =========================================================================== + +struct RtpsParticipant::NativeGoalHandle::State { + uint32_t goal_handle{0}; + std::vector goal; + RtpsParticipant *self{nullptr}; + std::string feedback_topic; + std::atomic done{false}; + std::atomic cancel_requested{false}; +}; + +struct RtpsParticipant::NativeActionServerContext { + RtpsParticipant *self{nullptr}; + std::string feedback_topic; + native_execute_callback_t execute{nullptr}; + std::atomic next_handle{1}; + // Owned execute workers (not detached): joined in stop() before the domain is + // torn down, reaped as they finish. See ActionExecThread / reap_and_store. + std::mutex threads_mutex; + std::vector exec_threads; + // Running goals keyed by handle, for routing cancel requests. weak so a + // finished goal's State can expire; the executing worker owns the strong ref. + std::mutex goals_mutex; + std::map> goals; +}; + +uint32_t RtpsParticipant::NativeGoalHandle::goal_handle() const { return state_->goal_handle; } +std::span RtpsParticipant::NativeGoalHandle::goal() const { + return {state_->goal.data(), state_->goal.size()}; +} +bool RtpsParticipant::NativeGoalHandle::is_canceling() const { + return state_->cancel_requested.load(); +} +void RtpsParticipant::NativeGoalHandle::publish_feedback(std::span feedback) const { + auto msg = rtps::rpc::native_make_feedback(state_->goal_handle, + rtps::rpc::NativeGoalStatus::EXECUTING, feedback); + state_->self->publish(state_->feedback_topic, {msg.data(), msg.size()}); +} +void RtpsParticipant::NativeGoalHandle::terminate(uint8_t status, + std::span result) const { + bool expected = false; + if (!state_->done.compare_exchange_strong(expected, true)) { + return; + } + auto msg = rtps::rpc::native_make_feedback( + state_->goal_handle, static_cast(status), result); + state_->self->publish(state_->feedback_topic, {msg.data(), msg.size()}); +} +void RtpsParticipant::NativeGoalHandle::succeed(std::span result) const { + terminate(static_cast(rtps::rpc::NativeGoalStatus::SUCCEEDED), result); +} +void RtpsParticipant::NativeGoalHandle::abort(std::span result) const { + terminate(static_cast(rtps::rpc::NativeGoalStatus::ABORTED), result); +} +void RtpsParticipant::NativeGoalHandle::canceled(std::span result) const { + terminate(static_cast(rtps::rpc::NativeGoalStatus::CANCELED), result); +} + +bool RtpsParticipant::add_native_action_server(const ActionConfig &config, + native_goal_callback_t on_goal, + native_execute_callback_t execute, + native_cancel_callback_t on_cancel) { + if (!started_) { + logger_.error("Cannot add native action server '{}': not started", config.action); + return false; + } + auto ctx = std::make_shared(); + ctx->self = this; + ctx->feedback_topic = rtps::rpc::native_feedback_topic(config.action); + ctx->execute = std::move(execute); + + if (!add_writer({ctx->feedback_topic, config.type_name, Reliability::RELIABLE})) { + logger_.error("Native action server '{}': feedback writer failed", config.action); + return false; + } + auto weak = std::weak_ptr(ctx); + // The send_goal native service: accept -> spawn execute -> reply goal_handle. + const bool ok = add_native_service_server( + {rtps::rpc::native_goal_service(config.action), config.type_name}, + [this, weak, on_goal](std::span goal) -> std::vector { + auto server = weak.lock(); + if (server == nullptr || (on_goal && !on_goal(goal))) { + return rtps::rpc::native_make_goal_reply(false, 0); + } + const uint32_t handle = server->next_handle.fetch_add(1); + auto gstate = std::make_shared(); + gstate->goal_handle = handle; + gstate->goal.assign(goal.begin(), goal.end()); + gstate->self = this; + gstate->feedback_topic = server->feedback_topic; + if (server->execute) { + { + std::lock_guard lock(server->goals_mutex); + server->goals[handle] = gstate; // weak; for cancel routing + } + // Own the worker (not detached) so stop() joins it before teardown. + auto finished = std::make_shared>(false); + std::weak_ptr weak_server = server; + std::thread worker([gstate, weak_server, finished, handle]() { + if (auto s = weak_server.lock()) { + s->execute(NativeGoalHandle(gstate)); + // Goal finished: drop it from the cancel-routing map. + std::lock_guard lock(s->goals_mutex); + s->goals.erase(handle); + } + finished->store(true); + }); + reap_and_store(server->threads_mutex, server->exec_threads, std::move(worker), finished); + } + return rtps::rpc::native_make_goal_reply(true, handle); + }); + if (!ok) { + logger_.error("Native action server '{}': goal service failed", config.action); + return false; + } + // The cancel native service: mark a running goal canceling (the execute + // callback observes is_canceling()); on_cancel, if set, gates acceptance. + const bool cancel_ok = add_native_service_server( + {rtps::rpc::native_cancel_service(config.action), config.type_name}, + [weak, on_cancel](std::span req) -> std::vector { + auto server = weak.lock(); + uint32_t handle = 0; + if (server == nullptr || !rtps::rpc::native_parse_cancel_request(req, handle)) { + return rtps::rpc::native_make_cancel_reply(false); + } + std::shared_ptr gstate; + { + std::lock_guard lock(server->goals_mutex); + auto it = server->goals.find(handle); + if (it != server->goals.end()) { + gstate = it->second.lock(); + } + } + if (!gstate) { + return rtps::rpc::native_make_cancel_reply(false); // unknown/finished goal + } + const bool accept = !on_cancel || on_cancel(handle); + if (accept) { + gstate->cancel_requested.store(true); + } + return rtps::rpc::native_make_cancel_reply(accept); + }); + if (!cancel_ok) { + logger_.error("Native action server '{}': cancel service failed", config.action); + return false; + } + native_action_servers_.push_back(std::move(ctx)); + logger_.info("Added native action server: '{}'", config.action); + return true; +} + +struct RtpsParticipant::NativeActionClient::Impl { + struct Goal { + feedback_callback_t on_feedback{nullptr}; + result_callback_t on_result{nullptr}; + }; + struct BufferedMsg { + rtps::rpc::NativeGoalStatus status{}; + std::vector payload; + }; + RtpsParticipant *self{nullptr}; + std::shared_ptr goal_client; + std::shared_ptr cancel_client; + std::mutex mutex; + std::map goals; + // Feedback/result can arrive before send_goal's reply installs the goal (the + // server starts executing immediately, so a fast native action may publish + // before the reply lands). Buffer such early messages by handle and replay + // them on registration. Bounded so a stray/unknown handle cannot grow it + // without limit. + std::map> pending_early; + size_t pending_early_count{0}; + static constexpr size_t kMaxPendingEarly = 64; + + // Route a parsed feedback/result message to the registered goal's callbacks; + // terminal status delivers the result and retires the goal. No-op for an + // unknown handle. Callbacks run outside the lock. + static void deliver(Impl *impl, uint32_t handle, rtps::rpc::NativeGoalStatus status, + std::span payload) { + const bool terminal = status == rtps::rpc::NativeGoalStatus::SUCCEEDED || + status == rtps::rpc::NativeGoalStatus::ABORTED || + status == rtps::rpc::NativeGoalStatus::CANCELED; + Goal g; + { + std::lock_guard lock(impl->mutex); + auto it = impl->goals.find(handle); + if (it == impl->goals.end()) { + return; + } + g = it->second; + if (terminal) { + impl->goals.erase(it); + } + } + if (terminal) { + if (g.on_result) { + g.on_result(static_cast(status), {payload.data(), payload.size()}); + } + } else if (g.on_feedback) { + g.on_feedback({payload.data(), payload.size()}); + } + } +}; + +RtpsParticipant::NativeActionClient::NativeActionClient(std::unique_ptr impl) + : impl_(std::move(impl)) {} +RtpsParticipant::NativeActionClient::~NativeActionClient() = default; + +bool RtpsParticipant::NativeActionClient::send_goal(std::span goal, + feedback_callback_t on_feedback, + result_callback_t on_result, + accepted_callback_t on_accepted) { + auto *impl = impl_.get(); + return impl->goal_client->call_async( + goal, [impl, on_feedback, on_result, on_accepted](std::span reply) { + bool accepted = false; + uint32_t handle = 0; + if (!rtps::rpc::native_parse_goal_reply(reply, accepted, handle) || !accepted) { + if (on_result) { + on_result(static_cast(rtps::rpc::NativeGoalStatus::ABORTED), {}); + } + return; + } + std::vector replay; + { + std::lock_guard lock(impl->mutex); + impl->goals[handle] = Impl::Goal{on_feedback, on_result}; + // Drain any feedback/result that arrived before this registration. + auto it = impl->pending_early.find(handle); + if (it != impl->pending_early.end()) { + replay = std::move(it->second); + impl->pending_early_count -= replay.size(); + impl->pending_early.erase(it); + } + } + if (on_accepted) { + on_accepted(handle); // hand the caller the goal_handle for cancel_goal() + } + for (auto &m : replay) { + Impl::deliver(impl, handle, m.status, m.payload); + } + }); +} + +bool RtpsParticipant::NativeActionClient::cancel_goal(uint32_t goal_handle) { + if (!impl_->cancel_client) { + return false; + } + return impl_->cancel_client->call_async(rtps::rpc::native_make_cancel_request(goal_handle), + [](std::span) {}); +} + +std::shared_ptr +RtpsParticipant::add_native_action_client(const ActionConfig &config) { + if (!started_) { + logger_.error("Cannot add native action client '{}': not started", config.action); + return nullptr; + } + auto impl = std::make_unique(); + impl->self = this; + impl->goal_client = + add_native_service_client({rtps::rpc::native_goal_service(config.action), config.type_name}); + impl->cancel_client = add_native_service_client( + {rtps::rpc::native_cancel_service(config.action), config.type_name}); + if (!impl->goal_client || !impl->cancel_client) { + logger_.error("Native action client '{}': goal/cancel client failed", config.action); + return nullptr; + } + NativeActionClient::Impl *raw = impl.get(); + // Feedback subscriber: route feedback/result by goal_handle; terminal status + // (>= SUCCEEDED) delivers the result and retires the goal. + if (!add_reader({rtps::rpc::native_feedback_topic(config.action), config.type_name, + Reliability::RELIABLE, [raw](std::span msg) { + uint32_t handle = 0; + rtps::rpc::NativeGoalStatus status{}; + std::vector payload; + if (!rtps::rpc::native_parse_feedback(msg, handle, status, payload)) { + return; + } + { + std::lock_guard lock(raw->mutex); + if (raw->goals.find(handle) == raw->goals.end()) { + // Goal not registered yet (its send_goal reply is still in + // flight): buffer this early message, bounded, for replay when + // send_goal installs the goal. See Impl::pending_early. + if (raw->pending_early_count < + NativeActionClient::Impl::kMaxPendingEarly) { + raw->pending_early[handle].push_back({status, std::move(payload)}); + ++raw->pending_early_count; + } + return; + } + } + NativeActionClient::Impl::deliver(raw, handle, status, payload); + }})) { + logger_.error("Native action client '{}': feedback reader failed", config.action); + return nullptr; + } + auto client = std::shared_ptr(new NativeActionClient(std::move(impl))); + native_action_clients_.push_back(client); + logger_.info("Added native action client: '{}'", config.action); + return client; +} +#endif // RTPS_WITH_RPC + +// stop() and ~RtpsParticipant are defined here (end of file) so every RPC +// context type the member containers point to is complete when their +// unique_ptr/shared_ptr elements are destroyed - see the note by the constructor. +void RtpsParticipant::stop() { + // Phase 1: flip started_ under mutex_ so no further publish()/add_*()/reply + // proceeds past its started_ check. + { + std::lock_guard lock(mutex_); + if (!started_) { + return; + } + started_ = false; + } + // Phase 2: invalidate deferred RPC replies. A service responder held by user + // code checks live_->alive under this lock before writing through its engine + // reply writer; flipping it here (before the domain and its writers are + // destroyed) turns any racing reply into a safe no-op, and holding the lock + // first waits for an in-flight reply to finish. + if (live_) { + std::lock_guard lock(live_->m); + live_->alive = false; + } + // Phase 3: stop the engine (no more reader/service callbacks fire), then join + // every owned action-execute worker so none touches this participant after + // the domain and its writers are gone. Done WITHOUT mutex_ held: a worker's + // final publish()/reply must be able to take mutex_/live_ and run to + // completion (as a no-op) so the thread can exit and be joined. The RPC + // container vectors are stable here - after phase 1 no add_*() can mutate them. + if (domain_) { + domain_->stop(); + } +#ifdef RTPS_WITH_RPC + const auto join_workers = [](std::mutex &m, std::vector &threads) { + std::lock_guard lock(m); + for (auto &t : threads) { + if (t.thread.joinable()) { + t.thread.join(); + } + } + threads.clear(); + }; + for (auto &ctx : action_servers_) { + if (!ctx) { + continue; + } + // Ask cooperative execute callbacks (those that poll is_canceling()) to wind + // down, then join. A callback that ignores the signal blocks stop() until it + // returns - execute callbacks must be finite / cancel-aware. + { + std::lock_guard lock(ctx->goals_mutex); + for (auto &kv : ctx->goals) { + kv.second->cancel_requested.store(true); + } + } + join_workers(ctx->threads_mutex, ctx->exec_threads); + } + for (auto &ctx : native_action_servers_) { + if (ctx) { + join_workers(ctx->threads_mutex, ctx->exec_threads); + } + } +#endif // RTPS_WITH_RPC + // Phase 4: tear the domain down and drop bookkeeping under mutex_. The engine + // owns the endpoint objects, so release our references before the domain (and + // with it every writer/reader and their callback registrations) goes away. + { + std::lock_guard lock(mutex_); + writers_.clear(); + participant_ = nullptr; + domain_.reset(); + reader_contexts_.clear(); +#ifdef RTPS_WITH_RPC + // RPC endpoints are owned by the (now-reset) domain; drop our bookkeeping. + // Execute workers were joined above, so nothing here races teardown. + action_clients_.clear(); + action_servers_.clear(); + service_clients_.clear(); + service_servers_.clear(); + native_action_clients_.clear(); + native_action_servers_.clear(); + native_service_clients_.clear(); + native_service_servers_.clear(); +#endif // RTPS_WITH_RPC + } + logger_.info("Stopped"); +} + +RtpsParticipant::~RtpsParticipant() { stop(); } + } // namespace espp diff --git a/doc/en/protocols/index.rst b/doc/en/protocols/index.rst index 4bc483fa9..4b506cd41 100644 --- a/doc/en/protocols/index.rst +++ b/doc/en/protocols/index.rst @@ -8,6 +8,7 @@ Higher-level network protocols and networking tools built on top of the :maxdepth: 1 rtps + rtps_rmi_ami rtsp remote_debug iperf_menu diff --git a/doc/en/protocols/rtps_rmi_ami.rst b/doc/en/protocols/rtps_rmi_ami.rst new file mode 100644 index 000000000..cedb8bddf --- /dev/null +++ b/doc/en/protocols/rtps_rmi_ami.rst @@ -0,0 +1,237 @@ +RTPS Services & Actions (RMI / AMI) +*********************************** + +The ``rtps_embedded`` component's :cpp:class:`espp::RtpsParticipant` facade adds +request/reply (**RMI** — Remote Method Invocation) and goal-oriented (**AMI** — +Asynchronous Method Invocation) messaging on top of its RTPS pub/sub, in two +flavours: + +- **ROS 2-interoperable** services and actions — byte-compatible with + ``rmw_fastrtps`` (validated against live ROS 2 Jazzy nodes, both directions). +- **Native** (espp ↔ espp) services and actions — a deliberately lean protocol + that trades ROS interop for a smaller footprint and simpler wire. + +Both flavours are *composition over the same reliable RTPS pub/sub* — no separate +transport. The full design and the wire-format captures that back it are in +``components/rtps_embedded/RMI_AMI_DESIGN.md``. + +Why services and actions matter +=============================== + +Pub/sub is fire-and-forget: a publisher never learns whether anyone acted on a +sample. Many robotics interactions are *requests* ("add these two ints", "move +to this pose") that need a **correlated reply**, and some are *long-running +goals* that need **progress feedback**, a **final result**, and **cancellation**. +ROS 2 models these as **services** and **actions**; this component provides both, +so an espp device can be a first-class ROS 2 service/action server or client, or +talk to another espp device with the leaner native protocol. + +The key structural fact (and why this was cheap to build): in ROS 2 a **service +is two topics + reply correlation**, and an **action is three services + two +topics** — *no new wire primitive*. Everything here is library code over pub/sub +plus one addition to the engine (carry a ``related_sample_identity`` inline-QoS on +a reply, so a client can match replies to requests). + +Two API levels +============== + +Like pub/sub (raw ``publish()``/``on_sample`` vs the typed ``Publisher`` / +``Subscriber``), the RMI/AMI layer has two levels: + +- **Typed, espp-idiomatic wrappers** (recommended): ``espp::ServiceServer`` / ``ServiceClient`` and ``ActionServer`` / ``ActionClient<...>`` in ``rtps_service.hpp`` / ``rtps_action.hpp``. + Reflectable structs are (de)serialized to CDR by the ``cdr`` component - your + code never touches bytes. Each takes a ``RtpsProtocol`` (``ROS2`` or ``NATIVE``), + so one class covers both flavours. +- **Byte-level methods** on ``RtpsParticipant`` (``add_service_server`` etc.): + CDR-encapsulated ``std::span`` in/out. Use these for dynamic + types, or when you already have the bytes. + +.. code-block:: cpp + + struct AddReq { int64_t a, b; }; + struct AddResp { int64_t sum; }; + espp::ServiceServer server(participant, { + .service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); + + espp::ServiceClient client(participant, { + .service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + if (auto resp = client.call(AddReq{7, 35}, 1s)) use(resp->sum); // -> 42 + + // Same classes, native protocol - just set .protocol: + espp::ServiceClient native(participant, + {.service = "/mul", .type_name = "espp::native::Mul", + .protocol = espp::RtpsProtocol::NATIVE}); + +The rest of this page shows the byte-level API to explain the wire; the typed +wrappers are thin layers over exactly these calls. + +Services (RMI) +============== + +A service is a request topic (``rq/Request``) + a reply topic +(``rr/Reply``), with each reply correlated to its request. Payloads are +CDR-encapsulated bytes, exactly like ``publish()`` / ``on_sample``. + +.. code-block:: cpp + + // Server: handler(request_cdr) -> reply_cdr + participant.add_service_server( + {"/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts"}, + [](std::span req) -> std::vector { + return make_reply_cdr(a + b); + }); + + // Client: three call styles. + auto client = participant.add_service_client( + {"/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts"}); + auto reply = client->call(req_cdr, 1s); // (1) blocking (RMI) + client->call_async(req_cdr, [](auto reply){ ... }); // (2) callback (AMI) + std::future<...> f = client->call_future(req_cdr); // (3) promise (AMI) + +The client offers **all three ergonomics** so callers pick what fits: block a +worker for a quick RPC, register a callback, or hold a ``std::future``. + +For a response that is not ready when the request arrives (e.g. an action's +``get_result``), use :cpp:func:`add_service_server_deferred`: the handler receives +a ``ServiceResponder`` it can store and fulfil later from any thread, so a slow +response never blocks an engine worker (which would deadlock other traffic). + +Correlation (ROS interop) +------------------------- + +``rmw_fastrtps`` correlates a reply to its request with a **related sample +identity** carried as inline QoS, under **both** PID ``0x0083`` (OMG standard) and +PID ``0x800f`` (eProsima legacy). The client stamps its request with +``{its reply-reader GUID, sequence = UNKNOWN}``; the server echoes +``{that GUID, the request's RTPS sequence number}`` on the reply; the client +matches replies against its pending table. This is confirmed byte-for-byte +against a live capture — and no SEDP type-hash exchange is needed: an espp +service even shows up in ``ros2 service list``. + +Actions (AMI) +============= + +An action expands to the standard five ROS 2 endpoints — three services +(``send_goal``, ``cancel_goal``, ``get_result``) plus two topics (``feedback``, +``status``) — driven by a goal state machine. All of it is library code over the +services above. + +.. code-block:: cpp + + // Server + participant.add_action_server( + {"/fibonacci", "example_interfaces::action::dds_::Fibonacci"}, + [](const auto &goal_id, std::span goal) { return true; }, // accept? + [](espp::RtpsParticipant::ActionGoalHandle h) { // execute (own thread) + h.publish_feedback(fb_cdr); + h.succeed(result_cdr); // or h.abort(...) / h.canceled(...) + }); + + // Client + auto action = participant.add_action_client( + {"/fibonacci", "example_interfaces::action::dds_::Fibonacci"}); + action->send_goal(goal_cdr, + [](std::span fb) { ... }, // per-feedback + [](int8_t status, std::span res) { ... }); // terminal result + +Goals are identified by a 16-byte UUID; feedback, status, and the result are all +routed to the right goal by that id. The result rides ``get_result`` (a deferred +service reply that completes when the goal terminates). + +.. note:: + + **Action result alignment limitation.** The action ``get_result`` envelope is + ``status:int8`` followed by the result message. The framework splices the + result's CDR bytes immediately after ``status`` + 3 bytes of padding (offset + 4), which is byte-exact only when the result message's **first field is at most + 4-byte aligned** (int32/uint32, arrays, strings, and structs of those - e.g. + Fibonacci's ``int32[]``). A result whose first field needs 8-byte alignment + (``int64``/``uint64``/``float64`` as the first member) would be placed at + offset 4 instead of the CDR-correct offset 8 and would mis-decode on a ROS 2 + peer. This affects both the byte-level and typed action APIs. If your result + starts with an 8-byte-aligned field, reorder it (put a 4-byte field first) or + wrap it in a leading 4-byte field. Goal and feedback payloads are unaffected + (they follow the 16-byte UUID, which is already 8-aligned). Services (request/ + reply) are unaffected. This is a known v1 limitation of the byte-splice + envelope; a future revision may build the envelope via full CDR serialization. + +Native (espp ↔ espp) services & actions +======================================= + +When both ends are espp and ROS interop is not needed, the **native** protocol is +leaner. Correlation is a **20-byte in-band header** prepended to the payload — +``client_prefix(12) + request_id(4) + op(1) + flags(1) + reserved(2)`` — so it +needs *no inline-QoS engine support* and rides plain reliable pub/sub on +``es_rq/`` / ``es_rr/`` topics (a distinct prefix, so it never aliases the ROS +``rq/`` / ``rr/`` topics). A native action collapses ROS's ~10 endpoints to ~3: +one goal service + one feedback topic that also carries the terminal result. + +.. code-block:: cpp + + participant.add_native_service_server({"/mul", "espp::native::Mul"}, handler); + auto c = participant.add_native_service_client({"/mul", "espp::native::Mul"}); + auto r = c->call(req_cdr, 1s); // same call / call_async ergonomics + + participant.add_native_action_server({"/countup", "espp::native::CountUp"}, + on_goal, execute); + auto a = participant.add_native_action_client({"/countup", "espp::native::CountUp"}); + a->send_goal(goal_cdr, on_feedback, on_result); + +Choosing a flavour +------------------ + +.. list-table:: + :header-rows: 1 + + * - Need + - Use + * - Talk to ROS 2 nodes / rclcpp / rclpy + - ``add_service_*`` / ``add_action_*`` (ROS-interoperable) + * - espp ↔ espp only, minimise endpoints / RAM + - ``add_native_service_*`` / ``add_native_action_*`` + +Python +====== + +The same APIs are exposed through the ``espp`` Python module (pybind11). +Callbacks run on engine threads but are invoked GIL-correctly, so plain Python +callables work. See ``python/rtps_rpc_demo.py`` for a runnable showcase of all +four mechanisms; the essence: + +.. code-block:: python + + import espp + p = espp.RtpsParticipant(espp.RtpsParticipant.Config()) + p.start() + p.add_service_server("/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts", + lambda req: make_reply(req)) + svc = p.add_service_client("/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts") + reply = svc.call(request_bytes, timeout=1.0) # bytes or None + +Payloads are CDR-encapsulated ``bytes`` (a 4-byte encapsulation header + CDR +body). Pack them with :mod:`struct` for simple fields, or with the ``cdr`` +component / ``pycdr2`` for real ROS 2 message types. + +Testing +======= + +Every mechanism is covered end-to-end: + +- **In-process loopbacks** (host, docker-free): ``rtps_service_loopback``, + ``rtps_action_loopback``, ``rtps_native_service_loopback``, + ``rtps_native_action_loopback``, and ``rtps_typed_rpc_loopback`` (the typed + ``ServiceServer/Client`` + ``ActionServer/Client`` wrappers, both protocols) — + plus wire-format unit tests (``rtps_service_naming``, ``rtps_action_naming``, + ``rtps_action_types``) checked byte-for-byte against live ROS 2 captures. +- **On-device**: the ``components/rtps_embedded/example`` (esp32) hosts a typed + ``/add_two_ints`` service and a ``/fibonacci`` action a ROS 2 client can drive. +- **Live ROS 2 interop** (dockerised ``rmw_fastrtps``, both directions): + ``ros2 service call`` ↔ espp server, espp client ↔ rclpy server, and the same + for actions (``ros2 action send_goal`` ↔ espp, espp ↔ rclpy). See + ``components/rtps_embedded/interop/``. +- **Python**: ``python/rtps_rpc_demo.py`` exercises all four mechanisms. diff --git a/lib/python_bindings/espp/rtps.py b/lib/python_bindings/espp/rtps.py index 9a30e38d3..0f915d42d 100644 --- a/lib/python_bindings/espp/rtps.py +++ b/lib/python_bindings/espp/rtps.py @@ -141,3 +141,183 @@ def _on_sample(data: bytes) -> None: reliable=reliable, on_sample=_on_sample, ) + + +# --------------------------------------------------------------------------- +# Services (RMI) and actions (AMI) - the typed Python counterparts to the C++ +# espp::ServiceServer/Client and espp::ActionServer/Client. Each pairs the +# byte-level RtpsParticipant RPC methods with a pycdr2 (or compatible) codec, so +# you deal in message objects instead of CDR bytes. Pass native=True to use the +# lean espp<->espp protocol instead of the ROS 2-interoperable one. +# --------------------------------------------------------------------------- + + +class ServiceServer: + """Answer requests with responses, (de)serializing message objects for you. + + :param participant: a started :class:`espp.RtpsParticipant`. + :param service: service name, e.g. ``"/add_two_ints"``. + :param type_name: base DDS service type, e.g. + ``"example_interfaces::srv::dds_::AddTwoInts"`` (any matching name for + native). + :param request_type: message class with ``.deserialize(bytes)``. + :param response_type: message class with ``.serialize()`` (documentary; the + handler returns an instance whose ``.serialize()`` is used). + :param handler: ``callable(request_msg) -> response_msg``. Runs on an engine + worker thread -- return promptly. + :param native: use the lean native protocol instead of ROS 2. + """ + + def __init__(self, participant, service, type_name, request_type, response_type, handler, + *, native: bool = False) -> None: + def _byte_handler(request_bytes: bytes) -> bytes: + try: + req = request_type.deserialize(request_bytes) + except Exception: + return b"" + return handler(req).serialize() + + add = participant.add_native_service_server if native else participant.add_service_server + self.valid = add(service, type_name, _byte_handler) + + +class ServiceClient: + """Call a service with a request object and get a response object back. + + Blocking :meth:`call` (RMI) and callback :meth:`call_async` (AMI). See + :class:`ServiceServer` for the constructor parameters. + """ + + def __init__(self, participant, service, type_name, request_type, response_type, + *, native: bool = False) -> None: + add = participant.add_native_service_client if native else participant.add_service_client + self._client = add(service, type_name) + self._response_type = response_type + self.valid = self._client is not None + + def call(self, request, timeout: float = 5.0): + """Blocking call. Returns the response object, or ``None`` on timeout.""" + reply = self._client.call(request.serialize(), timeout) + return None if reply is None else self._response_type.deserialize(reply) + + def call_async(self, request, on_response) -> bool: + """Async call: ``on_response(response_msg)`` when the reply arrives.""" + rt = self._response_type + return self._client.call_async(request.serialize(), lambda b: on_response(rt.deserialize(b))) + + def call_future(self, request): + """Async call returning a :class:`concurrent.futures.Future` of the + response object (result is ``None`` if the request could not be queued). + Use ``fut.result(timeout=...)``. Works for both protocols.""" + import concurrent.futures + + fut: concurrent.futures.Future = concurrent.futures.Future() + if not self.call_async(request, fut.set_result): + fut.set_result(None) + return fut + + +class GoalHandle: + """Typed view of a server-side goal handle, passed to an action ``execute`` + callback. Publish feedback and terminate the goal with message objects.""" + + def __init__(self, handle, goal_type, result_type, feedback_type) -> None: + self._handle = handle + self._result_type = result_type + self._feedback_type = feedback_type + self.goal = goal_type.deserialize(handle.goal()) + + def publish_feedback(self, feedback) -> None: + self._handle.publish_feedback(feedback.serialize()) + + def succeed(self, result) -> None: + self._handle.succeed(result.serialize()) + + def abort(self, result) -> None: + self._handle.abort(result.serialize()) + + def canceled(self, result) -> None: + """Terminate the goal CANCELED (in response to a cancel request). Works on + both the ROS 2 and native protocols.""" + canceled = getattr(self._handle, "canceled", None) + if canceled is not None: + canceled(result.serialize()) + + def is_canceling(self) -> bool: + """True if the client has requested cancellation of this goal (both the + ROS 2 and native protocols). Poll this in a long execute() and wind the + goal down - calling canceled() - when it becomes true.""" + return getattr(self._handle, "is_canceling", lambda: False)() + + +class ActionServer: + """Run long goals with typed goal / result / feedback messages. + + :param on_goal: ``callable(goal_msg) -> bool`` (accept/reject). + :param execute: ``callable(GoalHandle)`` run on its own thread. + + Other parameters mirror :class:`ServiceServer` (``action`` name + base + ``type_name`` + the three message classes). + """ + + def __init__(self, participant, action, type_name, goal_type, result_type, feedback_type, + on_goal, execute, *, native: bool = False) -> None: + def _byte_on_goal(goal_bytes: bytes) -> bool: + try: + return bool(on_goal(goal_type.deserialize(goal_bytes))) + except Exception: + return False + + def _byte_execute(handle) -> None: + execute(GoalHandle(handle, goal_type, result_type, feedback_type)) + + add = participant.add_native_action_server if native else participant.add_action_server + self.valid = add(action, type_name, _byte_on_goal, _byte_execute) + + +class ActionClient: + """Send typed goals and receive typed feedback + result.""" + + def __init__(self, participant, action, type_name, goal_type, result_type, feedback_type, + *, native: bool = False) -> None: + add = participant.add_native_action_client if native else participant.add_action_client + self._client = add(action, type_name) + self._native = native + self._result_type = result_type + self._feedback_type = feedback_type + self._last_goal = None # native: int handle; ROS 2: bytes goal id + self.valid = self._client is not None + + def send_goal(self, goal, on_feedback, on_result) -> bool: + """Send ``goal``; ``on_feedback(feedback_msg)`` per feedback, + ``on_result(status:int, result_msg)`` once at the end (result is ``None`` + if the goal was rejected). Remembers the accepted goal so cancel_goal() + can target it.""" + ft, rt = self._feedback_type, self._result_type + + def _fb(b: bytes) -> None: + try: + on_feedback(ft.deserialize(b)) + except Exception: + # Runs on an engine worker thread: a malformed / wrong-type + # feedback sample (or a raising user callback) must not kill it. + pass + + def _res(status: int, b: bytes) -> None: + on_result(status, rt.deserialize(b) if b else None) + + if self._native: + return self._client.send_goal( + goal.serialize(), _fb, _res, lambda handle: setattr(self, "_last_goal", handle)) + gid = self._client.send_goal(goal.serialize(), _fb, _res) + if gid is not None: + self._last_goal = gid + return gid is not None + + def cancel_goal(self) -> bool: + """Request cancellation of the most recently accepted goal (ROS 2 or + native). The server observes it via GoalHandle.is_canceling(). Returns + True if the cancel request was queued.""" + if self._last_goal is None: + return False + return self._client.cancel_goal(self._last_goal) diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp index ccc1a4029..d31e592af 100644 --- a/lib/python_bindings/rtps_bindings.cpp +++ b/lib/python_bindings/rtps_bindings.cpp @@ -12,6 +12,7 @@ // // It is kept out of the generated pybind_espp.cpp so regeneration never clobbers it. +#include #include #include #include @@ -47,6 +48,15 @@ inline std::shared_ptr make_gil_safe_holder(const py::function &fn }); } +// Same GIL-safe holder pattern for an arbitrary py::object (e.g. a +// concurrent.futures.Future captured into a background reply callback). +inline std::shared_ptr make_gil_safe_object(const py::object &obj) { + return std::shared_ptr(new py::object(obj), [](py::object *p) { + py::gil_scoped_acquire gil; + delete p; + }); +} + Rtps::sample_callback_t wrap_sample_callback(const py::function &fn) { if (!fn) { return {}; @@ -77,6 +87,42 @@ Rtps::matched_callback_t wrap_matched_callback(const py::function &fn) { }; } +std::vector to_vec(const py::bytes &b) { + std::string s = b; + return std::vector(s.begin(), s.end()); +} + +// A service handler: Python callable(bytes) -> bytes. Runs on an engine thread. +Rtps::service_handler_t wrap_service_handler(const py::function &fn) { + auto cb = make_gil_safe_holder(fn); + return [cb](std::span request) -> std::vector { + py::gil_scoped_acquire gil; + try { + py::object r = (*cb)(to_bytes(request)); + if (r.is_none()) { + return {}; + } + return to_vec(r.cast()); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("RtpsParticipant service handler"); + return {}; + } + }; +} + +// A reply callback for call_async: Python callable(bytes). +Rtps::ServiceClient::reply_callback_t wrap_reply_callback(const py::function &fn) { + auto cb = make_gil_safe_holder(fn); + return [cb](std::span reply) { + py::gil_scoped_acquire gil; + try { + (*cb)(to_bytes(reply)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("RtpsParticipant reply callback"); + } + }; +} + // Python-facing Config: like Rtps::Config but with py::function callbacks. struct PyRtpsConfig { std::string interface_address{}; @@ -176,4 +222,419 @@ void py_init_rtps(py::module &m) { }, py::arg("topic"), py::arg("data"), "Publish a CDR-encapsulated sample (bytes) on a topic added with add_writer()."); + + // ---- Services (RMI, ROS 2-interoperable) -------------------------------- + py::class_>( + rtps, "ServiceClient", "Handle for calling a ROS 2-interoperable service.") + .def( + "call", + [](Rtps::ServiceClient &self, const py::bytes &request, double timeout) -> py::object { + auto req = to_vec(request); + std::optional> r; + { + py::gil_scoped_release rel; + r = self.call(req, std::chrono::milliseconds(static_cast(timeout * 1000))); + } + return r ? py::object(to_bytes(*r)) : py::none(); + }, + py::arg("request"), py::arg("timeout") = 5.0, + "Blocking call (RMI). Returns the reply bytes, or None on timeout.") + .def( + "call_async", + [](Rtps::ServiceClient &self, const py::bytes &request, const py::function &on_reply) { + auto cb = wrap_reply_callback(on_reply); + auto req = to_vec(request); + py::gil_scoped_release rel; + return self.call_async(req, std::move(cb)); + }, + py::arg("request"), py::arg("on_reply"), + "Async call (AMI): on_reply(bytes) is invoked when the reply arrives.") + .def( + "call_future", + [](Rtps::ServiceClient &self, const py::bytes &request) { + py::object fut = py::module_::import("concurrent.futures").attr("Future")(); + auto fut_holder = make_gil_safe_object(fut); + auto req = to_vec(request); + bool queued; + { + py::gil_scoped_release rel; + queued = self.call_async(req, [fut_holder](std::span reply) { + py::gil_scoped_acquire gil; + try { + (*fut_holder).attr("set_result")(to_bytes(reply)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("ServiceClient.call_future"); + } + }); + } + if (!queued) { + fut.attr("set_result")(py::none()); + } + return fut; + }, + py::arg("request"), + "Async call (AMI): returns a concurrent.futures.Future for the reply bytes " + "(result is None if the request could not be queued). Use fut.result(timeout=...)."); + + rtps.def( + "add_service_server", + [](Rtps &self, const std::string &service, const std::string &type_name, + const py::function &handler) { + auto h = wrap_service_handler(handler); + py::gil_scoped_release rel; + return self.add_service_server({service, type_name}, std::move(h)); + }, + py::arg("service"), py::arg("type_name"), py::arg("handler"), + "Add a ROS 2 service server; handler(request_bytes) -> reply_bytes.") + .def( + "add_service_client", + [](Rtps &self, const std::string &service, const std::string &type_name) { + py::gil_scoped_release rel; + return self.add_service_client({service, type_name}); + }, + py::arg("service"), py::arg("type_name"), "Add a ROS 2 service client."); + + // ---- Actions (AMI, ROS 2-interoperable) --------------------------------- + py::class_( + rtps, "ActionGoalHandle", "Server-side handle to a running goal (in the execute callback).") + .def("goal", [](Rtps::ActionGoalHandle &h) { return to_bytes(h.goal()); }) + .def( + "publish_feedback", + [](Rtps::ActionGoalHandle &h, const py::bytes &fb) { + auto v = to_vec(fb); + py::gil_scoped_release rel; + h.publish_feedback(v); + }, + py::arg("feedback")) + .def( + "succeed", + [](Rtps::ActionGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.succeed(v); + }, + py::arg("result")) + .def( + "abort", + [](Rtps::ActionGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.abort(v); + }, + py::arg("result")) + .def( + "canceled", + [](Rtps::ActionGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.canceled(v); + }, + py::arg("result"), "Terminate the goal CANCELED (in response to a cancel request).") + .def("is_canceling", &Rtps::ActionGoalHandle::is_canceling); + + py::class_>( + rtps, "ActionClient", "Handle for driving a ROS 2-interoperable action.") + .def( + "send_goal", + [](Rtps::ActionClient &self, const py::bytes &goal, const py::function &on_feedback, + const py::function &on_result) -> py::object { + auto fb = make_gil_safe_holder(on_feedback); + auto rc = make_gil_safe_holder(on_result); + auto goal_v = to_vec(goal); + std::optional gid; + { + py::gil_scoped_release rel; + gid = self.send_goal( + goal_v, + [fb](std::span f) { + py::gil_scoped_acquire gil; + try { + (*fb)(to_bytes(f)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("action feedback"); + } + }, + [rc](int8_t status, std::span r) { + py::gil_scoped_acquire gil; + try { + (*rc)(status, to_bytes(r)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("action result"); + } + }); + } + if (!gid) { + return py::none(); + } + return py::object(py::bytes(reinterpret_cast(gid->data()), gid->size())); + }, + py::arg("goal"), py::arg("on_feedback"), py::arg("on_result"), + "Send a goal. on_feedback(bytes); on_result(status:int, bytes). Returns the goal id.") + .def( + "cancel_goal", + [](Rtps::ActionClient &self, const py::bytes &goal_id) { + auto id_v = to_vec(goal_id); + Rtps::GoalId id{}; + if (id_v.size() != id.size()) { + return false; + } + std::copy(id_v.begin(), id_v.end(), id.begin()); + py::gil_scoped_release rel; + return self.cancel_goal(id); + }, + py::arg("goal_id"), "Request cancellation of a goal by its id (from send_goal)."); + + rtps.def( + "add_action_server", + [](Rtps &self, const std::string &action, const std::string &type_name, + const py::function &on_goal, const py::function &execute) { + auto og = make_gil_safe_holder(on_goal); + auto ex = make_gil_safe_holder(execute); + py::gil_scoped_release rel; + return self.add_action_server( + {action, type_name}, + [og](const Rtps::GoalId &, std::span goal) -> bool { + py::gil_scoped_acquire gil; + try { + return (*og)(to_bytes(goal)).cast(); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("action on_goal"); + return false; + } + }, + [ex](Rtps::ActionGoalHandle h) { + py::gil_scoped_acquire gil; + try { + (*ex)(h); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("action execute"); + } + }); + }, + py::arg("action"), py::arg("type_name"), py::arg("on_goal"), py::arg("execute"), + "Add a ROS 2 action server. on_goal(goal_bytes)->bool; execute(ActionGoalHandle)."); + rtps.def( + "add_action_client", + [](Rtps &self, const std::string &action, const std::string &type_name) { + py::gil_scoped_release rel; + return self.add_action_client({action, type_name}); + }, + py::arg("action"), py::arg("type_name"), "Add a ROS 2 action client."); + + // ---- Native (espp<->espp) services + actions ---------------------------- + py::class_>( + rtps, "NativeServiceClient", "Handle for a lean native (espp<->espp) service.") + .def( + "call", + [](Rtps::NativeServiceClient &self, const py::bytes &request, + double timeout) -> py::object { + auto req = to_vec(request); + std::optional> r; + { + py::gil_scoped_release rel; + r = self.call(req, std::chrono::milliseconds(static_cast(timeout * 1000))); + } + return r ? py::object(to_bytes(*r)) : py::none(); + }, + py::arg("request"), py::arg("timeout") = 5.0) + .def( + "call_async", + [](Rtps::NativeServiceClient &self, const py::bytes &request, + const py::function &on_reply) { + auto cb = make_gil_safe_holder(on_reply); + auto req = to_vec(request); + py::gil_scoped_release rel; + return self.call_async(req, [cb](std::span reply) { + py::gil_scoped_acquire gil; + try { + (*cb)(to_bytes(reply)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native reply"); + } + }); + }, + py::arg("request"), py::arg("on_reply")) + .def( + "call_future", + [](Rtps::NativeServiceClient &self, const py::bytes &request) { + py::object fut = py::module_::import("concurrent.futures").attr("Future")(); + auto fut_holder = make_gil_safe_object(fut); + auto req = to_vec(request); + bool queued; + { + py::gil_scoped_release rel; + queued = self.call_async(req, [fut_holder](std::span reply) { + py::gil_scoped_acquire gil; + try { + (*fut_holder).attr("set_result")(to_bytes(reply)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("NativeServiceClient.call_future"); + } + }); + } + if (!queued) { + fut.attr("set_result")(py::none()); + } + return fut; + }, + py::arg("request"), + "Async call (AMI): returns a concurrent.futures.Future for the reply bytes."); + + py::class_>( + rtps, "NativeActionClient", "Handle for a lean native (espp<->espp) action.") + .def( + "send_goal", + [](Rtps::NativeActionClient &self, const py::bytes &goal, const py::function &on_feedback, + const py::function &on_result, const py::object &on_accepted) { + auto fb = make_gil_safe_holder(on_feedback); + auto rc = make_gil_safe_holder(on_result); + Rtps::NativeActionClient::accepted_callback_t acc = nullptr; + if (!on_accepted.is_none()) { + auto ac = make_gil_safe_holder(on_accepted.cast()); + acc = [ac](uint32_t handle) { + py::gil_scoped_acquire gil; + try { + (*ac)(handle); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native on_accepted"); + } + }; + } + auto goal_v = to_vec(goal); + py::gil_scoped_release rel; + return self.send_goal( + goal_v, + [fb](std::span f) { + py::gil_scoped_acquire gil; + try { + (*fb)(to_bytes(f)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native feedback"); + } + }, + [rc](uint8_t status, std::span r) { + py::gil_scoped_acquire gil; + try { + (*rc)(status, to_bytes(r)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native result"); + } + }, + std::move(acc)); + }, + py::arg("goal"), py::arg("on_feedback"), py::arg("on_result"), + py::arg("on_accepted") = py::none()) + .def( + "cancel_goal", + [](Rtps::NativeActionClient &self, uint32_t goal_handle) { + py::gil_scoped_release rel; + return self.cancel_goal(goal_handle); + }, + py::arg("goal_handle")); + + rtps.def( + "add_native_service_server", + [](Rtps &self, const std::string &service, const std::string &type_name, + const py::function &handler) { + auto h = wrap_service_handler(handler); + py::gil_scoped_release rel; + return self.add_native_service_server({service, type_name}, std::move(h)); + }, + py::arg("service"), py::arg("type_name"), py::arg("handler")) + .def( + "add_native_service_client", + [](Rtps &self, const std::string &service, const std::string &type_name) { + py::gil_scoped_release rel; + return self.add_native_service_client({service, type_name}); + }, + py::arg("service"), py::arg("type_name")) + .def( + "add_native_action_server", + [](Rtps &self, const std::string &action, const std::string &type_name, + const py::function &on_goal, const py::function &execute, + const py::object &on_cancel) { + auto og = make_gil_safe_holder(on_goal); + auto ex = make_gil_safe_holder(execute); + Rtps::native_cancel_callback_t oc = nullptr; + if (!on_cancel.is_none()) { + auto ocw = make_gil_safe_holder(on_cancel.cast()); + oc = [ocw](uint32_t handle) -> bool { + py::gil_scoped_acquire gil; + try { + return (*ocw)(handle).cast(); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native on_cancel"); + return false; + } + }; + } + py::gil_scoped_release rel; + return self.add_native_action_server( + {action, type_name}, + [og](std::span goal) -> bool { + py::gil_scoped_acquire gil; + try { + return (*og)(to_bytes(goal)).cast(); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native on_goal"); + return false; + } + }, + [ex](Rtps::NativeGoalHandle h) { + py::gil_scoped_acquire gil; + try { + (*ex)(h); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("native execute"); + } + }, + std::move(oc)); + }, + py::arg("action"), py::arg("type_name"), py::arg("on_goal"), py::arg("execute"), + py::arg("on_cancel") = py::none()) + .def( + "add_native_action_client", + [](Rtps &self, const std::string &action, const std::string &type_name) { + py::gil_scoped_release rel; + return self.add_native_action_client({action, type_name}); + }, + py::arg("action"), py::arg("type_name")); + + py::class_(rtps, "NativeGoalHandle", + "Server-side handle to a running native goal.") + .def("goal", [](Rtps::NativeGoalHandle &h) { return to_bytes(h.goal()); }) + .def("goal_handle", &Rtps::NativeGoalHandle::goal_handle) + .def("is_canceling", &Rtps::NativeGoalHandle::is_canceling) + .def( + "publish_feedback", + [](Rtps::NativeGoalHandle &h, const py::bytes &fb) { + auto v = to_vec(fb); + py::gil_scoped_release rel; + h.publish_feedback(v); + }, + py::arg("feedback")) + .def( + "succeed", + [](Rtps::NativeGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.succeed(v); + }, + py::arg("result")) + .def( + "abort", + [](Rtps::NativeGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.abort(v); + }, + py::arg("result")) + .def( + "canceled", + [](Rtps::NativeGoalHandle &h, const py::bytes &result) { + auto v = to_vec(result); + py::gil_scoped_release rel; + h.canceled(v); + }, + py::arg("result")); } diff --git a/pc/tests/rtps_action_interop_client.cpp b/pc/tests/rtps_action_interop_client.cpp new file mode 100644 index 000000000..c884299da --- /dev/null +++ b/pc/tests/rtps_action_interop_client.cpp @@ -0,0 +1,108 @@ +// espp action CLIENT for the ROS 2 interop matrix: drives a Fibonacci action +// hosted by a ROS 2 (rclpy) action server, and checks the result sequence. +// Exits 0 iff the terminal result equals Fibonacci(order) with status SUCCEEDED. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +void put_i32(std::vector &v, int32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); +} +int32_t get_i32(std::span p, size_t off) { + return static_cast( + static_cast(p[off]) | (static_cast(p[off + 1]) << 8) | + (static_cast(p[off + 2]) << 16) | (static_cast(p[off + 3]) << 24)); +} +std::vector encode_goal(int32_t order) { + std::vector v(kEncap, kEncap + 4); + put_i32(v, order); + return v; +} +std::vector decode_seq(std::span cdr) { + std::vector seq; + if (cdr.size() < 8) + return seq; + const uint32_t n = static_cast(get_i32(cdr, 4)); + for (uint32_t i = 0; i < n && 8 + (i + 1) * 4 <= cdr.size(); ++i) + seq.push_back(get_i32(cdr, 8 + i * 4)); + return seq; +} +} // namespace + +int main(int argc, char **argv) { + const char *action = (argc > 1) ? argv[1] : "/fibonacci"; + const char *type = (argc > 2) ? argv[2] : "example_interfaces::action::dds_::Fibonacci"; + const int32_t order = (argc > 3) ? std::atoi(argv[3]) : 5; + const int timeout_s = (argc > 4) ? std::atoi(argv[4]) : 30; + const char *interface_ip = (argc > 5) ? argv[5] : ""; + + espp::RtpsParticipant p( + {.interface_address = interface_ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!p.start()) { + std::printf("FAIL: start\n"); + return 1; + } + auto client = p.add_action_client({action, type}); + if (!client) { + std::printf("FAIL: add_action_client\n"); + return 1; + } + + std::this_thread::sleep_for(3s); // let the rclpy server + all 5 endpoints match + + std::mutex m; + std::condition_variable cv; + bool done = false; + int feedback_count = 0; + int8_t status = 0; + std::vector result; + + auto gid = client->send_goal( + encode_goal(order), + [&](std::span fb) { + std::lock_guard lk(m); + ++feedback_count; + (void)decode_seq(fb); + }, + [&](int8_t st, std::span res) { + std::lock_guard lk(m); + status = st; + result = decode_seq(res); + done = true; + cv.notify_one(); + }); + if (!gid.has_value()) { + std::printf("FAIL: send_goal\n"); + return 1; + } + + { + std::unique_lock lk(m); + cv.wait_for(lk, std::chrono::seconds(timeout_s), [&] { return done; }); + } + p.stop(); + + // Build the expected Fibonacci(order) sequence: [0, 1, 1, 2, ...]. + std::vector expected{0, 1}; + for (int32_t i = 1; i < order; ++i) + expected.push_back(expected[i] + expected[i - 1]); + + const bool ok = done && status == 4 /*SUCCEEDED*/ && result == expected && feedback_count > 0; + std::printf("client: status=%d seq_len=%zu feedback=%d => %s\n", (int)status, result.size(), + feedback_count, ok ? "PASS" : "FAIL"); + return ok ? 0 : 1; +} diff --git a/pc/tests/rtps_action_interop_server.cpp b/pc/tests/rtps_action_interop_server.cpp new file mode 100644 index 000000000..c8a1631b6 --- /dev/null +++ b/pc/tests/rtps_action_interop_server.cpp @@ -0,0 +1,87 @@ +// espp action SERVER for the ROS 2 interop matrix: hosts a Fibonacci action +// (example_interfaces/action/Fibonacci) so a ROS 2 client +// (`ros2 action send_goal -f /fibonacci ...` or an rclpy client) can drive it. +// Runs until killed. Prints each goal so the harness can confirm traffic. +// +// Fibonacci CDR (little-endian, post-encap): Goal = {order: int32}; +// Result/Feedback = {sequence: int32[]} (uint32 length prefix + elements). + +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +void put_i32(std::vector &v, int32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); +} +int32_t get_i32(std::span p, size_t off) { + return static_cast( + static_cast(p[off]) | (static_cast(p[off + 1]) << 8) | + (static_cast(p[off + 2]) << 16) | (static_cast(p[off + 3]) << 24)); +} +std::vector encode_seq(const std::vector &seq) { + std::vector v(kEncap, kEncap + 4); + put_i32(v, static_cast(seq.size())); + for (int32_t x : seq) + put_i32(v, x); + return v; +} +} // namespace + +int main(int argc, char **argv) { + const char *action = (argc > 1) ? argv[1] : "/fibonacci"; + const char *type = (argc > 2) ? argv[2] : "example_interfaces::action::dds_::Fibonacci"; + const int run_s = (argc > 3) ? std::atoi(argv[3]) : 40; + const char *interface_ip = (argc > 4) ? argv[4] : ""; + + espp::RtpsParticipant p( + {.interface_address = interface_ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!p.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + std::atomic handled{0}; + if (!p.add_action_server( + {action, type}, + [](const espp::RtpsParticipant::GoalId &, std::span goal) { + return goal.size() >= 8 && get_i32(goal, 4) > 0; + }, + [&handled](espp::RtpsParticipant::ActionGoalHandle h) { + const int32_t order = get_i32(h.goal(), 4); + std::vector seq{0, 1}; + for (int32_t i = 1; i < order; ++i) { + seq.push_back(seq[i] + seq[i - 1]); + auto fb = encode_seq(seq); + h.publish_feedback({fb.data(), fb.size()}); + std::this_thread::sleep_for(200ms); + } + auto result = encode_seq(seq); + h.succeed({result.data(), result.size()}); + handled.fetch_add(1); + std::printf("server: goal order=%d done, seq_len=%zu\n", order, seq.size()); + std::fflush(stdout); + })) { + std::printf("FAIL: add_action_server\n"); + return 1; + } + + std::printf("server: ready action=%s type=%s\n", action, type); + std::fflush(stdout); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(run_s); + while (std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(200ms); + } + p.stop(); + std::printf("server: handled %d goal(s)\n", handled.load()); + return handled.load() > 0 ? 0 : 2; +} diff --git a/pc/tests/rtps_action_loopback.cpp b/pc/tests/rtps_action_loopback.cpp new file mode 100644 index 000000000..43a5fdde2 --- /dev/null +++ b/pc/tests/rtps_action_loopback.cpp @@ -0,0 +1,150 @@ +// In-process action (AMI) loopback: one participant hosts a Fibonacci action +// server, another drives it as a client. Exercises the full M2 action path - +// mangling (M2.1), envelope codec (M2.2), the 3 services (send_goal/get_result/ +// cancel) + 2 topics (feedback/status), deferred get_result, and goal +// correlation by UUID - without ROS 2. Fibonacci CDR encoding matches +// example_interfaces/action/Fibonacci so the same payloads work against a real +// ROS 2 node in the docker interop leg. +// +// Exits 0 iff the result sequence is Fibonacci(order) and feedback was received. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; + +void put_i32(std::vector &v, int32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); +} +int32_t get_i32(std::span p, size_t off) { + return static_cast( + static_cast(p[off]) | (static_cast(p[off + 1]) << 8) | + (static_cast(p[off + 2]) << 16) | (static_cast(p[off + 3]) << 24)); +} +// Fibonacci Goal = { order: int32 }. +std::vector encode_goal(int32_t order) { + std::vector v(kEncap, kEncap + 4); + put_i32(v, order); + return v; +} +// Fibonacci Result/Feedback = { sequence: int32[] } (len-prefixed). +std::vector encode_seq(const std::vector &seq) { + std::vector v(kEncap, kEncap + 4); + put_i32(v, static_cast(seq.size())); + for (int32_t x : seq) + put_i32(v, x); + return v; +} +std::vector decode_seq(std::span cdr) { + std::vector seq; + if (cdr.size() < 8) + return seq; + const uint32_t n = static_cast(get_i32(cdr, 4)); + for (uint32_t i = 0; i < n && 8 + (i + 1) * 4 <= cdr.size(); ++i) + seq.push_back(get_i32(cdr, 8 + i * 4)); + return seq; +} +} // namespace + +int main() { + const espp::RtpsParticipant::ActionConfig cfg{"/fibonacci", + "example_interfaces::action::dds_::Fibonacci"}; + + espp::RtpsParticipant server({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant client({.log_level = espp::Logger::Verbosity::WARN}); + if (!server.start() || !client.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + // Server: compute the Fibonacci sequence, publishing feedback each step. + if (!server.add_action_server( + cfg, + [](const espp::RtpsParticipant::GoalId &, std::span goal) { + return goal.size() >= 8 && get_i32(goal, 4) > 0; // accept order > 0 + }, + [](espp::RtpsParticipant::ActionGoalHandle h) { + const int32_t order = get_i32(h.goal(), 4); + std::vector seq{0, 1}; + for (int32_t i = 1; i < order; ++i) { + seq.push_back(seq[i] + seq[i - 1]); + auto fb = encode_seq(seq); + h.publish_feedback({fb.data(), fb.size()}); + std::this_thread::sleep_for(100ms); + } + auto result = encode_seq(seq); + h.succeed({result.data(), result.size()}); + })) { + std::printf("FAIL: add_action_server\n"); + return 1; + } + + auto action = client.add_action_client(cfg); + if (!action) { + std::printf("FAIL: add_action_client\n"); + return 1; + } + + std::this_thread::sleep_for(2s); // SEDP match across all 5 endpoints + + std::mutex m; + std::condition_variable cv; + bool done = false; + int feedback_count = 0; + std::vector result_seq; + int8_t result_status = 0; + + const int32_t order = 5; + auto gid = action->send_goal( + encode_goal(order), + [&](std::span fb) { + std::lock_guard lk(m); + ++feedback_count; + (void)decode_seq(fb); + }, + [&](int8_t status, std::span result) { + std::lock_guard lk(m); + result_status = status; + result_seq = decode_seq(result); + done = true; + cv.notify_one(); + }); + if (!gid.has_value()) { + std::printf("FAIL: send_goal\n"); + return 1; + } + + { + std::unique_lock lk(m); + cv.wait_for(lk, 20s, [&] { return done; }); + } + + server.stop(); + client.stop(); + + // Fibonacci(5) = [0, 1, 1, 2, 3, 5]; feedback should have arrived (order-1 msgs). + const std::vector expected{0, 1, 1, 2, 3, 5}; + const bool result_ok = (result_status == 4 /*SUCCEEDED*/) && (result_seq == expected); + const bool feedback_ok = (feedback_count > 0); + std::printf("result status=%d seq_len=%zu feedback=%d => %s\n", (int)result_status, + result_seq.size(), feedback_count, (result_ok && feedback_ok) ? "PASS" : "FAIL"); + if (result_ok && feedback_ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_action_naming.cpp b/pc/tests/rtps_action_naming.cpp new file mode 100644 index 000000000..915dce9bb --- /dev/null +++ b/pc/tests/rtps_action_naming.cpp @@ -0,0 +1,62 @@ +// Unit test for ROS 2 action name/type mangling (rtps::rpc). Expected strings are +// taken verbatim from a live rmw_fastrtps (ROS 2 Jazzy) Fibonacci action capture +// (RMI_AMI_DESIGN.md 3.4). Header-only. + +#include +#include + +#include "rtps/rpc/action_naming.hpp" + +namespace { +int failures = 0; +void check(const std::string &got, const std::string &want, const char *what) { + if (got != want) { + std::printf("FAIL %s: got \"%s\" want \"%s\"\n", what, got.c_str(), want.c_str()); + ++failures; + } else { + std::printf("ok %s = \"%s\"\n", what, got.c_str()); + } +} +} // namespace + +int main() { + using namespace rtps::rpc; + + const std::string base = "example_interfaces::action::dds_::Fibonacci"; + + // send_goal service: rq/fibonacci/_action/send_goalRequest, _SendGoal_Request_. + check(service_request_topic(action_send_goal_service("/fibonacci")), + "rq/fibonacci/_action/send_goalRequest", "send_goal_req_topic"); + check(service_reply_topic(action_send_goal_service("/fibonacci")), + "rr/fibonacci/_action/send_goalReply", "send_goal_rep_topic"); + check(service_request_type(action_send_goal_type(base)), + "example_interfaces::action::dds_::Fibonacci_SendGoal_Request_", "send_goal_req_type"); + check(service_response_type(action_send_goal_type(base)), + "example_interfaces::action::dds_::Fibonacci_SendGoal_Response_", "send_goal_rep_type"); + + // get_result service. + check(service_request_topic(action_get_result_service("/fibonacci")), + "rq/fibonacci/_action/get_resultRequest", "get_result_req_topic"); + check(service_request_type(action_get_result_type(base)), + "example_interfaces::action::dds_::Fibonacci_GetResult_Request_", "get_result_req_type"); + + // cancel_goal service (fixed action_msgs type). + check(service_request_topic(action_cancel_goal_service("/fibonacci")), + "rq/fibonacci/_action/cancel_goalRequest", "cancel_goal_req_topic"); + check(service_request_type(action_cancel_goal_type()), + "action_msgs::srv::dds_::CancelGoal_Request_", "cancel_goal_req_type"); + + // topics. + check(action_feedback_topic("/fibonacci"), "rt/fibonacci/_action/feedback", "feedback_topic"); + check(action_status_topic("/fibonacci"), "rt/fibonacci/_action/status", "status_topic"); + check(action_feedback_type(base), "example_interfaces::action::dds_::Fibonacci_FeedbackMessage_", + "feedback_type"); + check(action_status_type(), "action_msgs::msg::dds_::GoalStatusArray_", "status_type"); + + if (failures == 0) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: %d\n", failures); + return 1; +} diff --git a/pc/tests/rtps_action_types.cpp b/pc/tests/rtps_action_types.cpp new file mode 100644 index 000000000..afeb812a7 --- /dev/null +++ b/pc/tests/rtps_action_types.cpp @@ -0,0 +1,146 @@ +// Validates the action envelope codec (rtps::rpc, action_types.hpp) against the +// EXACT bytes captured from a live rmw_fastrtps (ROS 2 Jazzy) Fibonacci action +// (RMI_AMI_DESIGN.md 3.4), goal UUID 93beb05274a946a6ae8f2540d15de68e, order=5. +// Confirms byte-for-byte wrap/parse of every action envelope. + +#include +#include +#include +#include +#include + +#include "rtps/rpc/action_types.hpp" + +namespace { +int failures = 0; + +std::vector hex(const char *s) { + std::vector v; + for (; s[0] && s[1]; s += 2) { + auto nib = [](char c) -> int { + if (c >= '0' && c <= '9') + return c - '0'; + if (c >= 'a' && c <= 'f') + return c - 'a' + 10; + return 0; + }; + v.push_back(static_cast((nib(s[0]) << 4) | nib(s[1]))); + } + return v; +} +void eq(const std::vector &got, const std::vector &want, const char *what) { + if (got == want) { + std::printf("ok %s\n", what); + return; + } + std::printf("FAIL %s\n got %zu bytes\n want %zu bytes\n", what, got.size(), want.size()); + ++failures; +} +void expect(bool cond, const char *what) { + if (cond) { + std::printf("ok %s\n", what); + } else { + std::printf("FAIL %s\n", what); + ++failures; + } +} +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +// kEncap + hex(h) as one vector - the shape of every CDR message here. +std::vector encap_hex(const char *h) { + std::vector v(kEncap, kEncap + 4); + const auto body = hex(h); + v.insert(v.end(), body.begin(), body.end()); + return v; +} +} // namespace + +int main() { + using namespace rtps::rpc; + const std::vector uuid_bytes = hex("93beb05274a946a6ae8f2540d15de68e"); + GoalUuid uuid{}; + std::copy(uuid_bytes.begin(), uuid_bytes.end(), uuid.begin()); + + // SendGoal_Request: UUID + int32 order=5. Captured serializedData (post-encap): + // 93beb05274a946a6ae8f2540d15de68e 05000000 + { + std::vector goal = encap_hex("05000000"); // order=5 as its own CDR msg + auto msg = wrap_send_goal_request(uuid, goal); + std::vector want = encap_hex("93beb05274a946a6ae8f2540d15de68e05000000"); + eq(msg, want, "wrap_send_goal_request"); + GoalUuid gid{}; + std::vector goal_out; + expect(unwrap_send_goal_request(msg, gid, goal_out) && gid == uuid && goal_out == goal, + "unwrap_send_goal_request roundtrip"); + } + + // SendGoal_Response: accepted=true, stamp. Captured: 01000000 a3927e6a dc014325 + { + auto msg = make_send_goal_response(true, 0x6a7e92a3, 0x254301dc); + std::vector want = encap_hex("01000000a3927e6adc014325"); + eq(msg, want, "make_send_goal_response"); + bool accepted = false; + expect(parse_send_goal_response(msg, accepted) && accepted, "parse_send_goal_response"); + } + + // GetResult_Request: just the UUID. + { + auto msg = make_get_result_request(uuid); + std::vector want(kEncap, kEncap + 4); + want.insert(want.end(), uuid_bytes.begin(), uuid_bytes.end()); + eq(msg, want, "make_get_result_request"); + GoalUuid gid{}; + expect(parse_get_result_request(msg, gid) && gid == uuid, "parse_get_result_request"); + } + + // GetResult_Response: status=SUCCEEDED(4), result=int32[] [0,1,1,2,3,5]. + // Captured: 04000000 06000000 00.. 01.. 01.. 02.. 03.. 05.. + { + std::vector result = + encap_hex("06000000000000000100000001000000020000000300000005000000"); + auto msg = wrap_get_result_response(GoalStatus::SUCCEEDED, result); + std::vector want = + encap_hex("0400000006000000000000000100000001000000020000000300000005000000"); + eq(msg, want, "wrap_get_result_response"); + GoalStatus st{}; + std::vector res_out; + expect(unwrap_get_result_response(msg, st, res_out) && st == GoalStatus::SUCCEEDED && + res_out == result, + "unwrap_get_result_response roundtrip"); + } + + // FeedbackMessage: UUID + int32[] [0,1,1]. Captured: + // 93beb05274a946a6ae8f2540d15de68e 03000000 000000000100000001000000 + { + std::vector fb = encap_hex("03000000000000000100000001000000"); + auto msg = wrap_feedback(uuid, fb); + std::vector want = + encap_hex("93beb05274a946a6ae8f2540d15de68e03000000000000000100000001000000"); + eq(msg, want, "wrap_feedback"); + } + + // GoalStatusArray: 1 entry, ACCEPTED. Captured: + // 01000000 93beb05274a946a6ae8f2540d15de68e a3927e6a 298f4425 01000000 + { + GoalStatusEntry e; + e.goal_id = uuid; + e.sec = 0x6a7e92a3; + e.nsec = 0x25448f29; + e.status = GoalStatus::ACCEPTED; + std::array entries{e}; + auto msg = make_goal_status_array(entries); + std::vector want = + encap_hex("0100000093beb05274a946a6ae8f2540d15de68ea3927e6a298f442501000000"); + eq(msg, want, "make_goal_status_array"); + std::vector out; + expect(parse_goal_status_array(msg, out) && out.size() == 1 && out[0].goal_id == uuid && + out[0].status == GoalStatus::ACCEPTED, + "parse_goal_status_array roundtrip"); + } + + if (failures == 0) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: %d\n", failures); + return 1; +} diff --git a/pc/tests/rtps_embedded_golden.cpp b/pc/tests/rtps_embedded_golden.cpp index 030b8e38b..8e93fb9a4 100644 --- a/pc/tests/rtps_embedded_golden.cpp +++ b/pc/tests/rtps_embedded_golden.cpp @@ -69,6 +69,23 @@ std::vector build_data() { return b.bytes; } +std::vector build_data_related_sample_identity() { + // DATA submessage carrying a related_sample_identity inline QoS (ROS 2 service + // request/reply correlation). Pins the inline-QoS layout: PID 0x0083 + 0x800f, + // each a 24-byte SampleIdentity {Guid, SequenceNumber}, then PID_SENTINEL, + // followed by the payload. The identity's guid uses kPrefix + a reader entity + // and sequence number {0, 1}, matching the shape seen in the live capture. + static constexpr uint8_t kPayload[] = {0x00, 0x01, 0x00, 0x00, 0x65, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + rtps::PayloadBuffer payload; + payload.append(kPayload, sizeof(kPayload)); + rtps::rpc::SampleIdentity related{rtps::Guid_t{kPrefix, kReaderId}, rtps::SequenceNumber_t{0, 1}}; + rtps::PayloadBuffer b; + rtps::MessageFactory::addSubMessageDataWithRelatedSampleIdentity( + b, payload, related, rtps::SequenceNumber_t{0, 1}, kWriterId, kReaderId); + return b.bytes; +} + std::vector build_data_frag() { // One DATA_FRAG submessage: fragment #1 of a 200000-byte sample split at a // 63000-byte fragment size, carrying a 10-byte ramp payload chunk. Pins the @@ -205,6 +222,8 @@ int main(int argc, char **argv) { {"info_dst", build_info_dst, kGolden_info_dst}, {"info_ts_invalid", build_info_ts_invalid, kGolden_info_ts_invalid}, {"data", build_data, kGolden_data}, + {"data_related_sample_identity", build_data_related_sample_identity, + kGolden_data_related_sample_identity}, {"data_frag", build_data_frag, kGolden_data_frag}, {"heartbeat", build_heartbeat, kGolden_heartbeat}, {"acknack", build_acknack, kGolden_acknack}, diff --git a/pc/tests/rtps_embedded_golden.inc b/pc/tests/rtps_embedded_golden.inc index 12eb92aca..4bbcce0e0 100644 --- a/pc/tests/rtps_embedded_golden.inc +++ b/pc/tests/rtps_embedded_golden.inc @@ -14,6 +14,15 @@ static constexpr uint8_t kGolden_data[] = { 0x00, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x68, 0x65, 0x6C, 0x6C, 0x6F, 0x00,}; +static constexpr uint8_t kGolden_data_related_sample_identity[] = { + 0x15, 0x07, 0x5C, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, + 0x00, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x83, 0x00, 0x18, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x09, 0x0A, 0x0B, 0x0C, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x00, 0x00, 0x0F, 0x80, 0x18, 0x00, 0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x00, 0x00, 0x02, 0x04, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,}; static constexpr uint8_t kGolden_data_frag[] = { 0x16, 0x01, 0x2A, 0x00, 0x00, 0x00, 0x1C, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, diff --git a/pc/tests/rtps_native_action_loopback.cpp b/pc/tests/rtps_native_action_loopback.cpp new file mode 100644 index 000000000..dd472d638 --- /dev/null +++ b/pc/tests/rtps_native_action_loopback.cpp @@ -0,0 +1,163 @@ +// In-process native (espp<->espp) action loopback: the lean AMI - one native +// send_goal service + one feedback topic carrying the terminal result. A +// "countup" action: the server counts 1..N, publishing each value as feedback, +// then succeeds with N. Exits 0 iff feedback arrived and the result equals N. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +void put_i32(std::vector &v, int32_t x) { + for (int i = 0; i < 4; ++i) + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); +} +int32_t get_i32(std::span p, size_t off) { + return static_cast( + static_cast(p[off]) | (static_cast(p[off + 1]) << 8) | + (static_cast(p[off + 2]) << 16) | (static_cast(p[off + 3]) << 24)); +} +std::vector encode_i32(int32_t x) { + std::vector v(kEncap, kEncap + 4); + put_i32(v, x); + return v; +} +} // namespace + +int main() { + const espp::RtpsParticipant::ActionConfig cfg{"/countup", "espp::native::CountUp"}; + + espp::RtpsParticipant server({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant client({.log_level = espp::Logger::Verbosity::WARN}); + if (!server.start() || !client.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + if (!server.add_native_action_server( + cfg, + [](std::span goal) { return goal.size() >= 8 && get_i32(goal, 4) > 0; }, + [](espp::RtpsParticipant::NativeGoalHandle h) { + const int32_t n = get_i32(h.goal(), 4); + for (int32_t i = 1; i <= n; ++i) { + // Cancel-aware: wind down (CANCELED) when the client cancels. + if (h.is_canceling()) { + auto partial = encode_i32(i - 1); + h.canceled({partial.data(), partial.size()}); + return; + } + auto fb = encode_i32(i); + h.publish_feedback({fb.data(), fb.size()}); + std::this_thread::sleep_for(80ms); + } + auto result = encode_i32(n); + h.succeed({result.data(), result.size()}); + })) { + std::printf("FAIL: add_native_action_server\n"); + return 1; + } + + auto action = client.add_native_action_client(cfg); + if (!action) { + std::printf("FAIL: add_native_action_client\n"); + return 1; + } + + std::this_thread::sleep_for(2s); // SEDP match + + std::mutex m; + std::condition_variable cv; + bool done = false; + int feedback_count = 0; + uint8_t status = 0; + int32_t result = 0; + + const int32_t n = 5; + action->send_goal( + encode_i32(n), + [&](std::span fb) { + std::lock_guard lk(m); + if (fb.size() >= 8) + ++feedback_count; + }, + [&](uint8_t st, std::span res) { + std::lock_guard lk(m); + status = st; + if (res.size() >= 8) + result = get_i32(res, 4); + done = true; + cv.notify_one(); + }); + + { + std::unique_lock lk(m); + cv.wait_for(lk, 15s, [&] { return done; }); + } + + const bool ok = done && status == 4 /*SUCCEEDED*/ && result == n && feedback_count > 0; + std::printf("native action: status=%d result=%d feedback=%d => %s\n", (int)status, result, + feedback_count, ok ? "PASS" : "FAIL"); + + // Phase 2: cancel a long-running goal mid-flight. The client learns the goal + // handle via on_accepted, waits for the first feedback, then cancel_goal()s it; + // the server's execute observes is_canceling() and ends CANCELED. + std::mutex m2; + std::condition_variable cv2; + bool done2 = false, have_handle = false; + int feedback2 = 0; + uint8_t status2 = 0; + uint32_t handle2 = 0; + action->send_goal( + encode_i32(1000), // long enough (1000 * 80ms) that it can't finish before cancel + [&](std::span fb) { + std::lock_guard lk(m2); + if (fb.size() >= 8) + ++feedback2; + cv2.notify_one(); + }, + [&](uint8_t st, std::span) { + std::lock_guard lk(m2); + status2 = st; + done2 = true; + cv2.notify_one(); + }, + [&](uint32_t handle) { + std::lock_guard lk(m2); + handle2 = handle; + have_handle = true; + cv2.notify_one(); + }); + { + std::unique_lock lk(m2); + cv2.wait_for(lk, 5s, [&] { return have_handle && feedback2 >= 1; }); + } + action->cancel_goal(handle2); + { + std::unique_lock lk(m2); + cv2.wait_for(lk, 10s, [&] { return done2; }); + } + const bool cancel_ok = done2 && status2 == 5 /*CANCELED*/; + std::printf("native cancel: handle=%u status=%d feedback=%d => %s\n", handle2, (int)status2, + feedback2, cancel_ok ? "PASS" : "FAIL"); + + server.stop(); + client.stop(); + + if (ok && cancel_ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_native_protocol.cpp b/pc/tests/rtps_native_protocol.cpp new file mode 100644 index 000000000..7e427a77d --- /dev/null +++ b/pc/tests/rtps_native_protocol.cpp @@ -0,0 +1,96 @@ +// Byte-level unit test for the native (espp<->espp) protocol codec +// (rtps/rpc/native_protocol.hpp): the 20-byte in-band request/reply header and +// the native-action goal-reply + feedback framing. No network - pure codec +// round-trips + fixed-layout assertions. Exits 0 iff every check passes. + +#include +#include +#include +#include +#include + +#include "rtps/rpc/native_protocol.hpp" + +namespace { +int failures = 0; +void expect(bool cond, const char *what) { + if (cond) { + std::printf("ok %s\n", what); + } else { + std::printf("FAIL %s\n", what); + ++failures; + } +} +} // namespace + +int main() { + using namespace rtps::rpc; + + // --- Request/reply header (20 bytes) round-trip + fixed layout. --- + { + NativeHeader h; + h.client_prefix = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; + h.request_id = 0x11223344; + h.op = NativeOp::REPLY; + h.flags = 0; + const std::vector payload{0x00, 0x01, 0x00, 0x00, 0xAA, 0xBB}; + auto frame = native_encode(h, payload); + + expect(frame.size() == NATIVE_HEADER_SIZE + payload.size(), "encode size"); + // request_id at offset 12 is little-endian. + expect(frame[12] == 0x44 && frame[13] == 0x33 && frame[14] == 0x22 && frame[15] == 0x11, + "request_id LE at offset 12"); + expect(frame[16] == static_cast(NativeOp::REPLY), "op at offset 16"); + expect(std::equal(h.client_prefix.begin(), h.client_prefix.end(), frame.begin()), + "client_prefix at offset 0"); + + NativeHeader out; + std::span out_payload; + expect(native_decode(frame, out, out_payload), "decode ok"); + expect(out.client_prefix == h.client_prefix && out.request_id == h.request_id && out.op == h.op, + "decode header round-trip"); + expect(out_payload.size() == payload.size() && + std::equal(out_payload.begin(), out_payload.end(), payload.begin()), + "decode payload round-trip"); + + // A frame shorter than the header must be rejected. + std::vector tooShort(NATIVE_HEADER_SIZE - 1, 0); + NativeHeader dummy; + std::span dummySpan; + expect(!native_decode(tooShort, dummy, dummySpan), "decode rejects short frame"); + } + + // --- Native-action send_goal reply { accepted, goal_handle }. --- + { + auto reply = native_make_goal_reply(true, 0x0A0B0C0D); + bool accepted = false; + uint32_t handle = 0; + expect(native_parse_goal_reply(reply, accepted, handle) && accepted && handle == 0x0A0B0C0D, + "goal reply round-trip (accepted)"); + auto rej = native_make_goal_reply(false, 0); + bool acc2 = true; + uint32_t h2 = 99; + expect(native_parse_goal_reply(rej, acc2, h2) && !acc2, "goal reply round-trip (rejected)"); + } + + // --- Native-action feedback/result { goal_handle, status, payload }. --- + { + const std::vector body{0x00, 0x01, 0x00, 0x00, 0xDE, 0xAD, 0xBE, 0xEF}; + auto msg = native_make_feedback(0x01020304, NativeGoalStatus::SUCCEEDED, body); + uint32_t handle = 0; + NativeGoalStatus status{}; + std::vector payload; + expect(native_parse_feedback(msg, handle, status, payload), "feedback parse ok"); + expect(handle == 0x01020304 && status == NativeGoalStatus::SUCCEEDED, "feedback handle+status"); + // payload is re-encapsulated (4-byte encap + the spliced body tail). + expect(payload.size() == body.size() && payload[4] == 0xDE && payload.back() == 0xEF, + "feedback payload round-trip"); + } + + if (failures == 0) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: %d\n", failures); + return 1; +} diff --git a/pc/tests/rtps_native_service_loopback.cpp b/pc/tests/rtps_native_service_loopback.cpp new file mode 100644 index 000000000..2b6e7849c --- /dev/null +++ b/pc/tests/rtps_native_service_loopback.cpp @@ -0,0 +1,120 @@ +// In-process native (espp<->espp) service loopback: exercises the lean +// request/reply protocol (20-byte in-band correlation header over plain pub/sub, +// no ROS mangling / inline QoS) with all three client call styles. Uses an +// add_two_ints service (two int64 in, one int64 out). Exits 0 iff sync, async, +// and future calls all return the correlated sum. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +void put_i64(std::vector &v, int64_t x) { + for (int i = 0; i < 8; ++i) + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); +} +int64_t get_i64(std::span p, size_t off) { + int64_t x = 0; + for (int i = 0; i < 8; ++i) + x |= static_cast(p[off + i]) << (8 * i); + return x; +} +std::vector request(int64_t a, int64_t b) { + std::vector v(kEncap, kEncap + 4); + put_i64(v, a); + put_i64(v, b); + return v; +} +std::vector response(int64_t sum) { + std::vector v(kEncap, kEncap + 4); + put_i64(v, sum); + return v; +} +} // namespace + +int main() { + const espp::RtpsParticipant::ServiceConfig cfg{"/add_two_ints", "espp::native::AddTwoInts"}; + + espp::RtpsParticipant server({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant client({.log_level = espp::Logger::Verbosity::WARN}); + if (!server.start() || !client.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + if (!server.add_native_service_server(cfg, [](std::span req) { + if (req.size() < 4 + 16) + return std::vector{}; + return response(get_i64(req, 4) + get_i64(req, 12)); + })) { + std::printf("FAIL: add_native_service_server\n"); + return 1; + } + auto call = client.add_native_service_client(cfg); + if (!call) { + std::printf("FAIL: add_native_service_client\n"); + return 1; + } + + std::this_thread::sleep_for(2s); // SEDP match on es_rq/es_rr + + // Sync. + bool sync_ok = false; + { + auto r = call->call(request(7, 35), 10s); + sync_ok = r && r->size() >= 12 && get_i64(*r, 4) == 42; + std::printf("native sync: 7+35 => %lld %s\n", r ? (long long)get_i64(*r, 4) : -1, + sync_ok ? "ok" : "FAIL"); + } + // Async. + bool async_ok = false; + { + std::mutex m; + std::condition_variable cv; + bool done = false; + int64_t got = 0; + call->call_async(request(1000, 337), [&](std::span r) { + if (r.size() >= 12) { + std::lock_guard lk(m); + got = get_i64(r, 4); + done = true; + } + cv.notify_one(); + }); + std::unique_lock lk(m); + if (cv.wait_for(lk, 10s, [&] { return done; })) + async_ok = (got == 1337); + std::printf("native async: 1000+337 => %lld %s\n", (long long)got, async_ok ? "ok" : "FAIL"); + } + // Future. + bool future_ok = false; + { + auto fut = call->call_future(request(500, 500)); + if (fut.wait_for(10s) == std::future_status::ready) { + auto r = fut.get(); + future_ok = r && r->size() >= 12 && get_i64(*r, 4) == 1000; + } + std::printf("native future: 500+500 => %s\n", future_ok ? "ok" : "FAIL"); + } + + server.stop(); + client.stop(); + + if (sync_ok && async_ok && future_ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_service_interop_client.cpp b/pc/tests/rtps_service_interop_client.cpp new file mode 100644 index 000000000..f5db12512 --- /dev/null +++ b/pc/tests/rtps_service_interop_client.cpp @@ -0,0 +1,79 @@ +// espp service CLIENT for the ROS 2 interop matrix: calls an add_two_ints +// service hosted by a ROS 2 (rclpy) server and checks the returned sum. Exits 0 +// iff the correlated reply equals a + b. +// +// AddTwoInts wire form (classic CDR, little-endian): request = int64 a + int64 b; +// response = int64 sum. Payloads include the 4-byte CDR encapsulation header. + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +void put_i64(std::vector &v, int64_t x) { + for (int i = 0; i < 8; ++i) { + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); + } +} +int64_t get_i64(std::span p, size_t off) { + int64_t x = 0; + for (int i = 0; i < 8; ++i) { + x |= static_cast(p[off + i]) << (8 * i); + } + return x; +} +} // namespace + +int main(int argc, char **argv) { + const char *service = (argc > 1) ? argv[1] : "/add_two_ints"; + const char *type = (argc > 2) ? argv[2] : "example_interfaces::srv::dds_::AddTwoInts"; + const int64_t a = (argc > 3) ? std::atoll(argv[3]) : 20; + const int64_t b = (argc > 4) ? std::atoll(argv[4]) : 22; + const int timeout_s = (argc > 5) ? std::atoi(argv[5]) : 30; + const char *interface_ip = (argc > 6) ? argv[6] : ""; + + espp::RtpsParticipant p( + {.interface_address = interface_ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!p.start()) { + std::printf("FAIL: start\n"); + return 1; + } + auto client = p.add_service_client({service, type}); + if (!client) { + std::printf("FAIL: add_service_client\n"); + return 1; + } + + std::vector request(kEncap, kEncap + 4); + put_i64(request, a); + put_i64(request, b); + + // Retry: the rclpy server may still be coming up / matching over SEDP. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(timeout_s); + int64_t sum = 0; + bool ok = false; + while (!ok && std::chrono::steady_clock::now() < deadline) { + auto reply = client->call(request, 3s); + if (reply.has_value() && reply->size() >= 4 + 8) { + sum = get_i64(*reply, 4); + ok = (sum == a + b); + } + if (!ok) { + std::this_thread::sleep_for(500ms); + } + } + + p.stop(); + std::printf("client: %lld + %lld = %lld (expected %lld) => %s\n", (long long)a, (long long)b, + (long long)sum, (long long)(a + b), ok ? "PASS" : "FAIL"); + return ok ? 0 : 1; +} diff --git a/pc/tests/rtps_service_interop_server.cpp b/pc/tests/rtps_service_interop_server.cpp new file mode 100644 index 000000000..3d4aa2895 --- /dev/null +++ b/pc/tests/rtps_service_interop_server.cpp @@ -0,0 +1,79 @@ +// espp service SERVER for the ROS 2 interop matrix: hosts an add_two_ints +// service (example_interfaces/srv/AddTwoInts) so a ROS 2 client +// (`ros2 service call /add_two_ints ...` or an rclpy client) can call it. Runs +// until killed. Prints each handled request so the harness can confirm traffic. +// +// AddTwoInts wire form (classic CDR, little-endian): request = int64 a + int64 b; +// response = int64 sum. Payloads include the 4-byte CDR encapsulation header. + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; +int64_t get_i64(std::span p, size_t off) { + int64_t x = 0; + for (int i = 0; i < 8; ++i) { + x |= static_cast(p[off + i]) << (8 * i); + } + return x; +} +std::vector encode_i64(int64_t v) { + std::vector b(kEncap, kEncap + 4); + for (int i = 0; i < 8; ++i) { + b.push_back(static_cast((v >> (8 * i)) & 0xFF)); + } + return b; +} +} // namespace + +int main(int argc, char **argv) { + const char *service = (argc > 1) ? argv[1] : "/add_two_ints"; + const char *type = (argc > 2) ? argv[2] : "example_interfaces::srv::dds_::AddTwoInts"; + const int run_s = (argc > 3) ? std::atoi(argv[3]) : 30; + const char *interface_ip = (argc > 4) ? argv[4] : ""; + + espp::RtpsParticipant p( + {.interface_address = interface_ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!p.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + std::atomic handled{0}; + if (!p.add_service_server( + {service, type}, [&](std::span req) -> std::vector { + if (req.size() < 4 + 16) { + return {}; + } + const int64_t a = get_i64(req, 4); + const int64_t b = get_i64(req, 12); + const int64_t sum = a + b; + handled.fetch_add(1); + std::printf("server: %lld + %lld = %lld\n", (long long)a, (long long)b, (long long)sum); + std::fflush(stdout); + return encode_i64(sum); + })) { + std::printf("FAIL: add_service_server\n"); + return 1; + } + + std::printf("server: ready service=%s type=%s\n", service, type); + std::fflush(stdout); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(run_s); + while (std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(200ms); + } + p.stop(); + std::printf("server: handled %d request(s)\n", handled.load()); + return handled.load() > 0 ? 0 : 2; +} diff --git a/pc/tests/rtps_service_loopback.cpp b/pc/tests/rtps_service_loopback.cpp new file mode 100644 index 000000000..3402d8152 --- /dev/null +++ b/pc/tests/rtps_service_loopback.cpp @@ -0,0 +1,192 @@ +// In-process service (RMI) loopback: one participant hosts an add_two_ints +// service server, another calls it as a client. Exercises the full M1 request/ +// reply path end to end - name mangling, the related_sample_identity inline QoS +// emit on both request and reply, the reader capturing it, and the pending- +// request correlation - without needing ROS 2. The wire encoding matches +// example_interfaces/srv/AddTwoInts (two int64 in, one int64 out) so the same +// payloads are valid against a real ROS 2 node in the docker interop leg. +// +// Exits 0 iff the correlated reply carries the expected sum. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +using ServiceConfig = espp::RtpsParticipant::ServiceConfig; + +// CDR_LE encapsulation header (little-endian, no options). +constexpr uint8_t kEncap[4] = {0x00, 0x01, 0x00, 0x00}; + +void put_i64(std::vector &v, int64_t x) { + for (int i = 0; i < 8; ++i) { + v.push_back(static_cast((x >> (8 * i)) & 0xFF)); + } +} +int64_t get_i64(std::span p, size_t off) { + int64_t x = 0; + for (int i = 0; i < 8; ++i) { + x |= static_cast(p[off + i]) << (8 * i); + } + return x; +} + +std::vector encode_request(int64_t a, int64_t b) { + std::vector v(kEncap, kEncap + 4); + put_i64(v, a); + put_i64(v, b); + return v; +} +std::vector encode_response(int64_t sum) { + std::vector v(kEncap, kEncap + 4); + put_i64(v, sum); + return v; +} +} // namespace + +int main() { + const ServiceConfig cfg{"/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts"}; + + espp::RtpsParticipant server({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant client({.log_level = espp::Logger::Verbosity::WARN}); + if (!server.start() || !client.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + // Server: sum = a + b. + if (!server.add_service_server(cfg, [](std::span req) -> std::vector { + if (req.size() < 4 + 16) { + return {}; + } + const int64_t a = get_i64(req, 4); + const int64_t b = get_i64(req, 12); + return encode_response(a + b); + })) { + std::printf("FAIL: add_service_server\n"); + return 1; + } + + auto call = client.add_service_client(cfg); + if (!call) { + std::printf("FAIL: add_service_client\n"); + return 1; + } + + std::this_thread::sleep_for(2s); // SEDP match on rq/ + rr/ topics + + // Synchronous call. + const int64_t a = 7, b = 35; + auto reply = call->call(encode_request(a, b), 10s); + bool ok = false; + if (!reply.has_value()) { + std::printf("FAIL: call timed out (no correlated reply)\n"); + } else if (reply->size() < 4 + 8) { + std::printf("FAIL: reply too short (%zu bytes)\n", reply->size()); + } else { + const int64_t sum = get_i64(*reply, 4); + ok = (sum == a + b); + std::printf("sync call: %lld + %lld = %lld (expected %lld) => %s\n", (long long)a, (long long)b, + (long long)sum, (long long)(a + b), ok ? "ok" : "MISMATCH"); + } + + // Asynchronous call: a second request must correlate independently. + bool async_ok = false; + { + std::mutex m; + std::condition_variable cv; + bool done = false; + int64_t got = 0; + const int64_t a2 = 1000, b2 = 337; + if (call->call_async(encode_request(a2, b2), [&](std::span rep) { + if (rep.size() >= 4 + 8) { + std::lock_guard lk(m); + got = get_i64(rep, 4); + done = true; + } + cv.notify_one(); + })) { + std::unique_lock lk(m); + if (cv.wait_for(lk, 10s, [&] { return done; })) { + async_ok = (got == a2 + b2); + } + std::printf("async call: got %lld (expected %lld) => %s\n", (long long)got, + (long long)(a2 + b2), async_ok ? "ok" : "MISMATCH/timeout"); + } + } + + // Deferred server: a separate service whose handler replies from another + // thread after a delay (exercises add_service_server_deferred + ServiceResponder). + bool deferred_ok = false; + { + const char *dsvc = "/add_two_ints_deferred"; + std::vector workers; + std::mutex wm; + server.add_service_server_deferred( + {dsvc, "example_interfaces::srv::dds_::AddTwoInts"}, + [&](std::span req, espp::RtpsParticipant::ServiceResponder responder) { + std::vector r(req.begin(), req.end()); + std::lock_guard lk(wm); + workers.emplace_back([r, responder]() { + std::this_thread::sleep_for(300ms); // reply later, off the worker thread + if (r.size() >= 4 + 16) { + responder.reply(encode_response(get_i64(r, 4) + get_i64(r, 12))); + } + }); + }); + auto dcall = client.add_service_client({dsvc, "example_interfaces::srv::dds_::AddTwoInts"}); + std::this_thread::sleep_for(2s); // discovery for the new endpoints + if (dcall) { + auto dreply = dcall->call(encode_request(11, 31), 10s); + if (dreply.has_value() && dreply->size() >= 4 + 8) { + deferred_ok = (get_i64(*dreply, 4) == 42); + } + std::printf("deferred call: 11 + 31 = %lld => %s\n", + reply.has_value() ? (long long)get_i64(*reply, 4) : -1, + deferred_ok ? "ok" : "MISMATCH/timeout"); + } + for (auto &t : workers) { + if (t.joinable()) + t.join(); + } + } + + // Future-based call: a third request must correlate independently. + bool future_ok = false; + { + const int64_t a3 = 500, b3 = 500; + auto fut = call->call_future(encode_request(a3, b3)); + if (fut.wait_for(10s) == std::future_status::ready) { + auto reply3 = fut.get(); + if (reply3.has_value() && reply3->size() >= 4 + 8) { + const int64_t sum = get_i64(*reply3, 4); + future_ok = (sum == a3 + b3); + std::printf("future call: got %lld (expected %lld) => %s\n", (long long)sum, + (long long)(a3 + b3), future_ok ? "ok" : "MISMATCH"); + } + } else { + std::printf("future call: timed out\n"); + } + } + + server.stop(); + client.stop(); + + if (ok && async_ok && future_ok && deferred_ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_service_naming.cpp b/pc/tests/rtps_service_naming.cpp new file mode 100644 index 000000000..299fb073e --- /dev/null +++ b/pc/tests/rtps_service_naming.cpp @@ -0,0 +1,48 @@ +// Unit test for ROS 2 service name/type mangling (rtps::rpc). The expected +// strings are taken verbatim from a live rmw_fastrtps (ROS 2 Jazzy) AddTwoInts +// capture (see components/rtps_embedded/RMI_AMI_DESIGN.md 3.1/3.2). Header-only, +// no engine/runtime needed. + +#include +#include + +#include "rtps/rpc/service_naming.hpp" + +namespace { +int failures = 0; +void check(const std::string &got, const std::string &want, const char *what) { + if (got != want) { + std::printf("FAIL %s: got \"%s\" want \"%s\"\n", what, got.c_str(), want.c_str()); + ++failures; + } else { + std::printf("ok %s = \"%s\"\n", what, got.c_str()); + } +} +} // namespace + +int main() { + using namespace rtps::rpc; + + const std::string base = "example_interfaces::srv::dds_::AddTwoInts"; + + // Captured wire strings. + check(service_request_topic("/add_two_ints"), "rq/add_two_intsRequest", "request_topic"); + check(service_reply_topic("/add_two_ints"), "rr/add_two_intsReply", "reply_topic"); + check(service_request_type(base), "example_interfaces::srv::dds_::AddTwoInts_Request_", + "request_type"); + check(service_response_type(base), "example_interfaces::srv::dds_::AddTwoInts_Response_", + "response_type"); + + // Namespaced service keeps internal slashes; leading slash stripped once. + check(service_request_topic("/ns/svc"), "rq/ns/svcRequest", "ns_request_topic"); + check(service_reply_topic("/ns/svc"), "rr/ns/svcReply", "ns_reply_topic"); + // No leading slash is accepted as-is. + check(service_request_topic("svc"), "rq/svcRequest", "noslash_request_topic"); + + if (failures == 0) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: %d\n", failures); + return 1; +} diff --git a/pc/tests/rtps_typed_rpc_loopback.cpp b/pc/tests/rtps_typed_rpc_loopback.cpp new file mode 100644 index 000000000..4c79f47f0 --- /dev/null +++ b/pc/tests/rtps_typed_rpc_loopback.cpp @@ -0,0 +1,133 @@ +// Showcase + self-test of the TYPED, espp-idiomatic RMI/AMI wrappers +// (espp::ServiceServer/ServiceClient, espp::ActionServer/ActionClient) - the +// service/action analogue of the typed Publisher/Subscriber. Reflectable +// structs are (de)serialized to CDR automatically by the `cdr` component, so +// application code never touches bytes. Exercises both the ROS 2-interoperable +// and the native protocol. Exits 0 iff every typed round-trip succeeds. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_action.hpp" +#include "rtps_service.hpp" + +using namespace std::chrono_literals; + +// Reflectable message structs - the fields map straight to CDR (ROS 2 wire form). +struct AddReq { + int64_t a; + int64_t b; +}; +struct AddResp { + int64_t sum; +}; +struct FibGoal { + int32_t order; +}; +struct FibSeq { + std::vector sequence; +}; // Result + Feedback + +int main() { + espp::RtpsParticipant server({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant client({.log_level = espp::Logger::Verbosity::WARN}); + if (!server.start() || !client.start()) { + std::printf("FAIL: start\n"); + return 1; + } + using P = espp::RtpsProtocol; + + bool svc_ros = false, svc_native = false, act_ros = false, act_native = false; + + // --- Typed service (ROS 2 + native): AddTwoInts --- + espp::ServiceServer ros_srv( + server, {.service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); + espp::ServiceServer nat_srv( + server, {.service = "/mul", + .type_name = "espp::native::Mul", + .handler = [](const AddReq &r) { return AddResp{r.a * r.b}; }, + .protocol = P::NATIVE}); + espp::ServiceClient ros_cli( + client, + {.service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + espp::ServiceClient nat_cli( + client, {.service = "/mul", .type_name = "espp::native::Mul", .protocol = P::NATIVE}); + + // --- Typed action (ROS 2 + native): Fibonacci-like sequence --- + auto make_exec = [](auto &h) { + const int32_t order = h.goal().order; + std::vector seq{0, 1}; + for (int32_t i = 1; i < order; ++i) { + seq.push_back(seq[i] + seq[i - 1]); + h.publish_feedback(FibSeq{seq}); + std::this_thread::sleep_for(40ms); + } + h.succeed(FibSeq{seq}); + }; + espp::ActionServer ros_act( + server, {.action = "/fibonacci", + .type_name = "example_interfaces::action::dds_::Fibonacci", + .on_goal = [](const FibGoal &g) { return g.order > 0; }, + .execute = make_exec}); + espp::ActionServer nat_act( + server, {.action = "/countup", + .type_name = "espp::native::CountUp", + .on_goal = [](const FibGoal &g) { return g.order > 0; }, + .execute = make_exec, + .protocol = P::NATIVE}); + espp::ActionClient ros_act_cli( + client, {.action = "/fibonacci", .type_name = "example_interfaces::action::dds_::Fibonacci"}); + espp::ActionClient nat_act_cli( + client, {.action = "/countup", .type_name = "espp::native::CountUp", .protocol = P::NATIVE}); + + std::this_thread::sleep_for(2s); // discovery + + // Services: blocking typed calls. + if (auto r = ros_cli.call(AddReq{7, 35}, 5s)) { + svc_ros = (r->sum == 42); + } + if (auto r = nat_cli.call(AddReq{6, 7}, 5s)) { + svc_native = (r->sum == 42); + } + std::printf("typed service ros=%d native=%d\n", svc_ros, svc_native); + + // Actions: typed feedback + result. + auto run_action = [](auto &cli, int order, const std::vector &expected) { + std::mutex m; + std::condition_variable cv; + bool done = false; + std::atomic fb{0}; + std::vector got; + espp::GoalStatus status{}; + cli.send_goal( + FibGoal{order}, [&](const FibSeq &) { fb.fetch_add(1); }, + [&](espp::GoalStatus st, const FibSeq &res) { + std::lock_guard lk(m); + status = st; + got = res.sequence; + done = true; + cv.notify_one(); + }); + std::unique_lock lk(m); + cv.wait_for(lk, 15s, [&] { return done; }); + return status == espp::GoalStatus::SUCCEEDED && got == expected && fb.load() > 0; + }; + act_ros = run_action(ros_act_cli, 5, {0, 1, 1, 2, 3, 5}); + act_native = run_action(nat_act_cli, 5, {0, 1, 1, 2, 3, 5}); + std::printf("typed action ros=%d native=%d\n", act_ros, act_native); + + server.stop(); + client.stop(); + + const bool ok = svc_ros && svc_native && act_ros && act_native; + std::printf("%s\n", ok ? "PASS" : "FAIL"); + return ok ? 0 : 1; +} diff --git a/pyproject.toml b/pyproject.toml index 5edf53b7e..9c860363d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -74,7 +74,10 @@ provider = "scikit_build_core.metadata.setuptools_scm" [tool.cibuildwheel] build = ["cp310-*", "cp311-*", "cp312-*", "cp313-*", "cp314-*"] skip = ["*-musllinux_*", "*_i686", "*-win32"] -test-command = "python -c \"import espp; print('espp', espp.__version__)\"" +# Smoke-test the wheel: import + assert the RTPS RMI/AMI binding surface is +# exposed (a functional in-process round-trip needs multicast, so it lives in the +# dockerised interop harness / python/rtps_rpc_demo.py, not here). +test-command = "python \"{project}/python/rtps_bindings_smoke.py\"" [tool.cibuildwheel.linux] # The repo is mounted at /project inside the manylinux container; git must diff --git a/python/rtps_bindings_smoke.py b/python/rtps_bindings_smoke.py new file mode 100644 index 000000000..da7664b2d --- /dev/null +++ b/python/rtps_bindings_smoke.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +"""Wheel smoke test: import espp and assert the RTPS RMI/AMI binding surface is +exposed. Used by cibuildwheel (see pyproject.toml [tool.cibuildwheel]). + +This is a binding-presence check, not a functional round-trip - a full in-process +demo needs multicast discovery and lives in the dockerised interop harness (see +python/rtps_rpc_demo.py). Catches a dropped/renamed binding after a build change. +""" + +import sys + +import espp + +p = espp.RtpsParticipant + +methods = [ + # pub/sub + "add_writer", "add_reader", "publish", + # services (RMI) - ROS 2 + native + "add_service_server", "add_service_client", + "add_native_service_server", "add_native_service_client", + # actions (AMI) - ROS 2 + native + "add_action_server", "add_action_client", + "add_native_action_server", "add_native_action_client", +] +classes = [ + "ServiceClient", "ActionClient", "ActionGoalHandle", + "NativeServiceClient", "NativeActionClient", "NativeGoalHandle", +] + +# Methods on the client / goal-handle classes (the full client call surface). +class_methods = { + "ServiceClient": ["call", "call_async", "call_future"], + "NativeServiceClient": ["call", "call_async", "call_future"], + "ActionClient": ["send_goal", "cancel_goal"], + "NativeActionClient": ["send_goal", "cancel_goal"], + "ActionGoalHandle": ["goal", "publish_feedback", "succeed", "abort", "canceled", "is_canceling"], + "NativeGoalHandle": ["goal", "publish_feedback", "succeed", "abort", "canceled", "is_canceling"], +} + +missing = [m for m in methods if not hasattr(p, m)] + [c for c in classes if not hasattr(p, c)] +for cls, method_names in class_methods.items(): + handle = getattr(p, cls, None) + missing += [f"{cls}.{m}" for m in method_names if handle is None or not hasattr(handle, m)] + +if missing: + print("espp RTPS binding surface INCOMPLETE, missing:", missing, file=sys.stderr) + sys.exit(1) + +print(f"espp {espp.__version__}: RTPS pub/sub + RMI/AMI (services, actions, native) bindings ok") diff --git a/python/rtps_rpc_demo.py b/python/rtps_rpc_demo.py new file mode 100644 index 000000000..b1089c03b --- /dev/null +++ b/python/rtps_rpc_demo.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +"""Showcase + self-test of the espp RTPS RMI/AMI APIs from Python. + +Demonstrates, in one process (two participants), every request/reply mechanism +the espp RtpsParticipant exposes, using the typed ``espp.rtps`` wrappers and +pycdr2 message schemas - so you work with message objects, never CDR bytes: + + 1. ROS 2-interoperable service (RMI) - espp.rtps.ServiceServer / ServiceClient + 2. ROS 2-interoperable action (AMI) - espp.rtps.ActionServer / ActionClient + 3. Native (espp<->espp) service (RMI) - the same wrappers with native=True + 4. Native (espp<->espp) action (AMI) - the same wrappers with native=True + +Message types are defined with pycdr2's ``make_idl_struct`` (the functional form +works on every Python, incl. 3.14; the ``@dataclass`` form does not). pycdr2 emits +ROS 2 / classic-CDR, so the AddTwoInts / Fibonacci schemas below match +example_interfaces on the wire. + +Requires pycdr2 (see python/requirements.txt). +Run: python/env/bin/python python/rtps_rpc_demo.py +Exit 0 iff every mechanism round-trips correctly. +""" + +import sys +import threading +import time + +import espp +import espp.rtps as rtps +from pycdr2 import make_idl_struct +from pycdr2.types import int32, int64, sequence + +# --- pycdr2 message schemas (fields map straight to CDR / the ROS 2 wire) ----- +# ROS 2 example_interfaces/srv/AddTwoInts. +AddReq = make_idl_struct("AddTwoInts_Request", "example_interfaces/srv/AddTwoInts_Request", + {"a": int64, "b": int64}) +AddResp = make_idl_struct("AddTwoInts_Response", "example_interfaces/srv/AddTwoInts_Response", + {"sum": int64}) +# ROS 2 example_interfaces/action/Fibonacci (Result + Feedback share the shape). +FibGoal = make_idl_struct("Fibonacci_Goal", "example_interfaces/action/Fibonacci_Goal", + {"order": int32}) +FibSeq = make_idl_struct("Fibonacci_Seq", "example_interfaces/action/Fibonacci_Seq", + {"sequence": sequence[int32]}) +# Native (espp<->espp) demo types - any consistent schema/name works. +MulResp = make_idl_struct("Mul_Response", "espp_examples/srv/Mul_Response", {"product": int64}) +CountGoal = make_idl_struct("CountUp_Goal", "espp_examples/action/CountUp_Goal", {"n": int32}) +CountVal = make_idl_struct("CountUp_Value", "espp_examples/action/CountUp_Value", {"value": int32}) + + +def main(): + server = espp.RtpsParticipant(espp.RtpsParticipant.Config(log_level=espp.Logger.Verbosity.warn)) + client = espp.RtpsParticipant(espp.RtpsParticipant.Config(log_level=espp.Logger.Verbosity.warn)) + # Explicit check (not assert: asserts are skipped under `python -O`). + if not server.start() or not client.start(): + print("FAIL: participants failed to start", file=sys.stderr) + server.stop() + client.stop() + return 1 + + results = {} + + # -- 1. ROS 2 service (RMI): sum = a + b -------------------------------- + rtps.ServiceServer(server, "/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts", + AddReq, AddResp, lambda req: AddResp(sum=req.a + req.b)) + svc = rtps.ServiceClient(client, "/add_two_ints", "example_interfaces::srv::dds_::AddTwoInts", + AddReq, AddResp) + + # -- 2. ROS 2 action (AMI): Fibonacci ----------------------------------- + def fib_execute(handle): + seq = [0, 1] + for i in range(1, handle.goal.order): + seq.append(seq[i] + seq[i - 1]) + handle.publish_feedback(FibSeq(sequence=seq)) + time.sleep(0.05) + handle.succeed(FibSeq(sequence=seq)) + + rtps.ActionServer(server, "/fibonacci", "example_interfaces::action::dds_::Fibonacci", + FibGoal, FibSeq, FibSeq, lambda goal: goal.order > 0, fib_execute) + act = rtps.ActionClient(client, "/fibonacci", "example_interfaces::action::dds_::Fibonacci", + FibGoal, FibSeq, FibSeq) + + # -- 3. Native service (RMI): product = a * b --------------------------- + rtps.ServiceServer(server, "/mul", "espp::native::Mul", AddReq, MulResp, + lambda req: MulResp(product=req.a * req.b), native=True) + nsvc = rtps.ServiceClient(client, "/mul", "espp::native::Mul", AddReq, MulResp, native=True) + + # -- 4. Native action (AMI): count up to n ------------------------------ + def count_execute(handle): + for i in range(1, handle.goal.n + 1): + if handle.is_canceling(): # cooperative cancel (native protocol too) + handle.canceled(CountVal(value=i - 1)) + return + handle.publish_feedback(CountVal(value=i)) + time.sleep(0.03) + handle.succeed(CountVal(value=handle.goal.n)) + + rtps.ActionServer(server, "/countup", "espp::native::CountUp", CountGoal, CountVal, CountVal, + lambda goal: goal.n > 0, count_execute, native=True) + nact = rtps.ActionClient(client, "/countup", "espp::native::CountUp", CountGoal, CountVal, + CountVal, native=True) + + time.sleep(2.0) # SEDP discovery for every endpoint + + # === 1. Service: blocking, async, and future (the three call styles) === + reply = svc.call(AddReq(a=7, b=35), timeout=5.0) + results["ros_service_sync"] = reply is not None and reply.sum == 42 + + ev = threading.Event() + got = {} + svc.call_async(AddReq(a=100, b=23), lambda r: (got.update(v=r.sum), ev.set())) + results["ros_service_async"] = ev.wait(5.0) and got.get("v") == 123 + + fut = svc.call_future(AddReq(a=40, b=2)) + fut_reply = fut.result(timeout=5.0) + results["ros_service_future"] = fut_reply is not None and fut_reply.sum == 42 + + # === 2. Action: feedback + result === + done = threading.Event() + fib = {"fb": 0, "status": 0, "seq": None} + act.send_goal( + FibGoal(order=5), + lambda f: fib.__setitem__("fb", fib["fb"] + 1), + lambda status, res: (fib.update(status=status, seq=res.sequence if res else None), done.set())) + ok = done.wait(10.0) + results["ros_action"] = (ok and fib["status"] == 4 and fib["seq"] == [0, 1, 1, 2, 3, 5] + and fib["fb"] > 0) + + # === 3. Native service === + nreply = nsvc.call(AddReq(a=6, b=7), timeout=5.0) + results["native_service"] = nreply is not None and nreply.product == 42 + + # === 4. Native action === + ndone = threading.Event() + cnt = {"fb": 0, "status": 0, "n": None} + nact.send_goal( + CountGoal(n=5), + lambda f: cnt.__setitem__("fb", cnt["fb"] + 1), + lambda status, res: (cnt.update(status=status, n=res.value if res else None), ndone.set())) + nok = ndone.wait(10.0) + results["native_action"] = nok and cnt["status"] == 4 and cnt["n"] == 5 and cnt["fb"] > 0 + + # === 5. Native action cancel: send a long goal, cancel it mid-flight === + ncdone = threading.Event() + ncancel = {"status": 0} + nact.send_goal( + CountGoal(n=100000), # long enough (100000 * 0.03s) to cancel before it finishes + lambda f: None, + lambda status, res: (ncancel.update(status=status), ncdone.set())) + time.sleep(0.3) # let the goal be accepted (so cancel_goal has its handle) + nact.cancel_goal() # cancels the most recently accepted goal + results["native_cancel"] = ncdone.wait(10.0) and ncancel["status"] == 5 # CANCELED + + server.stop() + client.stop() + + print("\n=== espp RTPS RMI/AMI demo results ===") + all_ok = True + for name, passed in results.items(): + print(f" {'PASS' if passed else 'FAIL'} {name}") + all_ok = all_ok and passed + print("=== %s ===" % ("ALL PASS" if all_ok else "FAILURES")) + return 0 if all_ok else 1 + + +if __name__ == "__main__": + sys.exit(main())