Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support feedback for action test. #373

Merged
merged 2 commits into from
Jan 9, 2025
Merged

Support feedback for action test. #373

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a66b14f
Support feedback for action test.
evshary Jan 7, 2025
301b1b7
Remove TODO.
evshary Jan 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 41 additions & 7 deletions zenoh-test-ros2dds/tests/test_ros_client_zenoh_action.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -48,6 +49,12 @@ pub struct FibonacciResult {
pub sequence: Vec<i32>,
}

#[derive(Serialize, Deserialize, PartialEq, Clone)]
pub struct FibonacciFeedback {
pub goal_id: [u8; 16],
pub sequence: Vec<i32>,
}

#[test]
fn test_ros_client_zenoh_action() {
let rt = tokio::runtime::Runtime::new().unwrap();
Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down
31 changes: 27 additions & 4 deletions zenoh-test-ros2dds/tests/test_zenoh_client_ros_action.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,12 @@ pub struct FibonacciResult {
pub sequence: Vec<i32>,
}

#[derive(Serialize, Deserialize, PartialEq, Clone)]
pub struct FibonacciFeedback {
pub goal_id: [u8; 16],
pub sequence: Vec<i32>,
}

#[test]
fn test_zenoh_client_ros_action() {
let rt = tokio::runtime::Runtime::new().unwrap();
Expand All @@ -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];

Expand All @@ -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(),
Expand All @@ -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;
Expand All @@ -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 };
Expand All @@ -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
Expand Down
Loading