Make rcl_service_t have stable location

This prevents another service initializing at the same location.
Therefore preventing conflicts in a trace.

r2r/src/nodes.rs

@@ -810,20 +810,33 @@ impl Node {
     where
         T: WrappedServiceTypeSupport,
     {
-        let service_handle = create_service_helper(
-            self.node_handle.as_mut(),
-            service_name,
-            T::get_ts(),
-            qos_profile,
-        )?;
         let (sender, receiver) = mpsc::channel::<ServiceRequest<T>>(10);
 
-        let ws = TypedService::<T> {
-            rcl_handle: service_handle,
+        // SAFETY: The `rcl_handle` is zero initialized (partial initialization) in this block.
+        let mut service_arc = Arc::new(Mutex::new(TypedService::<T> {
+            rcl_handle: unsafe { rcl_get_zero_initialized_service() },
             sender,
+        }));
+        let service_ref = Arc::get_mut(&mut service_arc)
+            .unwrap() // No other Arc should exist. The Arc was just created.
+            .get_mut()
+            .unwrap(); // The mutex was just created. It should not be poisoned.
+
+        // SAFETY:
+        // The service was zero initialized above.
+        // Full initialization happens in `create_service_helper``.
+        unsafe {
+            create_service_helper(
+                &mut service_ref.rcl_handle,
+                self.node_handle.as_mut(),
+                service_name,
+                T::get_ts(),
+                qos_profile,
+            )?;
         };
 
-        self.services.push(Arc::new(Mutex::new(ws)));
+        // Only push after full initialization.
+        self.services.push(service_arc);
         Ok(receiver)
     }
 

r2r/src/services.rs

@@ -111,11 +111,13 @@ where
     }
 }
 
-pub fn create_service_helper(
-    node: &mut rcl_node_t, service_name: &str, service_ts: *const rosidl_service_type_support_t,
-    qos_profile: QosProfile,
-) -> Result<rcl_service_t> {
-    let mut service_handle = unsafe { rcl_get_zero_initialized_service() };
+/// Initializes the service.
+///
+/// SAFETY: requires that the service handle is zero initialized by [`rcl_get_zero_initialized_service`].
+pub unsafe fn create_service_helper(
+    service_handle: &mut rcl_service_t, node: &mut rcl_node_t, service_name: &str,
+    service_ts: *const rosidl_service_type_support_t, qos_profile: QosProfile,
+) -> Result<()> {
     let service_name_c_string =
         CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
 
@@ -123,7 +125,7 @@ pub fn create_service_helper(
         let mut service_options = rcl_service_get_default_options();
         service_options.qos = qos_profile.into();
         rcl_service_init(
-            &mut service_handle,
+            service_handle,
             node,
             service_ts,
             service_name_c_string.as_ptr(),
@@ -131,7 +133,7 @@ pub fn create_service_helper(
         )
     };
     if result == RCL_RET_OK as i32 {
-        Ok(service_handle)
+        Ok(())
     } else {
         Err(Error::from_rcl_error(result))
     }