From f0075d8e9204d768097df55d67fabc890d7d91ba Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 20 Dec 2023 13:05:46 +0100 Subject: [PATCH 01/32] Add config option to msg gen macro to output `cxx_bridge` attribute --- Cargo.lock | 48 +++++++++---------- .../ros2-bridge/msg-gen-macro/Cargo.toml | 2 +- .../ros2-bridge/msg-gen-macro/src/lib.rs | 38 ++++++++++++++- .../ros2-bridge/msg-gen/src/types/package.rs | 10 +++- 4 files changed, 70 insertions(+), 28 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index e0e4ba103..8e0819923 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -524,7 +524,7 @@ checksum = "bc00ceb34980c03614e35a3a4e218276a0a824e911d07651cd0d858a51e8c0f0" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -954,7 +954,7 @@ dependencies = [ "heck 0.4.1", "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -1264,7 +1264,7 @@ dependencies = [ "proc-macro2", "quote", "scratch", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -1281,7 +1281,7 @@ checksum = "2fa16a70dd58129e4dfffdff535fb1bce66673f7bbeec4a5a1765a504e1ccd84" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -1736,7 +1736,7 @@ dependencies = [ "proc-macro2", "quote", "regex", - "syn 1.0.109", + "syn 2.0.41", "thiserror", ] @@ -1914,7 +1914,7 @@ checksum = "f95e2801cd355d4a1a3e3953ce6ee5ae9603a5c833455343a8bfe3f44d418246" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -2219,7 +2219,7 @@ checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -2289,7 +2289,7 @@ checksum = "ba330b70a5341d3bc730b8e205aaee97ddab5d9c448c4f51a7c2d924266fa8f9" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -3516,7 +3516,7 @@ checksum = "cfb77679af88f8b125209d354a202862602672222e7f2313fdd6dc349bad4712" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -3948,7 +3948,7 @@ dependencies = [ "pest_meta", "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -3989,7 +3989,7 @@ checksum = "4359fd9c9171ec6e8c62926d6faaf553a8dc3f64e1507e76da7911b4f6a04405" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -4288,7 +4288,7 @@ dependencies = [ "proc-macro2", "pyo3-macros-backend", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -4300,7 +4300,7 @@ dependencies = [ "heck 0.4.1", "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -4990,7 +4990,7 @@ checksum = "4eca7ac642d82aa35b60049a6eccb4be6be75e599bd2e9adb5f875a737654af2" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5012,7 +5012,7 @@ checksum = "8725e1dfadb3a50f7e5ce0b1a540466f6ed3fe7a0fca2ac2b8b831d31316bd00" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5243,7 +5243,7 @@ checksum = "7d395866cb6778625150f77a430cc0af764ce0300f6a3d3413477785fa34b6c7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5326,9 +5326,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.38" +version = "2.0.41" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e96b79aaa137db8f61e26363a0c9b47d8b4ec75da28b7d1d614c2303e232408b" +checksum = "44c8b28c477cc3bf0e7966561e3460130e1255f7a1cf71931075f1c5e7a7e269" dependencies = [ "proc-macro2", "quote", @@ -5462,7 +5462,7 @@ checksum = "10712f02019e9288794769fba95cd6847df9874d49d871d062172f9dd41bc4cc" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5586,7 +5586,7 @@ checksum = "630bdcf245f78637c13ec01ffae6187cca34625e8c63150d424b59e55af2675e" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5693,7 +5693,7 @@ checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -5902,7 +5902,7 @@ checksum = "f7e1ba1f333bd65ce3c9f27de592fcbc256dafe3af2717f56d7c87761fbaccf4" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", ] [[package]] @@ -6011,7 +6011,7 @@ dependencies = [ "once_cell", "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", "wasm-bindgen-shared", ] @@ -6045,7 +6045,7 @@ checksum = "54681b18a46765f095758388f2d0cf16eb8d4169b639ab575a8f5693af210c7b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.41", "wasm-bindgen-backend", "wasm-bindgen-shared", ] diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml b/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml index 434e42abc..5b7b7e075 100644 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml +++ b/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml @@ -12,7 +12,7 @@ nom = "7" proc-macro2 = "1.0" quote = "1.0" regex = "1" -syn = "1.0" +syn = { version = "2.0.41", features = ["parsing"] } thiserror = "1.0" dora-ros2-bridge-msg-gen = { path = "../msg-gen" } diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs index ae6d02ab5..93fb09dae 100644 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs @@ -12,9 +12,43 @@ use std::path::Path; use dora_ros2_bridge_msg_gen::get_packages; use proc_macro::TokenStream; use quote::quote; +use syn::{punctuated::Punctuated, MetaNameValue, Token}; + +struct Config { + create_cxx_bridge: bool, +} + +impl syn::parse::Parse for Config { + fn parse(input: syn::parse::ParseStream<'_>) -> syn::Result { + let punctuated = Punctuated::::parse_terminated(input)?; + + let mut config = Self { + create_cxx_bridge: false, + }; + + for item in &punctuated { + let ident = item.path.require_ident()?; + if ident == "cxx_bridge" { + let syn::Expr::Lit(lit) = &item.value else { + return Err(syn::Error::new_spanned(&item.value, "invalid value, expected bool")); + }; + let syn::Lit::Bool(b) = &lit.lit else { + return Err(syn::Error::new_spanned(&lit.lit, "invalid value, expected bool")); + }; + config.create_cxx_bridge = b.value; + } else { + return Err(syn::Error::new_spanned(&item.path, "invalid argument")); + } + } + + Ok(config) + } +} #[proc_macro] -pub fn msg_include_all(_input: TokenStream) -> TokenStream { +pub fn msg_include_all(input: TokenStream) -> TokenStream { + let config = syn::parse_macro_input!(input as Config); + let ament_prefix_path = std::env!("DETECTED_AMENT_PREFIX_PATH").trim(); if ament_prefix_path.is_empty() { quote! { @@ -32,7 +66,7 @@ pub fn msg_include_all(_input: TokenStream) -> TokenStream { let packages = get_packages(&paths) .unwrap() .iter() - .map(|v| v.token_stream()) + .map(|v| v.token_stream(config.create_cxx_bridge)) .collect::>(); (quote! { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs index 3d5c4944b..bbd163696 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs @@ -71,13 +71,21 @@ impl Package { } } - pub fn token_stream(&self) -> impl ToTokens { + pub fn token_stream(&self, gen_cxx_bridge: bool) -> impl ToTokens { let name = Ident::new(&self.name, Span::call_site()); let messages_block = self.messages_block(); let services_block = self.services_block(); let actions_block = self.actions_block(); + let attributes = if gen_cxx_bridge { + let namespace = &self.name; + quote! { #[cxx::bridge(namespace = #namespace)] } + } else { + quote! {} + }; + quote! { + #attributes pub mod #name { #messages_block #services_block From b9e5e533e0486a2cc6ce03bdc84ea7cf1361dec5 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 4 Jan 2024 18:27:37 +0100 Subject: [PATCH 02/32] Redesign ROS2 message struct generator to support `cxx_bridge` --- Cargo.lock | 1 + libraries/extensions/ros2-bridge/Cargo.toml | 2 + .../ros2-bridge/msg-gen-macro/src/lib.rs | 45 +++++++++- .../ros2-bridge/msg-gen/src/types/message.rs | 89 +++++++++++++++++-- .../ros2-bridge/msg-gen/src/types/package.rs | 71 ++++++++++++--- .../msg-gen/src/types/primitives.rs | 10 ++- .../msg-gen/src/types/sequences.rs | 4 +- libraries/extensions/ros2-bridge/src/lib.rs | 5 +- 8 files changed, 202 insertions(+), 25 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 8e0819923..ac8e78be7 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1696,6 +1696,7 @@ name = "dora-ros2-bridge" version = "0.1.0" dependencies = [ "array-init", + "cxx", "dora-daemon", "dora-ros2-bridge-msg-gen-macro", "eyre", diff --git a/libraries/extensions/ros2-bridge/Cargo.toml b/libraries/extensions/ros2-bridge/Cargo.toml index 7c55a1cb1..f0a7ac6ee 100644 --- a/libraries/extensions/ros2-bridge/Cargo.toml +++ b/libraries/extensions/ros2-bridge/Cargo.toml @@ -8,6 +8,7 @@ edition = "2021" [features] default = ["generate-messages"] generate-messages = ["dep:dora-ros2-bridge-msg-gen-macro"] +cxx-bridge = ["dep:cxx"] # enables examples that depend on a sourced ROS2 installation ros2-examples = ["eyre", "tokio", "dora-daemon"] @@ -24,6 +25,7 @@ tokio = { version = "1.29.1", features = ["full"], optional = true } dora-daemon = { path = "../../../binaries/daemon", optional = true } tracing = "0.1.37" tracing-subscriber = "0.3.17" +cxx = { version = "1.0", optional = true } [dev-dependencies] rand = "0.8.5" diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs index 93fb09dae..2a6c3b4c4 100644 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs @@ -63,14 +63,57 @@ pub fn msg_include_all(input: TokenStream) -> TokenStream { .map(Path::new) .collect::>(); + let message_structs = get_packages(&paths) + .unwrap() + .iter() + .map(|v| v.struct_token_stream(config.create_cxx_bridge)) + .collect::>(); + let aliases = get_packages(&paths) + .unwrap() + .iter() + .map(|v| v.aliases_token_stream()) + .collect::>(); let packages = get_packages(&paths) .unwrap() .iter() .map(|v| v.token_stream(config.create_cxx_bridge)) .collect::>(); + let (attributes, imports) = if config.create_cxx_bridge { + (quote! { #[cxx::bridge] }, quote! {}) + } else { + ( + quote! {}, + quote! { + use serde::{Serialize, Deserialize}; + }, + ) + }; + (quote! { - #(#packages)* + #attributes + pub mod ffi { + #imports + + #[derive(Debug, Default, Clone, PartialEq, Eq, Serialize, Deserialize)] + pub struct U16String { + chars: Vec, + } + + impl crate::_core::InternalDefault for U16String { + fn _default() -> Self { + Default::default() + } + } + + + #(#message_structs)* + } + + #(#aliases)* + + + // #(#packages)* }) .into() } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 72dc13370..96693f1a0 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -1,5 +1,6 @@ use heck::SnakeCase; use quote::{format_ident, quote, ToTokens}; +use syn::Ident; use super::{primitives::*, sequences::Array, ConstantType, MemberType}; @@ -122,9 +123,69 @@ pub struct Message { } impl Message { - pub fn token_stream_with_mod(&self) -> impl ToTokens { + pub fn struct_token_stream(&self, package_name: &Ident, gen_cxx_bridge: bool) -> impl ToTokens { + let cxx_name = format_ident!("{}", self.name); + let struct_raw_name = format_ident!("{package_name}__{}", self.name); + + let rust_type_def_inner = self.members.iter().map(|m| m.rust_type_def(&self.package)); + let constants_def_inner = self.constants.iter().map(|c| c.token_stream()); + let rust_type_default_inner = self.members.iter().map(|m| m.default_value()); + + let attributes = if gen_cxx_bridge { + quote! { + #[namespace = #package_name] + #[cxx_name = #cxx_name] + } + } else { + quote! {} + }; + + if self.members.is_empty() { + quote! {} + } else { + quote! { + #[allow(non_camel_case_types)] + #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] + #attributes + pub struct #struct_raw_name { + #(#rust_type_def_inner)* + } + + impl crate::_core::InternalDefault for #struct_raw_name { + fn _default() -> Self { + Self { + #(#rust_type_default_inner)* + } + } + } + + impl std::default::Default for #struct_raw_name { + #[inline] + fn default() -> Self { + crate::_core::InternalDefault::_default() + } + } + } + } + } + + pub fn alias_token_stream(&self, package_name: &Ident) -> impl ToTokens { + let cxx_name = format_ident!("{}", self.name); + let struct_raw_name = format_ident!("{package_name}__{}", self.name); + + if self.members.is_empty() { + quote! {} + } else { + quote! { + pub use super::super::ffi::#struct_raw_name as #cxx_name; + } + } + } + + pub fn token_stream_with_mod(&self, gen_cxx_bridge: bool) -> impl ToTokens { let mod_name = format_ident!("_{}", self.name.to_snake_case()); - let inner = self.token_stream(); + let inner = self.token_stream_args(gen_cxx_bridge); + quote! { pub use #mod_name::*; mod #mod_name { @@ -134,6 +195,10 @@ impl Message { } pub fn token_stream(&self) -> impl ToTokens { + self.token_stream_args(false) + } + + pub fn token_stream_args(&self, gen_cxx_bridge: bool) -> impl ToTokens { let rust_type = format_ident!("{}", self.name); let raw_type = format_ident!("{}_Raw", self.name); let raw_ref_type = format_ident!("{}_RawRef", self.name); @@ -144,6 +209,13 @@ impl Message { self.members.clone() }; + let attributes = if gen_cxx_bridge { + let namespace = &self.name; + quote! { #[cxx::bridge(namespace = #namespace)] } + } else { + quote! {} + }; + let rust_type_def_inner = self.members.iter().map(|m| m.rust_type_def(&self.package)); let constants_def_inner = self.constants.iter().map(|c| c.token_stream()); let rust_type_default_inner = self.members.iter().map(|m| m.default_value()); @@ -178,10 +250,15 @@ impl Message { FFIToRust as _FFIToRust, }; - #[allow(non_camel_case_types)] - #[derive(std::fmt::Debug, std::clone::Clone, std::cmp::PartialEq, serde::Serialize, serde::Deserialize)] - pub struct #rust_type { - #(#rust_type_def_inner)* + pub use self::t::#rust_type; + + #attributes + mod t { + #[allow(non_camel_case_types)] + #[derive(std::fmt::Debug, std::clone::Clone, std::cmp::PartialEq, serde::Serialize, serde::Deserialize)] + pub struct #rust_type { + #(#rust_type_def_inner)* + } } impl #rust_type { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs index bbd163696..50cabf2d7 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs @@ -26,13 +26,50 @@ impl Package { self.messages.is_empty() && self.services.is_empty() && self.actions.is_empty() } - fn messages_block(&self) -> impl ToTokens { + fn message_structs(&self, package_name: Ident, gen_cxx_bridge: bool) -> impl ToTokens { if self.messages.is_empty() { quote! { // empty msg } } else { - let items = self.messages.iter().map(|v| v.token_stream_with_mod()); + let items = self + .messages + .iter() + .map(|v| v.struct_token_stream(&package_name, gen_cxx_bridge)); + quote! { + #(#items)* + } + } + } + + fn message_aliases(&self, package_name: &Ident) -> impl ToTokens { + if self.messages.is_empty() { + quote! { + // empty msg + } + } else { + let items = self + .messages + .iter() + .map(|v| v.alias_token_stream(package_name)); + quote! { + pub mod msg { + #(#items)* + } + } + } + } + + fn messages_block(&self, gen_cxx_bridge: bool) -> impl ToTokens { + if self.messages.is_empty() { + quote! { + // empty msg + } + } else { + let items = self + .messages + .iter() + .map(|v| v.token_stream_with_mod(gen_cxx_bridge)); quote! { pub mod msg { #(#items)* @@ -71,21 +108,33 @@ impl Package { } } + pub fn struct_token_stream(&self, gen_cxx_bridge: bool) -> impl ToTokens { + let package_name = Ident::new(&self.name, Span::call_site()); + let message_structs = self.message_structs(package_name, gen_cxx_bridge); + + quote! { + #message_structs + } + } + + pub fn aliases_token_stream(&self) -> impl ToTokens { + let package_name = Ident::new(&self.name, Span::call_site()); + let aliases = self.message_aliases(&package_name); + + quote! { + pub mod #package_name { + #aliases + } + } + } + pub fn token_stream(&self, gen_cxx_bridge: bool) -> impl ToTokens { let name = Ident::new(&self.name, Span::call_site()); - let messages_block = self.messages_block(); + let messages_block = self.messages_block(gen_cxx_bridge); let services_block = self.services_block(); let actions_block = self.actions_block(); - let attributes = if gen_cxx_bridge { - let namespace = &self.name; - quote! { #[cxx::bridge(namespace = #namespace)] } - } else { - quote! {} - }; - quote! { - #attributes pub mod #name { #messages_block #services_block diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs index abf2747b1..98dc82352 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs @@ -133,7 +133,8 @@ impl NamedType { let package = Ident::new(package, Span::call_site()); let namespace = Ident::new("msg", Span::call_site()); let name = Ident::new(&self.0, Span::call_site()); - quote! { crate::#package::#namespace::#name } + let ident = format_ident!("{package}__{name}"); + quote! { #ident } } fn raw_type_tokens(&self, package: &str) -> impl ToTokens { @@ -175,7 +176,8 @@ impl NamespacedType { let package = Ident::new(&self.package, Span::call_site()); let namespace = Ident::new(&self.namespace, Span::call_site()); let name = Ident::new(&self.name, Span::call_site()); - quote! { crate::#package::#namespace::#name } + let ident = format_ident!("{package}__{name}"); + quote! { #ident } } fn raw_type_tokens(&self) -> impl ToTokens { @@ -215,9 +217,9 @@ impl GenericString { fn type_tokens(self) -> impl ToTokens { if self.is_wide() { - quote! { crate::_core::string::U16String } + quote! { U16String } } else { - quote! { ::std::string::String } + quote! { String } } } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/sequences.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/sequences.rs index 13a5c5f71..2c4c18720 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/sequences.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/sequences.rs @@ -41,7 +41,7 @@ pub struct Sequence { impl Sequence { pub fn type_tokens(&self, package: &str) -> impl ToTokens { let inner_type = self.value_type.type_tokens(package); - quote! { std::vec::Vec<#inner_type> } + quote! { Vec<#inner_type> } } pub fn raw_type_tokens(&self, package: &str) -> impl ToTokens { @@ -72,7 +72,7 @@ pub struct BoundedSequence { impl BoundedSequence { pub fn type_tokens(&self, package: &str) -> impl ToTokens { let inner_type = self.value_type.type_tokens(package); - quote! { std::vec::Vec<#inner_type> } + quote! { Vec<#inner_type> } } pub fn raw_type_tokens(&self, package: &str) -> impl ToTokens { diff --git a/libraries/extensions/ros2-bridge/src/lib.rs b/libraries/extensions/ros2-bridge/src/lib.rs index e2053e0b1..a5f3ecb8c 100644 --- a/libraries/extensions/ros2-bridge/src/lib.rs +++ b/libraries/extensions/ros2-bridge/src/lib.rs @@ -1,7 +1,10 @@ pub use ros2_client; pub use rustdds; -#[cfg(feature = "generate-messages")] +#[cfg(all(feature = "generate-messages", feature = "cxx-bridge"))] +dora_ros2_bridge_msg_gen_macro::msg_include_all!(cxx_bridge = true); + +#[cfg(all(feature = "generate-messages", not(feature = "cxx-bridge")))] dora_ros2_bridge_msg_gen_macro::msg_include_all!(); pub mod _core; From bd84baba28347205ff37282ac71c7cc53b4b44e1 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 5 Jan 2024 12:32:49 +0100 Subject: [PATCH 03/32] Fix impl blocks for generated messages --- .../ros2-bridge/msg-gen-macro/src/lib.rs | 19 +++++++----- .../ros2-bridge/msg-gen/src/types/message.rs | 21 +++++++++----- .../ros2-bridge/msg-gen/src/types/package.rs | 29 +++++++++---------- 3 files changed, 39 insertions(+), 30 deletions(-) diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs index 2a6c3b4c4..7ccdfc450 100644 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs @@ -66,8 +66,11 @@ pub fn msg_include_all(input: TokenStream) -> TokenStream { let message_structs = get_packages(&paths) .unwrap() .iter() - .map(|v| v.struct_token_stream(config.create_cxx_bridge)) + .map(|v| v.message_structs(config.create_cxx_bridge)) .collect::>(); + let message_struct_defs = message_structs.iter().map(|(s, _)| s); + let message_struct_impls = message_structs.iter().map(|(_, i)| i); + let aliases = get_packages(&paths) .unwrap() .iter() @@ -100,16 +103,18 @@ pub fn msg_include_all(input: TokenStream) -> TokenStream { chars: Vec, } - impl crate::_core::InternalDefault for U16String { - fn _default() -> Self { - Default::default() - } - } + #(#message_struct_defs)* + } - #(#message_structs)* + impl crate::_core::InternalDefault for ffi::U16String { + fn _default() -> Self { + Default::default() + } } + #(#message_struct_impls)* + #(#aliases)* diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 96693f1a0..60f0be0b6 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -123,7 +123,11 @@ pub struct Message { } impl Message { - pub fn struct_token_stream(&self, package_name: &Ident, gen_cxx_bridge: bool) -> impl ToTokens { + pub fn struct_token_stream( + &self, + package_name: &Ident, + gen_cxx_bridge: bool, + ) -> (impl ToTokens, impl ToTokens) { let cxx_name = format_ident!("{}", self.name); let struct_raw_name = format_ident!("{package_name}__{}", self.name); @@ -141,17 +145,18 @@ impl Message { }; if self.members.is_empty() { - quote! {} + (quote! {}, quote! {}) } else { - quote! { + let def = quote! { #[allow(non_camel_case_types)] #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] #attributes pub struct #struct_raw_name { #(#rust_type_def_inner)* } - - impl crate::_core::InternalDefault for #struct_raw_name { + }; + let impls = quote! { + impl crate::_core::InternalDefault for ffi::#struct_raw_name { fn _default() -> Self { Self { #(#rust_type_default_inner)* @@ -159,13 +164,15 @@ impl Message { } } - impl std::default::Default for #struct_raw_name { + impl std::default::Default for ffi::#struct_raw_name { #[inline] fn default() -> Self { crate::_core::InternalDefault::_default() } } - } + }; + + (def, impls) } } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs index 50cabf2d7..1e6e6fc88 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs @@ -26,19 +26,25 @@ impl Package { self.messages.is_empty() && self.services.is_empty() && self.actions.is_empty() } - fn message_structs(&self, package_name: Ident, gen_cxx_bridge: bool) -> impl ToTokens { + pub fn message_structs(&self, gen_cxx_bridge: bool) -> (impl ToTokens, impl ToTokens) { + let package_name = Ident::new(&self.name, Span::call_site()); if self.messages.is_empty() { - quote! { - // empty msg - } + // empty msg + (quote! {}, quote! {}) } else { let items = self .messages .iter() .map(|v| v.struct_token_stream(&package_name, gen_cxx_bridge)); - quote! { - #(#items)* - } + let defs = items.clone().map(|(def, _)| def); + let impls = items.clone().map(|(_, im)| im); + let def_tokens = quote! { + #(#defs)* + }; + let impl_tokens = quote! { + #(#impls)* + }; + (def_tokens, impl_tokens) } } @@ -108,15 +114,6 @@ impl Package { } } - pub fn struct_token_stream(&self, gen_cxx_bridge: bool) -> impl ToTokens { - let package_name = Ident::new(&self.name, Span::call_site()); - let message_structs = self.message_structs(package_name, gen_cxx_bridge); - - quote! { - #message_structs - } - } - pub fn aliases_token_stream(&self) -> impl ToTokens { let package_name = Ident::new(&self.name, Span::call_site()); let aliases = self.message_aliases(&package_name); From f9cfedc3029f9d43ffba6003754823dd2664ffe0 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 5 Jan 2024 13:26:12 +0100 Subject: [PATCH 04/32] Move code generation to `msg-gen` crate --- .../ros2-bridge/msg-gen-macro/src/lib.rs | 59 +---------------- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 65 +++++++++++++++++++ 2 files changed, 66 insertions(+), 58 deletions(-) diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs index 7ccdfc450..fb9ad9bac 100644 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs @@ -63,63 +63,6 @@ pub fn msg_include_all(input: TokenStream) -> TokenStream { .map(Path::new) .collect::>(); - let message_structs = get_packages(&paths) - .unwrap() - .iter() - .map(|v| v.message_structs(config.create_cxx_bridge)) - .collect::>(); - let message_struct_defs = message_structs.iter().map(|(s, _)| s); - let message_struct_impls = message_structs.iter().map(|(_, i)| i); - - let aliases = get_packages(&paths) - .unwrap() - .iter() - .map(|v| v.aliases_token_stream()) - .collect::>(); - let packages = get_packages(&paths) - .unwrap() - .iter() - .map(|v| v.token_stream(config.create_cxx_bridge)) - .collect::>(); - - let (attributes, imports) = if config.create_cxx_bridge { - (quote! { #[cxx::bridge] }, quote! {}) - } else { - ( - quote! {}, - quote! { - use serde::{Serialize, Deserialize}; - }, - ) - }; - - (quote! { - #attributes - pub mod ffi { - #imports - - #[derive(Debug, Default, Clone, PartialEq, Eq, Serialize, Deserialize)] - pub struct U16String { - chars: Vec, - } - - #(#message_struct_defs)* - } - - - impl crate::_core::InternalDefault for ffi::U16String { - fn _default() -> Self { - Default::default() - } - } - - #(#message_struct_impls)* - - #(#aliases)* - - - // #(#packages)* - }) - .into() + dora_ros2_bridge_msg_gen::gen(&paths, config.create_cxx_bridge).into() } } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index 313db9b35..3a2eb2448 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -7,7 +7,72 @@ clippy::nursery )] +use std::path::Path; + +use quote::quote; + pub mod parser; pub mod types; pub use crate::parser::get_packages; + +pub fn gen(paths: &[&Path], create_cxx_bridge: bool) -> proc_macro2::TokenStream { + let message_structs = get_packages(&paths) + .unwrap() + .iter() + .map(|v| v.message_structs(create_cxx_bridge)) + .collect::>(); + let message_struct_defs = message_structs.iter().map(|(s, _)| s); + let message_struct_impls = message_structs.iter().map(|(_, i)| i); + + let aliases = get_packages(&paths) + .unwrap() + .iter() + .map(|v| v.aliases_token_stream()) + .collect::>(); + let packages = get_packages(&paths) + .unwrap() + .iter() + .map(|v| v.token_stream(create_cxx_bridge)) + .collect::>(); + + let (attributes, imports) = if create_cxx_bridge { + (quote! { #[cxx::bridge] }, quote! {}) + } else { + ( + quote! {}, + quote! { + use serde::{Serialize, Deserialize}; + }, + ) + }; + + (quote! { + #attributes + pub mod ffi { + #imports + + #[derive(Debug, Default, Clone, PartialEq, Eq, Serialize, Deserialize)] + pub struct U16String { + chars: Vec, + } + + #(#message_struct_defs)* + } + + + impl crate::_core::InternalDefault for ffi::U16String { + fn _default() -> Self { + Default::default() + } + } + + #(#message_struct_impls)* + + #(#aliases)* + + + // #(#packages)* + }) + .into() +} From 4c64cad6edb5da2a5b5c4bdf302f938f3e67d955 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 5 Jan 2024 14:33:02 +0100 Subject: [PATCH 05/32] Replace proc macro by build script to generate C++ bindings The `cxx_build` crate does not support proc macro output (see https://github.com/dtolnay/cxx/issues/808), so we have to generate a standard Rust source file instead. --- Cargo.lock | 30 ++++---- Cargo.toml | 1 - examples/rust-ros2-dataflow/node/src/main.rs | 6 +- libraries/extensions/ros2-bridge/Cargo.toml | 12 +++- libraries/extensions/ros2-bridge/build.rs | 42 ++++++++++++ .../ros2-bridge/msg-gen-macro/Cargo.toml | 20 ------ .../ros2-bridge/msg-gen-macro/build.rs | 25 ------- .../ros2-bridge/msg-gen-macro/src/lib.rs | 68 ------------------- .../test_msgs/action/Fibonacci.action | 8 --- .../msg-gen-macro/test_msgs/msg/Arrays.msg | 34 ---------- .../test_msgs/msg/BasicTypes.msg | 13 ---- .../test_msgs/msg/BoundedSequences.msg | 34 ---------- .../msg-gen-macro/test_msgs/msg/Constants.msg | 13 ---- .../msg-gen-macro/test_msgs/msg/Defaults.msg | 13 ---- .../msg-gen-macro/test_msgs/msg/Empty.msg | 0 .../test_msgs/msg/MultiNested.msg | 10 --- .../msg-gen-macro/test_msgs/msg/Nested.msg | 1 - .../msg-gen-macro/test_msgs/msg/Strings.msg | 13 ---- .../test_msgs/msg/UnboundedSequences.msg | 34 ---------- .../msg-gen-macro/test_msgs/msg/WStrings.msg | 10 --- .../msg-gen-macro/test_msgs/srv/Arrays.srv | 63 ----------------- .../test_msgs/srv/BasicTypes.srv | 29 -------- .../msg-gen-macro/test_msgs/srv/Empty.srv | 1 - .../extensions/ros2-bridge/msg-gen/src/lib.rs | 18 ++--- .../ros2-bridge/msg-gen/src/parser/package.rs | 7 +- libraries/extensions/ros2-bridge/src/lib.rs | 8 +-- 26 files changed, 87 insertions(+), 426 deletions(-) create mode 100644 libraries/extensions/ros2-bridge/build.rs delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/build.rs delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/action/Fibonacci.action delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Arrays.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BasicTypes.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BoundedSequences.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Constants.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Defaults.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Empty.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/MultiNested.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Nested.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Strings.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/UnboundedSequences.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/WStrings.msg delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Arrays.srv delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/BasicTypes.srv delete mode 100644 libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Empty.srv diff --git a/Cargo.lock b/Cargo.lock index ac8e78be7..3dada54f2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1697,12 +1697,14 @@ version = "0.1.0" dependencies = [ "array-init", "cxx", + "cxx-build", "dora-daemon", - "dora-ros2-bridge-msg-gen-macro", + "dora-ros2-bridge-msg-gen", "eyre", "futures", "rand", "ros2-client", + "rust-format", "rustdds", "serde", "serde-big-array", @@ -1726,21 +1728,6 @@ dependencies = [ "thiserror", ] -[[package]] -name = "dora-ros2-bridge-msg-gen-macro" -version = "0.1.0" -dependencies = [ - "anyhow", - "dora-ros2-bridge-msg-gen", - "heck 0.3.3", - "nom", - "proc-macro2", - "quote", - "regex", - "syn 2.0.41", - "thiserror", -] - [[package]] name = "dora-ros2-bridge-python" version = "0.1.0" @@ -4679,6 +4666,17 @@ dependencies = [ "eyre", ] +[[package]] +name = "rust-format" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60e7c00b6c3bf5e38a880eec01d7e829d12ca682079f8238a464def3c4b31627" +dependencies = [ + "prettyplease", + "proc-macro2", + "syn 1.0.109", +] + [[package]] name = "rust-ros2-dataflow-example-node" version = "0.3.0" diff --git a/Cargo.toml b/Cargo.toml index b2d0ff30c..da1634263 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -27,7 +27,6 @@ members = [ "libraries/extensions/dora-record", "libraries/extensions/ros2-bridge", "libraries/extensions/ros2-bridge/msg-gen", - "libraries/extensions/ros2-bridge/msg-gen-macro", "libraries/extensions/ros2-bridge/python", ] diff --git a/examples/rust-ros2-dataflow/node/src/main.rs b/examples/rust-ros2-dataflow/node/src/main.rs index 15c360b62..6d20472fa 100644 --- a/examples/rust-ros2-dataflow/node/src/main.rs +++ b/examples/rust-ros2-dataflow/node/src/main.rs @@ -5,10 +5,12 @@ use dora_node_api::{ DoraNode, Event, }; use dora_ros2_bridge::{ - geometry_msgs::msg::{Twist, Vector3}, + messages::{ + geometry_msgs::msg::{Twist, Vector3}, + turtlesim::msg::Pose, + }, ros2_client::{self, ros2, NodeOptions}, rustdds::{self, policy}, - turtlesim::msg::Pose, }; use eyre::{eyre, Context}; diff --git a/libraries/extensions/ros2-bridge/Cargo.toml b/libraries/extensions/ros2-bridge/Cargo.toml index f0a7ac6ee..6b3b916bf 100644 --- a/libraries/extensions/ros2-bridge/Cargo.toml +++ b/libraries/extensions/ros2-bridge/Cargo.toml @@ -7,14 +7,13 @@ edition = "2021" [features] default = ["generate-messages"] -generate-messages = ["dep:dora-ros2-bridge-msg-gen-macro"] -cxx-bridge = ["dep:cxx"] +generate-messages = ["dep:dora-ros2-bridge-msg-gen", "dep:rust-format"] +cxx-bridge = ["dep:cxx", "dep:cxx-build"] # enables examples that depend on a sourced ROS2 installation ros2-examples = ["eyre", "tokio", "dora-daemon"] [dependencies] array-init = "2.1.0" -dora-ros2-bridge-msg-gen-macro = { path = "msg-gen-macro", optional = true } serde = { version = "1.0.164", features = ["derive"] } serde-big-array = "0.5.1" widestring = "1.0.2" @@ -30,3 +29,10 @@ cxx = { version = "1.0", optional = true } [dev-dependencies] rand = "0.8.5" futures = { version = "0.3.28", default-features = false } + +[build-dependencies] +dora-ros2-bridge-msg-gen = { path = "msg-gen", optional = true } +rust-format = { version = "0.3.4", features = [ + "pretty_please", +], optional = true } +cxx-build = { version = "1.0.73", optional = true } diff --git a/libraries/extensions/ros2-bridge/build.rs b/libraries/extensions/ros2-bridge/build.rs new file mode 100644 index 000000000..a8ec4c987 --- /dev/null +++ b/libraries/extensions/ros2-bridge/build.rs @@ -0,0 +1,42 @@ +use rust_format::Formatter; +use std::path::PathBuf; + +fn main() { + let create_cxx_bridge = cfg!(feature = "cxx-bridge"); + let paths = ament_prefix_paths(); + let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), create_cxx_bridge); + let generated_string = rust_format::PrettyPlease::default() + .format_tokens(generated) + .unwrap(); + let out_dir = PathBuf::from(std::env::var("OUT_DIR").unwrap()); + let target_file = out_dir.join("generated.rs"); + std::fs::write(&target_file, generated_string).unwrap(); + println!("cargo:rustc-env=GENERATED_PATH={}", target_file.display()); + + #[cfg(feature = "cxx-bridge")] + let _build = cxx_build::bridge(&target_file); +} + +fn ament_prefix_paths() -> Vec { + let ament_prefix_path: String = match std::env::var("AMENT_PREFIX_PATH") { + Ok(path) => path, + Err(std::env::VarError::NotPresent) => { + println!("cargo:warning='AMENT_PREFIX_PATH not set'"); + String::new() + } + Err(std::env::VarError::NotUnicode(s)) => { + panic!( + "AMENT_PREFIX_PATH is not valid unicode: `{}`", + s.to_string_lossy() + ); + } + }; + println!("cargo:rerun-if-env-changed=AMENT_PREFIX_PATH"); + + let paths: Vec<_> = ament_prefix_path.split(':').map(PathBuf::from).collect(); + for path in &paths { + println!("cargo:rerun-if-changed={}", path.display()); + } + + paths +} diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml b/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml deleted file mode 100644 index 5b7b7e075..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/Cargo.toml +++ /dev/null @@ -1,20 +0,0 @@ -[package] -name = "dora-ros2-bridge-msg-gen-macro" -version = "0.1.0" -edition = "2021" -authors = ["Yuma Hiramatsu "] -license = "Apache-2.0" - -[dependencies] -anyhow = "1.0" -heck = "0.3" -nom = "7" -proc-macro2 = "1.0" -quote = "1.0" -regex = "1" -syn = { version = "2.0.41", features = ["parsing"] } -thiserror = "1.0" -dora-ros2-bridge-msg-gen = { path = "../msg-gen" } - -[lib] -proc-macro = true diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/build.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/build.rs deleted file mode 100644 index 9d5bff751..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/build.rs +++ /dev/null @@ -1,25 +0,0 @@ -use std::path::Path; - -fn main() { - let ament_prefix_path = match std::env::var("AMENT_PREFIX_PATH") { - Ok(path) => path, - Err(std::env::VarError::NotPresent) => { - println!("cargo:warning='AMENT_PREFIX_PATH not set'"); - String::new() - } - Err(std::env::VarError::NotUnicode(s)) => { - panic!( - "AMENT_PREFIX_PATH is not valid unicode: `{}`", - s.to_string_lossy() - ); - } - }; - println!("cargo:rerun-if-env-changed=AMENT_PREFIX_PATH"); - - let paths = ament_prefix_path.split(':').map(Path::new); - for path in paths { - println!("cargo:rerun-if-changed={}", path.display()); - } - - println!("cargo:rustc-env=DETECTED_AMENT_PREFIX_PATH={ament_prefix_path}"); -} diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs deleted file mode 100644 index fb9ad9bac..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/src/lib.rs +++ /dev/null @@ -1,68 +0,0 @@ -// Based on https://github.com/rclrust/rclrust/tree/3a48dbb8f23a3d67d3031351da3ed236a354f039/rclrust-msg-gen - -#![warn( - rust_2018_idioms, - elided_lifetimes_in_paths, - clippy::all, - clippy::nursery -)] - -use std::path::Path; - -use dora_ros2_bridge_msg_gen::get_packages; -use proc_macro::TokenStream; -use quote::quote; -use syn::{punctuated::Punctuated, MetaNameValue, Token}; - -struct Config { - create_cxx_bridge: bool, -} - -impl syn::parse::Parse for Config { - fn parse(input: syn::parse::ParseStream<'_>) -> syn::Result { - let punctuated = Punctuated::::parse_terminated(input)?; - - let mut config = Self { - create_cxx_bridge: false, - }; - - for item in &punctuated { - let ident = item.path.require_ident()?; - if ident == "cxx_bridge" { - let syn::Expr::Lit(lit) = &item.value else { - return Err(syn::Error::new_spanned(&item.value, "invalid value, expected bool")); - }; - let syn::Lit::Bool(b) = &lit.lit else { - return Err(syn::Error::new_spanned(&lit.lit, "invalid value, expected bool")); - }; - config.create_cxx_bridge = b.value; - } else { - return Err(syn::Error::new_spanned(&item.path, "invalid argument")); - } - } - - Ok(config) - } -} - -#[proc_macro] -pub fn msg_include_all(input: TokenStream) -> TokenStream { - let config = syn::parse_macro_input!(input as Config); - - let ament_prefix_path = std::env!("DETECTED_AMENT_PREFIX_PATH").trim(); - if ament_prefix_path.is_empty() { - quote! { - /// **No messages are available because the `AMENT_PREFIX_PATH` environment variable - /// was not set during build.** - pub const AMENT_PREFIX_PATH_NOT_SET: () = (); - } - .into() - } else { - let paths = ament_prefix_path - .split(':') - .map(Path::new) - .collect::>(); - - dora_ros2_bridge_msg_gen::gen(&paths, config.create_cxx_bridge).into() - } -} diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/action/Fibonacci.action b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/action/Fibonacci.action deleted file mode 100644 index b2591f28e..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/action/Fibonacci.action +++ /dev/null @@ -1,8 +0,0 @@ -#goal definition -int32 order ---- -#result definition -int32[] sequence ---- -#feedback -int32[] sequence diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Arrays.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Arrays.msg deleted file mode 100644 index 4e2d280b4..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Arrays.msg +++ /dev/null @@ -1,34 +0,0 @@ -# Arrays of different types -bool[3] bool_values -byte[3] byte_values -char[3] char_values -float32[3] float32_values -float64[3] float64_values -int8[3] int8_values -uint8[3] uint8_values -int16[3] int16_values -uint16[3] uint16_values -int32[3] int32_values -uint32[3] uint32_values -int64[3] int64_values -uint64[3] uint64_values -string[3] string_values -BasicTypes[3] basic_types_values -Constants[3] constants_values -Defaults[3] defaults_values -bool[3] bool_values_default [false, true, false] -byte[3] byte_values_default [0, 1, 255] -char[3] char_values_default [0, 1, 127] -float32[3] float32_values_default [1.125, 0.0, -1.125] -float64[3] float64_values_default [3.1415, 0.0, -3.1415] -int8[3] int8_values_default [0, 127, -128] -uint8[3] uint8_values_default [0, 1, 255] -int16[3] int16_values_default [0, 32767, -32768] -uint16[3] uint16_values_default [0, 1, 65535] -int32[3] int32_values_default [0, 2147483647, -2147483648] -uint32[3] uint32_values_default [0, 1, 4294967295] -int64[3] int64_values_default [0, 9223372036854775807, -9223372036854775808] -uint64[3] uint64_values_default [0, 1, 18446744073709551615] -string[3] string_values_default ["", "max value", "min value"] -# Regression test: check alignment of basic field after an array field is correct -int32 alignment_check diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BasicTypes.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BasicTypes.msg deleted file mode 100644 index eaf9b9ef3..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BasicTypes.msg +++ /dev/null @@ -1,13 +0,0 @@ -bool bool_value -byte byte_value -char char_value -float32 float32_value -float64 float64_value -int8 int8_value -uint8 uint8_value -int16 int16_value -uint16 uint16_value -int32 int32_value -uint32 uint32_value -int64 int64_value -uint64 uint64_value diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BoundedSequences.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BoundedSequences.msg deleted file mode 100644 index e369ffc39..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/BoundedSequences.msg +++ /dev/null @@ -1,34 +0,0 @@ -# Bounded sequences of different types -bool[<=3] bool_values -byte[<=3] byte_values -char[<=3] char_values -float32[<=3] float32_values -float64[<=3] float64_values -int8[<=3] int8_values -uint8[<=3] uint8_values -int16[<=3] int16_values -uint16[<=3] uint16_values -int32[<=3] int32_values -uint32[<=3] uint32_values -int64[<=3] int64_values -uint64[<=3] uint64_values -string[<=3] string_values -BasicTypes[<=3] basic_types_values -Constants[<=3] constants_values -Defaults[<=3] defaults_values -bool[<=3] bool_values_default [false, true, false] -byte[<=3] byte_values_default [0, 1, 255] -char[<=3] char_values_default [0, 1, 127] -float32[<=3] float32_values_default [1.125, 0.0, -1.125] -float64[<=3] float64_values_default [3.1415, 0.0, -3.1415] -int8[<=3] int8_values_default [0, 127, -128] -uint8[<=3] uint8_values_default [0, 1, 255] -int16[<=3] int16_values_default [0, 32767, -32768] -uint16[<=3] uint16_values_default [0, 1, 65535] -int32[<=3] int32_values_default [0, 2147483647, -2147483648] -uint32[<=3] uint32_values_default [0, 1, 4294967295] -int64[<=3] int64_values_default [0, 9223372036854775807, -9223372036854775808] -uint64[<=3] uint64_values_default [0, 1, 18446744073709551615] -string[<=3] string_values_default ["", "max value", "min value"] -# Regression test: check alignment of basic field after a sequence field is correct -int32 alignment_check diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Constants.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Constants.msg deleted file mode 100644 index c99685935..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Constants.msg +++ /dev/null @@ -1,13 +0,0 @@ -bool BOOL_CONST=true -byte BYTE_CONST=50 -char CHAR_CONST=100 -float32 FLOAT32_CONST=1.125 -float64 FLOAT64_CONST=1.125 -int8 INT8_CONST=-50 -uint8 UINT8_CONST=200 -int16 INT16_CONST=-1000 -uint16 UINT16_CONST=2000 -int32 INT32_CONST=-30000 -uint32 UINT32_CONST=60000 -int64 INT64_CONST=-40000000 -uint64 UINT64_CONST=50000000 diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Defaults.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Defaults.msg deleted file mode 100644 index eb4832f54..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Defaults.msg +++ /dev/null @@ -1,13 +0,0 @@ -bool bool_value true -byte byte_value 50 -char char_value 100 -float32 float32_value 1.125 -float64 float64_value 1.125 -int8 int8_value -50 -uint8 uint8_value 200 -int16 int16_value -1000 -uint16 uint16_value 2000 -int32 int32_value -30000 -uint32 uint32_value 60000 -int64 int64_value -40000000 -uint64 uint64_value 50000000 diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Empty.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Empty.msg deleted file mode 100644 index e69de29bb..000000000 diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/MultiNested.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/MultiNested.msg deleted file mode 100644 index cf324dc8f..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/MultiNested.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Mulitple levels of nested messages -Arrays[3] array_of_arrays -BoundedSequences[3] array_of_bounded_sequences -UnboundedSequences[3] array_of_unbounded_sequences -Arrays[<=3] bounded_sequence_of_arrays -BoundedSequences[<=3] bounded_sequence_of_bounded_sequences -UnboundedSequences[<=3] bounded_sequence_of_unbounded_sequences -Arrays[] unbounded_sequence_of_arrays -BoundedSequences[] unbounded_sequence_of_bounded_sequences -UnboundedSequences[] unbounded_sequence_of_unbounded_sequences diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Nested.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Nested.msg deleted file mode 100644 index 9adb9a59d..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Nested.msg +++ /dev/null @@ -1 +0,0 @@ -BasicTypes basic_types_value diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Strings.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Strings.msg deleted file mode 100644 index e7f5e45fb..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/Strings.msg +++ /dev/null @@ -1,13 +0,0 @@ -string string_value -string string_value_default1 "Hello world!" -string string_value_default2 "Hello'world!" -string string_value_default3 'Hello"world!' -string string_value_default4 'Hello\'world!' -string string_value_default5 "Hello\"world!" -string STRING_CONST="Hello world!" -string<=22 bounded_string_value -string<=22 bounded_string_value_default1 "Hello world!" -string<=22 bounded_string_value_default2 "Hello'world!" -string<=22 bounded_string_value_default3 'Hello"world!' -string<=22 bounded_string_value_default4 'Hello\'world!' -string<=22 bounded_string_value_default5 "Hello\"world!" diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/UnboundedSequences.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/UnboundedSequences.msg deleted file mode 100644 index 36c02d96e..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/UnboundedSequences.msg +++ /dev/null @@ -1,34 +0,0 @@ -# Unbounded sequences of different types -bool[] bool_values -byte[] byte_values -char[] char_values -float32[] float32_values -float64[] float64_values -int8[] int8_values -uint8[] uint8_values -int16[] int16_values -uint16[] uint16_values -int32[] int32_values -uint32[] uint32_values -int64[] int64_values -uint64[] uint64_values -string[] string_values -BasicTypes[] basic_types_values -Constants[] constants_values -Defaults[] defaults_values -bool[] bool_values_default [false, true, false] -byte[] byte_values_default [0, 1, 255] -char[] char_values_default [0, 1, 127] -float32[] float32_values_default [1.125, 0.0, -1.125] -float64[] float64_values_default [3.1415, 0.0, -3.1415] -int8[] int8_values_default [0, 127, -128] -uint8[] uint8_values_default [0, 1, 255] -int16[] int16_values_default [0, 32767, -32768] -uint16[] uint16_values_default [0, 1, 65535] -int32[] int32_values_default [0, 2147483647, -2147483648] -uint32[] uint32_values_default [0, 1, 4294967295] -int64[] int64_values_default [0, 9223372036854775807, -9223372036854775808] -uint64[] uint64_values_default [0, 1, 18446744073709551615] -string[] string_values_default ["", "max value", "min value"] -# Regression test: check alignment of basic field after a sequence field is correct -int32 alignment_check diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/WStrings.msg b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/WStrings.msg deleted file mode 100644 index 666d0ec67..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/msg/WStrings.msg +++ /dev/null @@ -1,10 +0,0 @@ -wstring wstring_value -wstring wstring_value_default1 "Hello world!" -wstring wstring_value_default2 "Hellö wörld!" -wstring wstring_value_default3 "ハローワールド" -#wstring WSTRING_CONST="Hello world!" -#wstring<=22 bounded_wstring_value -#wstring<=22 bounded_wstring_value_default1 "Hello world!" -wstring[3] array_of_wstrings -wstring[<=3] bounded_sequence_of_wstrings -wstring[] unbounded_sequence_of_wstrings diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Arrays.srv b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Arrays.srv deleted file mode 100644 index 4c96820ae..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Arrays.srv +++ /dev/null @@ -1,63 +0,0 @@ -bool[3] bool_values -byte[3] byte_values -char[3] char_values -float32[3] float32_values -float64[3] float64_values -int8[3] int8_values -uint8[3] uint8_values -int16[3] int16_values -uint16[3] uint16_values -int32[3] int32_values -uint32[3] uint32_values -int64[3] int64_values -uint64[3] uint64_values -string[3] string_values -BasicTypes[3] basic_types_values -Constants[3] constants_values -Defaults[3] defaults_values -bool[3] bool_values_default [false, true, false] -byte[3] byte_values_default [0, 1, 255] -char[3] char_values_default [0, 1, 127] -float32[3] float32_values_default [1.125, 0.0, -1.125] -float64[3] float64_values_default [3.1415, 0.0, -3.1415] -int8[3] int8_values_default [0, 127, -128] -uint8[3] uint8_values_default [0, 1, 255] -int16[3] int16_values_default [0, 32767, -32768] -uint16[3] uint16_values_default [0, 1, 65535] -int32[3] int32_values_default [0, 2147483647, -2147483648] -uint32[3] uint32_values_default [0, 1, 4294967295] -int64[3] int64_values_default [0, 9223372036854775807, -9223372036854775808] -uint64[3] uint64_values_default [0, 1, 18446744073709551615] -string[3] string_values_default ["", "max value", "min value"] ---- -bool[3] bool_values -byte[3] byte_values -char[3] char_values -float32[3] float32_values -float64[3] float64_values -int8[3] int8_values -uint8[3] uint8_values -int16[3] int16_values -uint16[3] uint16_values -int32[3] int32_values -uint32[3] uint32_values -int64[3] int64_values -uint64[3] uint64_values -string[3] string_values -BasicTypes[3] basic_types_values -Constants[3] constants_values -Defaults[3] defaults_values -bool[3] bool_values_default [false, true, false] -byte[3] byte_values_default [0, 1, 255] -char[3] char_values_default [0, 1, 127] -float32[3] float32_values_default [1.125, 0.0, -1.125] -float64[3] float64_values_default [3.1415, 0.0, -3.1415] -int8[3] int8_values_default [0, 127, -128] -uint8[3] uint8_values_default [0, 1, 255] -int16[3] int16_values_default [0, 32767, -32768] -uint16[3] uint16_values_default [0, 1, 65535] -int32[3] int32_values_default [0, 2147483647, -2147483648] -uint32[3] uint32_values_default [0, 1, 4294967295] -int64[3] int64_values_default [0, 9223372036854775807, -9223372036854775808] -uint64[3] uint64_values_default [0, 1, 18446744073709551615] -string[3] string_values_default ["", "max value", "min value"] diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/BasicTypes.srv b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/BasicTypes.srv deleted file mode 100644 index d00cfd539..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/BasicTypes.srv +++ /dev/null @@ -1,29 +0,0 @@ -bool bool_value -byte byte_value -char char_value -float32 float32_value -float64 float64_value -int8 int8_value -uint8 uint8_value -int16 int16_value -uint16 uint16_value -int32 int32_value -uint32 uint32_value -int64 int64_value -uint64 uint64_value -string string_value ---- -bool bool_value -byte byte_value -char char_value -float32 float32_value -float64 float64_value -int8 int8_value -uint8 uint8_value -int16 int16_value -uint16 uint16_value -int32 int32_value -uint32 uint32_value -int64 int64_value -uint64 uint64_value -string string_value diff --git a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Empty.srv b/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Empty.srv deleted file mode 100644 index ed97d539c..000000000 --- a/libraries/extensions/ros2-bridge/msg-gen-macro/test_msgs/srv/Empty.srv +++ /dev/null @@ -1 +0,0 @@ ---- diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index 3a2eb2448..f372a1a16 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -16,8 +16,11 @@ pub mod types; pub use crate::parser::get_packages; -pub fn gen(paths: &[&Path], create_cxx_bridge: bool) -> proc_macro2::TokenStream { - let message_structs = get_packages(&paths) +pub fn gen

(paths: &[P], create_cxx_bridge: bool) -> proc_macro2::TokenStream +where + P: AsRef, +{ + let message_structs = get_packages(paths) .unwrap() .iter() .map(|v| v.message_structs(create_cxx_bridge)) @@ -25,12 +28,12 @@ pub fn gen(paths: &[&Path], create_cxx_bridge: bool) -> proc_macro2::TokenStream let message_struct_defs = message_structs.iter().map(|(s, _)| s); let message_struct_impls = message_structs.iter().map(|(_, i)| i); - let aliases = get_packages(&paths) + let aliases = get_packages(paths) .unwrap() .iter() .map(|v| v.aliases_token_stream()) .collect::>(); - let packages = get_packages(&paths) + let packages = get_packages(paths) .unwrap() .iter() .map(|v| v.token_stream(create_cxx_bridge)) @@ -47,9 +50,9 @@ pub fn gen(paths: &[&Path], create_cxx_bridge: bool) -> proc_macro2::TokenStream ) }; - (quote! { + quote! { #attributes - pub mod ffi { + mod ffi { #imports #[derive(Debug, Default, Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -73,6 +76,5 @@ pub fn gen(paths: &[&Path], create_cxx_bridge: bool) -> proc_macro2::TokenStream // #(#packages)* - }) - .into() + } } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/parser/package.rs b/libraries/extensions/ros2-bridge/msg-gen/src/parser/package.rs index 3c3f7b9f1..12e7da5ce 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/parser/package.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/parser/package.rs @@ -92,10 +92,13 @@ fn get_ros_msgs_each_package>(root_dir: P) -> Result Ok(packages) } -pub fn get_packages(paths: &[&Path]) -> Result> { +pub fn get_packages

(paths: &[P]) -> Result> +where + P: AsRef, +{ let mut packages = paths .iter() - .map(|&path| get_ros_msgs_each_package(path)) + .map(get_ros_msgs_each_package) .collect::>>()? .into_iter() .flatten() diff --git a/libraries/extensions/ros2-bridge/src/lib.rs b/libraries/extensions/ros2-bridge/src/lib.rs index a5f3ecb8c..7d29c7b15 100644 --- a/libraries/extensions/ros2-bridge/src/lib.rs +++ b/libraries/extensions/ros2-bridge/src/lib.rs @@ -1,10 +1,8 @@ pub use ros2_client; pub use rustdds; -#[cfg(all(feature = "generate-messages", feature = "cxx-bridge"))] -dora_ros2_bridge_msg_gen_macro::msg_include_all!(cxx_bridge = true); - -#[cfg(all(feature = "generate-messages", not(feature = "cxx-bridge")))] -dora_ros2_bridge_msg_gen_macro::msg_include_all!(); +pub mod messages { + include!(env!("GENERATED_PATH")); +} pub mod _core; From b5cbea1a209c188467562accbbec530eda2dbb33 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 5 Jan 2024 17:23:02 +0100 Subject: [PATCH 06/32] Include ros2-bridge header files in `dora-node-api-cxx` --- Cargo.lock | 1 + apis/c++/node/Cargo.toml | 2 + apis/c++/node/build.rs | 58 +++++++++++++++++++++ libraries/extensions/ros2-bridge/Cargo.toml | 1 + libraries/extensions/ros2-bridge/build.rs | 4 +- libraries/extensions/ros2-bridge/src/lib.rs | 2 +- 6 files changed, 65 insertions(+), 3 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 3dada54f2..20942d39a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1604,6 +1604,7 @@ dependencies = [ "cxx", "cxx-build", "dora-node-api", + "dora-ros2-bridge", "eyre", ] diff --git a/apis/c++/node/Cargo.toml b/apis/c++/node/Cargo.toml index 77e58c25e..347f07e4c 100644 --- a/apis/c++/node/Cargo.toml +++ b/apis/c++/node/Cargo.toml @@ -14,11 +14,13 @@ crate-type = ["staticlib"] [features] default = ["tracing"] tracing = ["dora-node-api/tracing"] +ros2-bridge = [] [dependencies] cxx = "1.0.73" dora-node-api = { workspace = true } eyre = "0.6.8" +dora-ros2-bridge = { workspace = true, features = ["cxx-bridge"] } [build-dependencies] cxx-build = "1.0.73" diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index dba00707d..f51d2af05 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -1,4 +1,62 @@ +use std::path::{Component, Path, PathBuf}; + fn main() { let _build = cxx_build::bridge("src/lib.rs"); println!("cargo:rerun-if-changed=src/lib.rs"); + + if cfg!(feature = "ros2-bridge") { + generate_ros2_message_header(); + } +} + +fn generate_ros2_message_header() { + let prefix = std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_PREFIX").unwrap(); + let include_dir = PathBuf::from(std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_DIR0").unwrap()); + let _crate_dir = std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_DIR1").unwrap(); + + let header_path = include_dir + .join(&prefix) + .join(local_relative_path(&include_dir)) + .ancestors() + .nth(2) + .unwrap() + .join("messages.rs.h"); + let code_path = include_dir + .parent() + .unwrap() + .join("sources") + .join(&prefix) + .join(local_relative_path(&include_dir)) + .ancestors() + .nth(2) + .unwrap() + .join("messages.rs.cc"); + + // copy message files to target directory + let root = Path::new(env!("CARGO_MANIFEST_DIR")) + .ancestors() + .nth(3) + .unwrap(); + let target_path = root + .join("target") + .join("cxxbridge") + .join("dora-node-api-cxx") + .join("src") + .join("messages.rs.h"); + + std::fs::copy(header_path, &target_path).unwrap(); + std::fs::copy(code_path, target_path.with_file_name("messages.rs.cc")).unwrap(); +} + +// copy from cxx-build source +fn local_relative_path(path: &Path) -> PathBuf { + let mut rel_path = PathBuf::new(); + for component in path.components() { + match component { + Component::Prefix(_) | Component::RootDir | Component::CurDir => {} + Component::ParentDir => drop(rel_path.pop()), // noop if empty + Component::Normal(name) => rel_path.push(name), + } + } + rel_path } diff --git a/libraries/extensions/ros2-bridge/Cargo.toml b/libraries/extensions/ros2-bridge/Cargo.toml index 6b3b916bf..2da5a5d31 100644 --- a/libraries/extensions/ros2-bridge/Cargo.toml +++ b/libraries/extensions/ros2-bridge/Cargo.toml @@ -2,6 +2,7 @@ name = "dora-ros2-bridge" version = "0.1.0" edition = "2021" +links = "dora-ros2-bridge" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/libraries/extensions/ros2-bridge/build.rs b/libraries/extensions/ros2-bridge/build.rs index a8ec4c987..44a948957 100644 --- a/libraries/extensions/ros2-bridge/build.rs +++ b/libraries/extensions/ros2-bridge/build.rs @@ -9,9 +9,9 @@ fn main() { .format_tokens(generated) .unwrap(); let out_dir = PathBuf::from(std::env::var("OUT_DIR").unwrap()); - let target_file = out_dir.join("generated.rs"); + let target_file = out_dir.join("messages.rs"); std::fs::write(&target_file, generated_string).unwrap(); - println!("cargo:rustc-env=GENERATED_PATH={}", target_file.display()); + println!("cargo:rustc-env=MESSAGES_PATH={}", target_file.display()); #[cfg(feature = "cxx-bridge")] let _build = cxx_build::bridge(&target_file); diff --git a/libraries/extensions/ros2-bridge/src/lib.rs b/libraries/extensions/ros2-bridge/src/lib.rs index 7d29c7b15..85fcf1163 100644 --- a/libraries/extensions/ros2-bridge/src/lib.rs +++ b/libraries/extensions/ros2-bridge/src/lib.rs @@ -2,7 +2,7 @@ pub use ros2_client; pub use rustdds; pub mod messages { - include!(env!("GENERATED_PATH")); + include!(env!("MESSAGES_PATH")); } pub mod _core; From 26b463bdfdb4412427757cb4f1a80650576c52a7 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 5 Jan 2024 17:46:33 +0100 Subject: [PATCH 07/32] Start creating a C++-ROS2 example --- Cargo.toml | 5 + examples/c++-ros2-dataflow/.gitignore | 1 + examples/c++-ros2-dataflow/dataflow.yml | 8 + .../c++-ros2-dataflow/node-rust-api/main.cc | 56 ++++++ examples/c++-ros2-dataflow/run.rs | 175 ++++++++++++++++++ 5 files changed, 245 insertions(+) create mode 100644 examples/c++-ros2-dataflow/.gitignore create mode 100644 examples/c++-ros2-dataflow/dataflow.yml create mode 100644 examples/c++-ros2-dataflow/node-rust-api/main.cc create mode 100644 examples/c++-ros2-dataflow/run.rs diff --git a/Cargo.toml b/Cargo.toml index da1634263..a23322947 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -138,3 +138,8 @@ path = "examples/multiple-daemons/run.rs" [[example]] name = "cmake-dataflow" path = "examples/cmake-dataflow/run.rs" + +[[example]] +name = "cxx-ros2-dataflow" +path = "examples/c++-ros2-dataflow/run.rs" +required-features = ["ros2-examples"] diff --git a/examples/c++-ros2-dataflow/.gitignore b/examples/c++-ros2-dataflow/.gitignore new file mode 100644 index 000000000..5761abcfd --- /dev/null +++ b/examples/c++-ros2-dataflow/.gitignore @@ -0,0 +1 @@ +*.o diff --git a/examples/c++-ros2-dataflow/dataflow.yml b/examples/c++-ros2-dataflow/dataflow.yml new file mode 100644 index 000000000..16f8a5e66 --- /dev/null +++ b/examples/c++-ros2-dataflow/dataflow.yml @@ -0,0 +1,8 @@ +nodes: + - id: cxx-node-rust-api + custom: + source: build/node_rust_api + inputs: + tick: dora/timer/millis/300 + outputs: + - counter diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc new file mode 100644 index 000000000..b301dad55 --- /dev/null +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -0,0 +1,56 @@ +#include "../build/dora-node-api.h" +#include "../build/messages.h" + +#include +#include + +int main() +{ + std::cout << "HELLO FROM C++" << std::endl; + + geometry_msgs::Twist twist = { + .linear = {.x = 1, .y = 0, .z = 0}, + .angular = {.x = 0, .y = 0, .z = 0.5}}; + + unsigned char counter = 0; + + auto dora_node = init_dora_node(); + + for (int i = 0; i < 20; i++) + { + + auto event = next_event(dora_node.events); + auto ty = event_type(event); + + if (ty == DoraEventType::AllInputsClosed) + { + break; + } + else if (ty == DoraEventType::Input) + { + auto input = event_as_input(std::move(event)); + + counter += 1; + + std::cout << "Received input " << std::string(input.id) << " (counter: " << (unsigned int)counter << ")" << std::endl; + + std::vector out_vec{counter}; + rust::Slice out_slice{out_vec.data(), out_vec.size()}; + auto result = send_output(dora_node.send_output, "counter", out_slice); + auto error = std::string(result.error); + if (!error.empty()) + { + std::cerr << "Error: " << error << std::endl; + return -1; + } + } + else + { + std::cerr << "Unknown event type " << static_cast(ty) << std::endl; + } + } + + std::cout << "GOODBYE FROM C++ node (using Rust API)" << std::endl; + + return 0; +} diff --git a/examples/c++-ros2-dataflow/run.rs b/examples/c++-ros2-dataflow/run.rs new file mode 100644 index 000000000..d859d466a --- /dev/null +++ b/examples/c++-ros2-dataflow/run.rs @@ -0,0 +1,175 @@ +use eyre::{bail, Context}; +use std::{env::consts::EXE_SUFFIX, path::Path}; +use tracing::metadata::LevelFilter; +use tracing_subscriber::Layer; + +#[derive(Debug, Clone, clap::Parser)] +pub struct Args { + #[clap(long)] + pub run_dora_runtime: bool, +} + +#[tokio::main] +async fn main() -> eyre::Result<()> { + let Args { run_dora_runtime } = clap::Parser::parse(); + + if run_dora_runtime { + return tokio::task::block_in_place(dora_daemon::run_dora_runtime); + } + set_up_tracing().wrap_err("failed to set up tracing")?; + + if cfg!(windows) { + tracing::error!( + "The c++ example does not work on Windows currently because of a linker error" + ); + return Ok(()); + } + + let root = Path::new(env!("CARGO_MANIFEST_DIR")); + let target = root.join("target"); + std::env::set_current_dir(root.join(file!()).parent().unwrap()) + .wrap_err("failed to set working dir")?; + + tokio::fs::create_dir_all("build").await?; + let build_dir = Path::new("build"); + + build_package("dora-node-api-cxx", &["ros2-bridge"]).await?; + let node_cxxbridge = target + .join("cxxbridge") + .join("dora-node-api-cxx") + .join("src"); + tokio::fs::copy( + node_cxxbridge.join("lib.rs.cc"), + build_dir.join("node-bridge.cc"), + ) + .await?; + tokio::fs::copy( + node_cxxbridge.join("lib.rs.h"), + build_dir.join("dora-node-api.h"), + ) + .await?; + tokio::fs::copy( + node_cxxbridge.join("messages.rs.cc"), + build_dir.join("messages.cc"), + ) + .await?; + tokio::fs::copy( + node_cxxbridge.join("messages.rs.h"), + build_dir.join("messages.h"), + ) + .await?; + + build_cxx_node( + root, + &[ + &dunce::canonicalize(Path::new("node-rust-api").join("main.cc"))?, + &dunce::canonicalize(build_dir.join("node-bridge.cc"))?, + ], + "node_rust_api", + &["-l", "dora_node_api_cxx"], + ) + .await?; + + let dataflow = Path::new("dataflow.yml").to_owned(); + dora_daemon::Daemon::run_dataflow(&dataflow).await?; + + Ok(()) +} + +async fn build_package(package: &str, features: &[&str]) -> eyre::Result<()> { + let cargo = std::env::var("CARGO").unwrap(); + let mut cmd = tokio::process::Command::new(&cargo); + cmd.arg("build"); + cmd.arg("--package").arg(package); + if !cmd.status().await?.success() { + bail!("failed to build {package}"); + }; + if !features.is_empty() { + cmd.arg("--features").arg(features.join(",")); + } + Ok(()) +} + +async fn build_cxx_node( + root: &Path, + paths: &[&Path], + out_name: &str, + args: &[&str], +) -> eyre::Result<()> { + let mut clang = tokio::process::Command::new("clang++"); + clang.args(paths); + clang.arg("-std=c++17"); + #[cfg(target_os = "linux")] + { + clang.arg("-l").arg("m"); + clang.arg("-l").arg("rt"); + clang.arg("-l").arg("dl"); + clang.arg("-pthread"); + } + #[cfg(target_os = "windows")] + { + clang.arg("-ladvapi32"); + clang.arg("-luserenv"); + clang.arg("-lkernel32"); + clang.arg("-lws2_32"); + clang.arg("-lbcrypt"); + clang.arg("-lncrypt"); + clang.arg("-lschannel"); + clang.arg("-lntdll"); + clang.arg("-liphlpapi"); + + clang.arg("-lcfgmgr32"); + clang.arg("-lcredui"); + clang.arg("-lcrypt32"); + clang.arg("-lcryptnet"); + clang.arg("-lfwpuclnt"); + clang.arg("-lgdi32"); + clang.arg("-lmsimg32"); + clang.arg("-lmswsock"); + clang.arg("-lole32"); + clang.arg("-lopengl32"); + clang.arg("-lsecur32"); + clang.arg("-lshell32"); + clang.arg("-lsynchronization"); + clang.arg("-luser32"); + clang.arg("-lwinspool"); + + clang.arg("-Wl,-nodefaultlib:libcmt"); + clang.arg("-D_DLL"); + clang.arg("-lmsvcrt"); + } + #[cfg(target_os = "macos")] + { + clang.arg("-framework").arg("CoreServices"); + clang.arg("-framework").arg("Security"); + clang.arg("-l").arg("System"); + clang.arg("-l").arg("resolv"); + clang.arg("-l").arg("pthread"); + clang.arg("-l").arg("c"); + clang.arg("-l").arg("m"); + } + clang.args(args); + clang.arg("-L").arg(root.join("target").join("debug")); + clang + .arg("--output") + .arg(Path::new("../build").join(format!("{out_name}{EXE_SUFFIX}"))); + if let Some(parent) = paths[0].parent() { + clang.current_dir(parent); + } + + if !clang.status().await?.success() { + bail!("failed to compile c++ node"); + }; + Ok(()) +} + +fn set_up_tracing() -> eyre::Result<()> { + use tracing_subscriber::prelude::__tracing_subscriber_SubscriberExt; + + let stdout_log = tracing_subscriber::fmt::layer() + .pretty() + .with_filter(LevelFilter::DEBUG); + let subscriber = tracing_subscriber::Registry::default().with(stdout_log); + tracing::subscriber::set_global_default(subscriber) + .context("failed to set tracing global subscriber") +} From d3d666040d30ab15407c4bba51c0afe5134baa2f Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 13:57:29 +0100 Subject: [PATCH 08/32] Fix building of ros2-bridge dependencies --- apis/c++/node/Cargo.toml | 6 ++++-- apis/c++/node/build.rs | 6 ++++-- examples/c++-ros2-dataflow/run.rs | 3 +++ 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/apis/c++/node/Cargo.toml b/apis/c++/node/Cargo.toml index 347f07e4c..7e75906f3 100644 --- a/apis/c++/node/Cargo.toml +++ b/apis/c++/node/Cargo.toml @@ -14,13 +14,15 @@ crate-type = ["staticlib"] [features] default = ["tracing"] tracing = ["dora-node-api/tracing"] -ros2-bridge = [] +ros2-bridge = ["dep:dora-ros2-bridge"] [dependencies] cxx = "1.0.73" dora-node-api = { workspace = true } eyre = "0.6.8" -dora-ros2-bridge = { workspace = true, features = ["cxx-bridge"] } +dora-ros2-bridge = { workspace = true, optional = true, features = [ + "cxx-bridge", +] } [build-dependencies] cxx-build = "1.0.73" diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index f51d2af05..fad587819 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -44,8 +44,10 @@ fn generate_ros2_message_header() { .join("src") .join("messages.rs.h"); - std::fs::copy(header_path, &target_path).unwrap(); - std::fs::copy(code_path, target_path.with_file_name("messages.rs.cc")).unwrap(); + std::fs::copy(&header_path, &target_path).unwrap(); + println!("cargo:rerun-if-changed={}", header_path.display()); + std::fs::copy(&code_path, target_path.with_file_name("messages.rs.cc")).unwrap(); + println!("cargo:rerun-if-changed={}", code_path.display()); } // copy from cxx-build source diff --git a/examples/c++-ros2-dataflow/run.rs b/examples/c++-ros2-dataflow/run.rs index d859d466a..0fba4291a 100644 --- a/examples/c++-ros2-dataflow/run.rs +++ b/examples/c++-ros2-dataflow/run.rs @@ -87,6 +87,9 @@ async fn build_package(package: &str, features: &[&str]) -> eyre::Result<()> { if !features.is_empty() { cmd.arg("--features").arg(features.join(",")); } + if !cmd.status().await?.success() { + bail!("failed to compile {package}"); + }; Ok(()) } From e818983549251e1d875253a7a52b025a8194a3e4 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 13:58:21 +0100 Subject: [PATCH 09/32] Generate topic definitions for all messages types --- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 59 +++++++++++-------- .../ros2-bridge/msg-gen/src/types/message.rs | 25 ++++++++ 2 files changed, 59 insertions(+), 25 deletions(-) diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index f372a1a16..f1360e063 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -9,7 +9,9 @@ use std::path::Path; +use proc_macro2::Span; use quote::quote; +use syn::Ident; pub mod parser; pub mod types; @@ -20,27 +22,36 @@ pub fn gen

(paths: &[P], create_cxx_bridge: bool) -> proc_macro2::TokenStream where P: AsRef, { - let message_structs = get_packages(paths) - .unwrap() - .iter() - .map(|v| v.message_structs(create_cxx_bridge)) - .collect::>(); - let message_struct_defs = message_structs.iter().map(|(s, _)| s); - let message_struct_impls = message_structs.iter().map(|(_, i)| i); - - let aliases = get_packages(paths) - .unwrap() - .iter() - .map(|v| v.aliases_token_stream()) - .collect::>(); - let packages = get_packages(paths) - .unwrap() - .iter() - .map(|v| v.token_stream(create_cxx_bridge)) - .collect::>(); + let packages = get_packages(paths).unwrap(); + let mut shared_type_defs = Vec::new(); + let mut message_struct_impls = Vec::new(); + let mut message_topic_defs = Vec::new(); + let mut message_topic_impls = Vec::new(); + let mut aliases = Vec::new(); + for package in &packages { + let package_name = Ident::new(&package.name, Span::call_site()); + for message in &package.messages { + let (def, imp) = message.struct_token_stream(&package_name, create_cxx_bridge); + shared_type_defs.push(def); + message_struct_impls.push(imp); + if create_cxx_bridge { + let (topic_def, topic_impl) = message.topic_def(&package_name); + message_topic_defs.push(topic_def); + message_topic_impls.push(topic_impl); + } + } + aliases.push(package.aliases_token_stream()); + } - let (attributes, imports) = if create_cxx_bridge { - (quote! { #[cxx::bridge] }, quote! {}) + let (attributes, imports_and_functions) = if create_cxx_bridge { + ( + quote! { #[cxx::bridge] }, + quote! { + extern "Rust" { + #(#message_topic_defs)* + } + }, + ) } else { ( quote! {}, @@ -53,14 +64,14 @@ where quote! { #attributes mod ffi { - #imports + #imports_and_functions #[derive(Debug, Default, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct U16String { chars: Vec, } - #(#message_struct_defs)* + #(#shared_type_defs)* } @@ -71,10 +82,8 @@ where } #(#message_struct_impls)* + #(#message_topic_impls)* #(#aliases)* - - - // #(#packages)* } } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 60f0be0b6..9f7fbb1c8 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -176,6 +176,31 @@ impl Message { } } + pub fn topic_def(&self, package_name: &Ident) -> (impl ToTokens, impl ToTokens) { + let topic_name = format_ident!("Topic__{package_name}__{}", self.name); + let fn_name = format_ident!("new__Topic__{package_name}__{}", self.name); + let cxx_topic_name = format_ident!("Topic_{}", self.name); + let cxx_fn_name = format!("create_{cxx_topic_name}"); + + let def = quote! { + #[namespace = #package_name] + #[cxx_name = #cxx_topic_name] + type #topic_name; + #[namespace = #package_name] + #[cxx_name = #cxx_fn_name] + fn #fn_name() -> Box<#topic_name>; + }; + let imp = quote! { + #[allow(non_camel_case_types)] + pub struct #topic_name; + #[allow(non_snake_case)] + pub fn #fn_name() -> Box<#topic_name> { + Box::new(#topic_name) + } + }; + (def, imp) + } + pub fn alias_token_stream(&self, package_name: &Ident) -> impl ToTokens { let cxx_name = format_ident!("{}", self.name); let struct_raw_name = format_ident!("{package_name}__{}", self.name); From 523d3544d280defabd8a3a0c006e258e1e6cd575 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 16:21:29 +0100 Subject: [PATCH 10/32] Re-export `dora_ros2_bridge` from c++ API to ensure that all symbols are linked --- apis/c++/node/src/lib.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index 77fd31970..6f6e5d599 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -5,6 +5,9 @@ use dora_node_api::{ }; use eyre::bail; +#[cfg(feature = "ros2-bridge")] +pub use dora_ros2_bridge as ros2; + #[cxx::bridge] #[allow(clippy::needless_lifetimes)] mod ffi { From 6153f6629af919de5193526409bd92ef7f182790 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 16:22:38 +0100 Subject: [PATCH 11/32] Export function to create ROS2 context --- .../c++-ros2-dataflow/node-rust-api/main.cc | 2 ++ libraries/extensions/ros2-bridge/Cargo.toml | 4 +-- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 29 ++++++++++++++++++- 3 files changed, 32 insertions(+), 3 deletions(-) diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index b301dad55..aa498f904 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -8,6 +8,8 @@ int main() { std::cout << "HELLO FROM C++" << std::endl; + auto ros2_context = init_ros2_context(); + geometry_msgs::Twist twist = { .linear = {.x = 1, .y = 0, .z = 0}, .angular = {.x = 0, .y = 0, .z = 0.5}}; diff --git a/libraries/extensions/ros2-bridge/Cargo.toml b/libraries/extensions/ros2-bridge/Cargo.toml index 2da5a5d31..28333d19f 100644 --- a/libraries/extensions/ros2-bridge/Cargo.toml +++ b/libraries/extensions/ros2-bridge/Cargo.toml @@ -9,9 +9,9 @@ links = "dora-ros2-bridge" [features] default = ["generate-messages"] generate-messages = ["dep:dora-ros2-bridge-msg-gen", "dep:rust-format"] -cxx-bridge = ["dep:cxx", "dep:cxx-build"] +cxx-bridge = ["dep:cxx", "dep:cxx-build", "dep:eyre"] # enables examples that depend on a sourced ROS2 installation -ros2-examples = ["eyre", "tokio", "dora-daemon"] +ros2-examples = ["dep:eyre", "tokio", "dora-daemon"] [dependencies] array-init = "2.1.0" diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index f1360e063..5fca7030d 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -43,14 +43,37 @@ where aliases.push(package.aliases_token_stream()); } - let (attributes, imports_and_functions) = if create_cxx_bridge { + let (attributes, imports_and_functions, cxx_bridge_impls) = if create_cxx_bridge { ( quote! { #[cxx::bridge] }, quote! { extern "Rust" { + type Ros2Context; + type Ros2Node; + fn init_ros2_context() -> Result>; + fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; + #(#message_topic_defs)* } }, + quote! { + struct Ros2Context(ros2_client::Context); + + fn init_ros2_context() -> eyre::Result> { + Ok(Box::new(Ros2Context(ros2_client::Context::new()?))) + } + + impl Ros2Context { + fn new_node(&self, name_space: &str, base_name: &str) -> eyre::Result> { + let name = ros2_client::NodeName::new(name_space, base_name).map_err(|e| eyre::eyre!(e))?; + let options = ros2_client::NodeOptions::new().enable_rosout(true); + let node = self.0.new_node(name, options)?; + Ok(Box::new(Ros2Node(node))) + } + } + + struct Ros2Node(ros2_client::Node); + }, ) } else { ( @@ -58,6 +81,7 @@ where quote! { use serde::{Serialize, Deserialize}; }, + quote! {}, ) }; @@ -82,8 +106,11 @@ where } #(#message_struct_impls)* + + #cxx_bridge_impls #(#message_topic_impls)* + #(#aliases)* } } From 26e78b546708450799320fff583246e77eb73adb Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 16:23:00 +0100 Subject: [PATCH 12/32] Fix: include generates `messages.cc` file during build --- examples/c++-ros2-dataflow/run.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/c++-ros2-dataflow/run.rs b/examples/c++-ros2-dataflow/run.rs index 0fba4291a..c9a7505f1 100644 --- a/examples/c++-ros2-dataflow/run.rs +++ b/examples/c++-ros2-dataflow/run.rs @@ -64,6 +64,7 @@ async fn main() -> eyre::Result<()> { &[ &dunce::canonicalize(Path::new("node-rust-api").join("main.cc"))?, &dunce::canonicalize(build_dir.join("node-bridge.cc"))?, + &dunce::canonicalize(build_dir.join("messages.cc"))?, ], "node_rust_api", &["-l", "dora_node_api_cxx"], From 7baecea71df811cbb654e83ecf3de0656152e6e5 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 11 Jan 2024 17:39:45 +0100 Subject: [PATCH 13/32] Provide functions to create topics and publishers --- .../c++-ros2-dataflow/node-rust-api/main.cc | 3 + .../extensions/ros2-bridge/msg-gen/src/lib.rs | 6 +- .../ros2-bridge/msg-gen/src/types/message.rs | 55 +++++++++++++++---- .../ros2-bridge/msg-gen/src/types/package.rs | 3 +- 4 files changed, 51 insertions(+), 16 deletions(-) diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index aa498f904..d58f91a31 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -9,6 +9,9 @@ int main() std::cout << "HELLO FROM C++" << std::endl; auto ros2_context = init_ros2_context(); + auto node = ros2_context->new_node("/ros2_demo", "turtle_teleop"); + auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", 0); + auto vel_publisher = node->create_publisher(vel_topic, 0); geometry_msgs::Twist twist = { .linear = {.x = 1, .y = 0, .z = 0}, diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index 5fca7030d..f7f3495c2 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -29,13 +29,12 @@ where let mut message_topic_impls = Vec::new(); let mut aliases = Vec::new(); for package in &packages { - let package_name = Ident::new(&package.name, Span::call_site()); for message in &package.messages { - let (def, imp) = message.struct_token_stream(&package_name, create_cxx_bridge); + let (def, imp) = message.struct_token_stream(&package.name, create_cxx_bridge); shared_type_defs.push(def); message_struct_impls.push(imp); if create_cxx_bridge { - let (topic_def, topic_impl) = message.topic_def(&package_name); + let (topic_def, topic_impl) = message.topic_def(&package.name); message_topic_defs.push(topic_def); message_topic_impls.push(topic_impl); } @@ -52,7 +51,6 @@ where type Ros2Node; fn init_ros2_context() -> Result>; fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; - #(#message_topic_defs)* } }, diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 9f7fbb1c8..4611386e5 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -125,7 +125,7 @@ pub struct Message { impl Message { pub fn struct_token_stream( &self, - package_name: &Ident, + package_name: &str, gen_cxx_bridge: bool, ) -> (impl ToTokens, impl ToTokens) { let cxx_name = format_ident!("{}", self.name); @@ -176,27 +176,62 @@ impl Message { } } - pub fn topic_def(&self, package_name: &Ident) -> (impl ToTokens, impl ToTokens) { + pub fn topic_def(&self, package_name: &str) -> (impl ToTokens, impl ToTokens) { + if self.members.is_empty() { + return (quote! {}, quote! {}); + }; + let topic_name = format_ident!("Topic__{package_name}__{}", self.name); - let fn_name = format_ident!("new__Topic__{package_name}__{}", self.name); let cxx_topic_name = format_ident!("Topic_{}", self.name); - let cxx_fn_name = format!("create_{cxx_topic_name}"); + let create_topic = format_ident!("new__Topic__{package_name}__{}", self.name); + let cxx_create_topic = format!("create_topic_{package_name}_{}", self.name); + + let publisher_name = format_ident!("Publisher__{package_name}__{}", self.name); + let cxx_publisher_name = format_ident!("Publisher_{}", self.name); + let create_publisher = format_ident!("new__Publisher__{package_name}__{}", self.name); + let cxx_create_publisher = format_ident!("create_publisher"); + let struct_raw_name = format_ident!("{package_name}__{}", self.name); + let self_name = &self.name; let def = quote! { #[namespace = #package_name] #[cxx_name = #cxx_topic_name] type #topic_name; + #[cxx_name = #cxx_create_topic] + fn #create_topic(self: &Ros2Node, name_space: &str, base_name: &str, qos: u32) -> Result>; + #[namespace = #package_name] - #[cxx_name = #cxx_fn_name] - fn #fn_name() -> Box<#topic_name>; + #[cxx_name = #cxx_publisher_name] + type #publisher_name; + #[cxx_name = #cxx_create_publisher] + fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: u32) -> Result>; }; let imp = quote! { #[allow(non_camel_case_types)] - pub struct #topic_name; - #[allow(non_snake_case)] - pub fn #fn_name() -> Box<#topic_name> { - Box::new(#topic_name) + pub struct #topic_name(rustdds::Topic); + + impl Ros2Node { + #[allow(non_snake_case)] + pub fn #create_topic(&self, name_space: &str, base_name: &str, qos: u32) -> eyre::Result> { + let name = ros2_client::Name::new(name_space, base_name).map_err(|e| eyre::eyre!(e))?; + let type_name = ros2_client::MessageTypeName::new(#package_name, #self_name); + let qos = Default::default(); // TODO + let topic = self.0.create_topic(&name, type_name, &qos)?; + Ok(Box::new(#topic_name(topic))) + } } + + #[allow(non_camel_case_types)] + pub struct #publisher_name(ros2_client::Subscription); + + impl Ros2Node { + #[allow(non_snake_case)] + pub fn #create_publisher(&mut self, topic: &Box<#topic_name>, qos: u32) -> eyre::Result> { + let subscription = self.0.create_subscription(&topic.0, None)?; // TODO + Ok(Box::new(#publisher_name(subscription))) + } + } + }; (def, imp) } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs index 1e6e6fc88..ab0ccb3c7 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/package.rs @@ -27,7 +27,6 @@ impl Package { } pub fn message_structs(&self, gen_cxx_bridge: bool) -> (impl ToTokens, impl ToTokens) { - let package_name = Ident::new(&self.name, Span::call_site()); if self.messages.is_empty() { // empty msg (quote! {}, quote! {}) @@ -35,7 +34,7 @@ impl Package { let items = self .messages .iter() - .map(|v| v.struct_token_stream(&package_name, gen_cxx_bridge)); + .map(|v| v.struct_token_stream(&self.name, gen_cxx_bridge)); let defs = items.clone().map(|(def, _)| def); let impls = items.clone().map(|(_, im)| im); let def_tokens = quote! { From 874ec96a3e59eb813823495a3fb862500467700b Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 12 Jan 2024 14:12:57 +0100 Subject: [PATCH 14/32] Provide ROS2 publish function to c++ --- .../c++-ros2-dataflow/node-rust-api/main.cc | 1 + .../ros2-bridge/msg-gen/src/types/message.rs | 27 +++++++++++++------ 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index d58f91a31..11bdf3f6f 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -23,6 +23,7 @@ int main() for (int i = 0; i < 20; i++) { + vel_publisher->publish(twist); auto event = next_event(dora_node.events); auto ty = event_type(event); diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 4611386e5..5a320e0a6 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -193,18 +193,24 @@ impl Message { let struct_raw_name = format_ident!("{package_name}__{}", self.name); let self_name = &self.name; + let publish = format_ident!("publish__{package_name}__{}", self.name); + let cxx_publish = format_ident!("publish"); + let def = quote! { #[namespace = #package_name] #[cxx_name = #cxx_topic_name] type #topic_name; #[cxx_name = #cxx_create_topic] fn #create_topic(self: &Ros2Node, name_space: &str, base_name: &str, qos: u32) -> Result>; + #[cxx_name = #cxx_create_publisher] + fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: u32) -> Result>; #[namespace = #package_name] #[cxx_name = #cxx_publisher_name] type #publisher_name; - #[cxx_name = #cxx_create_publisher] - fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: u32) -> Result>; + #[namespace = #package_name] + #[cxx_name = #cxx_publish] + fn #publish(self: &mut #publisher_name, message: #struct_raw_name) -> Result<()>; }; let imp = quote! { #[allow(non_camel_case_types)] @@ -219,16 +225,21 @@ impl Message { let topic = self.0.create_topic(&name, type_name, &qos)?; Ok(Box::new(#topic_name(topic))) } + + #[allow(non_snake_case)] + pub fn #create_publisher(&mut self, topic: &Box<#topic_name>, qos: u32) -> eyre::Result> { + let publisher = self.0.create_publisher(&topic.0, None)?; // TODO + Ok(Box::new(#publisher_name(publisher))) + } } #[allow(non_camel_case_types)] - pub struct #publisher_name(ros2_client::Subscription); + pub struct #publisher_name(ros2_client::Publisher); - impl Ros2Node { - #[allow(non_snake_case)] - pub fn #create_publisher(&mut self, topic: &Box<#topic_name>, qos: u32) -> eyre::Result> { - let subscription = self.0.create_subscription(&topic.0, None)?; // TODO - Ok(Box::new(#publisher_name(subscription))) + impl #publisher_name { + fn #publish(&mut self, message: ffi::#struct_raw_name) -> eyre::Result<()> { + use eyre::Context; + self.0.publish(message).context("publish failed") } } From c01880f70ac055a3482d8794eae9f67d104b9829 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 12 Jan 2024 15:33:02 +0100 Subject: [PATCH 15/32] Add support for setting QoS from C++ --- .../c++-ros2-dataflow/node-rust-api/main.cc | 10 +- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 114 ++++++++++++++++++ .../ros2-bridge/msg-gen/src/types/message.rs | 13 +- 3 files changed, 128 insertions(+), 9 deletions(-) diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index 11bdf3f6f..e77a22b3b 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -8,10 +8,16 @@ int main() { std::cout << "HELLO FROM C++" << std::endl; + auto qos = qos_default(); + qos.durability = Ros2Durability::Volatile; + qos.liveliness = Ros2Liveliness::Automatic; + qos.reliable = true; + qos.max_blocking_time = 0.1; + auto ros2_context = init_ros2_context(); auto node = ros2_context->new_node("/ros2_demo", "turtle_teleop"); - auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", 0); - auto vel_publisher = node->create_publisher(vel_topic, 0); + auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos); + auto vel_publisher = node->create_publisher(vel_topic, qos); geometry_msgs::Twist twist = { .linear = {.x = 1, .y = 0, .z = 0}, diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index f7f3495c2..eaea46df5 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -51,8 +51,37 @@ where type Ros2Node; fn init_ros2_context() -> Result>; fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; + fn qos_default() -> Ros2QosPolicies; #(#message_topic_defs)* } + + #[derive(Debug, Clone)] + pub struct Ros2QosPolicies { + pub durability: Ros2Durability, + pub liveliness: Ros2Liveliness, + pub lease_duration: f64, + pub reliable: bool, + pub max_blocking_time: f64, + pub keep_all: bool, + pub keep_last: i32, + } + + /// DDS 2.2.3.4 DURABILITY + #[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] + pub enum Ros2Durability { + Volatile, + TransientLocal, + Transient, + Persistent, + } + + /// DDS 2.2.3.11 LIVELINESS + #[derive(Copy, Clone, Debug, PartialEq)] + pub enum Ros2Liveliness { + Automatic, + ManualByParticipant, + ManualByTopic, + } }, quote! { struct Ros2Context(ros2_client::Context); @@ -71,6 +100,91 @@ where } struct Ros2Node(ros2_client::Node); + + fn qos_default() -> ffi::Ros2QosPolicies { + ffi::Ros2QosPolicies::new(None, None, None, None, None, None, None) + } + + impl ffi::Ros2QosPolicies { + pub fn new( + durability: Option, + liveliness: Option, + reliable: Option, + keep_all: Option, + lease_duration: Option, + max_blocking_time: Option, + keep_last: Option, + ) -> Self { + Self { + durability: durability.unwrap_or(ffi::Ros2Durability::Volatile), + liveliness: liveliness.unwrap_or(ffi::Ros2Liveliness::Automatic), + lease_duration: lease_duration.unwrap_or(f64::INFINITY), + reliable: reliable.unwrap_or(false), + max_blocking_time: max_blocking_time.unwrap_or(0.0), + keep_all: keep_all.unwrap_or(false), + keep_last: keep_last.unwrap_or(1), + } + } + } + + impl From for rustdds::QosPolicies { + fn from(value: ffi::Ros2QosPolicies) -> Self { + rustdds::QosPolicyBuilder::new() + .durability(value.durability.into()) + .liveliness(value.liveliness.convert(value.lease_duration)) + .reliability(if value.reliable { + rustdds::policy::Reliability::Reliable { + max_blocking_time: rustdds::Duration::from_frac_seconds( + value.max_blocking_time, + ), + } + } else { + rustdds::policy::Reliability::BestEffort + }) + .history(if value.keep_all { + rustdds::policy::History::KeepAll + } else { + rustdds::policy::History::KeepLast { + depth: value.keep_last, + } + }) + .build() + } + } + + + + impl From for rustdds::policy::Durability { + fn from(value: ffi::Ros2Durability) -> Self { + match value { + ffi::Ros2Durability::Volatile => rustdds::policy::Durability::Volatile, + ffi::Ros2Durability::TransientLocal => rustdds::policy::Durability::TransientLocal, + ffi::Ros2Durability::Transient => rustdds::policy::Durability::Transient, + ffi::Ros2Durability::Persistent => rustdds::policy::Durability::Persistent, + _ => unreachable!(), // required because enums are represented as integers in bridge + } + } + } + + + impl ffi::Ros2Liveliness { + fn convert(self, lease_duration: f64) -> rustdds::policy::Liveliness { + let lease_duration = if lease_duration.is_infinite() { + rustdds::Duration::INFINITE + } else { + rustdds::Duration::from_frac_seconds(lease_duration) + }; + match self { + ffi::Ros2Liveliness::Automatic => rustdds::policy::Liveliness::Automatic { lease_duration }, + ffi::Ros2Liveliness::ManualByParticipant => { + rustdds::policy::Liveliness::ManualByParticipant { lease_duration } + } + ffi::Ros2Liveliness::ManualByTopic => rustdds::policy::Liveliness::ManualByTopic { lease_duration }, + _ => unreachable!(), // required because enums are represented as integers in bridge + } + } + } + }, ) } else { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 5a320e0a6..4e363c6c8 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -201,9 +201,9 @@ impl Message { #[cxx_name = #cxx_topic_name] type #topic_name; #[cxx_name = #cxx_create_topic] - fn #create_topic(self: &Ros2Node, name_space: &str, base_name: &str, qos: u32) -> Result>; + fn #create_topic(self: &Ros2Node, name_space: &str, base_name: &str, qos: Ros2QosPolicies) -> Result>; #[cxx_name = #cxx_create_publisher] - fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: u32) -> Result>; + fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies) -> Result>; #[namespace = #package_name] #[cxx_name = #cxx_publisher_name] @@ -218,17 +218,16 @@ impl Message { impl Ros2Node { #[allow(non_snake_case)] - pub fn #create_topic(&self, name_space: &str, base_name: &str, qos: u32) -> eyre::Result> { + pub fn #create_topic(&self, name_space: &str, base_name: &str, qos: ffi::Ros2QosPolicies) -> eyre::Result> { let name = ros2_client::Name::new(name_space, base_name).map_err(|e| eyre::eyre!(e))?; let type_name = ros2_client::MessageTypeName::new(#package_name, #self_name); - let qos = Default::default(); // TODO - let topic = self.0.create_topic(&name, type_name, &qos)?; + let topic = self.0.create_topic(&name, type_name, &qos.into())?; Ok(Box::new(#topic_name(topic))) } #[allow(non_snake_case)] - pub fn #create_publisher(&mut self, topic: &Box<#topic_name>, qos: u32) -> eyre::Result> { - let publisher = self.0.create_publisher(&topic.0, None)?; // TODO + pub fn #create_publisher(&mut self, topic: &Box<#topic_name>, qos: ffi::Ros2QosPolicies) -> eyre::Result> { + let publisher = self.0.create_publisher(&topic.0, Some(qos.into()))?; Ok(Box::new(#publisher_name(publisher))) } } From 3a62f54daa4f7cec78a5f02084b1c2ddfbdcb01e Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 12 Jan 2024 15:53:47 +0100 Subject: [PATCH 16/32] Update c++ ros2 example to trigger random movements --- examples/c++-ros2-dataflow/dataflow.yml | 2 +- .../c++-ros2-dataflow/node-rust-api/main.cc | 37 +++++++++---------- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/examples/c++-ros2-dataflow/dataflow.yml b/examples/c++-ros2-dataflow/dataflow.yml index 16f8a5e66..e76f6d88d 100644 --- a/examples/c++-ros2-dataflow/dataflow.yml +++ b/examples/c++-ros2-dataflow/dataflow.yml @@ -3,6 +3,6 @@ nodes: custom: source: build/node_rust_api inputs: - tick: dora/timer/millis/300 + tick: dora/timer/millis/500 outputs: - counter diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index e77a22b3b..4b38c30ac 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -3,6 +3,7 @@ #include #include +#include int main() { @@ -19,18 +20,15 @@ int main() auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos); auto vel_publisher = node->create_publisher(vel_topic, qos); - geometry_msgs::Twist twist = { - .linear = {.x = 1, .y = 0, .z = 0}, - .angular = {.x = 0, .y = 0, .z = 0.5}}; - - unsigned char counter = 0; + std::random_device dev; + std::default_random_engine gen(dev()); + std::uniform_real_distribution<> dist(0., 1.); auto dora_node = init_dora_node(); + auto received_ticks = 0; - for (int i = 0; i < 20; i++) + for (int i = 0; i < 1000; i++) { - vel_publisher->publish(twist); - auto event = next_event(dora_node.events); auto ty = event_type(event); @@ -41,25 +39,24 @@ int main() else if (ty == DoraEventType::Input) { auto input = event_as_input(std::move(event)); + received_ticks += 1; - counter += 1; + std::cout << "Received input " << std::string(input.id) << std::endl; - std::cout << "Received input " << std::string(input.id) << " (counter: " << (unsigned int)counter << ")" << std::endl; - - std::vector out_vec{counter}; - rust::Slice out_slice{out_vec.data(), out_vec.size()}; - auto result = send_output(dora_node.send_output, "counter", out_slice); - auto error = std::string(result.error); - if (!error.empty()) - { - std::cerr << "Error: " << error << std::endl; - return -1; - } + geometry_msgs::Twist twist = { + .linear = {.x = dist(gen) + 1, .y = 0, .z = 0}, + .angular = {.x = 0, .y = 0, .z = (dist(gen) - 0.5) * 5.0}}; + vel_publisher->publish(twist); } else { std::cerr << "Unknown event type " << static_cast(ty) << std::endl; } + + if (received_ticks > 20) + { + break; + } } std::cout << "GOODBYE FROM C++ node (using Rust API)" << std::endl; From cf7e2b658a914f54a3007029cf7a7fe191377f64 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Fri, 12 Jan 2024 15:54:35 +0100 Subject: [PATCH 17/32] Hide method name warnings in generated publish code --- libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 4e363c6c8..db9cacf49 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -236,6 +236,7 @@ impl Message { pub struct #publisher_name(ros2_client::Publisher); impl #publisher_name { + #[allow(non_snake_case)] fn #publish(&mut self, message: ffi::#struct_raw_name) -> eyre::Result<()> { use eyre::Context; self.0.publish(message).context("publish failed") From 2bb519347fe18526c1e12aca5efafa1f3ddf62c9 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Thu, 18 Jan 2024 17:01:17 +0100 Subject: [PATCH 18/32] Fix ros2-bridge build without `generate-messages` feature again --- libraries/extensions/ros2-bridge/build.rs | 6 +++++- libraries/extensions/ros2-bridge/src/lib.rs | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/extensions/ros2-bridge/build.rs b/libraries/extensions/ros2-bridge/build.rs index 44a948957..260d317d7 100644 --- a/libraries/extensions/ros2-bridge/build.rs +++ b/libraries/extensions/ros2-bridge/build.rs @@ -1,7 +1,11 @@ -use rust_format::Formatter; use std::path::PathBuf; +#[cfg(not(feature = "generate-messages"))] +fn main() {} + +#[cfg(feature = "generate-messages")] fn main() { + use rust_format::Formatter; let create_cxx_bridge = cfg!(feature = "cxx-bridge"); let paths = ament_prefix_paths(); let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), create_cxx_bridge); diff --git a/libraries/extensions/ros2-bridge/src/lib.rs b/libraries/extensions/ros2-bridge/src/lib.rs index 85fcf1163..da41c5211 100644 --- a/libraries/extensions/ros2-bridge/src/lib.rs +++ b/libraries/extensions/ros2-bridge/src/lib.rs @@ -1,6 +1,7 @@ pub use ros2_client; pub use rustdds; +#[cfg(feature = "generate-messages")] pub mod messages { include!(env!("MESSAGES_PATH")); } From 20434e6492fc3cabe2325b4de973330aa3f4e139 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 24 Jan 2024 18:23:18 +0100 Subject: [PATCH 19/32] Move c++ ros2 bridge generation to node API crate to allow stream merging Stream merging based on the cxx crate requires the bridge and the api to share an `ExternalEvents` type, which is only possible if the two bridge modules are defined in the same crate. --- Cargo.lock | 36 ++-- Cargo.toml | 1 + apis/c++/node/Cargo.toml | 19 ++- apis/c++/node/build.rs | 154 ++++++++++++------ apis/c++/node/src/lib.rs | 46 +++++- libraries/extensions/ros2-bridge/Cargo.toml | 5 +- libraries/extensions/ros2-bridge/build.rs | 3 +- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 14 ++ .../ros2-bridge/msg-gen/src/types/message.rs | 42 +++++ 9 files changed, 246 insertions(+), 74 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 20942d39a..e9ab1d471 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -404,7 +404,7 @@ dependencies = [ "async-task", "concurrent-queue", "fastrand 2.0.1", - "futures-lite", + "futures-lite 1.13.0", "slab", ] @@ -419,7 +419,7 @@ dependencies = [ "async-io", "async-lock", "blocking", - "futures-lite", + "futures-lite 1.13.0", "once_cell", "tokio", ] @@ -434,7 +434,7 @@ dependencies = [ "autocfg", "cfg-if 1.0.0", "concurrent-queue", - "futures-lite", + "futures-lite 1.13.0", "log", "parking", "polling", @@ -465,7 +465,7 @@ dependencies = [ "blocking", "cfg-if 1.0.0", "event-listener 2.5.3", - "futures-lite", + "futures-lite 1.13.0", "rustix 0.37.25", "signal-hook", "windows-sys 0.48.0", @@ -498,7 +498,7 @@ dependencies = [ "futures-channel", "futures-core", "futures-io", - "futures-lite", + "futures-lite 1.13.0", "gloo-timers", "kv-log-macro", "log", @@ -747,7 +747,7 @@ dependencies = [ "async-task", "fastrand 2.0.1", "futures-io", - "futures-lite", + "futures-lite 1.13.0", "piper", "tracing", ] @@ -1605,7 +1605,12 @@ dependencies = [ "cxx-build", "dora-node-api", "dora-ros2-bridge", + "dora-ros2-bridge-msg-gen", "eyre", + "futures-lite 2.2.0", + "rust-format", + "serde", + "serde-big-array", ] [[package]] @@ -1697,8 +1702,6 @@ name = "dora-ros2-bridge" version = "0.1.0" dependencies = [ "array-init", - "cxx", - "cxx-build", "dora-daemon", "dora-ros2-bridge-msg-gen", "eyre", @@ -2200,6 +2203,19 @@ dependencies = [ "waker-fn", ] +[[package]] +name = "futures-lite" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "445ba825b27408685aaecefd65178908c36c6e96aaf6d8599419d46e624192ba" +dependencies = [ + "fastrand 2.0.1", + "futures-core", + "futures-io", + "parking", + "pin-project-lite", +] + [[package]] name = "futures-macro" version = "0.3.28" @@ -3849,9 +3865,9 @@ checksum = "b15813163c1d831bf4a13c3610c05c0d03b39feb07f7e09fa234dac9b15aaf39" [[package]] name = "parking" -version = "2.1.1" +version = "2.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e52c774a4c39359c1d1c52e43f73dd91a75a614652c825408eec30c95a9b2067" +checksum = "bb813b8af86854136c6922af0598d719255ecb2179515e6e7730d468f05c9cae" [[package]] name = "parking_lot" diff --git a/Cargo.toml b/Cargo.toml index a23322947..1760ac2f9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -58,6 +58,7 @@ dora-runtime = { version = "0.3.0", path = "binaries/runtime" } dora-daemon = { version = "0.3.0", path = "binaries/daemon" } dora-coordinator = { version = "0.3.0", path = "binaries/coordinator" } dora-ros2-bridge = { path = "libraries/extensions/ros2-bridge" } +dora-ros2-bridge-msg-gen = { path = "libraries/extensions/ros2-bridge/msg-gen" } dora-ros2-bridge-python = { path = "libraries/extensions/ros2-bridge/python" } arrow = "48.0.0" arrow-schema = "48.0.0" diff --git a/apis/c++/node/Cargo.toml b/apis/c++/node/Cargo.toml index 7e75906f3..53c0016ae 100644 --- a/apis/c++/node/Cargo.toml +++ b/apis/c++/node/Cargo.toml @@ -14,15 +14,26 @@ crate-type = ["staticlib"] [features] default = ["tracing"] tracing = ["dora-node-api/tracing"] -ros2-bridge = ["dep:dora-ros2-bridge"] +ros2-bridge = [ + "dep:dora-ros2-bridge", + "dep:dora-ros2-bridge-msg-gen", + "dep:rust-format", + "dep:serde", + "dep:serde-big-array", +] [dependencies] cxx = "1.0.73" dora-node-api = { workspace = true } eyre = "0.6.8" -dora-ros2-bridge = { workspace = true, optional = true, features = [ - "cxx-bridge", -] } +dora-ros2-bridge = { workspace = true, optional = true } +futures-lite = { version = "2.2" } +serde = { version = "1.0.164", features = ["derive"], optional = true } +serde-big-array = { version = "0.5.1", optional = true } [build-dependencies] cxx-build = "1.0.73" +dora-ros2-bridge-msg-gen = { workspace = true, optional = true } +rust-format = { version = "0.3.4", features = [ + "pretty_please", +], optional = true } diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index fad587819..e96aa8ef1 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -1,64 +1,118 @@ use std::path::{Component, Path, PathBuf}; fn main() { - let _build = cxx_build::bridge("src/lib.rs"); + let mut bridge_files = vec![PathBuf::from("src/lib.rs")]; + #[cfg(feature = "ros2-bridge")] + bridge_files.push(ros2::generate()); + + let _build = cxx_build::bridges(dbg!(&bridge_files)); println!("cargo:rerun-if-changed=src/lib.rs"); - if cfg!(feature = "ros2-bridge") { - generate_ros2_message_header(); - } + #[cfg(feature = "ros2-bridge")] + ros2::generate_ros2_message_header(bridge_files.last().unwrap()); + + // to avoid unnecessary `mut`` warning + bridge_files.clear(); } -fn generate_ros2_message_header() { - let prefix = std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_PREFIX").unwrap(); - let include_dir = PathBuf::from(std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_DIR0").unwrap()); - let _crate_dir = std::env::var("DEP_DORA_ROS2_BRIDGE_CXXBRIDGE_DIR1").unwrap(); +#[cfg(feature = "ros2-bridge")] +mod ros2 { + use std::path::{Component, Path, PathBuf}; - let header_path = include_dir - .join(&prefix) - .join(local_relative_path(&include_dir)) - .ancestors() - .nth(2) - .unwrap() - .join("messages.rs.h"); - let code_path = include_dir - .parent() - .unwrap() - .join("sources") - .join(&prefix) - .join(local_relative_path(&include_dir)) - .ancestors() - .nth(2) - .unwrap() - .join("messages.rs.cc"); + pub fn generate() -> PathBuf { + use rust_format::Formatter; + let paths = ament_prefix_paths(); + let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), true); + let generated_string = rust_format::PrettyPlease::default() + .format_tokens(generated) + .unwrap(); + let out_dir = PathBuf::from(std::env::var("OUT_DIR").unwrap()); + let target_file = out_dir.join("ros2_bindings.rs"); + std::fs::write(&target_file, generated_string).unwrap(); + println!( + "cargo:rustc-env=ROS2_BINDINGS_PATH={}", + target_file.display() + ); - // copy message files to target directory - let root = Path::new(env!("CARGO_MANIFEST_DIR")) - .ancestors() - .nth(3) - .unwrap(); - let target_path = root - .join("target") - .join("cxxbridge") - .join("dora-node-api-cxx") - .join("src") - .join("messages.rs.h"); + target_file + } - std::fs::copy(&header_path, &target_path).unwrap(); - println!("cargo:rerun-if-changed={}", header_path.display()); - std::fs::copy(&code_path, target_path.with_file_name("messages.rs.cc")).unwrap(); - println!("cargo:rerun-if-changed={}", code_path.display()); -} + fn ament_prefix_paths() -> Vec { + let ament_prefix_path: String = match std::env::var("AMENT_PREFIX_PATH") { + Ok(path) => path, + Err(std::env::VarError::NotPresent) => { + println!("cargo:warning='AMENT_PREFIX_PATH not set'"); + String::new() + } + Err(std::env::VarError::NotUnicode(s)) => { + panic!( + "AMENT_PREFIX_PATH is not valid unicode: `{}`", + s.to_string_lossy() + ); + } + }; + println!("cargo:rerun-if-env-changed=AMENT_PREFIX_PATH"); + + let paths: Vec<_> = ament_prefix_path.split(':').map(PathBuf::from).collect(); + for path in &paths { + println!("cargo:rerun-if-changed={}", path.display()); + } + + paths + } + + pub fn generate_ros2_message_header(source_file: &Path) { + let out_dir = source_file.parent().unwrap(); + let relative_path = local_relative_path(&source_file) + .ancestors() + .nth(2) + .unwrap() + .join("out"); + let header_path = out_dir + .join("cxxbridge") + .join("include") + .join("dora-node-api-cxx") + .join(&relative_path) + .join("ros2_bindings.rs.h"); + let code_path = out_dir + .join("cxxbridge") + .join("sources") + .join("dora-node-api-cxx") + .join(&relative_path) + .join("ros2_bindings.rs.cc"); + + // copy message files to target directory + let root = Path::new(env!("CARGO_MANIFEST_DIR")) + .ancestors() + .nth(3) + .unwrap(); + let target_path = root + .join("target") + .join("cxxbridge") + .join("dora-node-api-cxx") + .join("src") + .join("ros2_bindings.rs.h"); + + std::fs::copy(dbg!(&header_path), dbg!(&target_path)).unwrap(); + println!("cargo:rerun-if-changed={}", header_path.display()); + std::fs::copy( + &code_path, + target_path.with_file_name("ros2_bindings.rs.cc"), + ) + .unwrap(); + println!("cargo:rerun-if-changed={}", code_path.display()); + } -// copy from cxx-build source -fn local_relative_path(path: &Path) -> PathBuf { - let mut rel_path = PathBuf::new(); - for component in path.components() { - match component { - Component::Prefix(_) | Component::RootDir | Component::CurDir => {} - Component::ParentDir => drop(rel_path.pop()), // noop if empty - Component::Normal(name) => rel_path.push(name), + // copy from cxx-build source + fn local_relative_path(path: &Path) -> PathBuf { + let mut rel_path = PathBuf::new(); + for component in path.components() { + match component { + Component::Prefix(_) | Component::RootDir | Component::CurDir => {} + Component::ParentDir => drop(rel_path.pop()), // noop if empty + Component::Normal(name) => rel_path.push(name), + } } + rel_path } - rel_path } diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index 6f6e5d599..b25d91402 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -1,12 +1,24 @@ +use std::any::Any; + use dora_node_api::{ self, arrow::array::{AsArray, BinaryArray}, + merged::{MergeExternal, MergedEvent}, Event, EventStream, }; use eyre::bail; +use futures_lite::Stream; #[cfg(feature = "ros2-bridge")] -pub use dora_ros2_bridge as ros2; +use dora_ros2_bridge::_core; +#[cfg(feature = "ros2-bridge")] +pub use ros2::ExternalEvents; + +#[cfg(feature = "ros2-bridge")] +pub mod ros2 { + pub use dora_ros2_bridge::*; + include!(env!("ROS2_BINDINGS_PATH")); +} #[cxx::bridge] #[allow(clippy::needless_lifetimes)] @@ -34,14 +46,21 @@ mod ffi { error: String, } + extern "C++" { + #[cfg(feature = "ros2-bridge")] + type ExternalEvents = crate::ros2::ExternalEvents; + } + extern "Rust" { type Events; + // type ExternalEvents; + type MergedEvents; type OutputSender; type DoraEvent; fn init_dora_node() -> Result; - fn next_event(inputs: &mut Box) -> Box; + fn next(self: &mut Events) -> Box; fn event_type(event: &Box) -> DoraEventType; fn event_as_input(event: Box) -> Result; fn send_output( @@ -49,6 +68,9 @@ mod ffi { id: String, data: &[u8], ) -> DoraResult; + + #[cfg(feature = "ros2-bridge")] + fn merge_events(dora: Box, external: Box) -> Box; } } @@ -65,10 +87,26 @@ fn init_dora_node() -> eyre::Result { pub struct Events(EventStream); -fn next_event(events: &mut Box) -> Box { - Box::new(DoraEvent(events.0.recv())) +impl Events { + fn next(&mut self) -> Box { + Box::new(DoraEvent(self.0.recv())) + } } +#[cfg(feature = "ros2-bridge")] +#[allow(clippy::boxed_local)] +pub fn merge_events( + dora_events: Box, + external: Box, +) -> Box { + let merge_external = dora_events + .0 + .merge_external(external.events.0.as_event_stream()); + Box::new(MergedEvents(merge_external)) +} + +pub struct MergedEvents(Box>> + Unpin>); + pub struct DoraEvent(Option); fn event_type(event: &DoraEvent) -> ffi::DoraEventType { diff --git a/libraries/extensions/ros2-bridge/Cargo.toml b/libraries/extensions/ros2-bridge/Cargo.toml index 28333d19f..20b2d03f6 100644 --- a/libraries/extensions/ros2-bridge/Cargo.toml +++ b/libraries/extensions/ros2-bridge/Cargo.toml @@ -9,7 +9,6 @@ links = "dora-ros2-bridge" [features] default = ["generate-messages"] generate-messages = ["dep:dora-ros2-bridge-msg-gen", "dep:rust-format"] -cxx-bridge = ["dep:cxx", "dep:cxx-build", "dep:eyre"] # enables examples that depend on a sourced ROS2 installation ros2-examples = ["dep:eyre", "tokio", "dora-daemon"] @@ -25,15 +24,13 @@ tokio = { version = "1.29.1", features = ["full"], optional = true } dora-daemon = { path = "../../../binaries/daemon", optional = true } tracing = "0.1.37" tracing-subscriber = "0.3.17" -cxx = { version = "1.0", optional = true } [dev-dependencies] rand = "0.8.5" futures = { version = "0.3.28", default-features = false } [build-dependencies] -dora-ros2-bridge-msg-gen = { path = "msg-gen", optional = true } +dora-ros2-bridge-msg-gen = { workspace = true, optional = true } rust-format = { version = "0.3.4", features = [ "pretty_please", ], optional = true } -cxx-build = { version = "1.0.73", optional = true } diff --git a/libraries/extensions/ros2-bridge/build.rs b/libraries/extensions/ros2-bridge/build.rs index 260d317d7..dc3a701e3 100644 --- a/libraries/extensions/ros2-bridge/build.rs +++ b/libraries/extensions/ros2-bridge/build.rs @@ -6,9 +6,8 @@ fn main() {} #[cfg(feature = "generate-messages")] fn main() { use rust_format::Formatter; - let create_cxx_bridge = cfg!(feature = "cxx-bridge"); let paths = ament_prefix_paths(); - let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), create_cxx_bridge); + let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), false); let generated_string = rust_format::PrettyPlease::default() .format_tokens(generated) .unwrap(); diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index eaea46df5..efc0f4d46 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -49,12 +49,17 @@ where extern "Rust" { type Ros2Context; type Ros2Node; + type ExternalRos2Events; fn init_ros2_context() -> Result>; fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; fn qos_default() -> Ros2QosPolicies; #(#message_topic_defs)* } + pub struct ExternalEvents { + events: Box, + } + #[derive(Debug, Clone)] pub struct Ros2QosPolicies { pub durability: Ros2Durability, @@ -185,6 +190,15 @@ where } } + pub use ffi::ExternalEvents; + + pub struct ExternalRos2Events( + pub Box, + ); + + pub trait AsEventStream { + fn as_event_stream(self: Box) -> Box> + Unpin>; + } }, ) } else { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index db9cacf49..92dd29282 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -190,12 +190,20 @@ impl Message { let cxx_publisher_name = format_ident!("Publisher_{}", self.name); let create_publisher = format_ident!("new__Publisher__{package_name}__{}", self.name); let cxx_create_publisher = format_ident!("create_publisher"); + let struct_raw_name = format_ident!("{package_name}__{}", self.name); let self_name = &self.name; let publish = format_ident!("publish__{package_name}__{}", self.name); let cxx_publish = format_ident!("publish"); + let subscription_name = format_ident!("Subscription__{package_name}__{}", self.name); + let cxx_subscription_name = format_ident!("Subscription_{}", self.name); + let create_subscription = format_ident!("new__Subscription__{package_name}__{}", self.name); + let cxx_create_subscription = format_ident!("create_subscription"); + let event_stream = format_ident!("event_stream__{package_name}__{}", self.name); + let cxx_event_stream = format_ident!("event_stream"); + let def = quote! { #[namespace = #package_name] #[cxx_name = #cxx_topic_name] @@ -204,6 +212,8 @@ impl Message { fn #create_topic(self: &Ros2Node, name_space: &str, base_name: &str, qos: Ros2QosPolicies) -> Result>; #[cxx_name = #cxx_create_publisher] fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies) -> Result>; + #[cxx_name = #cxx_create_subscription] + fn #create_subscription(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies) -> Result>; #[namespace = #package_name] #[cxx_name = #cxx_publisher_name] @@ -211,6 +221,14 @@ impl Message { #[namespace = #package_name] #[cxx_name = #cxx_publish] fn #publish(self: &mut #publisher_name, message: #struct_raw_name) -> Result<()>; + + #[namespace = #package_name] + #[cxx_name = #cxx_subscription_name] + type #subscription_name; + + #[namespace = #package_name] + #[cxx_name = #cxx_event_stream] + fn #event_stream(subscription: Box<#subscription_name>) -> Box; }; let imp = quote! { #[allow(non_camel_case_types)] @@ -230,6 +248,12 @@ impl Message { let publisher = self.0.create_publisher(&topic.0, Some(qos.into()))?; Ok(Box::new(#publisher_name(publisher))) } + + #[allow(non_snake_case)] + pub fn #create_subscription(&mut self, topic: &Box<#topic_name>, qos: ffi::Ros2QosPolicies) -> eyre::Result> { + let subscription = self.0.create_subscription(&topic.0, Some(qos.into()))?; + Ok(Box::new(#subscription_name(subscription))) + } } #[allow(non_camel_case_types)] @@ -243,6 +267,24 @@ impl Message { } } + #[allow(non_camel_case_types)] + pub struct #subscription_name(ros2_client::Subscription); + + #[allow(non_snake_case)] + fn #event_stream(subscription: Box<#subscription_name>) -> Box { + Box::new(ExternalEvents { events: Box::new(ExternalRos2Events(subscription)) }) + } + + impl AsEventStream for #subscription_name { + fn as_event_stream(self: Box) -> Box> + Unpin> { + let stream = futures_lite::stream::unfold(self.0, |sub| async { + let item = sub.async_take().await; + let item_boxed: Box = Box::new(item); + Some((item_boxed, sub)) + }); + Box::new(Box::pin(stream)) + } + } }; (def, imp) } From 0d75f5391f4ed5eafdda6c4d9c5be321872fbccb Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 7 Feb 2024 15:28:34 +0100 Subject: [PATCH 20/32] First implementation of subscription stream merging and downcasting --- apis/c++/node/build.rs | 31 ++++- apis/c++/node/src/lib.rs | 129 ++++++++++++++---- examples/c++-ros2-dataflow/dataflow.yml | 2 +- .../c++-ros2-dataflow/node-rust-api/main.cc | 68 ++++++--- examples/c++-ros2-dataflow/run.rs | 26 ++-- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 8 ++ .../ros2-bridge/msg-gen/src/types/message.rs | 21 +++ 7 files changed, 213 insertions(+), 72 deletions(-) diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index e96aa8ef1..d60150d07 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -1,17 +1,35 @@ -use std::path::{Component, Path, PathBuf}; +use std::path::{Path, PathBuf}; fn main() { let mut bridge_files = vec![PathBuf::from("src/lib.rs")]; #[cfg(feature = "ros2-bridge")] bridge_files.push(ros2::generate()); - let _build = cxx_build::bridges(dbg!(&bridge_files)); + let _build = cxx_build::bridges(&bridge_files); println!("cargo:rerun-if-changed=src/lib.rs"); + // rename header files + let root = Path::new(env!("CARGO_MANIFEST_DIR")) + .ancestors() + .nth(3) + .unwrap(); + let src_dir = root + .join("target") + .join("cxxbridge") + .join("dora-node-api-cxx") + .join("src"); + let target_dir = src_dir.parent().unwrap(); + std::fs::copy(src_dir.join("lib.rs.h"), target_dir.join("dora-node-api.h")).unwrap(); + std::fs::copy( + src_dir.join("lib.rs.cc"), + target_dir.join("dora-node-api.cc"), + ) + .unwrap(); + #[cfg(feature = "ros2-bridge")] ros2::generate_ros2_message_header(bridge_files.last().unwrap()); - // to avoid unnecessary `mut`` warning + // to avoid unnecessary `mut` warning bridge_files.clear(); } @@ -90,14 +108,13 @@ mod ros2 { .join("target") .join("cxxbridge") .join("dora-node-api-cxx") - .join("src") - .join("ros2_bindings.rs.h"); + .join("dora-ros2-bindings.h"); - std::fs::copy(dbg!(&header_path), dbg!(&target_path)).unwrap(); + std::fs::copy(&header_path, &target_path).unwrap(); println!("cargo:rerun-if-changed={}", header_path.display()); std::fs::copy( &code_path, - target_path.with_file_name("ros2_bindings.rs.cc"), + target_path.with_file_name("dora-ros2-bindings.cc"), ) .unwrap(); println!("cargo:rerun-if-changed={}", code_path.display()); diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index b25d91402..d4c755ea2 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -3,22 +3,13 @@ use std::any::Any; use dora_node_api::{ self, arrow::array::{AsArray, BinaryArray}, - merged::{MergeExternal, MergedEvent}, + merged::MergedEvent, Event, EventStream, }; use eyre::bail; -use futures_lite::Stream; #[cfg(feature = "ros2-bridge")] use dora_ros2_bridge::_core; -#[cfg(feature = "ros2-bridge")] -pub use ros2::ExternalEvents; - -#[cfg(feature = "ros2-bridge")] -pub mod ros2 { - pub use dora_ros2_bridge::*; - include!(env!("ROS2_BINDINGS_PATH")); -} #[cxx::bridge] #[allow(clippy::needless_lifetimes)] @@ -47,16 +38,18 @@ mod ffi { } extern "C++" { - #[cfg(feature = "ros2-bridge")] + #[allow(dead_code)] type ExternalEvents = crate::ros2::ExternalEvents; + #[allow(dead_code)] + type Ros2Event = crate::ros2::Ros2Event; } extern "Rust" { type Events; - // type ExternalEvents; type MergedEvents; type OutputSender; type DoraEvent; + type MergedDoraEvent; fn init_dora_node() -> Result; @@ -69,8 +62,32 @@ mod ffi { data: &[u8], ) -> DoraResult; - #[cfg(feature = "ros2-bridge")] fn merge_events(dora: Box, external: Box) -> Box; + fn next(self: &mut MergedEvents) -> Box; + + fn is_ros2(event: &Box) -> bool; + fn downcast_ros2(event: Box) -> Result>; + fn is_dora(event: &Box) -> bool; + fn downcast_dora(event: Box) -> Result>; + } +} + +#[cfg(feature = "ros2-bridge")] +pub mod ros2 { + pub use dora_ros2_bridge::*; + include!(env!("ROS2_BINDINGS_PATH")); +} + +/// Dummy placeholder. +#[cfg(not(feature = "ros2-bridge"))] +#[cxx::bridge] +#[allow(clippy::needless_lifetimes)] +mod ros2 { + pub struct ExternalEvents { + dummy: u8, + } + pub struct Ros2Event { + dummy: u8, } } @@ -93,20 +110,6 @@ impl Events { } } -#[cfg(feature = "ros2-bridge")] -#[allow(clippy::boxed_local)] -pub fn merge_events( - dora_events: Box, - external: Box, -) -> Box { - let merge_external = dora_events - .0 - .merge_external(external.events.0.as_event_stream()); - Box::new(MergedEvents(merge_external)) -} - -pub struct MergedEvents(Box>> + Unpin>); - pub struct DoraEvent(Option); fn event_type(event: &DoraEvent) -> ffi::DoraEventType { @@ -147,3 +150,75 @@ fn send_output(sender: &mut Box, id: String, data: &[u8]) -> ffi:: }; ffi::DoraResult { error } } + +#[cfg(feature = "ros2-bridge")] +#[allow(clippy::boxed_local)] +pub fn merge_events( + dora_events: Box, + external: Box, +) -> Box { + use dora_node_api::merged::MergeExternal; + + let merge_external = dora_events + .0 + .merge_external(external.events.0.as_event_stream()); + Box::new(MergedEvents(Box::new(futures_lite::stream::block_on( + merge_external, + )))) +} + +/// Dummy +#[cfg(not(feature = "ros2-bridge"))] +#[allow(clippy::boxed_local)] +pub fn merge_events( + dora_events: Box, + _external: Box, +) -> Box { + use dora_node_api::merged::MergeExternal; + + let merge_external = dora_events.0.merge_external(futures_lite::stream::empty()); + Box::new(MergedEvents(Box::new(futures_lite::stream::block_on( + merge_external, + )))) +} + +pub struct MergedEvents(Box>> + Unpin>); + +impl MergedEvents { + fn next(&mut self) -> Box { + let event = self.0.next(); + Box::new(MergedDoraEvent(event)) + } +} + +pub struct MergedDoraEvent(Option>>); + +fn is_ros2(event: &Box) -> bool { + match event.0 { + Some(MergedEvent::External(_)) => true, + _ => false, + } +} + +fn downcast_ros2(event: Box) -> eyre::Result> { + match event.0 { + Some(MergedEvent::External(event)) => Ok(Box::new(ros2::Ros2Event { + event: Box::new(ros2::ExternalRos2Event(event)), + })), + _ => eyre::bail!("not an external event"), + } +} + +fn is_dora(event: &Box) -> bool { + match event.0 { + Some(MergedEvent::Dora(_)) => true, + _ => false, + } +} + +fn downcast_dora(event: Box) -> eyre::Result> { + match event.0 { + Some(MergedEvent::Dora(event)) => Ok(Box::new(DoraEvent(Some(event)))), + _ => eyre::bail!("not an external event"), + } +} diff --git a/examples/c++-ros2-dataflow/dataflow.yml b/examples/c++-ros2-dataflow/dataflow.yml index e76f6d88d..7639c6036 100644 --- a/examples/c++-ros2-dataflow/dataflow.yml +++ b/examples/c++-ros2-dataflow/dataflow.yml @@ -5,4 +5,4 @@ nodes: inputs: tick: dora/timer/millis/500 outputs: - - counter + - pose diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index 4b38c30ac..9e7a107ac 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -1,5 +1,5 @@ #include "../build/dora-node-api.h" -#include "../build/messages.h" +#include "../build/dora-ros2-bindings.h" #include #include @@ -19,43 +19,69 @@ int main() auto node = ros2_context->new_node("/ros2_demo", "turtle_teleop"); auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos); auto vel_publisher = node->create_publisher(vel_topic, qos); + auto pose_topic = node->create_topic_turtlesim_Pose("/turtle1", "pose", qos); + auto pose_subscription = node->create_subscription(pose_topic, qos); std::random_device dev; std::default_random_engine gen(dev()); std::uniform_real_distribution<> dist(0., 1.); auto dora_node = init_dora_node(); + + std::cout << "MERGING EVENTS" << std::endl; + auto merged_events = merge_events(std::move(dora_node.events), event_stream(std::move(pose_subscription))); + std::cout << "MERGED EVENTS" << std::endl; + auto received_ticks = 0; for (int i = 0; i < 1000; i++) { - auto event = next_event(dora_node.events); - auto ty = event_type(event); + auto event = merged_events->next(); - if (ty == DoraEventType::AllInputsClosed) + if (is_ros2(event)) { - break; + auto ros2_event = downcast_ros2(std::move(event)); + if (turtlesim::is_Pose(ros2_event)) + { + auto pose = turtlesim::downcast_Pose(std::move(ros2_event)); + std::cout << "Received Pose { x: " << pose->x << ", y: " << pose->y << " }" << std::endl; + } + else + { + std::cout << "received unexpected ros2 input" << std::endl; + } } - else if (ty == DoraEventType::Input) + else if (is_dora(event)) { - auto input = event_as_input(std::move(event)); - received_ticks += 1; + auto dora_event = downcast_dora(std::move(event)); - std::cout << "Received input " << std::string(input.id) << std::endl; + auto ty = event_type(dora_event); - geometry_msgs::Twist twist = { - .linear = {.x = dist(gen) + 1, .y = 0, .z = 0}, - .angular = {.x = 0, .y = 0, .z = (dist(gen) - 0.5) * 5.0}}; - vel_publisher->publish(twist); - } - else - { - std::cerr << "Unknown event type " << static_cast(ty) << std::endl; - } + if (ty == DoraEventType::AllInputsClosed) + { + break; + } + else if (ty == DoraEventType::Input) + { + auto input = event_as_input(std::move(dora_event)); + received_ticks += 1; - if (received_ticks > 20) - { - break; + std::cout << "Received input " << std::string(input.id) << std::endl; + + geometry_msgs::Twist twist = { + .linear = {.x = dist(gen) + 1, .y = 0, .z = 0}, + .angular = {.x = 0, .y = 0, .z = (dist(gen) - 0.5) * 5.0}}; + vel_publisher->publish(twist); + } + else + { + std::cerr << "Unknown event type " << static_cast(ty) << std::endl; + } + + if (received_ticks > 20) + { + break; + } } } diff --git a/examples/c++-ros2-dataflow/run.rs b/examples/c++-ros2-dataflow/run.rs index c9a7505f1..5ec35bb50 100644 --- a/examples/c++-ros2-dataflow/run.rs +++ b/examples/c++-ros2-dataflow/run.rs @@ -34,28 +34,25 @@ async fn main() -> eyre::Result<()> { let build_dir = Path::new("build"); build_package("dora-node-api-cxx", &["ros2-bridge"]).await?; - let node_cxxbridge = target - .join("cxxbridge") - .join("dora-node-api-cxx") - .join("src"); + let node_cxxbridge = target.join("cxxbridge").join("dora-node-api-cxx"); tokio::fs::copy( - node_cxxbridge.join("lib.rs.cc"), - build_dir.join("node-bridge.cc"), + node_cxxbridge.join("dora-node-api.cc"), + build_dir.join("dora-node-api.cc"), ) .await?; tokio::fs::copy( - node_cxxbridge.join("lib.rs.h"), + node_cxxbridge.join("dora-node-api.h"), build_dir.join("dora-node-api.h"), ) .await?; tokio::fs::copy( - node_cxxbridge.join("messages.rs.cc"), - build_dir.join("messages.cc"), + node_cxxbridge.join("dora-ros2-bindings.cc"), + build_dir.join("dora-ros2-bindings.cc"), ) .await?; tokio::fs::copy( - node_cxxbridge.join("messages.rs.h"), - build_dir.join("messages.h"), + node_cxxbridge.join("dora-ros2-bindings.h"), + build_dir.join("dora-ros2-bindings.h"), ) .await?; @@ -63,8 +60,8 @@ async fn main() -> eyre::Result<()> { root, &[ &dunce::canonicalize(Path::new("node-rust-api").join("main.cc"))?, - &dunce::canonicalize(build_dir.join("node-bridge.cc"))?, - &dunce::canonicalize(build_dir.join("messages.cc"))?, + &dunce::canonicalize(build_dir.join("dora-ros2-bindings.cc"))?, + &dunce::canonicalize(build_dir.join("dora-node-api.cc"))?, ], "node_rust_api", &["-l", "dora_node_api_cxx"], @@ -82,9 +79,6 @@ async fn build_package(package: &str, features: &[&str]) -> eyre::Result<()> { let mut cmd = tokio::process::Command::new(&cargo); cmd.arg("build"); cmd.arg("--package").arg(package); - if !cmd.status().await?.success() { - bail!("failed to build {package}"); - }; if !features.is_empty() { cmd.arg("--features").arg(features.join(",")); } diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index efc0f4d46..be4f03969 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -50,6 +50,7 @@ where type Ros2Context; type Ros2Node; type ExternalRos2Events; + type ExternalRos2Event; fn init_ros2_context() -> Result>; fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; fn qos_default() -> Ros2QosPolicies; @@ -60,6 +61,10 @@ where events: Box, } + pub struct Ros2Event { + event: Box, + } + #[derive(Debug, Clone)] pub struct Ros2QosPolicies { pub durability: Ros2Durability, @@ -191,6 +196,7 @@ where } pub use ffi::ExternalEvents; + pub use ffi::Ros2Event; pub struct ExternalRos2Events( pub Box, @@ -199,6 +205,8 @@ where pub trait AsEventStream { fn as_event_stream(self: Box) -> Box> + Unpin>; } + + pub struct ExternalRos2Event(pub Box); }, ) } else { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 92dd29282..1ce74e16e 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -204,6 +204,11 @@ impl Message { let event_stream = format_ident!("event_stream__{package_name}__{}", self.name); let cxx_event_stream = format_ident!("event_stream"); + let is = format_ident!("is__{package_name}__{}", self.name); + let cxx_is = format_ident!("is_{}", self.name); + let downcast = format_ident!("downcast__{package_name}__{}", self.name); + let cxx_downcast = format_ident!("downcast_{}", self.name); + let def = quote! { #[namespace = #package_name] #[cxx_name = #cxx_topic_name] @@ -229,6 +234,13 @@ impl Message { #[namespace = #package_name] #[cxx_name = #cxx_event_stream] fn #event_stream(subscription: Box<#subscription_name>) -> Box; + + #[namespace = #package_name] + #[cxx_name = #cxx_is] + fn #is(event: &Box) -> bool; + #[namespace = #package_name] + #[cxx_name = #cxx_downcast] + fn #downcast(event: Box) -> Result>; }; let imp = quote! { #[allow(non_camel_case_types)] @@ -275,6 +287,15 @@ impl Message { Box::new(ExternalEvents { events: Box::new(ExternalRos2Events(subscription)) }) } + #[allow(non_snake_case)] + fn #is(event: &Box) -> bool { + event.event.0.is::() + } + #[allow(non_snake_case)] + fn #downcast(event: Box) -> eyre::Result> { + event.event.0.downcast().map_err(|_| eyre::eyre!("downcast failed")) + } + impl AsEventStream for #subscription_name { fn as_event_stream(self: Box) -> Box> + Unpin> { let stream = futures_lite::stream::unfold(self.0, |sub| async { From 0074ffd6e8e0746b1f75fab28fc6b3151d0256d5 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Tue, 13 Feb 2024 17:32:40 +0100 Subject: [PATCH 21/32] Refactor C++ ROS2 subscription API to make downcasts work Use the subscriber type for downcasting to ensure that the correct type is used. Also, store an unique ID per subscriber to differentiate subscriptions of same type after merging. --- Cargo.lock | 9 +- apis/c++/node/Cargo.toml | 2 + apis/c++/node/build.rs | 18 ++- apis/c++/node/src/lib.rs | 137 ++++++++---------- .../c++-ros2-dataflow/node-rust-api/main.cc | 37 ++--- .../extensions/ros2-bridge/msg-gen/src/lib.rs | 29 +--- .../ros2-bridge/msg-gen/src/types/message.rs | 81 ++++++----- rust-toolchain.toml | 2 +- 8 files changed, 144 insertions(+), 171 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index e9ab1d471..56486dea7 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1608,6 +1608,7 @@ dependencies = [ "dora-ros2-bridge-msg-gen", "eyre", "futures-lite 2.2.0", + "prettyplease", "rust-format", "serde", "serde-big-array", @@ -4863,9 +4864,9 @@ checksum = "ef703b7cb59335eae2eb93ceb664c0eb7ea6bf567079d843e09420219668e072" [[package]] name = "safer-ffi" -version = "0.1.3" +version = "0.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e9c1d19b288ca9898cd421c7b105fb7269918a7f8e9253a991e228981ca421ad" +checksum = "4483c5ab47f222d2c297e73a520c9003e09e2fe1f1b04edcb572e6939f303003" dependencies = [ "inventory 0.1.11", "inventory 0.3.12", @@ -4881,9 +4882,9 @@ dependencies = [ [[package]] name = "safer_ffi-proc_macros" -version = "0.1.3" +version = "0.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2d7a04caa3ca2224f5ea4ddd850e2629c3b36b2b83621f87a8303bf41020110" +checksum = "bf04ebd3786110e64269a74eea58c5564dd92a1e790c0f6f9871d6fe1b8e34db" dependencies = [ "macro_rules_attribute", "prettyplease", diff --git a/apis/c++/node/Cargo.toml b/apis/c++/node/Cargo.toml index 53c0016ae..ff27a2e88 100644 --- a/apis/c++/node/Cargo.toml +++ b/apis/c++/node/Cargo.toml @@ -18,6 +18,7 @@ ros2-bridge = [ "dep:dora-ros2-bridge", "dep:dora-ros2-bridge-msg-gen", "dep:rust-format", + "dep:prettyplease", "dep:serde", "dep:serde-big-array", ] @@ -37,3 +38,4 @@ dora-ros2-bridge-msg-gen = { workspace = true, optional = true } rust-format = { version = "0.3.4", features = [ "pretty_please", ], optional = true } +prettyplease = { version = "0.1", features = ["verbatim"], optional = true } diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index d60150d07..3a0e9c4f6 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -80,6 +80,8 @@ mod ros2 { } pub fn generate_ros2_message_header(source_file: &Path) { + use std::io::Write as _; + let out_dir = source_file.parent().unwrap(); let relative_path = local_relative_path(&source_file) .ancestors() @@ -112,12 +114,18 @@ mod ros2 { std::fs::copy(&header_path, &target_path).unwrap(); println!("cargo:rerun-if-changed={}", header_path.display()); - std::fs::copy( - &code_path, - target_path.with_file_name("dora-ros2-bindings.cc"), - ) - .unwrap(); + + let mut node_header = + std::fs::File::open(target_path.with_file_name("dora-node-api.h")).unwrap(); + let mut code_file = std::fs::File::open(&code_path).unwrap(); println!("cargo:rerun-if-changed={}", code_path.display()); + let mut code_target_file = + std::fs::File::create(target_path.with_file_name("dora-ros2-bindings.cc")).unwrap(); + + // copy both the node header and the code file to prevent import errors + std::io::copy(&mut node_header, &mut code_target_file).unwrap(); + std::io::copy(&mut code_file, &mut code_target_file).unwrap(); + code_target_file.flush().unwrap(); } // copy from cxx-build source diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index d4c755ea2..99a3b1cfd 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -3,13 +3,14 @@ use std::any::Any; use dora_node_api::{ self, arrow::array::{AsArray, BinaryArray}, - merged::MergedEvent, + merged::{MergeExternal, MergedEvent}, Event, EventStream, }; use eyre::bail; #[cfg(feature = "ros2-bridge")] use dora_ros2_bridge::_core; +use futures_lite::{Stream, StreamExt}; #[cxx::bridge] #[allow(clippy::needless_lifetimes)] @@ -37,22 +38,24 @@ mod ffi { error: String, } - extern "C++" { - #[allow(dead_code)] - type ExternalEvents = crate::ros2::ExternalEvents; - #[allow(dead_code)] - type Ros2Event = crate::ros2::Ros2Event; + pub struct CombinedEvents { + events: Box, + } + + pub struct CombinedEvent { + event: Box, } extern "Rust" { type Events; - type MergedEvents; type OutputSender; type DoraEvent; + type MergedEvents; type MergedDoraEvent; fn init_dora_node() -> Result; + fn dora_events_into_combined(events: Box) -> CombinedEvents; fn next(self: &mut Events) -> Box; fn event_type(event: &Box) -> DoraEventType; fn event_as_input(event: Box) -> Result; @@ -62,13 +65,10 @@ mod ffi { data: &[u8], ) -> DoraResult; - fn merge_events(dora: Box, external: Box) -> Box; - fn next(self: &mut MergedEvents) -> Box; + fn next(self: &mut CombinedEvents) -> CombinedEvent; - fn is_ros2(event: &Box) -> bool; - fn downcast_ros2(event: Box) -> Result>; - fn is_dora(event: &Box) -> bool; - fn downcast_dora(event: Box) -> Result>; + fn is_dora(self: &CombinedEvent) -> bool; + fn downcast_dora(event: CombinedEvent) -> Result>; } } @@ -78,19 +78,6 @@ pub mod ros2 { include!(env!("ROS2_BINDINGS_PATH")); } -/// Dummy placeholder. -#[cfg(not(feature = "ros2-bridge"))] -#[cxx::bridge] -#[allow(clippy::needless_lifetimes)] -mod ros2 { - pub struct ExternalEvents { - dummy: u8, - } - pub struct Ros2Event { - dummy: u8, - } -} - fn init_dora_node() -> eyre::Result { let (node, events) = dora_node_api::DoraNode::init_from_env()?; let events = Events(events); @@ -110,6 +97,16 @@ impl Events { } } +fn dora_events_into_combined(events: Box) -> ffi::CombinedEvents { + let events = events.0.map(MergedEvent::Dora); + ffi::CombinedEvents { + events: Box::new(MergedEvents { + events: Some(Box::new(events)), + next_id: 1, + }), + } +} + pub struct DoraEvent(Option); fn event_type(event: &DoraEvent) -> ffi::DoraEventType { @@ -151,73 +148,57 @@ fn send_output(sender: &mut Box, id: String, data: &[u8]) -> ffi:: ffi::DoraResult { error } } -#[cfg(feature = "ros2-bridge")] -#[allow(clippy::boxed_local)] -pub fn merge_events( - dora_events: Box, - external: Box, -) -> Box { - use dora_node_api::merged::MergeExternal; - - let merge_external = dora_events - .0 - .merge_external(external.events.0.as_event_stream()); - Box::new(MergedEvents(Box::new(futures_lite::stream::block_on( - merge_external, - )))) -} - -/// Dummy -#[cfg(not(feature = "ros2-bridge"))] -#[allow(clippy::boxed_local)] -pub fn merge_events( - dora_events: Box, - _external: Box, -) -> Box { - use dora_node_api::merged::MergeExternal; - - let merge_external = dora_events.0.merge_external(futures_lite::stream::empty()); - Box::new(MergedEvents(Box::new(futures_lite::stream::block_on( - merge_external, - )))) +pub struct MergedEvents { + events: Option> + Unpin>>, + next_id: u32, } -pub struct MergedEvents(Box>> + Unpin>); - impl MergedEvents { - fn next(&mut self) -> Box { - let event = self.0.next(); - Box::new(MergedDoraEvent(event)) + fn next(&mut self) -> MergedDoraEvent { + let event = futures_lite::future::block_on(self.events.as_mut().unwrap().next()); + MergedDoraEvent(event) } -} -pub struct MergedDoraEvent(Option>>); + pub fn merge(&mut self, events: impl Stream> + Unpin + 'static) -> u32 { + let id = self.next_id; + self.next_id += 1; + let events = Box::pin(events.map(move |event| ExternalEvent { event, id })); + + let inner = self.events.take().unwrap(); + let merged: Box + Unpin + 'static> = + Box::new(inner.merge_external(events).map(|event| match event { + MergedEvent::Dora(event) => MergedEvent::Dora(event), + MergedEvent::External(event) => MergedEvent::External(event.flatten()), + })); + self.events = Some(merged); -fn is_ros2(event: &Box) -> bool { - match event.0 { - Some(MergedEvent::External(_)) => true, - _ => false, + id } } -fn downcast_ros2(event: Box) -> eyre::Result> { - match event.0 { - Some(MergedEvent::External(event)) => Ok(Box::new(ros2::Ros2Event { - event: Box::new(ros2::ExternalRos2Event(event)), - })), - _ => eyre::bail!("not an external event"), +impl ffi::CombinedEvents { + fn next(&mut self) -> ffi::CombinedEvent { + ffi::CombinedEvent { + event: Box::new(self.events.next()), + } } } -fn is_dora(event: &Box) -> bool { - match event.0 { - Some(MergedEvent::Dora(_)) => true, - _ => false, +pub struct MergedDoraEvent(Option>); + +pub struct ExternalEvent { + pub event: Box, + pub id: u32, +} + +impl ffi::CombinedEvent { + fn is_dora(&self) -> bool { + matches!(&self.event.0, Some(MergedEvent::Dora(_))) } } -fn downcast_dora(event: Box) -> eyre::Result> { - match event.0 { +fn downcast_dora(event: ffi::CombinedEvent) -> eyre::Result> { + match event.event.0 { Some(MergedEvent::Dora(event)) => Ok(Box::new(DoraEvent(Some(event)))), _ => eyre::bail!("not an external event"), } diff --git a/examples/c++-ros2-dataflow/node-rust-api/main.cc b/examples/c++-ros2-dataflow/node-rust-api/main.cc index 9e7a107ac..4d8d0cb77 100644 --- a/examples/c++-ros2-dataflow/node-rust-api/main.cc +++ b/examples/c++-ros2-dataflow/node-rust-api/main.cc @@ -9,6 +9,9 @@ int main() { std::cout << "HELLO FROM C++" << std::endl; + auto dora_node = init_dora_node(); + auto merged_events = dora_events_into_combined(std::move(dora_node.events)); + auto qos = qos_default(); qos.durability = Ros2Durability::Volatile; qos.liveliness = Ros2Liveliness::Automatic; @@ -20,38 +23,19 @@ int main() auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos); auto vel_publisher = node->create_publisher(vel_topic, qos); auto pose_topic = node->create_topic_turtlesim_Pose("/turtle1", "pose", qos); - auto pose_subscription = node->create_subscription(pose_topic, qos); + auto pose_subscription = node->create_subscription(pose_topic, qos, merged_events); std::random_device dev; std::default_random_engine gen(dev()); std::uniform_real_distribution<> dist(0., 1.); - auto dora_node = init_dora_node(); - - std::cout << "MERGING EVENTS" << std::endl; - auto merged_events = merge_events(std::move(dora_node.events), event_stream(std::move(pose_subscription))); - std::cout << "MERGED EVENTS" << std::endl; - auto received_ticks = 0; for (int i = 0; i < 1000; i++) { - auto event = merged_events->next(); + auto event = merged_events.next(); - if (is_ros2(event)) - { - auto ros2_event = downcast_ros2(std::move(event)); - if (turtlesim::is_Pose(ros2_event)) - { - auto pose = turtlesim::downcast_Pose(std::move(ros2_event)); - std::cout << "Received Pose { x: " << pose->x << ", y: " << pose->y << " }" << std::endl; - } - else - { - std::cout << "received unexpected ros2 input" << std::endl; - } - } - else if (is_dora(event)) + if (event.is_dora()) { auto dora_event = downcast_dora(std::move(event)); @@ -83,6 +67,15 @@ int main() break; } } + else if (pose_subscription->matches(event)) + { + auto pose = pose_subscription->downcast(std::move(event)); + std::cout << "Received pose x:" << pose.x << ", y:" << pose.y << std::endl; + } + else + { + std::cout << "received unexpected event" << std::endl; + } } std::cout << "GOODBYE FROM C++ node (using Rust API)" << std::endl; diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index be4f03969..5c86823c4 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -46,23 +46,19 @@ where ( quote! { #[cxx::bridge] }, quote! { + extern "C++" { + type CombinedEvents = crate::ffi::CombinedEvents; + type CombinedEvent = crate::ffi::CombinedEvent; + } + extern "Rust" { type Ros2Context; type Ros2Node; - type ExternalRos2Events; - type ExternalRos2Event; fn init_ros2_context() -> Result>; fn new_node(self: &Ros2Context, name_space: &str, base_name: &str) -> Result>; fn qos_default() -> Ros2QosPolicies; - #(#message_topic_defs)* - } - - pub struct ExternalEvents { - events: Box, - } - pub struct Ros2Event { - event: Box, + #(#message_topic_defs)* } #[derive(Debug, Clone)] @@ -194,19 +190,6 @@ where } } } - - pub use ffi::ExternalEvents; - pub use ffi::Ros2Event; - - pub struct ExternalRos2Events( - pub Box, - ); - - pub trait AsEventStream { - fn as_event_stream(self: Box) -> Box> + Unpin>; - } - - pub struct ExternalRos2Event(pub Box); }, ) } else { diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs index 1ce74e16e..b8d811731 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/message.rs @@ -192,22 +192,22 @@ impl Message { let cxx_create_publisher = format_ident!("create_publisher"); let struct_raw_name = format_ident!("{package_name}__{}", self.name); + let struct_raw_name_str = struct_raw_name.to_string(); let self_name = &self.name; let publish = format_ident!("publish__{package_name}__{}", self.name); let cxx_publish = format_ident!("publish"); let subscription_name = format_ident!("Subscription__{package_name}__{}", self.name); + let subscription_name_str = subscription_name.to_string(); let cxx_subscription_name = format_ident!("Subscription_{}", self.name); let create_subscription = format_ident!("new__Subscription__{package_name}__{}", self.name); let cxx_create_subscription = format_ident!("create_subscription"); - let event_stream = format_ident!("event_stream__{package_name}__{}", self.name); - let cxx_event_stream = format_ident!("event_stream"); - let is = format_ident!("is__{package_name}__{}", self.name); - let cxx_is = format_ident!("is_{}", self.name); + let matches = format_ident!("matches__{package_name}__{}", self.name); + let cxx_matches = format_ident!("matches"); let downcast = format_ident!("downcast__{package_name}__{}", self.name); - let cxx_downcast = format_ident!("downcast_{}", self.name); + let cxx_downcast = format_ident!("downcast"); let def = quote! { #[namespace = #package_name] @@ -218,7 +218,7 @@ impl Message { #[cxx_name = #cxx_create_publisher] fn #create_publisher(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies) -> Result>; #[cxx_name = #cxx_create_subscription] - fn #create_subscription(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies) -> Result>; + fn #create_subscription(self: &mut Ros2Node, topic: &Box<#topic_name>, qos: Ros2QosPolicies, events: &mut CombinedEvents) -> Result>; #[namespace = #package_name] #[cxx_name = #cxx_publisher_name] @@ -232,15 +232,11 @@ impl Message { type #subscription_name; #[namespace = #package_name] - #[cxx_name = #cxx_event_stream] - fn #event_stream(subscription: Box<#subscription_name>) -> Box; - - #[namespace = #package_name] - #[cxx_name = #cxx_is] - fn #is(event: &Box) -> bool; + #[cxx_name = #cxx_matches] + fn #matches(self: &#subscription_name, event: &CombinedEvent) -> bool; #[namespace = #package_name] #[cxx_name = #cxx_downcast] - fn #downcast(event: Box) -> Result>; + fn #downcast(self: &#subscription_name, event: CombinedEvent) -> Result<#struct_raw_name>; }; let imp = quote! { #[allow(non_camel_case_types)] @@ -262,9 +258,16 @@ impl Message { } #[allow(non_snake_case)] - pub fn #create_subscription(&mut self, topic: &Box<#topic_name>, qos: ffi::Ros2QosPolicies) -> eyre::Result> { - let subscription = self.0.create_subscription(&topic.0, Some(qos.into()))?; - Ok(Box::new(#subscription_name(subscription))) + pub fn #create_subscription(&mut self, topic: &Box<#topic_name>, qos: ffi::Ros2QosPolicies, events: &mut crate::ffi::CombinedEvents) -> eyre::Result> { + let subscription = self.0.create_subscription::(&topic.0, Some(qos.into()))?; + let stream = futures_lite::stream::unfold(subscription, |sub| async { + let item = sub.async_take().await; + let item_boxed: Box = Box::new(item); + Some((item_boxed, sub)) + }); + let id = events.events.merge(Box::pin(stream)); + + Ok(Box::new(#subscription_name { id })) } } @@ -280,30 +283,32 @@ impl Message { } #[allow(non_camel_case_types)] - pub struct #subscription_name(ros2_client::Subscription); - - #[allow(non_snake_case)] - fn #event_stream(subscription: Box<#subscription_name>) -> Box { - Box::new(ExternalEvents { events: Box::new(ExternalRos2Events(subscription)) }) - } - - #[allow(non_snake_case)] - fn #is(event: &Box) -> bool { - event.event.0.is::() - } - #[allow(non_snake_case)] - fn #downcast(event: Box) -> eyre::Result> { - event.event.0.downcast().map_err(|_| eyre::eyre!("downcast failed")) + pub struct #subscription_name { + id: u32, } - impl AsEventStream for #subscription_name { - fn as_event_stream(self: Box) -> Box> + Unpin> { - let stream = futures_lite::stream::unfold(self.0, |sub| async { - let item = sub.async_take().await; - let item_boxed: Box = Box::new(item); - Some((item_boxed, sub)) - }); - Box::new(Box::pin(stream)) + impl #subscription_name { + #[allow(non_snake_case)] + fn #matches(&self, event: &crate::ffi::CombinedEvent) -> bool { + match &event.event.as_ref().0 { + Some(crate::MergedEvent::External(event)) if event.id == self.id => true, + _ => false + } + } + #[allow(non_snake_case)] + fn #downcast(&self, event: crate::ffi::CombinedEvent) -> eyre::Result { + use eyre::WrapErr; + + match (*event.event).0 { + Some(crate::MergedEvent::External(event)) if event.id == self.id => { + let result = event.event.downcast::>() + .map_err(|_| eyre::eyre!("downcast to {} failed", #struct_raw_name_str))?; + + let (data, _info) = result.with_context(|| format!("failed to receive {} event", #subscription_name_str))?; + Ok(data) + }, + _ => eyre::bail!("not a {} event", #subscription_name_str), + } } } }; diff --git a/rust-toolchain.toml b/rust-toolchain.toml index 0538cafab..0a2102d4f 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -1,3 +1,3 @@ [toolchain] -channel = "1.70" +channel = "1.76" components = ["rustfmt", "clippy"] From 3e7b73a9ca60561ac64393d3ed797391a807b272 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 12:13:28 +0100 Subject: [PATCH 22/32] Update c++-dataflow example for new event iteration syntax --- examples/c++-dataflow/node-rust-api/main.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/c++-dataflow/node-rust-api/main.cc b/examples/c++-dataflow/node-rust-api/main.cc index 314155239..9407650a6 100644 --- a/examples/c++-dataflow/node-rust-api/main.cc +++ b/examples/c++-dataflow/node-rust-api/main.cc @@ -13,7 +13,7 @@ int main() for (int i = 0; i < 20; i++) { - auto event = next_event(dora_node.events); + auto event = dora_node.events->next(); auto ty = event_type(event); if (ty == DoraEventType::AllInputsClosed) From dc8a7948359198215819b67adfb7635a05a043fd Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 12:13:51 +0100 Subject: [PATCH 23/32] Fix some dead code warnings --- libraries/extensions/ros2-bridge/msg-gen/src/lib.rs | 3 +-- .../extensions/ros2-bridge/msg-gen/src/types/primitives.rs | 2 -- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs index 5c86823c4..54e7155c7 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/lib.rs @@ -9,9 +9,7 @@ use std::path::Path; -use proc_macro2::Span; use quote::quote; -use syn::Ident; pub mod parser; pub mod types; @@ -46,6 +44,7 @@ where ( quote! { #[cxx::bridge] }, quote! { + #[allow(dead_code)] extern "C++" { type CombinedEvents = crate::ffi::CombinedEvents; type CombinedEvent = crate::ffi::CombinedEvent; diff --git a/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs b/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs index 98dc82352..56d001498 100644 --- a/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs +++ b/libraries/extensions/ros2-bridge/msg-gen/src/types/primitives.rs @@ -131,7 +131,6 @@ pub struct NamedType(pub String); impl NamedType { fn type_tokens(&self, package: &str) -> impl ToTokens { let package = Ident::new(package, Span::call_site()); - let namespace = Ident::new("msg", Span::call_site()); let name = Ident::new(&self.0, Span::call_site()); let ident = format_ident!("{package}__{name}"); quote! { #ident } @@ -174,7 +173,6 @@ pub struct NamespacedType { impl NamespacedType { fn type_tokens(&self) -> impl ToTokens { let package = Ident::new(&self.package, Span::call_site()); - let namespace = Ident::new(&self.namespace, Span::call_site()); let name = Ident::new(&self.name, Span::call_site()); let ident = format_ident!("{package}__{name}"); quote! { #ident } From 2eeb40b1ffd3ca2a6a59c091b5b4379e5a742125 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 12:14:38 +0100 Subject: [PATCH 24/32] Fix operator API: Box optional error string since `Option` is not FFI-safe --- apis/c++/node/src/lib.rs | 7 ++++- apis/c/operator/operator_types.h | 2 +- apis/rust/operator/src/lib.rs | 5 +--- apis/rust/operator/src/raw.rs | 10 +++---- apis/rust/operator/types/src/lib.rs | 30 ++++++++++++++++++--- binaries/runtime/src/operator/shared_lib.rs | 28 +++++++------------ libraries/arrow-convert/src/from_impls.rs | 2 +- 7 files changed, 49 insertions(+), 35 deletions(-) diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index 99a3b1cfd..33705e5b5 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -123,7 +123,12 @@ fn event_type(event: &DoraEvent) -> ffi::DoraEventType { } fn event_as_input(event: Box) -> eyre::Result { - let Some(Event::Input { id, metadata: _, data }) = event.0 else { + let Some(Event::Input { + id, + metadata: _, + data, + }) = event.0 + else { bail!("not an input event"); }; let data: Option<&BinaryArray> = data.as_binary_opt(); diff --git a/apis/c/operator/operator_types.h b/apis/c/operator/operator_types.h index c6e3c6f8c..9cf2f3d21 100644 --- a/apis/c/operator/operator_types.h +++ b/apis/c/operator/operator_types.h @@ -34,7 +34,7 @@ typedef struct Vec_uint8 { /** */ typedef struct DoraResult { /** */ - Vec_uint8_t error; + Vec_uint8_t * error; } DoraResult_t; /** */ diff --git a/apis/rust/operator/src/lib.rs b/apis/rust/operator/src/lib.rs index fd770c78c..262fcdd3f 100644 --- a/apis/rust/operator/src/lib.rs +++ b/apis/rust/operator/src/lib.rs @@ -64,9 +64,6 @@ impl DoraOutputSender<'_> { open_telemetry_context: String::new().into(), // TODO }, }); - match result.error { - None => Ok(()), - Some(error) => Err(error.into()), - } + result.into_result() } } diff --git a/apis/rust/operator/src/raw.rs b/apis/rust/operator/src/raw.rs index 9a05a5e3f..2ff89c64d 100644 --- a/apis/rust/operator/src/raw.rs +++ b/apis/rust/operator/src/raw.rs @@ -24,7 +24,7 @@ pub unsafe fn dora_init_operator() -> DoraInitResult { pub unsafe fn dora_drop_operator(operator_context: *mut c_void) -> DoraResult { let raw: *mut O = operator_context.cast(); - unsafe { Box::from_raw(raw) }; + drop(unsafe { Box::from_raw(raw) }); DoraResult { error: None } } @@ -40,10 +40,10 @@ pub unsafe fn dora_on_event( let event_variant = if let Some(input) = &mut event.input { let Some(data_array) = input.data_array.take() else { return OnEventResult { - result: DoraResult { error: Some("data already taken".to_string().into()) }, + result: DoraResult::from_error("data already taken".to_string()), status: DoraStatus::Continue, }; - }; + }; let data = arrow::ffi::from_ffi(data_array, &input.schema); match data { @@ -73,9 +73,7 @@ pub unsafe fn dora_on_event( status, }, Err(error) => OnEventResult { - result: DoraResult { - error: Some(error.into()), - }, + result: DoraResult::from_error(error), status: DoraStatus::Stop, }, } diff --git a/apis/rust/operator/types/src/lib.rs b/apis/rust/operator/types/src/lib.rs index af44c5bec..e7f5cb8a1 100644 --- a/apis/rust/operator/types/src/lib.rs +++ b/apis/rust/operator/types/src/lib.rs @@ -14,7 +14,7 @@ use safer_ffi::{ closure::ArcDynFn1, derive_ReprC, ffi_export, }; -use std::path::Path; +use std::{ops::Deref, path::Path}; #[derive_ReprC] #[ffi_export] @@ -43,7 +43,31 @@ pub struct DoraDropOperator { #[repr(C)] #[derive(Debug)] pub struct DoraResult { - pub error: Option, + pub error: Option>, +} + +impl DoraResult { + pub const SUCCESS: Self = Self { error: None }; + + pub fn from_error(error: String) -> Self { + Self { + error: Some(Box::new(safer_ffi::String::from(error)).into()), + } + } + + pub fn error(&self) -> Option<&str> { + self.error.as_deref().map(|s| s.deref()) + } + + pub fn into_result(self) -> Result<(), String> { + match self.error { + None => Ok(()), + Some(error) => { + let converted = safer_ffi::boxed::Box_::into(error); + Err((*converted).into()) + } + } + } } #[derive_ReprC] @@ -174,7 +198,7 @@ pub unsafe fn dora_send_operator_output( match result() { Ok(output) => send_output.send_output.call(output), Err(error) => DoraResult { - error: Some(error.into()), + error: Some(Box::new(safer_ffi::String::from(error)).into()), }, } } diff --git a/binaries/runtime/src/operator/shared_lib.rs b/binaries/runtime/src/operator/shared_lib.rs index ca758c4fa..495c1b6c7 100644 --- a/binaries/runtime/src/operator/shared_lib.rs +++ b/binaries/runtime/src/operator/shared_lib.rs @@ -98,7 +98,7 @@ impl<'lib> SharedLibraryOperator<'lib> { let raw = match result.error { Some(error) => { let _ = init_done.send(Err(eyre!(error.to_string()))); - bail!("init_operator failed: {}", String::from(error)) + bail!("init_operator failed: {}", *error) } None => operator_context, }; @@ -126,11 +126,7 @@ impl<'lib> SharedLibraryOperator<'lib> { let arrow_array = match arrow::ffi::from_ffi(data_array, &schema) { Ok(a) => a, - Err(err) => { - return DoraResult { - error: Some(err.to_string().into()), - } - } + Err(err) => return DoraResult::from_error(err.to_string()), }; let total_len = required_data_size(&arrow_array); @@ -138,11 +134,7 @@ impl<'lib> SharedLibraryOperator<'lib> { let type_info = match copy_array_into_sample(&mut sample, &arrow_array) { Ok(t) => t, - Err(err) => { - return DoraResult { - error: Some(err.to_string().into()), - } - } + Err(err) => return DoraResult::from_error(err.to_string()), }; let event = OperatorEvent::Output { @@ -157,18 +149,16 @@ impl<'lib> SharedLibraryOperator<'lib> { .blocking_send(event) .map_err(|_| eyre!("failed to send output to runtime")); - let error = match result { - Ok(()) => None, - Err(_) => Some(String::from("runtime process closed unexpectedly").into()), - }; - - DoraResult { error } + match result { + Ok(()) => DoraResult::SUCCESS, + Err(_) => DoraResult::from_error("runtime process closed unexpectedly".into()), + } }); let reason = loop { #[allow(unused_mut)] let Ok(mut event) = self.incoming_events.recv() else { - break StopReason::InputsClosed + break StopReason::InputsClosed; }; let span = span!(tracing::Level::TRACE, "on_event", input_id = field::Empty); @@ -261,7 +251,7 @@ impl<'lib> SharedLibraryOperator<'lib> { ) }; match error { - Some(error) => bail!("on_input failed: {}", String::from(error)), + Some(error) => bail!("on_input failed: {}", *error), None => match status { DoraStatus::Continue => {} DoraStatus::Stop => break StopReason::ExplicitStop, diff --git a/libraries/arrow-convert/src/from_impls.rs b/libraries/arrow-convert/src/from_impls.rs index 8d918a3aa..db484c6b3 100644 --- a/libraries/arrow-convert/src/from_impls.rs +++ b/libraries/arrow-convert/src/from_impls.rs @@ -1,5 +1,5 @@ use arrow::{ - array::{make_array, Array, AsArray, PrimitiveArray, StringArray}, + array::{Array, AsArray, PrimitiveArray, StringArray}, datatypes::ArrowPrimitiveType, }; use eyre::ContextCompat; From de6917ae00133b6037b1933bf32bcfcaf78472b5 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 14:55:57 +0100 Subject: [PATCH 25/32] C++ API: Add back `next_event` function for backwards compatibility --- apis/c++/node/src/lib.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/apis/c++/node/src/lib.rs b/apis/c++/node/src/lib.rs index 33705e5b5..d5c03528f 100644 --- a/apis/c++/node/src/lib.rs +++ b/apis/c++/node/src/lib.rs @@ -57,6 +57,7 @@ mod ffi { fn dora_events_into_combined(events: Box) -> CombinedEvents; fn next(self: &mut Events) -> Box; + fn next_event(events: &mut Box) -> Box; fn event_type(event: &Box) -> DoraEventType; fn event_as_input(event: Box) -> Result; fn send_output( @@ -97,6 +98,10 @@ impl Events { } } +fn next_event(events: &mut Box) -> Box { + events.next() +} + fn dora_events_into_combined(events: Box) -> ffi::CombinedEvents { let events = events.0.map(MergedEvent::Dora); ffi::CombinedEvents { From 3b1777c2e240c72101900ebb922b4a990d38a999 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 15:42:22 +0100 Subject: [PATCH 26/32] Fix: Respect `CARGO_TARGET_DIR` if set --- apis/c++/node/build.rs | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index 3a0e9c4f6..0706053ba 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -9,12 +9,7 @@ fn main() { println!("cargo:rerun-if-changed=src/lib.rs"); // rename header files - let root = Path::new(env!("CARGO_MANIFEST_DIR")) - .ancestors() - .nth(3) - .unwrap(); - let src_dir = root - .join("target") + let src_dir = target_dir() .join("cxxbridge") .join("dora-node-api-cxx") .join("src"); @@ -33,6 +28,18 @@ fn main() { bridge_files.clear(); } +fn target_dir() -> PathBuf { + std::env::var("CARGO_TARGET_DIR") + .map(PathBuf::from) + .unwrap_or_else(|_| { + let root = Path::new(env!("CARGO_MANIFEST_DIR")) + .ancestors() + .nth(3) + .unwrap(); + root.join("target") + }) +} + #[cfg(feature = "ros2-bridge")] mod ros2 { use std::path::{Component, Path, PathBuf}; @@ -102,12 +109,7 @@ mod ros2 { .join("ros2_bindings.rs.cc"); // copy message files to target directory - let root = Path::new(env!("CARGO_MANIFEST_DIR")) - .ancestors() - .nth(3) - .unwrap(); - let target_path = root - .join("target") + let target_path = target_dir() .join("cxxbridge") .join("dora-node-api-cxx") .join("dora-ros2-bindings.h"); From 2d6b5b98079690189ccd9816d59af2d821c75d75 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Wed, 14 Feb 2024 18:00:17 +0100 Subject: [PATCH 27/32] Start documenting C++ node API --- apis/c++/node/README.md | 64 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 apis/c++/node/README.md diff --git a/apis/c++/node/README.md b/apis/c++/node/README.md new file mode 100644 index 000000000..b7192b545 --- /dev/null +++ b/apis/c++/node/README.md @@ -0,0 +1,64 @@ +# Dora Node API for C++ + +## Build + +- Clone the `dora` repository: + ```bash + > git clone https://github.com/dora-rs/dora.git + > cd dora + ``` +- Build the `dora-node-api-cxx` package: + ```bash + cargo build --package dora-node-api-cxx + ``` + - This will result in `dora-node-api.h` and `dora-node-api.cc` files in the `target/cxxbridge/dora-node-api-cxx` directory. +- Include the `dora-node-api.h` header file in your source file. +- Add the `dora-node-api.cc` file to your compile and link steps. + +### Build with ROS2 Bridge + +Dora features an experimental ROS2 Bridge that enables dora nodes to publish and subscribe to ROS2 topics. +To enable the bridge, use these steps: + +- Clone the `dora` repository (see above). +- Source the ROS2 setup files (see [ROS2 docs](https://docs.ros.org/en/rolling/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html#source-the-setup-files)) +- Optional: Source package-specific ROS2 setup files if you want to use custom package-specific ROS2 messages in the bridge (see [ROS2 docs](https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html#source-the-setup-file)) +- Build the `dora-node-api-cxx` package **with the `ros2-bridge` feature enabled**: + ```bash + cargo build --package dora-node-api-cxx --feature ros2-bridge + ``` + - In addition to the `dora-node-api.h` and `dora-node-api.cc` files, this will place a `dora-ros2-bindings.h` and a `dora-ros2-bindings.cc` file in the `target/cxxbridge/dora-node-api-cxx` directory. +- Include both the `dora-node-api.h` and the `dora-ros2-bindings.h` header files in your source file. +- Add the `dora-node-api.cc` and `dora-ros2-bindings.cc` files to your compile and link steps. + +## Usage + +### Init Dora Node + +TODO + +### Receiving Events + +TODO + +### Sending Outputs + +TODO + +## Using the ROS2 Bridge + +### Initializing the ROS2 Context + +TODO + +### Creating Topics + +TODO + +### Publish + +TODO + +### Subscriptions + +TODO From da44f752357de667cabe1edc898b7744c9df621b Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Tue, 27 Feb 2024 08:39:31 +0100 Subject: [PATCH 28/32] Run c++ ros2 example on CI --- .github/workflows/ci.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index aad0b15a5..70234f29d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -191,6 +191,14 @@ jobs: # Reset only the turtlesim instance as it is not destroyed at the end of the previous job source /opt/ros/humble/setup.bash && ros2 service call /reset std_srvs/srv/Empty & cargo run --example python-ros2-dataflow --features="ros2-examples" + - name: "c++-ros2-dataflow" + timeout-minutes: 30 + env: + QT_QPA_PLATFORM: offscreen + run: | + # Reset only the turtlesim instance as it is not destroyed at the end of the previous job + source /opt/ros/humble/setup.bash && ros2 service call /reset std_srvs/srv/Empty & + cargo run --example cxx-ros2-dataflow --features="ros2-examples" bench: name: "Bench" From 58d13c06fadbf4b8edeb0797eaad96c53ab6537c Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Tue, 27 Feb 2024 08:43:44 +0100 Subject: [PATCH 29/32] Add missing import --- apis/c++/node/build.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/apis/c++/node/build.rs b/apis/c++/node/build.rs index 0706053ba..767bac2ec 100644 --- a/apis/c++/node/build.rs +++ b/apis/c++/node/build.rs @@ -42,6 +42,7 @@ fn target_dir() -> PathBuf { #[cfg(feature = "ros2-bridge")] mod ros2 { + use super::target_dir; use std::path::{Component, Path, PathBuf}; pub fn generate() -> PathBuf { From 57de69738fa812a1af78030f1c3d5768452a7540 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Tue, 27 Feb 2024 08:44:06 +0100 Subject: [PATCH 30/32] Update `run.rs` to work with latest master changes --- examples/c++-ros2-dataflow/run.rs | 40 +++++++++++++------------------ 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/examples/c++-ros2-dataflow/run.rs b/examples/c++-ros2-dataflow/run.rs index 5ec35bb50..4d5d242f4 100644 --- a/examples/c++-ros2-dataflow/run.rs +++ b/examples/c++-ros2-dataflow/run.rs @@ -1,22 +1,10 @@ +use dora_tracing::set_up_tracing; use eyre::{bail, Context}; use std::{env::consts::EXE_SUFFIX, path::Path}; -use tracing::metadata::LevelFilter; -use tracing_subscriber::Layer; - -#[derive(Debug, Clone, clap::Parser)] -pub struct Args { - #[clap(long)] - pub run_dora_runtime: bool, -} #[tokio::main] async fn main() -> eyre::Result<()> { - let Args { run_dora_runtime } = clap::Parser::parse(); - - if run_dora_runtime { - return tokio::task::block_in_place(dora_daemon::run_dora_runtime); - } - set_up_tracing().wrap_err("failed to set up tracing")?; + set_up_tracing("c++-ros2-dataflow-exaple").wrap_err("failed to set up tracing")?; if cfg!(windows) { tracing::error!( @@ -69,7 +57,7 @@ async fn main() -> eyre::Result<()> { .await?; let dataflow = Path::new("dataflow.yml").to_owned(); - dora_daemon::Daemon::run_dataflow(&dataflow).await?; + run_dataflow(&dataflow).await?; Ok(()) } @@ -161,13 +149,17 @@ async fn build_cxx_node( Ok(()) } -fn set_up_tracing() -> eyre::Result<()> { - use tracing_subscriber::prelude::__tracing_subscriber_SubscriberExt; - - let stdout_log = tracing_subscriber::fmt::layer() - .pretty() - .with_filter(LevelFilter::DEBUG); - let subscriber = tracing_subscriber::Registry::default().with(stdout_log); - tracing::subscriber::set_global_default(subscriber) - .context("failed to set tracing global subscriber") +async fn run_dataflow(dataflow: &Path) -> eyre::Result<()> { + let cargo = std::env::var("CARGO").unwrap(); + let mut cmd = tokio::process::Command::new(&cargo); + cmd.arg("run"); + cmd.arg("--package").arg("dora-cli"); + cmd.arg("--") + .arg("daemon") + .arg("--run-dataflow") + .arg(dataflow); + if !cmd.status().await?.success() { + bail!("failed to run dataflow"); + }; + Ok(()) } From 70fb49bdd568745d56b59408b84d61ebfcf0d9f1 Mon Sep 17 00:00:00 2001 From: Philipp Oppermann Date: Tue, 27 Feb 2024 15:29:49 +0100 Subject: [PATCH 31/32] Write usage instructions for C++ ROS2 bridge --- apis/c++/node/README.md | 182 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 174 insertions(+), 8 deletions(-) diff --git a/apis/c++/node/README.md b/apis/c++/node/README.md index b7192b545..111d0741c 100644 --- a/apis/c++/node/README.md +++ b/apis/c++/node/README.md @@ -1,6 +1,8 @@ # Dora Node API for C++ -## Build +Dora supports nodes written in C++ through this API crate. + +## Build - Clone the `dora` repository: ```bash @@ -33,32 +35,196 @@ To enable the bridge, use these steps: ## Usage +The `dora-node-api.h` header provides various functions to interact with Dora. + ### Init Dora Node -TODO +All nodes need to register themselves with Dora at startup. +To do that, call the `init_dora_node()` function. +The function returns a `DoraNode` instance, which gives access to dora events and enables sending Dora outputs. + +```c++ +auto dora_node = init_dora_node(); +``` ### Receiving Events -TODO +The `dora_node.events` field is a stream of incoming events. +To wait for the next incoming event, call `dora_node.events->next()`: + +```c++ +auto event = dora_node.events->next(); +``` + +The `next` function returns an opaque `DoraEvent` type, which cannot be inspected from C++ directly. +Instead, use the following functions to read and destructure the event: + +- `event_type(event)` returns a `DoraEventType`, which describes the kind of event. For example, an event could be an input or a stop instruction. + - when receiving a `DoraEventType::AllInputsClosed`, the node should exit and not call `next` anymore +- Events of type `DoraEventType::Input` can be downcasted using `event_as_input`: + ```c++ + auto input = event_as_input(std::move(event)); + ``` + The function returns a `DoraInput` instance, which has an `id` and `data` field. + - The input `id` can be converted to a C++ string through `std::string(input.id)`. + - The `data` of inputs is currently of type [`rust::Vec`](https://cxx.rs/binding/vec.html). Use the provided methods for reading or converting the data. + - **Note:** In the future, we plan to change the data type to the [Apache Arrow](https://arrow.apache.org/) data format to support typed inputs. ### Sending Outputs -TODO +Nodes can send outputs using the `send_output` output function and the `dora_node.send_output` field. +Note that all outputs need to be listed in the dataflow YAML declaration file, otherwise an error will occur. + +**Example:** + +```c++ +// the data you want to send (NOTE: only byte vectors are supported right now) +std::vector out_vec{42}; +// create a Rust slice from the output vector +rust::Slice out_slice{out_vec.data(), out_vec.size()}; +// send the slice as output +auto result = send_output(dora_node.send_output, "output_id", out_slice); + +// check for errors +auto error = std::string(result.error); +if (!error.empty()) +{ + std::cerr << "Error: " << error << std::endl; + return -1; +} +``` ## Using the ROS2 Bridge +The `dora-ros2-bindings.h` contains function and struct definitions that allow interacting with ROS2 nodes. +Currently, the bridge supports publishing and subscribing to ROS2 topics. +In the future, we plan to support ROS2 services and ROS2 actions as well. + ### Initializing the ROS2 Context -TODO +The first step is to initialize a ROS2 context: + +```c++ +auto ros2_context = init_ros2_context(); +``` + +### Creating Nodes + +After initializing a ROS2 context, you can use it to create ROS2 nodes: + +```c++ +auto node = ros2_context->new_node("/ros2_demo", "turtle_teleop"); +``` + +The first argument is the namespace of the node and the second argument is its name. ### Creating Topics -TODO +After creating a node, you can use one of the `create_topic_` functions to create a topic on it. +The `` describes the message type that will be sent on the topic. +The Dora ROS2 bridge automatically creates `create_topic_` functions for all messages types found in the sourced ROS2 environment. + +```c++ +auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos_default()); +``` + +The first argument is the namespace of the topic and the second argument is its name. +The third argument is the QoS (quality of service) setting for the topic. +It can be adjusted as desired, for example: + +```c++ +auto qos = qos_default(); +qos.durability = Ros2Durability::Volatile; +qos.liveliness = Ros2Liveliness::Automatic; +auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos); +``` ### Publish -TODO +After creating a topic, it is possible to publish messages on it. +First, create a publisher: + +```c++ +auto vel_publisher = node->create_publisher(vel_topic, qos); +``` + +The returned publisher is typed by the chosen topic. +It will only accept messages of the topic's type, otherwise a compile error will occur. + +After creating a publisher, you can use the `publish` function to publish one or more messages. +For example: + +```c++ +geometry_msgs::Twist twist = { + .linear = {.x = 1, .y = 0, .z = 0}, + .angular = {.x = 0, .y = 0, .z = 0.5} +}; +vel_publisher->publish(twist); +``` + +The `geometry_msgs::Twist` struct is automatically generated from the sourced ROS2 environment. +Since the publisher is typed, its `publish` method only accepts `geometry_msgs::Twist` messages. + ### Subscriptions -TODO +Subscribing to a topic is possible through the `create_subscription` function on nodes: + +```c++ +auto pose_topic = node->create_topic_turtlesim_Pose("/turtle1", "pose", qos_default()); +auto pose_subscription = node->create_subscription(pose_topic, qos_default(), event_stream); +``` + +The `topic` is the topic you want to subscribe to, created using a `create_topic_` function. +The second argument is the quality of service setting, which can be customized as described above. + +The third parameter is the event stream that the received messages should be merged into. +You can create such a event stream from Dora's event stream using the `dora_events_into_combined` function: + +```c++ +auto event_stream = dora_events_into_combined(std::move(dora_node.events)); +``` + +Multiple subscriptions can be merged into the same event stream. + +#### Receiving Messages from Merged Event Stream + +The merged event stream will receive all incoming events of the node, including Dora events and ROS2 messages. +To wait for the next incoming event, use its `next` method: + +```c++ +auto event = event_stream.next(); +``` + +This returns a `event` instance of type `CombinedEvent`, which can be downcasted to Dora events or ROS2 messages. +To handle an event, you should check it's type and then downcast it: + +- To check for a Dora event, you can use the `is_dora()` function. If it returns `true`, you can downcast the combined event to a Dora event using the `downcast_dora` function. +- ROS2 subscriptions support a `matches` function to check whether a combined event is an instance of the respective ROS2 subscription. If it returns true, you can downcast the event to the respective ROS2 message struct using the subscription's `downcast` function. + +**Example:** + +```c++ +if (event.is_dora()) +{ + auto dora_event = downcast_dora(std::move(event)); + // handle dora_event as described above + auto ty = event_type(dora_event); + if (ty == DoraEventType::Input) + { + auto input = event_as_input(std::move(dora_event)); + // etc + } + // .. else if +} +else if (pose_subscription->matches(event)) +{ + auto pose = pose_subscription->downcast(std::move(event)); + std::cout << "Received pose x:" << pose.x << ", y:" << pose.y << std::endl; +} +else +{ + std::cout << "received unexpected event" << std::endl; +} +``` + From f258fbe3f6feb0a62eb01b65134892414f6e4503 Mon Sep 17 00:00:00 2001 From: Haixuan Xavier Tao Date: Wed, 28 Feb 2024 09:24:03 +0100 Subject: [PATCH 32/32] Fix Typo --- apis/c++/node/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apis/c++/node/README.md b/apis/c++/node/README.md index 111d0741c..c18fb2e0a 100644 --- a/apis/c++/node/README.md +++ b/apis/c++/node/README.md @@ -27,7 +27,7 @@ To enable the bridge, use these steps: - Optional: Source package-specific ROS2 setup files if you want to use custom package-specific ROS2 messages in the bridge (see [ROS2 docs](https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html#source-the-setup-file)) - Build the `dora-node-api-cxx` package **with the `ros2-bridge` feature enabled**: ```bash - cargo build --package dora-node-api-cxx --feature ros2-bridge + cargo build --package dora-node-api-cxx --features ros2-bridge ``` - In addition to the `dora-node-api.h` and `dora-node-api.cc` files, this will place a `dora-ros2-bindings.h` and a `dora-ros2-bindings.cc` file in the `target/cxxbridge/dora-node-api-cxx` directory. - Include both the `dora-node-api.h` and the `dora-ros2-bindings.h` header files in your source file.