do_not_use_testing_rclrs/node/builder.rs
1use std::ffi::CString;
2use std::sync::{Arc, Mutex};
3
4use crate::rcl_bindings::*;
5use crate::{
6 ClockType, Context, Node, ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult,
7 QOS_PROFILE_CLOCK,
8};
9
10/// A builder for creating a [`Node`][1].
11///
12/// The builder pattern allows selectively setting some fields, and leaving all others at their default values.
13/// This struct instance can be created via [`Node::builder()`][2].
14///
15/// The default values for optional fields are:
16/// - `namespace: "/"`
17/// - `use_global_arguments: true`
18/// - `arguments: []`
19/// - `enable_rosout: true`
20/// - `clock_type: ClockType::RosTime`
21/// - `clock_qos: QOS_PROFILE_CLOCK`
22///
23/// # Example
24/// ```
25/// # use rclrs::{Context, NodeBuilder, Node, RclrsError};
26/// let context = Context::new([])?;
27/// // Building a node in a single expression
28/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?;
29/// assert_eq!(node.name(), "foo_node");
30/// assert_eq!(node.namespace(), "/bar");
31/// // Building a node via Node::builder()
32/// let node = Node::builder(&context, "bar_node").build()?;
33/// assert_eq!(node.name(), "bar_node");
34/// // Building a node step-by-step
35/// let mut builder = Node::builder(&context, "goose");
36/// builder = builder.namespace("/duck/duck");
37/// let node = builder.build()?;
38/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose");
39/// # Ok::<(), RclrsError>(())
40/// ```
41///
42/// [1]: crate::Node
43/// [2]: crate::Node::builder
44pub struct NodeBuilder {
45 context: Arc<Mutex<rcl_context_t>>,
46 name: String,
47 namespace: String,
48 use_global_arguments: bool,
49 arguments: Vec<String>,
50 enable_rosout: bool,
51 clock_type: ClockType,
52 clock_qos: QoSProfile,
53}
54
55impl NodeBuilder {
56 /// Creates a builder for a node with the given name.
57 ///
58 /// See the [`Node` docs][1] for general information on node names.
59 ///
60 /// # Rules for valid node names
61 ///
62 /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2]
63 /// function. They are:
64 /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters
65 /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH`
66 /// - Must not start with a number
67 ///
68 /// Note that node name validation is delayed until [`NodeBuilder::build()`][3].
69 ///
70 /// # Example
71 /// ```
72 /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode};
73 /// let context = Context::new([])?;
74 /// // This is a valid node name
75 /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok());
76 /// // This is another valid node name (although not a good one)
77 /// assert!(NodeBuilder::new(&context, "_______").build().is_ok());
78 /// // This is an invalid node name
79 /// assert!(matches!(
80 /// NodeBuilder::new(&context, "röböt")
81 /// .build()
82 /// .unwrap_err(),
83 /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. }
84 /// ));
85 /// # Ok::<(), RclrsError>(())
86 /// ```
87 ///
88 /// [1]: crate::Node#naming
89 /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3
90 /// [3]: NodeBuilder::build
91 pub fn new(context: &Context, name: &str) -> NodeBuilder {
92 NodeBuilder {
93 context: context.rcl_context_mtx.clone(),
94 name: name.to_string(),
95 namespace: "/".to_string(),
96 use_global_arguments: true,
97 arguments: vec![],
98 enable_rosout: true,
99 clock_type: ClockType::RosTime,
100 clock_qos: QOS_PROFILE_CLOCK,
101 }
102 }
103
104 /// Sets the node namespace.
105 ///
106 /// See the [`Node` docs][1] for general information on namespaces.
107 ///
108 /// # Rules for valid namespaces
109 ///
110 /// The rules for a valid node namespace are based on the [rules for a valid topic][2]
111 /// and are checked by the [`rmw_validate_namespace()`][3] function. However, a namespace
112 /// without a leading forward slash is automatically changed to have a leading forward slash
113 /// before it is checked with this function.
114 ///
115 /// Thus, the effective rules are:
116 /// - Must contain only the `a-z`, `A-Z`, `0-9`, `_`, and `/` characters
117 /// - Must not have a number at the beginning, or after a `/`
118 /// - Must not contain two or more `/` characters in a row
119 /// - Must not have a `/` character at the end, except if `/` is the full namespace
120 ///
121 /// Note that namespace validation is delayed until [`NodeBuilder::build()`][4].
122 ///
123 /// # Example
124 /// ```
125 /// # use rclrs::{Context, Node, RclrsError, RclReturnCode};
126 /// let context = Context::new([])?;
127 /// // This is a valid namespace
128 /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace");
129 /// assert!(builder_ok_ns.build().is_ok());
130 /// // This is an invalid namespace
131 /// assert!(matches!(
132 /// Node::builder(&context, "my_node")
133 /// .namespace("/10_percent_luck/20_percent_skill")
134 /// .build()
135 /// .unwrap_err(),
136 /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. }
137 /// ));
138 /// // A missing forward slash at the beginning is automatically added
139 /// assert_eq!(
140 /// Node::builder(&context, "my_node")
141 /// .namespace("foo")
142 /// .build()?
143 /// .namespace(),
144 /// "/foo"
145 /// );
146 /// # Ok::<(), RclrsError>(())
147 /// ```
148 ///
149 /// [1]: crate::Node#naming
150 /// [2]: http://design.ros2.org/articles/topic_and_service_names.html
151 /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49
152 /// [4]: NodeBuilder::build
153 pub fn namespace(mut self, namespace: &str) -> Self {
154 self.namespace = namespace.to_string();
155 self
156 }
157
158 /// Enables or disables using global arguments.
159 ///
160 /// The "global" arguments are those used in [creating the context][1].
161 ///
162 /// # Example
163 /// ```
164 /// # use rclrs::{Context, Node, NodeBuilder, RclrsError};
165 /// let context_args = ["--ros-args", "--remap", "__node:=your_node"]
166 /// .map(String::from);
167 /// let context = Context::new(context_args)?;
168 /// // Ignore the global arguments:
169 /// let node_without_global_args =
170 /// rclrs::create_node_builder(&context, "my_node")
171 /// .use_global_arguments(false)
172 /// .build()?;
173 /// assert_eq!(node_without_global_args.name(), "my_node");
174 /// // Do not ignore the global arguments:
175 /// let node_with_global_args =
176 /// rclrs::create_node_builder(&context, "my_other_node")
177 /// .use_global_arguments(true)
178 /// .build()?;
179 /// assert_eq!(node_with_global_args.name(), "your_node");
180 /// # Ok::<(), RclrsError>(())
181 /// ```
182 ///
183 /// [1]: crate::Context::new
184 pub fn use_global_arguments(mut self, enable: bool) -> Self {
185 self.use_global_arguments = enable;
186 self
187 }
188
189 /// Sets node-specific command line arguments.
190 ///
191 /// These arguments are parsed the same way as those for [`Context::new()`][1].
192 /// However, the node-specific command line arguments have higher precedence than the arguments
193 /// used in creating the context.
194 ///
195 /// For more details about command line arguments, see [here][2].
196 ///
197 /// # Example
198 /// ```
199 /// # use rclrs::{Context, Node, NodeBuilder, RclrsError};
200 /// // Usually, this would change the name of "my_node" to "context_args_node":
201 /// let context_args = ["--ros-args", "--remap", "my_node:__node:=context_args_node"]
202 /// .map(String::from);
203 /// let context = Context::new(context_args)?;
204 /// // But the node arguments will change it to "node_args_node":
205 /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"]
206 /// .map(String::from);
207 /// let node =
208 /// rclrs::create_node_builder(&context, "my_node")
209 /// .arguments(node_args)
210 /// .build()?;
211 /// assert_eq!(node.name(), "node_args_node");
212 /// # Ok::<(), RclrsError>(())
213 /// ```
214 ///
215 /// [1]: crate::Context::new
216 /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html
217 pub fn arguments(mut self, arguments: impl IntoIterator<Item = String>) -> Self {
218 self.arguments = arguments.into_iter().collect();
219 self
220 }
221
222 /// Enables or disables logging to rosout.
223 ///
224 /// When enabled, log messages are published to the `/rosout` topic in addition to
225 /// standard output.
226 ///
227 /// This option is currently unused in `rclrs`.
228 pub fn enable_rosout(mut self, enable: bool) -> Self {
229 self.enable_rosout = enable;
230 self
231 }
232
233 /// Sets the node's clock type.
234 pub fn clock_type(mut self, clock_type: ClockType) -> Self {
235 self.clock_type = clock_type;
236 self
237 }
238
239 /// Sets the QoSProfile for the clock subscription.
240 pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
241 self.clock_qos = clock_qos;
242 self
243 }
244
245 /// Builds the node instance.
246 ///
247 /// Node name and namespace validation is performed in this method.
248 ///
249 /// For example usage, see the [`NodeBuilder`][1] docs.
250 ///
251 /// [1]: crate::NodeBuilder
252 pub fn build(&self) -> Result<Arc<Node>, RclrsError> {
253 let node_name =
254 CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul {
255 err,
256 s: self.name.clone(),
257 })?;
258 let node_namespace =
259 CString::new(self.namespace.as_str()).map_err(|err| RclrsError::StringContainsNul {
260 err,
261 s: self.namespace.clone(),
262 })?;
263 let rcl_node_options = self.create_rcl_node_options()?;
264 let rcl_context = &mut *self.context.lock().unwrap();
265
266 // SAFETY: Getting a zero-initialized value is always safe.
267 let mut rcl_node = unsafe { rcl_get_zero_initialized_node() };
268 unsafe {
269 // SAFETY: The rcl_node is zero-initialized as expected by this function.
270 // The strings and node options are copied by this function, so we don't need
271 // to keep them alive.
272 // The rcl_context has to be kept alive because it is co-owned by the node.
273 rcl_node_init(
274 &mut rcl_node,
275 node_name.as_ptr(),
276 node_namespace.as_ptr(),
277 rcl_context,
278 &rcl_node_options,
279 )
280 .ok()?;
281 };
282
283 let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
284 let _parameter = ParameterInterface::new(
285 &rcl_node_mtx,
286 &rcl_node_options.arguments,
287 &rcl_context.global_arguments,
288 )?;
289 let node = Arc::new(Node {
290 rcl_node_mtx,
291 rcl_context_mtx: self.context.clone(),
292 clients_mtx: Mutex::new(vec![]),
293 guard_conditions_mtx: Mutex::new(vec![]),
294 services_mtx: Mutex::new(vec![]),
295 subscriptions_mtx: Mutex::new(vec![]),
296 _time_source: TimeSource::builder(self.clock_type)
297 .clock_qos(self.clock_qos)
298 .build(),
299 _parameter,
300 });
301 node._time_source.attach_node(&node);
302 Ok(node)
303 }
304
305 /// Creates a rcl_node_options_t struct from this builder.
306 ///
307 /// Any fields not present in the builder will have their default value.
308 /// For detail about default values, see [`NodeBuilder`][1] docs.
309 ///
310 /// [1]: crate::NodeBuilder
311 fn create_rcl_node_options(&self) -> Result<rcl_node_options_t, RclrsError> {
312 // SAFETY: No preconditions for this function.
313 let mut rcl_node_options = unsafe { rcl_node_get_default_options() };
314
315 let cstring_args = self
316 .arguments
317 .iter()
318 .map(|s| match CString::new(s.as_str()) {
319 Ok(cstr) => Ok(cstr),
320 Err(err) => Err(RclrsError::StringContainsNul { s: s.clone(), err }),
321 })
322 .collect::<Result<Vec<_>, _>>()?;
323
324 let cstring_arg_ptrs = cstring_args.iter().map(|s| s.as_ptr()).collect::<Vec<_>>();
325 unsafe {
326 // SAFETY: This function does not store the ephemeral cstring_args_ptrs
327 // pointers. We are passing in a zero-initialized arguments struct as expected.
328 rcl_parse_arguments(
329 cstring_arg_ptrs.len() as i32,
330 cstring_arg_ptrs.as_ptr(),
331 rcutils_get_default_allocator(),
332 &mut rcl_node_options.arguments,
333 )
334 }
335 .ok()?;
336
337 rcl_node_options.use_global_arguments = self.use_global_arguments;
338 rcl_node_options.enable_rosout = self.enable_rosout;
339 // SAFETY: No preconditions for this function.
340 rcl_node_options.allocator = unsafe { rcutils_get_default_allocator() };
341
342 Ok(rcl_node_options)
343 }
344}
345
346impl Drop for rcl_node_options_t {
347 fn drop(&mut self) {
348 // SAFETY: Do not finish this struct except here.
349 unsafe {
350 // This also finalizes the `rcl_arguments_t` contained in `rcl_node_options_t`.
351 rcl_node_options_fini(self).ok().unwrap();
352 }
353 }
354}