feat: Initial implementation of ROS to OPC UA method calls

.github/workflows/ci.yml

@@ -14,9 +14,12 @@ jobs:
   build:
     needs: build_dev_container
     uses: ./.github/workflows/ci_build.yml
-  test:
+  unit_test:
     needs: build_dev_container
-    uses: ./.github/workflows/ci_test.yml
+    uses: ./.github/workflows/ci_unit_test.yml
+  integration_test:
+    needs: build_dev_container
+    uses: ./.github/workflows/ci_integration_test.yml
   lint:
     needs: build_dev_container
     uses: ./.github/workflows/ci_lint.yml

.github/workflows/ci_integration_test.yml

@@ -0,0 +1,21 @@
+name: CI Integration Test
+'on':
+  workflow_call: null
+jobs:
+  test:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout (GitHub)
+        uses: actions/checkout@v4
+      - name: Login to GitHub Container Registry
+        uses: docker/login-action@v3
+        with:
+          registry: ghcr.io
+          username: ${{ github.repository_owner }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+      - name: Cargo integration tests
+        uses: devcontainers/[email protected]
+        with:
+          cacheFrom: ghcr.io/vorausrobotik/voraus-ros-bridge-dev
+          runCmd: cargo test --test '*' --verbose -- --test-threads=1
+          push: never

.github/workflows/ci_test.yml → .github/workflows/ci_unit_test.yml

@@ -1,4 +1,4 @@
-name: CI Test
+name: CI Unit Test
 'on':
   workflow_call: null
 jobs:
@@ -13,9 +13,9 @@ jobs:
           registry: ghcr.io
           username: ${{ github.repository_owner }}
           password: ${{ secrets.GITHUB_TOKEN }}
-      - name: Cargo test
+      - name: Cargo unit tests
         uses: devcontainers/[email protected]
         with:
           cacheFrom: ghcr.io/vorausrobotik/voraus-ros-bridge-dev
-          runCmd: cargo test --verbose
+          runCmd: cargo test --bins --verbose
           push: never

Cargo.toml

@@ -18,13 +18,16 @@ opcua = { version = "0.12.0", features = ["client", "console-logging"] }
 # Explicitly pinning openssl >= 0.10.66 is required, as long as opcua is not 0.13.0, yet.
 openssl = { version = "0.10.66" }
 std_msgs = { version = "4.2.4" }
+std_srvs = { version = "4.2.4" }
 sensor_msgs = { version = "4.2.4" }
 builtin_interfaces = { version = "1.2.1" }
 voraus_interfaces = { version = "0.1.0" }
 rclrs = { version = "0.4.1" }
 rosidl_runtime_rs = { version = "0.4.1" }
 log = "0.4.22"
 env_logger = "0.11.5"
+tokio = "1.38.0"
+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/main.rs

@@ -1,15 +1,18 @@
-mod simple_opc_ua_subscriber;
+mod opc_ua_client;
 
 use env_logger::{Builder, Env};
-use log::debug;
+use log::{debug, info};
+use opc_ua_client::OPCUAClient;
 use opcua::types::Variant;
 use rclrs::{create_node, Context, RclrsError};
-use ros_service_server::handle_service;
-use simple_opc_ua_subscriber::SimpleSubscriber;
-use std::{env, sync::Arc};
+use ros_services::ROSServices;
+use std::{
+    env,
+    sync::{Arc, Mutex},
+};
 
 mod ros_publisher;
-mod ros_service_server;
+mod ros_services;
 
 use ros_publisher::{create_joint_state_msg, RosPublisher};
 
@@ -22,14 +25,18 @@ fn main() -> Result<(), RclrsError> {
     let node_copy = Arc::clone(&node);
     let joint_state_publisher = Arc::new(RosPublisher::new(&node, "joint_states").unwrap());
 
-    let _server = node_copy
-        .create_service::<voraus_interfaces::srv::Voraus, _>("add_two_ints", handle_service)?;
-
-    let mut simple_subscriber = SimpleSubscriber::new("opc.tcp://127.0.0.1:4855");
-    let Ok(_connection_result) = simple_subscriber.connect() else {
+    let opc_ua_client = Arc::new(Mutex::new(OPCUAClient::new("opc.tcp://127.0.0.1:4855")));
+    let Ok(_connection_result) = opc_ua_client.lock().unwrap().connect() else {
         panic!("Connection could not be established, but is required.");
     };
 
+    let ros_services = Arc::new(ROSServices::new(Arc::clone(&opc_ua_client)));
+    let _enable_impedance_control =
+        node_copy.create_service::<std_srvs::srv::Empty, _>("enable_impedance_control", {
+            let rsc = Arc::clone(&ros_services);
+            move |request_header, request| rsc.enable_impedance_control(request_header, request)
+        });
+
     let callback = {
         let provider = Arc::clone(&joint_state_publisher);
         move |x: Variant| {
@@ -52,10 +59,22 @@ fn main() -> Result<(), RclrsError> {
                 .expect("Error while publishing.")
         }
     };
-    simple_subscriber
+    opc_ua_client
+        .lock()
+        .unwrap()
         .create_subscription(1, "100111", callback, 10)
         .expect("ERROR: Got an error while subscribing to variables");
     // Loops forever. The publish thread will call the callback with changes on the variables
-    simple_subscriber.run();
+    info!("Starting OPC UA client");
+    let _session = opc_ua_client.lock().unwrap().run_async();
+    info!("Spinning ROS");
     rclrs::spin(node_copy)
 }
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn test_dummy() {
+        assert_eq!(1, 1);
+    }
+}

src/simple_opc_ua_subscriber.rs → src/opc_ua_client.rs

@@ -1,18 +1,19 @@
 use std::sync::Arc;
 
-use log::{debug, error};
+use log::debug;
 use opcua::client::prelude::{
-    Client, ClientBuilder, DataChangeCallback, IdentityToken, MonitoredItem, MonitoredItemService,
-    Session, SubscriptionService,
+    Client, ClientBuilder, DataChangeCallback, IdentityToken, MethodService, MonitoredItem,
+    MonitoredItemService, Session, SessionCommand, SubscriptionService,
 };
 use opcua::crypto::SecurityPolicy;
 use opcua::sync::RwLock;
 use opcua::types::{
-    MessageSecurityMode, MonitoredItemCreateRequest, NodeId, StatusCode, TimestampsToReturn,
-    UserTokenPolicy, Variant,
+    CallMethodRequest, MessageSecurityMode, MonitoredItemCreateRequest, NodeId, StatusCode,
+    TimestampsToReturn, UserTokenPolicy, Variant,
 };
+use tokio::sync::oneshot;
 
-pub struct SimpleSubscriber {
+pub struct OPCUAClient {
     endpoint: String,
     session: Option<Arc<RwLock<Session>>>,
 }
@@ -25,7 +26,7 @@ fn extract_value(item: &MonitoredItem) -> Variant {
         .expect("No value found - check value bit in EncodingMask.")
 }
 
-impl SimpleSubscriber {
+impl OPCUAClient {
     pub fn new<S: Into<String>>(endpoint: S) -> Self {
         Self {
             endpoint: endpoint.into(),
@@ -35,9 +36,9 @@ impl SimpleSubscriber {
 
     pub fn connect(&mut self) -> Result<(), &str> {
         let mut client: Client = ClientBuilder::new()
-            .application_name("Simple Subscriber")
-            .application_uri("urn:SimpleSubscriber")
-            .product_uri("urn:SimpleSubscriber")
+            .application_name("voraus ROS brigde")
+            .application_uri("urn:vorausRosBridge")
+            .product_uri("urn:vorausRosBridge")
             .trust_server_certs(true)
             .create_sample_keypair(true)
             .session_retry_limit(5)
@@ -117,12 +118,27 @@ impl SimpleSubscriber {
         Ok(())
     }
 
-    pub fn run(self) {
-        match self.session {
-            Some(session) => Session::run(session),
-            None => {
-                error!("Could not run inexistent session.");
-            }
-        }
+    pub fn call_method<T>(&self, object_id: NodeId, method_id: NodeId, args: Vec<T>)
+    where
+        T: Into<Variant>,
+    {
+        let cloned_session_lock = self.session.clone().unwrap();
+        let session = cloned_session_lock.read();
+        let _arguments: Option<Vec<Variant>> = Some(args.into_iter().map(Into::into).collect());
+        let method = CallMethodRequest {
+            object_id,
+            method_id,
+            input_arguments: None,
+        };
+        let result = session.call(method).unwrap();
+        debug!("result of call: {:?}", result);
+    }
+
+    pub fn run_async(&self) -> oneshot::Sender<SessionCommand> {
+        let cloned_session_lock = self
+            .session
+            .clone()
+            .expect("The session should be connected. Please call connect() first.");
+        Session::run_async(cloned_session_lock)
     }
 }

src/ros_services.rs

@@ -0,0 +1,34 @@
+use std::sync::{Arc, Mutex};
+
+use log::info;
+use opcua::types::NodeId;
+use std_srvs::srv::{Empty_Request, Empty_Response};
+
+use crate::opc_ua_client::OPCUAClient;
+
+pub struct ROSServices {
+    opc_ua_client: Arc<Mutex<OPCUAClient>>,
+}
+
+impl ROSServices {
+    pub fn new(opc_ua_client: Arc<Mutex<OPCUAClient>>) -> Self {
+        Self { opc_ua_client }
+    }
+
+    pub fn enable_impedance_control(
+        &self,
+        _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
+            .lock()
+            .unwrap()
+            .call_method(object_id, method_id, vec![()]);
+        Empty_Response {
+            structure_needs_at_least_one_member: 0,
+        }
+    }
+}

tests/helpers/mod.rs

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