diff --git a/examples/rust-ros2-dataflow/README.md b/examples/rust-ros2-dataflow/README.md new file mode 100644 index 00000000..6c8445e9 --- /dev/null +++ b/examples/rust-ros2-dataflow/README.md @@ -0,0 +1,21 @@ +# `rust-ros2-dataflow` Example + +This example shows how to publish/subscribe to both ROS2 and Dora. The dataflow consists of a single node that sends random movement commands to the [ROS2 `turtlesim_node`](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html). + +## Setup + +This 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) +- Follow tasks 1 and 2 of the [ROS2 turtlesim tutorial](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html#id3) + - Install the turtlesim package + - Start the turtlesim node through `ros2 run turtlesim turtlesim_node` + +## Running + +After sourcing the ROS2 installation and starting the `turtlesim` node, you can run this example to move the turtle in random directions: + +``` +cargo run --example rust-ros2-dataflow --features ros2-examples +``` diff --git a/examples/rust-ros2-dataflow/dataflow.yml b/examples/rust-ros2-dataflow/dataflow.yml new file mode 100644 index 00000000..83583f75 --- /dev/null +++ b/examples/rust-ros2-dataflow/dataflow.yml @@ -0,0 +1,9 @@ +nodes: + - id: rust-node + custom: + build: cargo build -p rust-ros2-dataflow-example-node --features ros2 + source: ../../target/debug/rust-ros2-dataflow-example-node + inputs: + tick: dora/timer/millis/500 + outputs: + - pose diff --git a/examples/rust-ros2-dataflow/node/Cargo.toml b/examples/rust-ros2-dataflow/node/Cargo.toml new file mode 100644 index 00000000..daa2929c --- /dev/null +++ b/examples/rust-ros2-dataflow/node/Cargo.toml @@ -0,0 +1,23 @@ +[package] +name = "rust-ros2-dataflow-example-node" +version.workspace = true +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[features] +# enables binaries that depend on a sourced ROS2 installation +ros2 = [] + +[[bin]] +name = "rust-ros2-dataflow-example-node" +required-features = ["ros2"] + +[dependencies] +dora-node-api = { version = "0.2.4", features = ["tracing"] } +eyre = "0.6.8" +futures = "0.3.21" +rand = "0.8.5" +tokio = { version = "1.24.2", features = ["rt", "macros"] } +dora-ros2-bridge = { workspace = true } +serde_json = "1.0.99" diff --git a/examples/rust-ros2-dataflow/node/src/main.rs b/examples/rust-ros2-dataflow/node/src/main.rs new file mode 100644 index 00000000..313cc47d --- /dev/null +++ b/examples/rust-ros2-dataflow/node/src/main.rs @@ -0,0 +1,134 @@ +use dora_node_api::{ + self, + dora_core::config::DataId, + merged::{MergeExternal, MergedEvent}, + DoraNode, Event, +}; +use dora_ros2_bridge::{ + geometry_msgs::msg::{Twist, Vector3}, + ros2_client::{self, ros2, NodeOptions}, + rustdds::{self, policy}, + turtlesim::msg::Pose, +}; +use eyre::Context; + +fn main() -> eyre::Result<()> { + let mut ros_node = init_ros_node()?; + let turtle_vel_publisher = create_vel_publisher(&mut ros_node)?; + let turtle_pose_reader = create_pose_reader(&mut ros_node)?; + + let output = DataId::from("pose".to_owned()); + + let (mut node, dora_events) = DoraNode::init_from_env()?; + + let merged = dora_events.merge_external(Box::pin(turtle_pose_reader.async_stream())); + let mut events = futures::executor::block_on_stream(merged); + + for i in 0..1000 { + let event = match events.next() { + Some(input) => input, + None => break, + }; + + match event { + MergedEvent::Dora(event) => match event { + Event::Input { + id, + metadata: _, + data: _, + } => match id.as_str() { + "tick" => { + let direction = Twist { + linear: Vector3 { + x: rand::random::() + 1.0, + ..Default::default() + }, + angular: Vector3 { + z: (rand::random::() - 0.5) * 5.0, + ..Default::default() + }, + }; + println!("tick {i}, sending {direction:?}"); + turtle_vel_publisher.publish(direction).unwrap(); + } + other => eprintln!("Ignoring unexpected input `{other}`"), + }, + Event::Stop => println!("Received manual stop"), + other => eprintln!("Received unexpected input: {other:?}"), + }, + MergedEvent::External(pose) => { + println!("received pose event: {pose:?}"); + if let Ok((pose, _)) = pose { + let serialized = serde_json::to_string(&pose)?; + node.send_output_bytes( + output.clone(), + Default::default(), + serialized.len(), + serialized.as_bytes(), + )?; + } + } + } + } + + Ok(()) +} + +fn init_ros_node() -> eyre::Result { + let ros_context = ros2_client::Context::new().unwrap(); + + ros_context + .new_node( + "turtle_teleop", // name + "/ros2_demo", // namespace + NodeOptions::new().enable_rosout(true), + ) + .context("failed to create ros2 node") +} + +fn create_vel_publisher( + ros_node: &mut ros2_client::Node, +) -> eyre::Result> { + let topic_qos: rustdds::QosPolicies = { + rustdds::QosPolicyBuilder::new() + .durability(policy::Durability::Volatile) + .liveliness(policy::Liveliness::Automatic { + lease_duration: ros2::Duration::DURATION_INFINITE, + }) + .reliability(policy::Reliability::Reliable { + max_blocking_time: ros2::Duration::from_millis(100), + }) + .history(policy::History::KeepLast { depth: 1 }) + .build() + }; + + let turtle_cmd_vel_topic = ros_node + .create_topic( + "/turtle1/cmd_vel", + String::from("geometry_msgs::msg::dds_::Twist_"), + &topic_qos, + ) + .context("failed to create topic")?; + + // The point here is to publish Twist for the turtle + let turtle_cmd_vel_writer = ros_node + .create_publisher::(&turtle_cmd_vel_topic, None) + .context("failed to create publisher")?; + Ok(turtle_cmd_vel_writer) +} + +fn create_pose_reader( + ros_node: &mut ros2_client::Node, +) -> eyre::Result> { + let turtle_pose_topic = ros_node + .create_topic( + "/turtle1/pose", + String::from("turtlesim::msg::dds_::Pose_"), + &Default::default(), + ) + .context("failed to create topic")?; + let turtle_pose_reader = ros_node + .create_subscription::(&turtle_pose_topic, None) + .context("failed to create subscription")?; + Ok(turtle_pose_reader) +} diff --git a/examples/rust-ros2-dataflow/run.rs b/examples/rust-ros2-dataflow/run.rs new file mode 100644 index 00000000..2a0cc731 --- /dev/null +++ b/examples/rust-ros2-dataflow/run.rs @@ -0,0 +1,34 @@ +use eyre::WrapErr; +use std::path::Path; + +#[tokio::main] +async fn main() -> eyre::Result<()> { + let root = Path::new(env!("CARGO_MANIFEST_DIR")); + std::env::set_current_dir(root.join(file!()).parent().unwrap()) + .wrap_err("failed to set working dir")?; + + let dataflow = Path::new("dataflow.yml"); + build_dataflow(dataflow).await?; + + run_dataflow(dataflow).await?; + + Ok(()) +} + +async fn build_dataflow(dataflow: &Path) -> eyre::Result<()> { + let mut cmd = tokio::process::Command::new("dora"); + cmd.arg("build").arg(dataflow); + if !cmd.status().await?.success() { + eyre::bail!("failed to build dataflow"); + }; + Ok(()) +} + +async fn run_dataflow(dataflow: &Path) -> eyre::Result<()> { + let mut cmd = tokio::process::Command::new("dora-daemon"); + cmd.arg("--run-dataflow").arg(dataflow); + if !cmd.status().await?.success() { + eyre::bail!("failed to build dataflow"); + }; + Ok(()) +}