mod builder;
mod graph;
use std::cmp::PartialEq;
use std::ffi::CStr;
use std::fmt;
use std::sync::{Arc, Mutex, Weak};
use std::vec::Vec;
use libc::c_char;
use rosidl_runtime_rs::Message;
pub use self::builder::*;
pub use self::graph::*;
use crate::rcl_bindings::*;
use crate::{
Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile,
RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback,
ToResult,
};
impl Drop for rcl_node_t {
fn drop(&mut self) {
unsafe { rcl_node_fini(self).ok().unwrap() };
}
}
unsafe impl Send for rcl_node_t {}
pub struct Node {
pub(crate) rcl_node_mtx: Arc<Mutex<rcl_node_t>>,
pub(crate) rcl_context_mtx: Arc<Mutex<rcl_context_t>>,
pub(crate) clients: Vec<Weak<dyn ClientBase>>,
pub(crate) guard_conditions: Vec<Weak<GuardCondition>>,
pub(crate) services: Vec<Weak<dyn ServiceBase>>,
pub(crate) subscriptions: Vec<Weak<dyn SubscriptionBase>>,
_parameter_map: ParameterOverrideMap,
}
impl Eq for Node {}
impl PartialEq for Node {
fn eq(&self, other: &Self) -> bool {
Arc::ptr_eq(&self.rcl_node_mtx, &other.rcl_node_mtx)
}
}
impl fmt::Debug for Node {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> {
f.debug_struct("Node")
.field("fully_qualified_name", &self.fully_qualified_name())
.finish()
}
}
impl Node {
#[allow(clippy::new_ret_no_self)]
pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
Self::builder(context, node_name).build()
}
pub fn name(&self) -> String {
self.call_string_getter(rcl_node_get_name)
}
pub fn namespace(&self) -> String {
self.call_string_getter(rcl_node_get_namespace)
}
pub fn fully_qualified_name(&self) -> String {
self.call_string_getter(rcl_node_get_fully_qualified_name)
}
fn call_string_getter(
&self,
getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char,
) -> String {
unsafe { call_string_getter_with_handle(&*self.rcl_node_mtx.lock().unwrap(), getter) }
}
pub fn create_client<T>(&mut self, topic: &str) -> Result<Arc<Client<T>>, RclrsError>
where
T: rosidl_runtime_rs::Service,
{
let client = Arc::new(Client::<T>::new(self, topic)?);
self.clients
.push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
Ok(client)
}
pub fn create_guard_condition(&mut self) -> Arc<GuardCondition> {
let guard_condition = Arc::new(GuardCondition::new_with_rcl_context(
&mut self.rcl_context_mtx.lock().unwrap(),
None,
));
self.guard_conditions
.push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
guard_condition
}
pub fn create_guard_condition_with_callback<F>(&mut self, callback: F) -> Arc<GuardCondition>
where
F: Fn() + Send + Sync + 'static,
{
let guard_condition = Arc::new(GuardCondition::new_with_rcl_context(
&mut self.rcl_context_mtx.lock().unwrap(),
Some(Box::new(callback) as Box<dyn Fn() + Send + Sync>),
));
self.guard_conditions
.push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
guard_condition
}
pub fn create_publisher<T>(
&self,
topic: &str,
qos: QoSProfile,
) -> Result<Publisher<T>, RclrsError>
where
T: Message,
{
Publisher::<T>::new(self, topic, qos)
}
pub fn create_service<T, F>(
&mut self,
topic: &str,
callback: F,
) -> Result<Arc<Service<T>>, RclrsError>
where
T: rosidl_runtime_rs::Service,
F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send,
{
let service = Arc::new(Service::<T>::new(self, topic, callback)?);
self.services
.push(Arc::downgrade(&service) as Weak<dyn ServiceBase>);
Ok(service)
}
pub fn create_subscription<T, Args>(
&mut self,
topic: &str,
qos: QoSProfile,
callback: impl SubscriptionCallback<T, Args>,
) -> Result<Arc<Subscription<T>>, RclrsError>
where
T: Message,
{
let subscription = Arc::new(Subscription::<T>::new(self, topic, qos, callback)?);
self.subscriptions
.push(Arc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
Ok(subscription)
}
pub(crate) fn live_subscriptions(&self) -> Vec<Arc<dyn SubscriptionBase>> {
self.subscriptions
.iter()
.filter_map(Weak::upgrade)
.collect()
}
pub(crate) fn live_clients(&self) -> Vec<Arc<dyn ClientBase>> {
self.clients.iter().filter_map(Weak::upgrade).collect()
}
pub(crate) fn live_guard_conditions(&self) -> Vec<Arc<GuardCondition>> {
self.guard_conditions
.iter()
.filter_map(Weak::upgrade)
.collect()
}
pub(crate) fn live_services(&self) -> Vec<Arc<dyn ServiceBase>> {
self.services.iter().filter_map(Weak::upgrade).collect()
}
pub fn domain_id(&self) -> usize {
let rcl_node = &*self.rcl_node_mtx.lock().unwrap();
let mut domain_id: usize = 0;
let ret = unsafe {
rcl_node_get_domain_id(rcl_node, &mut domain_id)
};
debug_assert_eq!(ret, 0);
domain_id
}
pub fn builder(context: &Context, node_name: &str) -> NodeBuilder {
NodeBuilder::new(context, node_name)
}
}
unsafe fn call_string_getter_with_handle(
rcl_node: &rcl_node_t,
getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char,
) -> String {
let char_ptr = getter(rcl_node);
debug_assert!(!char_ptr.is_null());
let cstr = CStr::from_ptr(char_ptr);
cstr.to_string_lossy().into_owned()
}
#[cfg(test)]
mod tests {
use super::*;
fn assert_send<T: Send>() {}
fn assert_sync<T: Sync>() {}
#[test]
fn node_is_send_and_sync() {
assert_send::<Node>();
assert_sync::<Node>();
}
}