WIP: test: Add e2e test for ros service calls

Cargo.toml

@@ -27,6 +27,7 @@ rosidl_runtime_rs = { version = "0.4.1" }
 tokio = "1.38.0"
 log = "0.4.22"
 env_logger = "0.11.5"
+subprocess = "0.2.9"
 
 [dev-dependencies]
 rclrs = { version = "0.4.1" }

examples/opc_ua/rapid-clock.rs

@@ -11,11 +11,7 @@ fn rapid_clock() {
     let mut server =
         Server::new(ServerConfig::load(&PathBuf::from("tests/resources/clock.conf")).unwrap());
 
-    let namespace = {
-        let address_space = server.address_space();
-        let mut address_space = address_space.write();
-        address_space.register_namespace("urn:rapid-clock").unwrap()
-    };
+    let namespace = 1u16;
 
     add_timed_variable(&mut server, namespace);
 

src/opc_ua_client.rs

@@ -1,6 +1,6 @@
 use std::sync::Arc;
 
-use log::debug;
+use log::{debug, info};
 use opcua::client::prelude::{
     Client, ClientBuilder, DataChangeCallback, IdentityToken, MethodService, MonitoredItem,
     MonitoredItemService, Session, SessionCommand, SubscriptionService,
@@ -131,7 +131,7 @@ impl OPCUAClient {
             input_arguments: None,
         };
         let result = session.call(method).unwrap();
-        debug!("result of call: {:?}", result);
+        info!("result of call: {:?}", result);
     }
 
     pub fn run_async(&self) -> oneshot::Sender<SessionCommand> {

src/ros_services.rs

@@ -1,5 +1,6 @@
 use std::sync::{Arc, Mutex};
 
+use log::info;
 use opcua::types::NodeId;
 use std_srvs::srv::{Empty_Request, Empty_Response};
 
@@ -19,6 +20,7 @@ impl ROSServices {
         _request_header: &rclrs::rmw_request_id_t,
         _request: Empty_Request,
     ) -> Empty_Response {
+        info!("ROS service called");
         let object_id = NodeId::new(1, 100182);
         let method_id = NodeId::new(1, 100263);
         self.opc_ua_client

tests/helpers/mod.rs

@@ -1,2 +1,3 @@
 pub mod opc_ua_publisher_single_linear;
+pub mod ros_service_caller;
 pub mod ros_subscriber;

tests/helpers/opc_ua_publisher_single_linear.rs

@@ -1,5 +1,14 @@
-use opcua::server::prelude::{Config, DateTime, NodeId, Server, ServerConfig, Variable};
+use log::info;
+use opcua::server::callbacks;
+use opcua::server::prelude::{
+    Config, DateTime, EventNotifier, MethodBuilder, NodeId, ObjectBuilder, Server, ServerConfig,
+    Variable,
+};
+use opcua::server::session::SessionManager;
+use opcua::sync::RwLock;
+use opcua::types::{CallMethodRequest, CallMethodResult, ObjectId, StatusCode};
 use std::path::PathBuf;
+use std::sync::Arc;
 use std::thread;
 
 pub async fn run_rapid_clock() -> thread::JoinHandle<()> {
@@ -17,6 +26,7 @@ fn rapid_clock() {
     let questionable_namespace = 1u16;
 
     add_timed_variable(&mut server, questionable_namespace);
+    add_no_op_method(&mut server, questionable_namespace);
 
     server.run();
 }
@@ -60,3 +70,40 @@ fn add_timed_variable(server: &mut Server, namespace: u16) {
         });
     }
 }
+
+fn add_no_op_method(server: &mut Server, namespace: u16) {
+    let address_space = server.address_space();
+    let mut address_space = address_space.write();
+
+    let object_id = NodeId::new(namespace, 100182);
+    ObjectBuilder::new(&object_id, "Functions", "Functions")
+        .event_notifier(EventNotifier::SUBSCRIBE_TO_EVENTS)
+        .organized_by(ObjectId::ObjectsFolder)
+        .insert(&mut address_space);
+
+    // NoOp has 0 inputs and 0 outputs
+    let method_id = NodeId::new(namespace, 100263);
+    MethodBuilder::new(&method_id, "NoOp", "NoOp")
+        .component_of(object_id.clone())
+        .callback(Box::new(NoOp))
+        .insert(&mut address_space);
+}
+
+struct NoOp;
+
+impl callbacks::Method for NoOp {
+    fn call(
+        &mut self,
+        _session_id: &NodeId,
+        _session_map: Arc<RwLock<SessionManager>>,
+        _request: &CallMethodRequest,
+    ) -> Result<CallMethodResult, StatusCode> {
+        info!("NoOp method called");
+        Ok(CallMethodResult {
+            status_code: StatusCode::Good,
+            input_argument_results: None,
+            input_argument_diagnostic_infos: None,
+            output_arguments: None,
+        })
+    }
+}

tests/helpers/ros_service_caller.rs

@@ -0,0 +1,62 @@
+use rclrs::{Node, RclrsError};
+use std::{
+    env,
+    sync::{Arc, Mutex},
+    time::Duration,
+};
+
+pub struct ServiceCaller {
+    pub client: Arc<rclrs::Client<std_srvs::srv::Empty>>,
+    pub node: Arc<Node>,
+    pub request: std_srvs::srv::Empty_Request,
+    pub number_of_calls: Arc<Mutex<u32>>,
+}
+
+impl ServiceCaller {
+    pub fn new(service: &str) -> Result<Arc<Self>, RclrsError> {
+        let context = rclrs::Context::new(env::args())?;
+
+        let node = rclrs::create_node(&context, "minimal_client")?;
+
+        let client = node.create_client::<std_srvs::srv::Empty>(service)?;
+
+        let request = std_srvs::srv::Empty_Request {
+            structure_needs_at_least_one_member: 0,
+        };
+        let action_caller = Arc::new(ServiceCaller {
+            client,
+            node,
+            request,
+            number_of_calls: Arc::new(Mutex::new(0)),
+        });
+
+        Ok(action_caller)
+    }
+
+    pub fn start(&self) {
+        println!("Starting ROS Service Client");
+
+        while !self.client.service_is_ready().unwrap() {
+            std::thread::sleep(std::time::Duration::from_millis(10));
+        }
+        println!("ROS Service Client is ready");
+    }
+    pub fn call(&self) {
+        println!("Calling a ROS Service");
+        let num_calls_clone = Arc::clone(&self.number_of_calls);
+        self.client
+            .async_send_request_with_callback(
+                &self.request,
+                move |_response: std_srvs::srv::Empty_Response| {
+                    println!("Got an answer");
+                    let mut num_calls = num_calls_clone.lock().unwrap();
+                    *num_calls += 1;
+                },
+            )
+            .unwrap();
+
+        let timeout = Some(Duration::new(5, 0));
+        let node_clone = Arc::clone(&self.node);
+        rclrs::spin_once(node_clone, timeout).unwrap();
+    }
+}

tests/test_bridge.rs

@@ -1,32 +1,83 @@
 use std::{
-    process::Command,
+    env,
+    path::PathBuf,
     sync::{atomic::Ordering, Arc},
     time::Duration,
 };
 
+use common::wait_for_function_to_pass;
+use subprocess::{Popen, PopenConfig, Redirection};
+
+pub mod common;
 pub mod helpers;
 
+struct ManagedRosBridge {
+    process: Popen,
+}
+
+impl ManagedRosBridge {
+    fn new() -> subprocess::Result<Self> {
+        // Start the ROS Brigde
+        // We can't use ros2 run here because of https://github.com/ros2/ros2cli/issues/895
+        let root_dir = env::var("CARGO_MANIFEST_DIR").expect("CARGO_MANIFEST_DIR is not set");
+        let mut path_to_executable = PathBuf::from(root_dir);
+        path_to_executable
+            .push("install/voraus-ros-bridge/lib/voraus-ros-bridge/voraus-ros-bridge");
+
+        let process = Popen::create(
+            &[path_to_executable],
+            PopenConfig {
+                stdout: Redirection::Pipe,
+                stderr: Redirection::Pipe,
+                detached: false,
+                ..Default::default()
+            },
+        )?;
+
+        Ok(ManagedRosBridge { process })
+    }
+
+    fn is_running(&mut self) -> bool {
+        self.process.poll().is_none()
+    }
+
+    fn get_std_err(&mut self) -> Option<String> {
+        let (_out, err) = self
+            .process
+            .communicate(None)
+            .expect("Failed to capture output");
+        err
+    }
+
+    fn terminate(&mut self) {
+        let _ = self.process.terminate();
+        let _ = self.process.wait();
+    }
+}
+
+impl Drop for ManagedRosBridge {
+    fn drop(&mut self) {
+        if self.is_running() {
+            self.terminate();
+        }
+    }
+}
+
 #[tokio::test]
 async fn e2e_opc_ua_var_to_ros_topic() {
     // Start the OPC UA server in the background
     helpers::opc_ua_publisher_single_linear::run_rapid_clock().await;
 
-    println!("Starting ros bridge");
-    let mut bridge = Command::new("ros2")
-        .args(["run"])
-        .args(["voraus-ros-bridge"])
-        .args(["voraus-ros-bridge"])
-        .spawn()
-        .expect("Failed to run command");
+    let mut _bridge_process = ManagedRosBridge::new().expect("Failed to start subprocess");
 
     // Spawn ROS Subscriber against a sample topic
-    println!("Creating Subscription");
     let subscription = Arc::new(
         helpers::ros_subscriber::Subscriber::new("joint_states_subscriber", "joint_states")
             .unwrap(),
     );
-    let timeout = Some(Duration::new(5, 0));
-    rclrs::spin_once(subscription.node.clone(), timeout).expect("Could not spin");
+    let clone = Arc::clone(&subscription.node);
+    // TODO: Figure out why this takes almost 10 s
+    rclrs::spin_once(clone, Some(Duration::from_secs(25))).expect("Could not spin");
 
     let mut number_of_messages_received = subscription.num_messages.load(Ordering::SeqCst);
     let first_value = *subscription
@@ -40,7 +91,7 @@ async fn e2e_opc_ua_var_to_ros_topic() {
         .unwrap();
     assert_eq!(number_of_messages_received, 1);
 
-    rclrs::spin_once(subscription.node.clone(), timeout).expect("Could not spin");
+    rclrs::spin_once(subscription.node.clone(), Some(Duration::new(5, 0))).expect("Could not spin");
 
     number_of_messages_received = subscription.num_messages.load(Ordering::SeqCst);
     assert_eq!(number_of_messages_received, 2);
@@ -61,8 +112,34 @@ async fn e2e_opc_ua_var_to_ros_topic() {
         second_value,
         first_value
     );
-    bridge
-        .kill()
-        .expect("voraus-ros-bridge process could not be killed.");
-    println!("done");
+}
+
+#[tokio::test]
+async fn e2e_ros_service_to_opc_ua_call() {
+    // Start the OPC UA server in the background
+    helpers::opc_ua_publisher_single_linear::run_rapid_clock().await;
+
+    let mut bridge_process = ManagedRosBridge::new().expect("Failed to start subprocess");
+
+    let service_caller = Arc::new(
+        helpers::ros_service_caller::ServiceCaller::new("enable_impedance_control").unwrap(),
+    );
+
+    // TODO: Figure out why this takes almost 10 s
+    service_caller.start();
+    assert!(*service_caller.number_of_calls.lock().unwrap() == 0);
+    service_caller.call();
+    println!("call done");
+
+    wait_for_function_to_pass(
+        || *service_caller.number_of_calls.lock().unwrap() == 1,
+        5000,
+    )
+    .unwrap();
+    println!("get_std");
+    bridge_process.terminate();
+    assert!(bridge_process
+        .get_std_err()
+        .unwrap()
+        .contains("ROS service called"));
 }