Add service_is_ready() (#339)

* Add check if service is ready

* Update changelog

* Fix unneeded return

---------

Co-authored-by: carter <carterjschultz@gmail.com>
Co-authored-by: carter <carter@amprobotics.com>

examples/minimal_client_service/src/minimal_client.rs

@@ -13,7 +13,9 @@ fn main() -> Result<(), Error> {
 
     println!("Starting client");
 
-    std::thread::sleep(std::time::Duration::from_millis(500));
+    while !client.service_is_ready()? {
+        std::thread::sleep(std::time::Duration::from_millis(10));
+    }
 
     client.async_send_request_with_callback(
         &request,

examples/minimal_client_service/src/minimal_client_async.rs

@@ -12,7 +12,9 @@ async fn main() -> Result<(), Error> {
 
     println!("Starting client");
 
-    std::thread::sleep(std::time::Duration::from_millis(500));
+    while !client.service_is_ready()? {
+        std::thread::sleep(std::time::Duration::from_millis(10));
+    }
 
     let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 };
 

rclrs/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package rclrs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+Unreleased
+----------------
+* Service clients now support service_is_ready to check if a service server is present ahead of calling (`#399 <https://github.com/ros2-rust/ros2_rust/pull/339>`_)
+
 0.3 (2022-07-22)
 ----------------
 * Loaned messages (zero-copy) (`#212 <https://github.com/ros2-rust/ros2_rust/pull/212>`_)

rclrs/src/client.rs

@@ -238,6 +238,24 @@ where
         .ok()?;
         Ok((T::Response::from_rmw_message(response_out), request_id_out))
     }
+
+    /// Check if a service server is available.
+    ///
+    /// Will return true if there is a service server available, false if unavailable.
+    ///
+    pub fn service_is_ready(&self) -> Result<bool, RclrsError> {
+        let mut is_ready = false;
+        let client = &mut *self.handle.rcl_client_mtx.lock().unwrap();
+        let node = &mut *self.handle.rcl_node_mtx.lock().unwrap();
+
+        unsafe {
+            // SAFETY both node and client are guaranteed to be valid here
+            // client is guaranteed to have been generated with node
+            rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready)
+        }
+        .ok()?;
+        Ok(is_ready)
+    }
 }
 
 impl<T> ClientBase for Client<T>