diff --git a/rclrs/action_demo/Cargo.toml b/rclrs/action_demo/Cargo.toml new file mode 100644 index 0000000..1eb8b17 --- /dev/null +++ b/rclrs/action_demo/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "action_demo" +version = "0.1.0" +edition = "2024" + +[[bin]] +name = "action_client" +path = "src/action_client.rs" + +[[bin]] +name = "action_server" +path = "src/action_server.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} +tokio = "*" +rclrs = "0.7" +futures = "*" +example_interfaces = "*" + +# This specific version is compatible with Rust 1.75 +backtrace = "=0.3.74" \ No newline at end of file diff --git a/rclrs/action_demo/package.xml b/rclrs/action_demo/package.xml new file mode 100644 index 0000000..917fa9b --- /dev/null +++ b/rclrs/action_demo/package.xml @@ -0,0 +1,19 @@ + + + + examples_rclrs_action_demo + Nicolas Daube + 0.0.0 + Package containing an example of how to use actions in rclrs. + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/rclrs/action_demo/src/action_client.rs b/rclrs/action_demo/src/action_client.rs new file mode 100644 index 0000000..2af69ba --- /dev/null +++ b/rclrs/action_demo/src/action_client.rs @@ -0,0 +1,46 @@ +use rclrs::*; +use anyhow::{Result, Error}; +use example_interfaces::action::{Fibonacci, Fibonacci_Goal}; +use std::time::Duration; +use futures::StreamExt; + +fn main() -> Result<(), Error> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + + let node = executor.create_node("action_client")?; + log_info!(node.logger(), "Action client node starting ..."); + + let client = node + .create_action_client::(&"action_name") + .unwrap(); + + log_info!(node.logger(), "Wait for action server ..."); // Completly arbitrary + std::thread::sleep(Duration::from_secs(1)); + + let request = client.request_goal(Fibonacci_Goal { order: 10 }); + + let promise = executor.commands().run(async move { + let mut goal_client_stream = request.await.unwrap().stream(); + while let Some(event) = goal_client_stream.next().await { + match event { + GoalEvent::Feedback(feedback) => { + log_info!(node.logger(), "Received feedback: {}", feedback.sequence.last().unwrap()); + } + GoalEvent::Result((status, result)) => { + match status { + GoalStatusCode::Succeeded =>{log_info!(node.logger(), "Goal succeeded, complete result {:?}", result.sequence)} + GoalStatusCode::Cancelled =>{log_info!(node.logger(), "Goal canceled before end, result {:?}", result.sequence)} + _ => {}, + } + + return; + } + _ => {}, + } + } + }); + + executor.spin(SpinOptions::default().until_promise_resolved(promise)); + Ok(()) +} \ No newline at end of file diff --git a/rclrs/action_demo/src/action_server.rs b/rclrs/action_demo/src/action_server.rs new file mode 100644 index 0000000..4308bd9 --- /dev/null +++ b/rclrs/action_demo/src/action_server.rs @@ -0,0 +1,90 @@ +use rclrs::*; +use tokio::sync::mpsc::unbounded_channel; +use std::time::Duration; +use anyhow::{Error, Result}; +use example_interfaces::action::{Fibonacci, Fibonacci_Feedback, Fibonacci_Result}; + +async fn fibonacci_action(node: Node, handle: RequestedGoal) -> TerminatedGoal { + let goal_order = handle.goal().order; // Get the fibonacci order inside the requested goal + log_info!(node.logger(), "Received goal with order: {} from client", goal_order); + + // Reject the forbidden goal + if goal_order < 0 { + log_error!(node.logger(), "Rejecting goal, can't compute Fibonacci sequence of negative number"); + return handle.reject(); + } + + let mut result = Fibonacci_Result::default(); // Initialize the result variable + let mut sequence = Vec::new(); // Initialize the feedback + + // Notifying the acceptance of the goal and starting the execution phase + let executing = match handle.accept().begin() { + BeginAcceptedGoal::Execute(executing) => executing, + BeginAcceptedGoal::Cancel(cancelling) => { + return cancelling.cancelled_with(result); + } + }; + + let (sender, mut receiver) = unbounded_channel(); + + // Execution thread computing the actual Fibonacci sequence + std::thread::spawn(move || { + let mut previous = 0; + let mut current = 1; + + for _ in 0..goal_order { + if let Err(_) = sender.send(current) { + return; + } + + let next = previous + current; + previous = current; + current = next; + std::thread::sleep(Duration::from_secs(1)); + } + }); + + // Consuming the successive results comming from the execution thread + loop { + match executing.unless_cancel_requested(receiver.recv()).await { + Ok(Some(next)) => { + // Still executing, push the current result as feedback + sequence.push(next); + executing.publish_feedback(Fibonacci_Feedback { + sequence: sequence.clone(), + }); + } + Ok(None) => { + // The end of the sequence is reached, return result + log_info!(node.logger(), "Sequence end reached, action succeeded"); + result.sequence = sequence; + return executing.succeeded_with(result); + } + Err(_) => { + // Cancel received, end the current execution + log_warn!(node.logger(), "Goal cancelled"); + let cancelling = executing.begin_cancelling(); + result.sequence = sequence; + return cancelling.cancelled_with(result); + } + } + } +} + +fn main() -> Result<(), Error> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + + let node = executor.create_node("action_server")?; + log_info!(node.logger(), "Action server node starting ..."); + + let action_node = node.clone(); + let _action = node.create_action_server( + &"action_name", + move |handle| { + fibonacci_action(action_node.clone(), handle) + }).unwrap(); + + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) +}