Action API and example for dora-ros2-bridge

apis/c++/node/build.rs

@@ -35,6 +35,8 @@ fn main() {
     )
     .unwrap();
 
+    std::fs::copy(src_dir.join("../../rust/cxx.h"), target_dir.join("cxx.h")).unwrap();
+
     #[cfg(feature = "ros2-bridge")]
     ros2::generate_ros2_message_header(&target_dir);
 
@@ -70,11 +72,7 @@ fn origin_dir() -> PathBuf {
 
 #[cfg(feature = "ros2-bridge")]
 mod ros2 {
-    use super::origin_dir;
-    use std::{
-        io::{BufRead, BufReader},
-        path::{Component, Path, PathBuf},
-    };
+    use std::path::{Path, PathBuf};
 
     pub fn generate() -> Vec<PathBuf> {
         use rust_format::Formatter;
@@ -171,8 +169,6 @@ mod ros2 {
     }
 
     pub fn generate_ros2_message_header(target_path: &Path) {
-        use std::io::Write as _;
-
         let default_target = std::env::var("CARGO_TARGET_DIR")
             .map(PathBuf::from)
             .unwrap_or_else(|_| {
@@ -183,7 +179,6 @@ mod ros2 {
                 root.join("target")
             });
         let out_dir_str = std::env::var("OUT_DIR").unwrap();
-        let out_dir = PathBuf::from(&out_dir_str);
         let relative_dir = PathBuf::from(out_dir_str.strip_prefix("/").unwrap());
         let header_path = default_target
             .join("cxxbridge")
@@ -194,17 +189,4 @@ mod ros2 {
 
         copy_dir_all(&header_path, &target_path).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
-    }
 }

examples/ros2-bridge/c++/action-client/.gitignore

@@ -0,0 +1,2 @@
+*.o
+/build

examples/ros2-bridge/c++/action-client/dataflow.yml

@@ -0,0 +1,6 @@
+nodes:
+  - id: cxx-node-rust-api
+    path: build/node_rust_api
+    inputs:
+      tick: dora/timer/millis/500
+    outputs:

examples/ros2-bridge/c++/action-client/main.cc

@@ -0,0 +1,109 @@
+#include "cxx.h"
+#include "dora-node-api.h"
+#include "ros2-bridge/impl.h"
+#include "ros2-bridge/msg/example_interfaces.h"
+
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <vector>
+using namespace std::chrono_literals;
+
+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 service_qos = qos_default();
+    service_qos.durability = Ros2Durability::Volatile;
+    service_qos.liveliness = Ros2Liveliness::Automatic;
+    service_qos.reliable = true;
+    service_qos.max_blocking_time = 0.1;
+    auto qos = actionqos_default();
+    qos.goal_service = service_qos;
+    qos.result_service = service_qos;
+    qos.cancel_service = service_qos;
+    qos.feedback_subscription = service_qos;
+    qos.status_subscription = service_qos;
+
+    auto ros2_context = init_ros2_context();
+    auto node = ros2_context->new_node("/dora_ros2_bridge", "action_client");
+    auto client = example_interfaces::create_Fibonacci_action_client(
+        *node, "/", "fibonacci", qos, merged_events);
+
+    client->wait_for_action(node);
+    std::unique_ptr<::rust::Box<::ActionGoalId>> goal_id;
+    for(int count = 0; count < 3;) {
+        auto event = merged_events.next();
+        if (event.is_dora()) { // Event from Dora
+            auto dora_event = downcast_dora(std::move(event));
+            auto ty = event_type(dora_event);
+
+            if (ty == DoraEventType::Input) {
+                auto input = event_as_input(std::move(dora_event));
+                if (!goal_id) {
+                    auto goal = example_interfaces::Fibonacci_Goal{ .order = 10 + count };
+                    goal_id = std::make_unique<::rust::Box<::ActionGoalId>>(
+                        client->send_goal(goal));
+                    std::cout << "Goal have been sent" << std::endl;
+                    client->request_result(*goal_id);
+                }
+            } else if (ty == DoraEventType::Stop || ty == DoraEventType::AllInputsClosed) {
+                std::cout << "Received stop event" << std::endl;
+                break;
+            }
+        } else if (client->matches_result(event)) { // Result Event from ActionClient
+            std::cout << "Get result event" << std::endl;
+
+            auto result_event = client->downcast_result(std::move(event));
+
+            std::cout << "Result: ";
+            for(auto& num: result_event->get_result().sequence) {
+                std::cout << num << " ";
+            }
+            std::cout << std::endl;
+
+            auto status = result_event->get_status();
+
+            std::cout << "Status: " << static_cast<::std::int16_t>(status) << std::endl;
+            if(status == ::ActionStatusEnum::Succeeded) {
+                std::cout << "Goal succeeded" << std::endl;
+                goal_id = nullptr;
+                count += 1;
+            } else if(status == ::ActionStatusEnum::Aborted) {
+                std::cerr << "Goal aborted" << std::endl;
+            } else if(status == ::ActionStatusEnum::Canceled) {
+                std::cerr << "Goal canceled" << std::endl;
+            }
+        } else if (client->matches_status(event)) { // Feedback Event from ActionClient
+            std::cout << "Get status event" << std::endl;
+
+            auto status_event = client->downcast_status(std::move(event));
+
+            std::cout << "Status: " << static_cast<::std::int16_t>(status_event->get_status()) << std::endl;
+        } else if (client->matches_feedback(event)) { // Status Event from ActionClient
+            std::cout << "Get feedback event" << std::endl;
+
+            auto feedback_event = client->downcast_feedback(std::move(event));
+
+            if (!feedback_event->matches_goal(*goal_id)) { // skip if the goal id does not match
+                std::cout << "Feedback not belongs to current requesting goal" << std::endl;
+                continue;
+            }
+
+            if(feedback_event->matches_goal(*goal_id)) {
+                auto feedback = feedback_event->get_feedback();
+                std::cout << "Feedback: ";
+                for(auto& num: feedback.sequence) {
+                    std::cout << num << " ";
+                }
+            }
+            std::cout << std::endl;
+
+        }
+    }
+    std::cout << "GOODBYE FROM C++ node (using Rust API)" << std::endl;
+    return 0;
+}

examples/ros2-bridge/c++/action-client/run.rs

@@ -0,0 +1,158 @@
+use eyre::{Context, bail};
+use std::{env::consts::EXE_SUFFIX, path::Path};
+
+use process_wrap::std::{
+    ProcessGroup, StdChildWrapper as ChildWrapper, StdCommandWrap as CommandWrap,
+};
+
+fn main() -> eyre::Result<()> {
+    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")).join("../../../");
+    let target = root.join("target");
+    std::env::set_current_dir(root.join(file!()).parent().unwrap())
+        .wrap_err("failed to set working dir")?;
+
+    std::fs::create_dir_all("build")?;
+
+    build_package("dora-node-api-cxx", &["ros2-bridge"])?;
+    let node_cxxbridge = target
+        .join("cxxbridge")
+        .join("dora-node-api-cxx")
+        .join("install");
+
+    build_cxx_node(
+        &root,
+        &[
+            &dunce::canonicalize(Path::new("./").join("main.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("dora-node-api.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/example_interfaces.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/impl.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/action_msgs.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/builtin_interfaces.cc"))?,
+            &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/unique_identifier_msgs.cc"))?,
+        ],
+        "node_rust_api",
+        &[
+            "-I",
+            node_cxxbridge.as_os_str().to_str().unwrap(),
+            "-l",
+            "dora_node_api_cxx",
+        ],
+    )?;
+
+    let dataflow_task = std::thread::spawn(|| {
+        dora_cli::run("dataflow.yml".to_string(), false).unwrap();
+    });
+
+    let mut ros_task = run_ros_node(
+        "examples_rclcpp_minimal_action_server",
+        "action_server_member_functions",
+    )?;
+
+    let dataflow_task_res = dataflow_task.join();
+
+    ros_task.kill()?;
+
+    dataflow_task_res.map_err(|e| eyre::eyre!("the dataflow thread failed: {:?}", e))?;
+    Ok(())
+}
+
+fn run_ros_node(package: &str, node: &str) -> eyre::Result<Box<dyn ChildWrapper>> {
+    let mut command = CommandWrap::with_new("ros2", |cmd| {
+        cmd.arg("run");
+        cmd.arg(package).arg(node);
+    });
+    command.wrap(ProcessGroup::leader());
+    command
+        .spawn()
+        .map_err(|e| eyre::eyre!("failed to spawn ros node: {}", e))
+}
+
+fn build_package(package: &str, features: &[&str]) -> eyre::Result<()> {
+    let cargo = std::env::var("CARGO").unwrap();
+    let mut cmd = std::process::Command::new(&cargo);
+    cmd.arg("build");
+    cmd.arg("--package").arg(package);
+    if !features.is_empty() {
+        cmd.arg("--features").arg(features.join(","));
+    }
+    if !cmd.status()?.success() {
+        bail!("failed to compile {package}");
+    };
+    Ok(())
+}
+
+fn build_cxx_node(root: &Path, paths: &[&Path], out_name: &str, args: &[&str]) -> eyre::Result<()> {
+    let mut clang = std::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("-l").arg("z");
+        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()?.success() {
+        bail!("failed to compile c++ node");
+    };
+    Ok(())
+}

examples/ros2-bridge/c++/turtle/run.rs

@@ -30,7 +30,7 @@ fn main() -> eyre::Result<()> {
     build_cxx_node(
         &root,
         &[
-            &dunce::canonicalize(Path::new("node-rust-api").join("main.cc"))?,
+            &dunce::canonicalize(Path::new("./").join("main.cc"))?,
             &dunce::canonicalize(node_cxxbridge.join("dora-node-api.cc"))?,
             &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/sensor_msgs.cc"))?,
             &dunce::canonicalize(node_cxxbridge.join("ros2-bridge/msg/geometry_msgs.cc"))?,
@@ -150,7 +150,7 @@ fn build_cxx_node(root: &Path, paths: &[&Path], out_name: &str, args: &[&str]) -
     clang.arg("-L").arg(root.join("target").join("debug"));
     clang
         .arg("--output")
-        .arg(Path::new("../build").join(format!("{out_name}{EXE_SUFFIX}")));
+        .arg(Path::new("./build").join(format!("{out_name}{EXE_SUFFIX}")));
     if let Some(parent) = paths[0].parent() {
         clang.current_dir(parent);
     }

examples/ros2-bridge/rust/README.md

@@ -0,0 +1,17 @@
+# `ros2-bridge` Rust Examples
+
+These examples show how to interact between ROS2 and Dora with Rust, including topic pub/sub service client/server and action client/server.
+The dataflow consists of the [ROS2's examples package](https://index.ros.org/r/examples/).
+
+## Setup
+
+These examples requires a sourced ROS2 installation.
+
+- To set up ROS2, follow the [ROS2 installation](https://docs.ros.org/en/iron/Installation.html) guide.
+- Don't forget to [source the ROS2 setup files](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html#source-the-setup-files)
+- Install the corresponding ROS2 examples package
+
+## Running
+
+- Run the corresponding ROS2 node.
+- Run the Dora dataflow.