Use correct service mapping based on RMW_IMPLEMENTATION and ROS distro

.github/workflows/ci.yml

@@ -199,7 +199,7 @@ jobs:
         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"
+          source /opt/ros/humble/setup.bash && cargo run --example cxx-ros2-dataflow --features="ros2-examples"
 
   bench:
     name: "Bench"

apis/c++/node/build.rs

@@ -51,7 +51,7 @@ mod ros2 {
     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 = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), true).unwrap();
         let generated_string = rust_format::PrettyPlease::default()
             .format_tokens(generated)
             .unwrap();
@@ -81,6 +81,8 @@ mod ros2 {
             }
         };
         println!("cargo:rerun-if-env-changed=AMENT_PREFIX_PATH");
+        println!("cargo:rerun-if-env-changed=RMW_IMPLEMENTATION");
+        println!("cargo:rerun-if-env-changed=ROS_DISTRO");
 
         let paths: Vec<_> = ament_prefix_path.split(':').map(PathBuf::from).collect();
         for path in &paths {

libraries/extensions/ros2-bridge/build.rs

@@ -7,7 +7,7 @@
 fn main() {
     use rust_format::Formatter;
     let paths = ament_prefix_paths();
-    let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), false);
+    let generated = dora_ros2_bridge_msg_gen::gen(paths.as_slice(), false).unwrap();
     let generated_string = rust_format::PrettyPlease::default()
         .format_tokens(generated)
         .unwrap();
@@ -20,7 +20,7 @@
     let _build = cxx_build::bridge(&target_file);
 }
 
 fn ament_prefix_paths() -> Vec<PathBuf> {
     let ament_prefix_path: String = match std::env::var("AMENT_PREFIX_PATH") {
         Ok(path) => path,
         Err(std::env::VarError::NotPresent) => {

libraries/extensions/ros2-bridge/msg-gen/src/lib.rs

@@ -16,7 +16,7 @@ pub mod types;
 
 pub use crate::parser::get_packages;
 
-pub fn gen<P>(paths: &[P], create_cxx_bridge: bool) -> proc_macro2::TokenStream
+pub fn gen<P>(paths: &[P], create_cxx_bridge: bool) -> anyhow::Result<proc_macro2::TokenStream>
 where
     P: AsRef<Path>,
 {
@@ -48,7 +48,7 @@ where
             service_impls.push(imp);
             if create_cxx_bridge {
                 let (service_creation_def, service_creation_impl) =
-                    service.cxx_service_creation_functions(&package.name);
+                    service.cxx_service_creation_functions(&package.name)?;
                 service_creation_defs.push(service_creation_def);
                 service_creation_impls.push(service_creation_impl);
             }
@@ -240,7 +240,7 @@ where
         )
     };
 
-    quote! {
+    let generated = quote! {
         #attributes
         mod ffi {
             #imports_and_functions
@@ -271,5 +271,6 @@ where
         #(#service_impls)*
 
         #(#aliases)*
-    }
+    };
+    Ok(generated)
 }

libraries/extensions/ros2-bridge/msg-gen/src/types/service.rs

@@ -79,7 +79,7 @@ impl Service {
     pub fn cxx_service_creation_functions(
         &self,
         package_name: &str,
-    ) -> (impl ToTokens, impl ToTokens) {
+    ) -> anyhow::Result<(impl ToTokens, impl ToTokens)> {
         let client_name = format_ident!("Client__{package_name}__{}", self.name);
         let cxx_client_name = format_ident!("Client_{}", self.name);
         let create_client = format_ident!("new_Client__{package_name}__{}", self.name);
@@ -102,6 +102,32 @@ impl Service {
         let downcast = format_ident!("downcast__{package_name}__{}", self.name);
         let cxx_downcast = format_ident!("downcast");
 
+        let ros_service_mapping = {
+            let enhanced = format_ident!("Enhanced");
+            let cyclone = format_ident!("Cyclone");
+            match std::env::var("RMW_IMPLEMENTATION") {
+                Ok(middleware) => match middleware.as_str() {
+                    "rmw_fastrtps_cpp" => enhanced,
+                    "rmw_cyclonedds_cpp" => cyclone,
+                    other => anyhow::bail!("unsupported RMW_IMPLEMENTATION `{other}`"),
+                },
+                Err(std::env::VarError::NotPresent) => match std::env::var("ROS_DISTRO") {
+                    Ok(distro) => match distro.as_str() {
+                        "humble" | "iron" => enhanced,
+                        "galactic" => cyclone,
+                        other => anyhow::bail!("unsupported ROS_DISTRO `{other}`"),
+                    },
+                    Err(std::env::VarError::NotPresent) => anyhow::bail!("no ROS_DISTRO set"),
+                    Err(std::env::VarError::NotUnicode(_)) => {
+                        anyhow::bail!("ROS_DISTRO is not valid unicode")
+                    }
+                },
+                Err(std::env::VarError::NotUnicode(other)) => {
+                    anyhow::bail!("RMW_IMPLEMENTATION is not valid unicode `{:?}`", other)
+                }
+            }
+        };
+
         let def = quote! {
             #[namespace = #package_name]
             #[cxx_name = #cxx_client_name]
@@ -130,7 +156,7 @@ impl Service {
                     use futures::StreamExt as _;
 
                     let client = self.node.create_client::< #package :: service :: #self_name >(
-                        ros2_client::ServiceMapping::Enhanced,
+                        ros2_client::ServiceMapping:: #ros_service_mapping,
                         &ros2_client::Name::new(name_space, base_name).unwrap(),
                         &ros2_client::ServiceTypeName::new(#package_name, #self_name_str),
                         qos.clone().into(),
@@ -222,7 +248,7 @@ impl Service {
                 }
             }
         };
-        (def, imp)
+        Ok((def, imp))
     }
 
     pub fn token_stream_with_mod(&self) -> impl ToTokens {