feat: Implement examplary topics

Cargo.toml

@@ -20,6 +20,7 @@ openssl = { version = "0.10.66" }
 std_msgs = { version = "4.2.4" }
 std_srvs = { version = "4.2.4" }
 sensor_msgs = { version = "4.2.4" }
+geometry_msgs = { version = "4.2.4" }
 builtin_interfaces = { version = "1.2.1" }
 voraus_interfaces = { version = "0.1.0" }
 rclrs = { version = "0.4.1" }

src/main.rs

@@ -1,80 +1,48 @@
 mod opc_ua_client;
 
 use env_logger::{Builder, Env};
-use log::{debug, info};
+use log::info;
 use opc_ua_client::OPCUAClient;
-use opcua::types::Variant;
 use rclrs::{create_node, Context, RclrsError};
+use register_subscriptions::register_opcua_subscriptions;
 use ros_services::ROSServices;
 use std::{
     env,
     sync::{Arc, Mutex},
 };
 
+mod register_subscriptions;
+mod ros_message_creation;
 mod ros_publisher;
 mod ros_services;
 
-use ros_publisher::{create_joint_state_msg, RosPublisher};
-
 fn main() -> Result<(), RclrsError> {
     Builder::from_env(Env::default().default_filter_or("info"))
         .filter_module("opcua", log::LevelFilter::Warn)
         .init();
     let context = Context::new(env::args()).unwrap();
-    let node = create_node(&context, "voraus_bridge_node")?;
-    let node_copy = Arc::clone(&node);
-    let joint_state_publisher = Arc::new(RosPublisher::new(&node, "joint_states").unwrap());
+    let ros_node = create_node(&context, "voraus_bridge_node")?;
+    let ros_node_copy_spin = Arc::clone(&ros_node);
+    let ros_node_copy_service = Arc::clone(&ros_node);
 
-    let opc_ua_client = Arc::new(Mutex::new(OPCUAClient::new("opc.tcp://127.0.0.1:4855")));
+    let opc_ua_client = Arc::new(Mutex::new(OPCUAClient::new("opc.tcp://127.0.0.1:48401")));
     let Ok(_connection_result) = opc_ua_client.lock().unwrap().connect() else {
         panic!("Connection could not be established, but is required.");
     };
+    let opc_ua_client_copy_run = Arc::clone(&opc_ua_client);
+    let opc_ua_client_copy_services = Arc::clone(&opc_ua_client);
+
+    register_opcua_subscriptions(ros_node, opc_ua_client);
 
-    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 ros_services = Arc::new(ROSServices::new(opc_ua_client_copy_services));
+    let _enable_impedance_control = ros_node_copy_service
+        .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| {
-            debug!("Value = {:?}", &x);
-            let mut data_value: Vec<f64> = vec![];
-            match x {
-                Variant::Array(unwrapped) => {
-                    unwrapped.values.into_iter().for_each(|value| {
-                        let unpacked_value = value
-                            .as_f64()
-                            .expect("This should have been encapsulated f64 but wasn't.");
-                        data_value.push(unpacked_value)
-                    });
-                }
-                _ => panic!("Expected an array"),
-            };
-            let j_msg = create_joint_state_msg(data_value);
-            provider
-                .publish_data(&j_msg)
-                .expect("Error while publishing.")
-        }
-    };
-    opc_ua_client
-        .lock()
-        .unwrap()
-        .create_subscription(1, 100111, callback, 10.0)
-        .expect("ERROR: Got an error while subscribing to variables");
-    // Loops forever. The publish thread will call the callback with changes on the variables
     info!("Starting OPC UA client");
-    let _session = opc_ua_client.lock().unwrap().run_async();
+    let _session = opc_ua_client_copy_run.lock().unwrap().run_async();
     info!("Spinning ROS");
-    rclrs::spin(node_copy)
-}
-
-#[cfg(test)]
-mod tests {
-    #[test]
-    fn test_dummy() {
-        assert_eq!(1, 1);
-    }
+    rclrs::spin(ros_node_copy_spin)
 }

src/register_subscriptions.rs

@@ -0,0 +1,144 @@
+use std::sync::{Arc, Mutex};
+
+use geometry_msgs::msg::{TwistStamped, WrenchStamped};
+use rclrs::Node;
+
+use crate::{
+    opc_ua_client::OPCUAClient,
+    ros_message_creation::{create_twist_stamped_msg, create_wrench_stamped_msg},
+    ros_publisher::{unpack_data, JointStatesBuffer, RosPublisher, TCPPoseBuffer},
+};
+
+pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mutex<OPCUAClient>>) {
+    let ros_node_copy_joint_states_buffer = Arc::clone(&ros_node);
+    let ros_node_copy_tcp_pose_buffer = Arc::clone(&ros_node);
+
+    let joint_states_buffer = Arc::new(Mutex::new(JointStatesBuffer::new(
+        ros_node_copy_joint_states_buffer,
+    )));
+    let joint_states_buffer_copy_position = Arc::clone(&joint_states_buffer);
+    let joint_states_buffer_copy_velocity = Arc::clone(&joint_states_buffer);
+    let joint_states_buffer_copy_effort = Arc::clone(&joint_states_buffer);
+
+    let tcp_pose_buffer = Arc::new(Mutex::new(TCPPoseBuffer::new(
+        ros_node_copy_tcp_pose_buffer,
+    )));
+    let tcp_pose_buffer_copy_pose = Arc::clone(&tcp_pose_buffer);
+    let tcp_pose_buffer_copy_quaternion = Arc::clone(&tcp_pose_buffer);
+
+    let tcp_twist_publisher: Arc<Mutex<RosPublisher<TwistStamped>>> = Arc::new(Mutex::new(
+        RosPublisher::new(&ros_node, "tcp_twist").unwrap(),
+    ));
+    let tcp_wrench_publisher: Arc<Mutex<RosPublisher<WrenchStamped>>> = Arc::new(Mutex::new(
+        RosPublisher::new(&ros_node, "tcp_wrench").unwrap(),
+    ));
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100111,
+            move |x| {
+                joint_states_buffer_copy_position
+                    .lock()
+                    .unwrap()
+                    .on_position_change(x);
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100115,
+            move |x| {
+                joint_states_buffer_copy_velocity
+                    .lock()
+                    .unwrap()
+                    .on_velocity_change(x);
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100113,
+            move |x| {
+                joint_states_buffer_copy_effort
+                    .lock()
+                    .unwrap()
+                    .on_effort_change(x);
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100707,
+            move |x| tcp_pose_buffer_copy_pose.lock().unwrap().on_pose_change(x),
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100710,
+            move |x| {
+                tcp_pose_buffer_copy_quaternion
+                    .lock()
+                    .unwrap()
+                    .on_quaternion_change(x)
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100708,
+            move |x| {
+                tcp_twist_publisher
+                    .lock()
+                    .unwrap()
+                    .publish_data(&create_twist_stamped_msg(unpack_data(x)))
+                    .unwrap()
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+
+    opc_ua_client
+        .lock()
+        .unwrap()
+        .create_subscription(
+            1,
+            100711,
+            move |x| {
+                tcp_wrench_publisher
+                    .lock()
+                    .unwrap()
+                    .publish_data(&create_wrench_stamped_msg(unpack_data(x)))
+                    .unwrap()
+            },
+            0.0,
+        )
+        .expect("ERROR: Got an error while subscribing to variable");
+}