do_not_use_testing_rclrs/node/
graph.rs

1use std::collections::HashMap;
2use std::ffi::{CStr, CString};
3use std::slice;
4
5use crate::rcl_bindings::*;
6use crate::{Node, RclrsError, ToResult};
7
8impl Drop for rmw_names_and_types_t {
9    fn drop(&mut self) {
10        // SAFETY: No preconditions for this function.
11        unsafe {
12            rcl_names_and_types_fini(self).ok().unwrap();
13        }
14    }
15}
16
17impl Drop for rmw_topic_endpoint_info_array_t {
18    fn drop(&mut self) {
19        // SAFETY: No preconditions for this function.
20        unsafe {
21            rmw_topic_endpoint_info_array_fini(self, &mut rcutils_get_default_allocator())
22                .ok()
23                .unwrap();
24        }
25    }
26}
27
28impl Drop for rcutils_string_array_t {
29    fn drop(&mut self) {
30        // SAFETY: No preconditions for this function.
31        unsafe {
32            rcutils_string_array_fini(self);
33        }
34    }
35}
36
37/// Stores a list of types associated with each topic.
38pub type TopicNamesAndTypes = HashMap<String, Vec<String>>;
39
40/// Stores a node's name and namespace
41#[derive(Debug, PartialEq, Eq)]
42pub struct NodeNameInfo {
43    /// The name of the node
44    pub name: String,
45    /// The namespace of the node
46    pub namespace: String,
47}
48
49/// Contains topic endpoint information
50#[derive(Debug, PartialEq, Eq)]
51pub struct TopicEndpointInfo {
52    /// The name of the endpoint node
53    pub node_name: String,
54    /// The namespace of the endpoint node
55    pub node_namespace: String,
56    /// The type of the topic
57    pub topic_type: String,
58}
59
60impl Node {
61    /// Returns a list of topic names and types for publishers associated with a node.
62    pub fn get_publisher_names_and_types_by_node(
63        &self,
64        node: &str,
65        namespace: &str,
66    ) -> Result<TopicNamesAndTypes, RclrsError> {
67        // SAFETY: Forwarding arguments to the inner C function is safe
68        unsafe extern "C" fn wrapper(
69            node: *const rcl_node_t,
70            allocator: *mut rcl_allocator_t,
71            node_name: *const ::std::os::raw::c_char,
72            node_namespace: *const ::std::os::raw::c_char,
73            topic_names_and_types: *mut rcl_names_and_types_t,
74        ) -> rcl_ret_t {
75            rcl_get_publisher_names_and_types_by_node(
76                node,
77                allocator,
78                false,
79                node_name,
80                node_namespace,
81                topic_names_and_types,
82            )
83        }
84
85        self.get_names_and_types_by_node(node, namespace, wrapper)
86    }
87
88    /// Returns a list of topic names and types for subscriptions associated with a node.
89    pub fn get_subscription_names_and_types_by_node(
90        &self,
91        node: &str,
92        namespace: &str,
93    ) -> Result<TopicNamesAndTypes, RclrsError> {
94        // SAFETY: Forwarding arguments to the inner C function is safe
95        unsafe extern "C" fn wrapper(
96            node: *const rcl_node_t,
97            allocator: *mut rcl_allocator_t,
98            node_name: *const ::std::os::raw::c_char,
99            node_namespace: *const ::std::os::raw::c_char,
100            topic_names_and_types: *mut rcl_names_and_types_t,
101        ) -> rcl_ret_t {
102            rcl_get_subscriber_names_and_types_by_node(
103                node,
104                allocator,
105                false,
106                node_name,
107                node_namespace,
108                topic_names_and_types,
109            )
110        }
111
112        self.get_names_and_types_by_node(node, namespace, wrapper)
113    }
114
115    /// Returns a list of topic names and types for services associated with a node.
116    pub fn get_service_names_and_types_by_node(
117        &self,
118        node: &str,
119        namespace: &str,
120    ) -> Result<TopicNamesAndTypes, RclrsError> {
121        self.get_names_and_types_by_node(node, namespace, rcl_get_service_names_and_types_by_node)
122    }
123
124    /// Returns a list of topic names and types for clients associated with a node.
125    pub fn get_client_names_and_types_by_node(
126        &self,
127        node: &str,
128        namespace: &str,
129    ) -> Result<TopicNamesAndTypes, RclrsError> {
130        self.get_names_and_types_by_node(node, namespace, rcl_get_client_names_and_types_by_node)
131    }
132
133    /// Returns a list of all topic names and their types.
134    pub fn get_topic_names_and_types(&self) -> Result<TopicNamesAndTypes, RclrsError> {
135        // SAFETY: Getting a zero-initialized value is always safe
136        let mut rcl_names_and_types = unsafe { rmw_get_zero_initialized_names_and_types() };
137
138        // SAFETY: rcl_names_and_types is zero-initialized as expected by this call
139        unsafe {
140            rcl_get_topic_names_and_types(
141                &*self.rcl_node_mtx.lock().unwrap(),
142                &mut rcutils_get_default_allocator(),
143                false,
144                &mut rcl_names_and_types,
145            )
146            .ok()?
147        };
148
149        Ok(convert_names_and_types(rcl_names_and_types))
150    }
151
152    /// Returns a list of service names and types for this node.
153    pub fn get_service_names_and_types(&self) -> Result<TopicNamesAndTypes, RclrsError> {
154        self.get_service_names_and_types_by_node(&self.name(), &self.namespace())
155    }
156
157    /// Returns a list of all node names.
158    pub fn get_node_names(&self) -> Result<Vec<NodeNameInfo>, RclrsError> {
159        // SAFETY: Getting a zero-initialized value is always safe
160        let (mut rcl_names, mut rcl_namespaces) = unsafe {
161            (
162                rcutils_get_zero_initialized_string_array(),
163                rcutils_get_zero_initialized_string_array(),
164            )
165        };
166
167        // SAFETY: node_names and node_namespaces are zero-initialized as expected by this call.
168        unsafe {
169            rcl_get_node_names(
170                &*self.rcl_node_mtx.lock().unwrap(),
171                rcutils_get_default_allocator(),
172                &mut rcl_names,
173                &mut rcl_namespaces,
174            )
175            .ok()?;
176        };
177
178        // SAFETY: Because the previous function call successfully returned, the names and
179        // namespaces are populated with valid data
180        let (names_slice, namespaces_slice) = unsafe {
181            (
182                slice::from_raw_parts(rcl_names.data, rcl_names.size),
183                slice::from_raw_parts(rcl_namespaces.data, rcl_namespaces.size),
184            )
185        };
186
187        // SAFETY: Because rcl_get_node_names successfully returned, the name and namespace pointers
188        // point to valid C strings
189        let zipped_names = names_slice
190            .iter()
191            .zip(namespaces_slice.iter())
192            .map(|(name, namespace)| unsafe {
193                NodeNameInfo {
194                    name: CStr::from_ptr(*name).to_string_lossy().into_owned(),
195                    namespace: CStr::from_ptr(*namespace).to_string_lossy().into_owned(),
196                }
197            })
198            .collect();
199
200        Ok(zipped_names)
201    }
202
203    /// Returns a list of all node names with enclaves.
204    pub fn get_node_names_with_enclaves(&self) -> Result<Vec<(NodeNameInfo, String)>, RclrsError> {
205        // SAFETY: Getting a zero-initialized value is always safe
206        let (mut rcl_names, mut rcl_namespaces, mut rcl_enclaves) = unsafe {
207            (
208                rcutils_get_zero_initialized_string_array(),
209                rcutils_get_zero_initialized_string_array(),
210                rcutils_get_zero_initialized_string_array(),
211            )
212        };
213
214        // SAFETY: The node_names, namespaces, and enclaves are zero-initialized as expected by this call.
215        unsafe {
216            rcl_get_node_names_with_enclaves(
217                &*self.rcl_node_mtx.lock().unwrap(),
218                rcutils_get_default_allocator(),
219                &mut rcl_names,
220                &mut rcl_namespaces,
221                &mut rcl_enclaves,
222            )
223            .ok()?;
224        };
225
226        // SAFETY: The previous function successfully returned, so the arrays are valid
227        let (names_slice, namespaces_slice, enclaves_slice) = unsafe {
228            (
229                slice::from_raw_parts(rcl_names.data, rcl_names.size),
230                slice::from_raw_parts(rcl_namespaces.data, rcl_namespaces.size),
231                slice::from_raw_parts(rcl_enclaves.data, rcl_enclaves.size),
232            )
233        };
234
235        // SAFETY: The rcl function call successfully returned, so each element of the arrays points to
236        // a valid C string
237        let zipped_names = names_slice
238            .iter()
239            .zip(namespaces_slice.iter())
240            .zip(enclaves_slice.iter())
241            .map(|((name, namespace), enclave)| unsafe {
242                (
243                    NodeNameInfo {
244                        name: CStr::from_ptr(*name).to_string_lossy().into_owned(),
245                        namespace: CStr::from_ptr(*namespace).to_string_lossy().into_owned(),
246                    },
247                    CStr::from_ptr(*enclave).to_string_lossy().into_owned(),
248                )
249            })
250            .collect();
251
252        Ok(zipped_names)
253    }
254
255    /// Counts the number of publishers for a given topic.
256    pub fn count_publishers(&self, topic: &str) -> Result<usize, RclrsError> {
257        let topic_name = CString::new(topic).map_err(|err| RclrsError::StringContainsNul {
258            s: topic.to_string(),
259            err,
260        })?;
261        let mut count: usize = 0;
262
263        // SAFETY: The topic_name string was correctly allocated previously
264        unsafe {
265            rcl_count_publishers(
266                &*self.rcl_node_mtx.lock().unwrap(),
267                topic_name.as_ptr(),
268                &mut count,
269            )
270            .ok()?
271        };
272        Ok(count)
273    }
274
275    /// Counts the number of subscriptions for a given topic.
276    pub fn count_subscriptions(&self, topic: &str) -> Result<usize, RclrsError> {
277        let topic_name = CString::new(topic).map_err(|err| RclrsError::StringContainsNul {
278            s: topic.to_string(),
279            err,
280        })?;
281        let mut count: usize = 0;
282
283        // SAFETY: The topic_name string was correctly allocated previously
284        unsafe {
285            rcl_count_subscribers(
286                &*self.rcl_node_mtx.lock().unwrap(),
287                topic_name.as_ptr(),
288                &mut count,
289            )
290            .ok()?
291        };
292        Ok(count)
293    }
294
295    /// Returns topic publisher info.
296    pub fn get_publishers_info_by_topic(
297        &self,
298        topic: &str,
299    ) -> Result<Vec<TopicEndpointInfo>, RclrsError> {
300        self.get_publisher_subscriber_info_by_topic(topic, rcl_get_publishers_info_by_topic)
301    }
302
303    /// Returns topic subscriptions info.
304    pub fn get_subscriptions_info_by_topic(
305        &self,
306        topic: &str,
307    ) -> Result<Vec<TopicEndpointInfo>, RclrsError> {
308        self.get_publisher_subscriber_info_by_topic(topic, rcl_get_subscriptions_info_by_topic)
309    }
310
311    /// Returns an rcl names_and_types function, without a "no_demangle" argument.
312    fn get_names_and_types_by_node(
313        &self,
314        node: &str,
315        namespace: &str,
316        getter: unsafe extern "C" fn(
317            *const rcl_node_t,
318            *mut rcl_allocator_t,
319            *const ::std::os::raw::c_char,
320            *const ::std::os::raw::c_char,
321            *mut rcl_names_and_types_t,
322        ) -> rcl_ret_t,
323    ) -> Result<TopicNamesAndTypes, RclrsError> {
324        // SAFETY: Getting a zero-initialized value is always safe.
325        let mut rcl_names_and_types = unsafe { rmw_get_zero_initialized_names_and_types() };
326
327        let node_name = CString::new(node).map_err(|err| RclrsError::StringContainsNul {
328            s: node.to_string(),
329            err,
330        })?;
331        let node_namespace =
332            CString::new(namespace).map_err(|err| RclrsError::StringContainsNul {
333                s: namespace.to_string(),
334                err,
335            })?;
336
337        // SAFETY: node_name and node_namespace have been zero-initialized.
338        unsafe {
339            getter(
340                &*self.rcl_node_mtx.lock().unwrap(),
341                &mut rcutils_get_default_allocator(),
342                node_name.as_ptr(),
343                node_namespace.as_ptr(),
344                &mut rcl_names_and_types,
345            )
346        };
347
348        Ok(convert_names_and_types(rcl_names_and_types))
349    }
350
351    /// Returns publisher or subscriber info by topic.
352    fn get_publisher_subscriber_info_by_topic(
353        &self,
354        topic: &str,
355        getter: unsafe extern "C" fn(
356            *const rcl_node_t,
357            *mut rcl_allocator_t,
358            *const ::std::os::raw::c_char,
359            bool,
360            *mut rcl_topic_endpoint_info_array_t,
361        ) -> rcl_ret_t,
362    ) -> Result<Vec<TopicEndpointInfo>, RclrsError> {
363        let topic = CString::new(topic).map_err(|err| RclrsError::StringContainsNul {
364            s: topic.to_string(),
365            err,
366        })?;
367
368        // SAFETY: Getting a zero-initialized value is always safe.
369        let mut rcl_publishers_info =
370            unsafe { rmw_get_zero_initialized_topic_endpoint_info_array() };
371
372        // SAFETY: topic has been zero-initialized
373        unsafe {
374            getter(
375                &*self.rcl_node_mtx.lock().unwrap(),
376                &mut rcutils_get_default_allocator(),
377                topic.as_ptr(),
378                false,
379                &mut rcl_publishers_info,
380            )
381            .ok()?;
382        }
383
384        // SAFETY: The previous call returned successfully, so the slice is valid
385        let topic_endpoint_infos_slice = unsafe {
386            slice::from_raw_parts(rcl_publishers_info.info_array, rcl_publishers_info.size)
387        };
388
389        // SAFETY: Because the rcl call returned successfully, each element of the slice points
390        // to a valid topic_endpoint_info object, which contains valid C strings
391        let topic_endpoint_infos_vec = topic_endpoint_infos_slice
392            .iter()
393            .map(|info| {
394                let (node_name, node_namespace, topic_type) = unsafe {
395                    (
396                        CStr::from_ptr(info.node_name)
397                            .to_string_lossy()
398                            .into_owned(),
399                        CStr::from_ptr(info.node_namespace)
400                            .to_string_lossy()
401                            .into_owned(),
402                        CStr::from_ptr(info.topic_type)
403                            .to_string_lossy()
404                            .into_owned(),
405                    )
406                };
407                TopicEndpointInfo {
408                    node_name,
409                    node_namespace,
410                    topic_type,
411                }
412            })
413            .collect();
414
415        Ok(topic_endpoint_infos_vec)
416    }
417}
418
419/// Converts a rmw_names_and_types_t object to a HashMap.
420fn convert_names_and_types(
421    rcl_names_and_types: rmw_names_and_types_t,
422) -> HashMap<String, Vec<String>> {
423    let mut names_and_types: TopicNamesAndTypes = HashMap::new();
424
425    // SAFETY: Safe if the rcl_names_and_types arg has been initialized by the caller
426    let name_slice = unsafe {
427        slice::from_raw_parts(
428            rcl_names_and_types.names.data,
429            rcl_names_and_types.names.size,
430        )
431    };
432
433    for (idx, name) in name_slice.iter().enumerate() {
434        // SAFETY: The slice contains valid C string pointers if it was populated by the caller
435        let name: String = unsafe {
436            let cstr = CStr::from_ptr(*name);
437            cstr.to_string_lossy().into_owned()
438        };
439
440        // SAFETY: Safe as long as rcl_names_and_types was populated by the caller
441        let types: Vec<String> = unsafe {
442            let p = rcl_names_and_types.types.add(idx);
443            slice::from_raw_parts((*p).data, (*p).size)
444                .iter()
445                .map(|s| {
446                    let cstr = CStr::from_ptr(*s);
447                    cstr.to_string_lossy().into_owned()
448                })
449                .collect()
450        };
451
452        names_and_types.insert(name, types);
453    }
454
455    names_and_types
456}
457
458#[cfg(test)]
459mod tests {
460
461    use super::*;
462    use crate::Context;
463
464    #[test]
465    fn test_graph_empty() {
466        let context = Context::new([]).unwrap();
467        let node_name = "test_publisher_names_and_types";
468        let node = Node::new(&context, node_name).unwrap();
469
470        // Test that the graph has no publishers
471        let names_and_topics = node
472            .get_publisher_names_and_types_by_node(node_name, "")
473            .unwrap();
474
475        assert_eq!(names_and_topics.len(), 0);
476
477        let num_publishers = node.count_publishers("/test").unwrap();
478
479        assert_eq!(num_publishers, 0);
480
481        let publisher_infos = node.get_publishers_info_by_topic("test").unwrap();
482
483        assert!(publisher_infos.is_empty());
484
485        // Test that the graph has no subscriptions
486        let names_and_topics = node
487            .get_subscription_names_and_types_by_node(node_name, "")
488            .unwrap();
489
490        assert_eq!(names_and_topics.len(), 0);
491
492        let num_subscriptions = node.count_subscriptions("/test").unwrap();
493
494        assert_eq!(num_subscriptions, 0);
495
496        let subscription_infos = node.get_subscriptions_info_by_topic("test").unwrap();
497
498        assert!(subscription_infos.is_empty());
499
500        // Test that the graph has no services
501        let names_and_topics = node
502            .get_service_names_and_types_by_node(node_name, "")
503            .unwrap();
504
505        assert_eq!(names_and_topics.len(), 0);
506
507        let names_and_topics = node.get_service_names_and_types().unwrap();
508
509        assert_eq!(names_and_topics.len(), 0);
510
511        // Test that the graph has no clients
512        let names_and_topics = node
513            .get_client_names_and_types_by_node(node_name, "")
514            .unwrap();
515
516        assert_eq!(names_and_topics.len(), 0);
517
518        // Test that the graph has no topics
519        let names_and_topics = node.get_topic_names_and_types().unwrap();
520
521        assert_eq!(names_and_topics.len(), 0);
522    }
523
524    #[test]
525    fn test_node_names() {
526        let context = Context::new([]).unwrap();
527        let node_name = "test_node_names";
528        let node = Node::new(&context, node_name).unwrap();
529
530        let names_and_namespaces = node.get_node_names().unwrap();
531
532        // The tests are executed in parallel, so we might see some other nodes
533        // in here. That's why we can't use assert_eq.
534        assert!(names_and_namespaces.contains(&NodeNameInfo {
535            name: node_name.to_string(),
536            namespace: "/".to_string()
537        }));
538    }
539
540    #[test]
541    fn test_node_names_with_enclaves() {
542        let context = Context::new([]).unwrap();
543        let node_name = "test_node_names_with_enclaves";
544        let node = Node::new(&context, node_name).unwrap();
545
546        let names_and_namespaces = node.get_node_names_with_enclaves().unwrap();
547
548        // The tests are executed in parallel, so we might see some other nodes
549        // in here. That's why we can't use assert_eq.
550        assert!(names_and_namespaces.contains(&(
551            NodeNameInfo {
552                name: node_name.to_string(),
553                namespace: "/".to_string()
554            },
555            "/".to_string()
556        )));
557    }
558}
do_not_use_testing_rclrs/node/graph.rs

do_not_use_testing_rclrs/node/
graph.rs
