diff --git a/zenoh-test-ros2dds/tests/test_ros_client_zenoh_action.rs b/zenoh-test-ros2dds/tests/test_ros_client_zenoh_action.rs index ab250ea..2c17b25 100644 --- a/zenoh-test-ros2dds/tests/test_ros_client_zenoh_action.rs +++ b/zenoh-test-ros2dds/tests/test_ros_client_zenoh_action.rs @@ -17,6 +17,7 @@ pub mod common; use std::time::Duration; use common::DEFAULT_TIMEOUT; +use futures::StreamExt; use r2r::{self}; use serde_derive::{Deserialize, Serialize}; use zenoh::Wait; @@ -48,6 +49,12 @@ pub struct FibonacciResult { pub sequence: Vec, } +#[derive(Serialize, Deserialize, PartialEq, Clone)] +pub struct FibonacciFeedback { + pub goal_id: [u8; 16], + pub sequence: Vec, +} + #[test] fn test_ros_client_zenoh_action() { let rt = tokio::runtime::Runtime::new().unwrap(); @@ -62,19 +69,29 @@ fn test_ros_client_zenoh_action() { // We send request 5 and expect result [0, 1, 1, 2, 3, 5] let action_request = 5; let action_result = vec![0, 1, 1, 2, 3, 5]; + let feedback = vec![0, 1, 1, 2, 3]; // Zenoh action server - // Note that we just create send_goal and get_result to implement the minimal action server - // TODO: We should also test `_action/feedback` + // Note that we just create send_goal, get_result and feedback to implement the minimal action server let session = zenoh::open(zenoh::Config::default()).await.unwrap(); let send_goal_expr = TEST_ACTION_R2Z.to_string() + "/_action/send_goal"; let get_result_expr = TEST_ACTION_R2Z.to_string() + "/_action/get_result"; + let feedback_expr = TEST_ACTION_R2Z.to_string() + "/_action/feedback"; + // feedback publisher + let feedback_publisher = session + .declare_publisher(feedback_expr.clone()) + .await + .unwrap(); + // send_goal let _send_goal_server = session .declare_queryable(send_goal_expr.clone()) .callback(move |query| { let send_goal: FibonacciSendGoal = cdr::deserialize(&query.payload().unwrap().to_bytes()).unwrap(); - println!("Receive {:?}: {:?}", send_goal.goal_id, send_goal.goal); + println!( + "Receive goal request: order {:?}, goal id {:?}", + send_goal.goal, send_goal.goal_id + ); assert_eq!(send_goal.goal, action_request); // Reply to the action client @@ -89,14 +106,28 @@ fn test_ros_client_zenoh_action() { }) .await .unwrap(); - let sequence = action_result.clone(); + // get_result + let feedback_seq = feedback.clone(); + let result_seq = action_result.clone(); let _get_result_server = session .declare_queryable(get_result_expr.clone()) .callback(move |query| { + let req_result: ActionResultRequest = + cdr::deserialize(&query.payload().unwrap().to_bytes()).unwrap(); + // Publish feedback + let buf = cdr::serialize::<_, _, cdr::CdrLe>( + &FibonacciFeedback { + goal_id: req_result.goal_id, + sequence: feedback_seq.clone(), + }, + cdr::Infinite, + ) + .unwrap(); + feedback_publisher.put(buf).wait().unwrap(); // Reply the get result let get_result_response = FibonacciResult { status: 4, - sequence: sequence.clone(), + sequence: result_seq.clone(), }; let payload = cdr::serialize::<_, _, cdr::CdrLe>(&get_result_response, cdr::Infinite) @@ -127,10 +158,13 @@ fn test_ros_client_zenoh_action() { let my_goal = r2r::example_interfaces::action::Fibonacci::Goal { order: action_request, }; - let (_goal, result_fut, mut _feedback) = + let (_goal, result_fut, mut feedback_fut) = client.send_goal_request(my_goal).unwrap().await.unwrap(); + let feedback_result = feedback_fut.next().await.unwrap(); + println!("Receive feedback {:?}", feedback_result); + assert_eq!(feedback_result.sequence, feedback); let (goal_status, result) = result_fut.await.unwrap(); - println!("Received result {:?}, status {:?}", result, goal_status); + println!("Receive result {:?}, status {:?}", result, goal_status); assert_eq!(result.sequence, action_result); // Tell the main test thread, we're completed diff --git a/zenoh-test-ros2dds/tests/test_zenoh_client_ros_action.rs b/zenoh-test-ros2dds/tests/test_zenoh_client_ros_action.rs index 293775d..a29c69f 100644 --- a/zenoh-test-ros2dds/tests/test_zenoh_client_ros_action.rs +++ b/zenoh-test-ros2dds/tests/test_zenoh_client_ros_action.rs @@ -47,6 +47,12 @@ pub struct FibonacciResult { pub sequence: Vec, } +#[derive(Serialize, Deserialize, PartialEq, Clone)] +pub struct FibonacciFeedback { + pub goal_id: [u8; 16], + pub sequence: Vec, +} + #[test] fn test_zenoh_client_ros_action() { let rt = tokio::runtime::Runtime::new().unwrap(); @@ -61,6 +67,7 @@ fn test_zenoh_client_ros_action() { // We send request 5 and expect result [0, 1, 1, 2, 3, 5] let action_request = 5; let action_result = vec![0, 1, 1, 2, 3, 5]; + let feedback = vec![0, 1, 1, 2, 3]; // Random goal id let goal_id = [1; 16]; @@ -74,14 +81,19 @@ fn test_zenoh_client_ros_action() { ) .unwrap(); let sequence = action_result.clone(); + let feedback_seq = feedback.clone(); tokio::spawn(async move { while let Some(req) = action_server.next().await { println!( - r#"Receive goal request with order {}, goal id: {}"#, + "Receive goal request: order {}, goal id {}", req.goal.order, req.uuid ); assert_eq!(req.goal.order, action_request); let (mut recv_goal, mut _cancel) = req.accept().unwrap(); + let feedback_data = r2r::example_interfaces::action::Fibonacci::Feedback { + sequence: feedback_seq.clone(), + }; + recv_goal.publish_feedback(feedback_data).unwrap(); recv_goal .succeed(r2r::example_interfaces::action::Fibonacci::Result { sequence: sequence.clone(), @@ -96,12 +108,13 @@ fn test_zenoh_client_ros_action() { }); // Zenoh action client - // TODO: We should also test `_action/feedback` let session = zenoh::open(zenoh::Config::default()).await.unwrap(); let send_goal_expr = TEST_ACTION_Z2R.to_string() + "/_action/send_goal"; let get_result_expr = TEST_ACTION_Z2R.to_string() + "/_action/get_result"; + let feedback_expr = TEST_ACTION_Z2R.to_string() + "/_action/feedback"; let send_goal_client = session.declare_querier(send_goal_expr).await.unwrap(); let get_result_client = session.declare_querier(get_result_expr).await.unwrap(); + let subscriber = session.declare_subscriber(feedback_expr).await.unwrap(); // Wait for the environment to be ready tokio::time::sleep(Duration::from_secs(1)).await; @@ -117,7 +130,14 @@ fn test_zenoh_client_ros_action() { let reader = reply_sample.result().unwrap().payload().reader(); let reply: ActionSendGoalResponse = cdr::deserialize_from(reader, cdr::size::Infinite).unwrap(); - println!("The result of SendGoal: {:?}", reply.accept); + println!("Receive the result of SendGoal: {:?}", reply.accept); + + // Receive feedback + let sample = subscriber.recv_async().await.unwrap(); + let feedback_result: FibonacciFeedback = + cdr::deserialize_from(sample.payload().reader(), cdr::size::Infinite).unwrap(); + println!("Receive feedback {:?}", feedback_result.sequence); + assert_eq!(feedback_result.sequence, feedback); // Get the result from ROS 2 action server let req = ActionResultRequest { goal_id }; @@ -126,7 +146,10 @@ fn test_zenoh_client_ros_action() { let reply_sample = recv_handler.recv().unwrap(); let reader = reply_sample.result().unwrap().payload().reader(); let reply: FibonacciResult = cdr::deserialize_from(reader, cdr::size::Infinite).unwrap(); - println!("The result: {:?} {:?}", reply.status, reply.sequence); + println!( + "Receive result: {:?}, status {:?}", + reply.sequence, reply.status + ); assert_eq!(reply.sequence, action_result); // Tell the main test thread, we're completed