Introduce InitOptions to allow manually setting domain ID

Signed-off-by: Michael X. Grey <[email protected]>

Author: mxgrey

rclrs/src/context.rs

@@ -55,10 +55,10 @@ pub(crate) struct ContextHandle {
 impl Context {
     /// Creates a new context.
     ///
-    /// Usually, this would be called with `std::env::args()`, analogously to `rclcpp::init()`.
+    /// Usually this would be called with `std::env::args()`, analogously to `rclcpp::init()`.
     /// See also the official "Passing ROS arguments to nodes via the command-line" tutorial.
     ///
-    /// Creating a context can fail in case the args contain invalid ROS arguments.
+    /// Creating a context will fail if the args contain invalid ROS arguments.
     ///
     /// # Example
     /// ```
@@ -68,6 +68,21 @@ impl Context {
     /// assert!(Context::new(invalid_remapping).is_err());
     /// ```
     pub fn new(args: impl IntoIterator<Item = String>) -> Result<Self, RclrsError> {
+        Self::new_with_options(args, InitOptions::new())
+    }
+
+    /// Same as [`Context::new`] except you can additionally provide initialization options.
+    ///
+    /// # Example
+    /// ```
+    /// use rclrs::{Context, InitOptions};
+    /// let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(5))).unwrap();
+    /// assert_eq!(context.domain_id(), 5);
+    /// ````
+    pub fn new_with_options(
+        args: impl IntoIterator<Item=String>,
+        options: InitOptions,
+    ) -> Result<Self, RclrsError> {
         // SAFETY: Getting a zero-initialized value is always safe
         let mut rcl_context = unsafe { rcl_get_zero_initialized_context() };
         let cstring_args: Vec<CString> = args
@@ -84,11 +99,7 @@ impl Context {
         unsafe {
             // SAFETY: No preconditions for this function.
             let allocator = rcutils_get_default_allocator();
-            // SAFETY: Getting a zero-initialized value is always safe.
-            let mut rcl_init_options = rcl_get_zero_initialized_init_options();
-            // SAFETY: Passing in a zero-initialized value is expected.
-            // In the case where this returns not ok, there's nothing to clean up.
-            rcl_init_options_init(&mut rcl_init_options, allocator).ok()?;
+            let mut rcl_init_options = options.into_rcl(allocator)?;
             // SAFETY: This function does not store the ephemeral init_options and c_args
             // pointers. Passing in a zero-initialized rcl_context is expected.
             let ret = rcl_init(
@@ -115,6 +126,25 @@ impl Context {
         })
     }
 
+    /// Returns the ROS domain ID that the context is using.
+    ///
+    /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1].
+    /// It can be set through the `ROS_DOMAIN_ID` environment variable.
+    ///
+    /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Domain-ID.html
+    pub fn domain_id(&self) -> u8 {
+        let mut domain_id: usize = 0;
+        let ret = unsafe {
+            rcl_context_get_domain_id(
+                &mut *self.handle.rcl_context.lock().unwrap(),
+                &mut domain_id
+            )
+        };
+
+        debug_assert_eq!(ret, 0);
+        domain_id as u8
+    }
+
     /// Checks if the context is still valid.
     ///
     /// This will return `false` when a signal has caused the context to shut down (currently
@@ -128,6 +158,59 @@ impl Context {
     }
 }
 
+/// Additional options for initializing the Context.
+#[derive(Default, Clone)]
+pub struct InitOptions {
+    /// The domain ID that should be used by the Context. Set to None to ask for
+    /// the default behavior, which is to set the domain ID according to the
+    /// [ROS_DOMAIN_ID][1] environment variable.
+    ///
+    /// [1]: https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Domain-ID.html#the-ros-domain-id
+    domain_id: Option<u8>,
+}
+
+impl InitOptions {
+    /// Create a new InitOptions with all default values.
+    pub fn new() -> InitOptions {
+        Self::default()
+    }
+
+    /// Transform an InitOptions into a new one with a certain domain_id
+    pub fn with_domain_id(mut self, domain_id: Option<u8>) -> InitOptions {
+        self.domain_id = domain_id;
+        self
+    }
+
+    /// Set the domain_id of an InitOptions, or reset it to the default behavior
+    /// (determined by environment variables) by providing None.
+    pub fn set_domain_id(&mut self, domain_id: Option<u8>) {
+        self.domain_id = domain_id;
+    }
+
+    /// Get the domain_id that will be provided by these InitOptions.
+    pub fn domain_id(&self) -> Option<u8> {
+        self.domain_id
+    }
+
+    fn into_rcl(self, allocator: rcutils_allocator_s) -> Result<rcl_init_options_t, RclrsError> {
+        unsafe {
+            // SAFETY: Getting a zero-initialized value is always safe.
+            let mut rcl_init_options = rcl_get_zero_initialized_init_options();
+            // SAFETY: Passing in a zero-initialized value is expected.
+            // In the case where this returns not ok, there's nothing to clean up.
+            rcl_init_options_init(&mut rcl_init_options, allocator).ok()?;
+
+            // We only need to set the domain_id if the user asked for something
+            // other than None. When the user asks for None, that is equivalent
+            // to the default value in rcl_init_options.
+            if let Some(domain_id) = self.domain_id {
+                rcl_init_options_set_domain_id(&mut rcl_init_options, domain_id as usize);
+            }
+            return Ok(rcl_init_options);
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;