use std::time::{Duration, Instant};
use mio::{Events, Poll, PollOpt, Ready, Token};
use serde::{Deserialize, Serialize};
use ros2_client::{
AService, Context, Message, Name, Node, NodeName, NodeOptions, ServiceMapping, ServiceTypeName,
};
use rustdds::{policy, QosPolicies, QosPolicyBuilder};
const RESPONSE_TOKEN: Token = Token(7);
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct AddTwoIntsRequest {
pub a: i64,
pub b: i64,
}
impl Message for AddTwoIntsRequest {}
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct AddTwoIntsResponse {
pub sum: i64,
}
impl Message for AddTwoIntsResponse {}
fn main() {
pretty_env_logger::init();
println!(">>> ros2_service starting...");
let mut node = create_node();
let service_qos = create_qos();
println!(">>> ros2_service node started");
let client = node
.create_client::<AService<AddTwoIntsRequest, AddTwoIntsResponse>>(
ServiceMapping::Enhanced,
&Name::new("/", "add_two_ints").unwrap(),
&ServiceTypeName::new("example_interfaces", "AddTwoInts"),
service_qos.clone(),
service_qos,
)
.unwrap();
println!(">>> ros2_service client created");
let poll = Poll::new().unwrap();
poll
.register(&client, RESPONSE_TOKEN, Ready::readable(), PollOpt::edge())
.unwrap();
let mut request_generator = 0;
let mut request_sent_at = Instant::now();
loop {
let mut events = Events::with_capacity(100);
poll
.poll(&mut events, Some(Duration::from_secs(1)))
.unwrap();
for event in events.iter() {
match event.token() {
RESPONSE_TOKEN => {
while let Ok(Some((id, response))) = client.receive_response() {
println!(
">>> Response received: response: {:?} - response id: {:?}, ",
response, id,
);
}
}
_ => println!(">>> Unknown poll token {:?}", event.token()),
}
}
let now = Instant::now(); if now.duration_since(request_sent_at) > Duration::from_secs(2) {
request_sent_at = now;
println!(">>> request sending...");
request_generator += 3;
let a = request_generator % 5;
let b = request_generator % 7;
match client.send_request(AddTwoIntsRequest { a, b }) {
Ok(id) => {
println!(">>> request sent a={} b={}, {:?}", a, b, id);
}
Err(e) => {
println!(">>> request sending error {:?}", e);
}
}
}
}
}
fn create_qos() -> QosPolicies {
let service_qos: QosPolicies = {
QosPolicyBuilder::new()
.reliability(policy::Reliability::Reliable {
max_blocking_time: rustdds::Duration::from_millis(100),
})
.history(policy::History::KeepLast { depth: 1 })
.build()
};
service_qos
}
fn create_node() -> Node {
let context = Context::new().unwrap();
context
.new_node(
NodeName::new("/rustdds", "rustdds_client").unwrap(),
NodeOptions::new().enable_rosout(true),
)
.unwrap()
}