Enable ROS2 service calls from C++ nodes

apis/c++/node/README.md

@@ -254,3 +254,68 @@ assert((sensor_msgs::const_NavSatStatus_STATUS_NO_FIX() == -1));
 ```
 
 (Note: Exposing them as C++ constants is not possible because it's [not supported by `cxx` yet](https://github.com/dtolnay/cxx/issues/1051).)
+
+### Service Clients
+
+To create a service client, use one of the `create_client_<TYPE>` functions.
+The `<TYPE>` describes the service type, which specifies the request and response types.
+The Dora ROS2 bridge automatically creates `create_client_<TYPE>` functions for all service types found in the sourced ROS2 environment.
+
+```c++
+auto add_two_ints = node->create_client_example_interfaces_AddTwoInts(
+  "/",
+  "add_two_ints",
+  qos,
+  merged_events
+);
+```
+
+- The first argument is the namespace of the service and the second argument is its name.
+- The third argument is the QoS (quality of service) setting for the service.
+  It can be set to `qos_default()` or adjusted as desired, for example:
+  ```c++
+  auto qos = qos_default();
+  qos.reliable = true;
+  qos.max_blocking_time = 0.1;
+  qos.keep_last = 1;
+  ```
+- The last argument is the [combined event stream](#combined-event-streams) that the received service responses should be merged into.
+
+#### Waiting for the Service
+
+In order to achieve reliable service communication, it is recommended to wait until the service is available before sending requests.
+Use the `wait_for_service()` method for that, e.g.:
+
+```c++
+add_two_ints->wait_for_service(node)
+```
+
+The given `node` must be the node on which the service was created.
+
+#### Sending Requests
+
+To send a request, use the `send_request` method:
+
+```c++
+add_two_ints->send_request(request);
+```
+
+The method sends the request asynchronously without waiting for a response.
+When the response is received, it is automatically sent to the [combined event stream](#combined-event-streams) that was given on client creation.
+
+#### Receiving Responses
+
+See the [_"Receiving Messages from Combined Event Stream"_](#receiving-messages-from-combined-event-stream) section for how to receive events from the combined event stream.
+To check if a received event is a service response, use the `matches` method.
+If it returns `true`, you can use the `downcast` method to convert the event to the correct service response type.
+
+Example:
+
+```c++
+if (add_two_ints->matches(event))
+{
+    auto response = add_two_ints->downcast(std::move(event));
+    std::cout << "Received sum response with value " << response.sum << std::endl;
+    ...
+}
+```

examples/c++-ros2-dataflow/README.md

@@ -12,8 +12,9 @@ This examples requires a sourced ROS2 installation.
   - Install the turtlesim package
   - Start the turtlesim node through `ros2 run turtlesim turtlesim_node`
 
-## Running turtlesim example
+## Running pub/sub example
 
+A ROS2 client to pubish turtlesim ROS2 messages and a DORA node can subscribe and visualize it.
 
 From terminal 1 , sourcing the ROS2 installation and start ROS2 turtlesim window
 ```
@@ -27,3 +28,28 @@ source /opt/ros/galactic/setup.bash
 cargo run --example cxx-ros2-dataflow --features ros2-examples
 ```
 And you will see the turtle move a few steps.
+
+## Running service example
+The current service code example is a service client. To test with service server we can test with either ROS2 demo or ros2-client
+- if using ROS2 demo the the command line is:
+```
+ros2 run demo_nodes_cpp add_two_ints_server
+```
+
+start DORA service client from another terminal
+```
+cargo run --example cxx-ros2-dataflow --features ros2-examples
+```
+
+- if using ros2-client the command line is:
+```
+cargo run --example=ros2_service_server
+```
+
+then start DORA service client from another terminal
+```
+cargo run --example cxx-ros2-dataflow --features ros2-examples
+```
+
+
+

examples/c++-ros2-dataflow/node-rust-api/main.cc

@@ -29,7 +29,15 @@ int main()
     std::default_random_engine gen(dev());
     std::uniform_real_distribution<> dist(0., 1.);
 
+    auto service_qos = qos_default();
+    service_qos.reliable = true;
+    service_qos.max_blocking_time = 0.1;
+    service_qos.keep_last = 1;
+    auto add_two_ints = node->create_client_example_interfaces_AddTwoInts("/", "add_two_ints", service_qos, merged_events);
+    add_two_ints->wait_for_service(node);
+
     auto received_ticks = 0;
+    auto responses_received = 0;
 
     for (int i = 0; i < 1000; i++)
     {
@@ -56,6 +64,9 @@ int main()
                     .linear = {.x = dist(gen) + 1, .y = 0, .z = 0},
                     .angular = {.x = 0, .y = 0, .z = (dist(gen) - 0.5) * 5.0}};
                 vel_publisher->publish(twist);
+
+                example_interfaces::AddTwoInts_Request request = {.a = 4, .b = 5};
+                add_two_ints->send_request(request);
             }
             else
             {
@@ -72,12 +83,22 @@ int main()
             auto pose = pose_subscription->downcast(std::move(event));
             std::cout << "Received pose x:" << pose.x << ", y:" << pose.y << std::endl;
         }
+        else if (add_two_ints->matches(event))
+        {
+            auto response = add_two_ints->downcast(std::move(event));
+            assert(response.sum == 9);
+            std::cout << "Received correct sum response from add_two_ints" << std::endl;
+            responses_received += 1;
+        }
         else
         {
             std::cout << "received unexpected event" << std::endl;
         }
     }
 
+    std::cout << "Received " << responses_received << " service responses" << std::endl;
+    assert(responses_received > 0);
+
     // try to access a constant for testing
     assert((sensor_msgs::const_NavSatStatus_STATUS_NO_FIX() == -1));
 

libraries/extensions/ros2-bridge/Cargo.toml

@@ -24,6 +24,9 @@ tokio = { version = "1.29.1", features = ["full"], optional = true }
 dora-daemon = { path = "../../../binaries/daemon", optional = true }
 tracing = "0.1.37"
 tracing-subscriber = "0.3.17"
+flume = "0.11.0"
+futures = { version = "0.3.21", features = ["thread-pool"] }
+futures-timer = "3.0.3"
 
 [dev-dependencies]
 rand = "0.8.5"

libraries/extensions/ros2-bridge/msg-gen/src/lib.rs

@@ -27,6 +27,8 @@ where
     let mut message_topic_impls = Vec::new();
     let mut service_defs = Vec::new();
     let mut service_impls = Vec::new();
+    let mut service_creation_defs = Vec::new();
+    let mut service_creation_impls = Vec::new();
     let mut aliases = Vec::new();
     for package in &packages {
         for message in &package.messages {
@@ -44,6 +46,12 @@ where
             let (def, imp) = service.struct_token_stream(&package.name, create_cxx_bridge);
             service_defs.push(def);
             service_impls.push(imp);
+            if create_cxx_bridge {
+                let (service_creation_def, service_creation_impl) =
+                    service.cxx_service_creation_functions(&package.name);
+                service_creation_defs.push(service_creation_def);
+                service_creation_impls.push(service_creation_impl);
+            }
         }
 
         aliases.push(package.aliases_token_stream());
@@ -67,6 +75,7 @@ where
                     fn qos_default() -> Ros2QosPolicies;
 
                     #(#message_topic_defs)*
+                    #(#service_creation_defs)*
                 }
 
                 #[derive(Debug, Clone)]
@@ -98,22 +107,43 @@ where
                 }
             },
             quote! {
-                struct Ros2Context(ros2_client::Context);
+                struct Ros2Context{
+                    context: ros2_client::Context,
+                    executor: std::sync::Arc<futures::executor::ThreadPool>,
+                }
 
                 fn init_ros2_context() -> eyre::Result<Box<Ros2Context>> {
-                    Ok(Box::new(Ros2Context(ros2_client::Context::new()?)))
+                    Ok(Box::new(Ros2Context{
+                        context: ros2_client::Context::new()?,
+                        executor: std::sync::Arc::new(futures::executor::ThreadPool::new()?),
+                    }))
                 }
 
                 impl Ros2Context {
                     fn new_node(&self, name_space: &str, base_name: &str) -> eyre::Result<Box<Ros2Node>> {
+                        use futures::task::SpawnExt as _;
+                        use eyre::WrapErr as _;
+
                         let name = ros2_client::NodeName::new(name_space, base_name).map_err(|e| eyre::eyre!(e))?;
                         let options = ros2_client::NodeOptions::new().enable_rosout(true);
-                        let node = self.0.new_node(name, options)?;
-                        Ok(Box::new(Ros2Node(node)))
+                        let mut node = self.context.new_node(name, options)?;
+
+                        let spinner = node.spinner();
+                        self.executor.spawn(async {
+                            if let Err(err) = spinner.spin().await {
+                                eprintln!("ros2 spinner failed: {err:?}");
+                            }
+                        })
+                        .context("failed to spawn ros2 spinner")?;
+
+                        Ok(Box::new(Ros2Node{ node, executor: self.executor.clone(), }))
                     }
                 }
 
-                struct Ros2Node(ros2_client::Node);
+                struct Ros2Node {
+                    node : ros2_client::Node,
+                    executor: std::sync::Arc<futures::executor::ThreadPool>,
+                }
 
                 fn qos_default() -> ffi::Ros2QosPolicies {
                     ffi::Ros2QosPolicies::new(None, None, None, None, None, None, None)
@@ -235,6 +265,7 @@ where
 
         #cxx_bridge_impls
         #(#message_topic_impls)*
+        #(#service_creation_impls)*
 
 
         #(#service_impls)*