Instead of exposing an Option<QosProfile> I now make the user input an actual QosProfile. I also did the same for the clients and modified some of the examples.

This commit is contained in:
Tijl Jappens 2024-05-15 08:43:35 +00:00
parent 862281f814
commit a6d15e77c2
9 changed files with 35 additions and 37 deletions

View File

@ -2,7 +2,7 @@ use futures::{executor::LocalPool, task::LocalSpawnExt, Future};
use std::io::Write; use std::io::Write;
use r2r::example_interfaces::srv::AddTwoInts; use r2r::{example_interfaces::srv::AddTwoInts, QosProfile};
async fn requester_task( async fn requester_task(
node_available: impl Future<Output = r2r::Result<()>>, c: r2r::Client<AddTwoInts::Service>, node_available: impl Future<Output = r2r::Result<()>>, c: r2r::Client<AddTwoInts::Service>,
@ -29,7 +29,7 @@ async fn requester_task(
fn main() -> Result<(), Box<dyn std::error::Error>> { fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?; let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?; let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints")?; let client = node.create_client::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?;
let service_available = r2r::Node::is_available(&client)?; let service_available = r2r::Node::is_available(&client)?;

View File

@ -1,6 +1,6 @@
use futures::{executor::LocalPool, select, stream::StreamExt, task::LocalSpawnExt, FutureExt}; use futures::{executor::LocalPool, select, stream::StreamExt, task::LocalSpawnExt, FutureExt};
use r2r::example_interfaces::srv::AddTwoInts; use r2r::{example_interfaces::srv::AddTwoInts, QosProfile};
/// ///
/// This example demonstrates how we can chain async service calls. /// This example demonstrates how we can chain async service calls.
@ -11,9 +11,9 @@ use r2r::example_interfaces::srv::AddTwoInts;
fn main() -> Result<(), Box<dyn std::error::Error>> { fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?; let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?; let mut node = r2r::Node::create(ctx, "testnode", "")?;
let mut service = node.create_service::<AddTwoInts::Service>("/add_two_ints")?; let mut service = node.create_service::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?;
let service_delayed = node.create_service::<AddTwoInts::Service>("/add_two_ints_delayed")?; let service_delayed = node.create_service::<AddTwoInts::Service>("/add_two_ints_delayed", QosProfile::default())?;
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints_delayed")?; let client = node.create_client::<AddTwoInts::Service>("/add_two_ints_delayed", QosProfile::default())?;
let mut timer = node.create_wall_timer(std::time::Duration::from_millis(250))?; let mut timer = node.create_wall_timer(std::time::Duration::from_millis(250))?;
let mut timer2 = node.create_wall_timer(std::time::Duration::from_millis(2000))?; let mut timer2 = node.create_wall_timer(std::time::Duration::from_millis(2000))?;
// wait for service to be available // wait for service to be available

View File

@ -1,3 +1,5 @@
use r2r::QosProfile;
#[tokio::main] #[tokio::main]
async fn main() -> Result<(), Box<dyn std::error::Error>> { async fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?; let ctx = r2r::Context::create()?;
@ -5,7 +7,7 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
let duration = std::time::Duration::from_millis(2500); let duration = std::time::Duration::from_millis(2500);
use r2r::example_interfaces::srv::AddTwoInts; use r2r::example_interfaces::srv::AddTwoInts;
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints")?; let client = node.create_client::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?;
let mut timer = node.create_wall_timer(duration)?; let mut timer = node.create_wall_timer(duration)?;
let waiting = r2r::Node::is_available(&client)?; let waiting = r2r::Node::is_available(&client)?;

View File

@ -75,7 +75,7 @@ async fn client(arc_node: Arc<Mutex<r2r::Node>>) -> Result<(), r2r::Error> {
let (client, mut timer, service_available) = { let (client, mut timer, service_available) = {
// Limiting the scope when locking the arc // Limiting the scope when locking the arc
let mut node = arc_node.lock().unwrap(); let mut node = arc_node.lock().unwrap();
let client = node.create_client::<AddTwoInts::Service>("/add_two_ints")?; let client = node.create_client::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?;
let timer = node.create_wall_timer(std::time::Duration::from_secs(2))?; let timer = node.create_wall_timer(std::time::Duration::from_secs(2))?;
let service_available = r2r::Node::is_available(&client)?; let service_available = r2r::Node::is_available(&client)?;
(client, timer, service_available) (client, timer, service_available)
@ -98,7 +98,7 @@ async fn service(arc_node: Arc<Mutex<r2r::Node>>) -> Result<(), r2r::Error> {
let mut service = { let mut service = {
// Limiting the scope when locking the arc // Limiting the scope when locking the arc
let mut node = arc_node.lock().unwrap(); let mut node = arc_node.lock().unwrap();
node.create_service::<AddTwoInts::Service>("/add_two_ints")? node.create_service::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?
}; };
loop { loop {
match service.next().await { match service.next().await {

View File

@ -1,4 +1,5 @@
use futures::stream::StreamExt; use futures::stream::StreamExt;
use r2r::QosProfile;
#[tokio::main] #[tokio::main]
async fn main() -> Result<(), Box<dyn std::error::Error>> { async fn main() -> Result<(), Box<dyn std::error::Error>> {
@ -6,7 +7,7 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut node = r2r::Node::create(ctx, "testnode", "")?; let mut node = r2r::Node::create(ctx, "testnode", "")?;
use r2r::example_interfaces::srv::AddTwoInts; use r2r::example_interfaces::srv::AddTwoInts;
let mut service = node.create_service::<AddTwoInts::Service>("/add_two_ints")?; let mut service = node.create_service::<AddTwoInts::Service>("/add_two_ints", QosProfile::default())?;
let handle = tokio::task::spawn_blocking(move || loop { let handle = tokio::task::spawn_blocking(move || loop {
node.spin_once(std::time::Duration::from_millis(100)); node.spin_once(std::time::Duration::from_millis(100));

View File

@ -1,4 +1,5 @@
use futures::{executor::LocalPool, task::LocalSpawnExt, Future}; use futures::{executor::LocalPool, task::LocalSpawnExt, Future};
use r2r::QosProfile;
async fn requester_task( async fn requester_task(
node_available: impl Future<Output = r2r::Result<()>>, c: r2r::ClientUntyped, node_available: impl Future<Output = r2r::Result<()>>, c: r2r::ClientUntyped,
@ -25,7 +26,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?; let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?; let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client = let client =
node.create_client_untyped("/add_two_ints", "example_interfaces/srv/AddTwoInts")?; node.create_client_untyped("/add_two_ints", "example_interfaces/srv/AddTwoInts", QosProfile::default())?;
let service_available = r2r::Node::is_available(&client)?; let service_available = r2r::Node::is_available(&client)?;

View File

@ -6,7 +6,7 @@ use std::{
sync::{Mutex, Weak}, sync::{Mutex, Weak},
}; };
use crate::{error::*, msg_types::*}; use crate::{error::*, msg_types::*, QosProfile};
use r2r_rcl::*; use r2r_rcl::*;
/// ROS service client. /// ROS service client.
@ -319,14 +319,15 @@ impl Client_ for UntypedClient_ {
} }
pub fn create_client_helper( pub fn create_client_helper(
node: *mut rcl_node_t, service_name: &str, service_ts: *const rosidl_service_type_support_t, node: *mut rcl_node_t, service_name: &str, service_ts: *const rosidl_service_type_support_t, qos_profile: QosProfile,
) -> Result<rcl_client_t> { ) -> Result<rcl_client_t> {
let mut client_handle = unsafe { rcl_get_zero_initialized_client() }; let mut client_handle = unsafe { rcl_get_zero_initialized_client() };
let service_name_c_string = let service_name_c_string =
CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe { let result = unsafe {
let client_options = rcl_client_get_default_options(); let mut client_options = rcl_client_get_default_options();
client_options.qos = qos_profile.into();
rcl_client_init( rcl_client_init(
&mut client_handle, &mut client_handle,
node, node,

View File

@ -278,7 +278,7 @@ impl Node {
"{}/set_parameters", "{}/set_parameters",
node_name node_name
), ),
None)?; QosProfile::default())?;
let params = self.params.clone(); let params = self.params.clone();
let params_struct_clone = params_struct.clone(); let params_struct_clone = params_struct.clone();
@ -338,11 +338,9 @@ impl Node {
// rcl_interfaces/srv/GetParameters // rcl_interfaces/srv/GetParameters
let get_params_request_stream = self let get_params_request_stream = self
.create_service::<rcl_interfaces::srv::GetParameters::Service>(&format!( .create_service::<rcl_interfaces::srv::GetParameters::Service>(
"{}/get_parameters", &format!("{}/get_parameters",node_name),
node_name QosProfile::default())?;
),
None)?;
let params = self.params.clone(); let params = self.params.clone();
let params_struct_clone = params_struct.clone(); let params_struct_clone = params_struct.clone();
@ -381,7 +379,7 @@ impl Node {
// rcl_interfaces/srv/ListParameters // rcl_interfaces/srv/ListParameters
use rcl_interfaces::srv::ListParameters; use rcl_interfaces::srv::ListParameters;
let list_params_request_stream = self let list_params_request_stream = self
.create_service::<ListParameters::Service>(&format!("{}/list_parameters", node_name), None)?; .create_service::<ListParameters::Service>(&format!("{}/list_parameters", node_name), QosProfile::default())?;
let params = self.params.clone(); let params = self.params.clone();
let list_params_future = list_params_request_stream.for_each( let list_params_future = list_params_request_stream.for_each(
@ -395,8 +393,7 @@ impl Node {
// rcl_interfaces/srv/DescribeParameters // rcl_interfaces/srv/DescribeParameters
use rcl_interfaces::srv::DescribeParameters; use rcl_interfaces::srv::DescribeParameters;
let desc_params_request_stream = self.create_service::<DescribeParameters::Service>( let desc_params_request_stream = self.create_service::<DescribeParameters::Service>(
&format!("{node_name}/describe_parameters"), &format!("{node_name}/describe_parameters"), QosProfile::default())?;
None)?;
let params = self.params.clone(); let params = self.params.clone();
let desc_params_future = desc_params_request_stream.for_each( let desc_params_future = desc_params_request_stream.for_each(
@ -411,7 +408,7 @@ impl Node {
use rcl_interfaces::srv::GetParameterTypes; use rcl_interfaces::srv::GetParameterTypes;
let get_param_types_request_stream = self.create_service::<GetParameterTypes::Service>( let get_param_types_request_stream = self.create_service::<GetParameterTypes::Service>(
&format!("{node_name}/get_parameter_types"), &format!("{node_name}/get_parameter_types"),
None)?; QosProfile::default())?;
let params = self.params.clone(); let params = self.params.clone();
let get_param_types_future = get_param_types_request_stream.for_each( let get_param_types_future = get_param_types_request_stream.for_each(
@ -639,7 +636,7 @@ impl Node {
/// This function returns a `Stream` of `ServiceRequest`:s. Call /// This function returns a `Stream` of `ServiceRequest`:s. Call
/// `respond` on the Service Request to send the reply. /// `respond` on the Service Request to send the reply.
pub fn create_service<T: 'static>( pub fn create_service<T: 'static>(
&mut self, service_name: &str, qos_profile: Option<QosProfile> &mut self, service_name: &str, qos_profile: QosProfile,
) -> Result<impl Stream<Item = ServiceRequest<T>> + Unpin> ) -> Result<impl Stream<Item = ServiceRequest<T>> + Unpin>
where where
T: WrappedServiceTypeSupport, T: WrappedServiceTypeSupport,
@ -661,12 +658,13 @@ impl Node {
/// Create a ROS service client. /// Create a ROS service client.
/// ///
/// A service client is used to make requests to a ROS service server. /// A service client is used to make requests to a ROS service server.
pub fn create_client<T: 'static>(&mut self, service_name: &str) -> Result<Client<T>> pub fn create_client<T: 'static>(
&mut self, service_name: &str, qos_profile: QosProfile,) -> Result<Client<T>>
where where
T: WrappedServiceTypeSupport, T: WrappedServiceTypeSupport,
{ {
let client_handle = let client_handle =
create_client_helper(self.node_handle.as_mut(), service_name, T::get_ts())?; create_client_helper(self.node_handle.as_mut(), service_name, T::get_ts(), qos_profile)?;
let ws = TypedClient::<T> { let ws = TypedClient::<T> {
rcl_handle: client_handle, rcl_handle: client_handle,
response_channels: Vec::new(), response_channels: Vec::new(),
@ -686,11 +684,11 @@ impl Node {
/// with `serde_json::Value`s instead of concrete types. Useful /// with `serde_json::Value`s instead of concrete types. Useful
/// when you cannot know the type of the message at compile time. /// when you cannot know the type of the message at compile time.
pub fn create_client_untyped( pub fn create_client_untyped(
&mut self, service_name: &str, service_type: &str, &mut self, service_name: &str, service_type: &str, qos_profile: QosProfile,
) -> Result<ClientUntyped> { ) -> Result<ClientUntyped> {
let service_type = UntypedServiceSupport::new_from(service_type)?; let service_type = UntypedServiceSupport::new_from(service_type)?;
let client_handle = let client_handle =
create_client_helper(self.node_handle.as_mut(), service_name, service_type.ts)?; create_client_helper(self.node_handle.as_mut(), service_name, service_type.ts, qos_profile)?;
let client = UntypedClient_ { let client = UntypedClient_ {
service_type, service_type,
rcl_handle: client_handle, rcl_handle: client_handle,

View File

@ -113,20 +113,15 @@ where
} }
pub fn create_service_helper( pub fn create_service_helper(
node: &mut rcl_node_t, service_name: &str, service_ts: *const rosidl_service_type_support_t, qos_profile: Option<QosProfile> node: &mut rcl_node_t, service_name: &str, service_ts: *const rosidl_service_type_support_t, qos_profile: QosProfile,
) -> Result<rcl_service_t> { ) -> Result<rcl_service_t> {
let mut service_handle = unsafe { rcl_get_zero_initialized_service() }; let mut service_handle = unsafe { rcl_get_zero_initialized_service() };
let service_name_c_string = let service_name_c_string =
CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
let result = unsafe { let result = unsafe {
let service_options= let mut service_options = rcl_service_get_default_options();
match qos_profile { service_options.qos = qos_profile.into();
Some(profile) => {let mut service_options = rcl_service_get_default_options();
service_options.qos = profile.into();
service_options}
None => {rcl_service_get_default_options()}
};
rcl_service_init( rcl_service_init(
&mut service_handle, &mut service_handle,
node, node,