feat: Add move joints

README.md

@@ -25,6 +25,7 @@ Currently, the following voraus.core functionality is exposed to the ROS 2 ecosy
 - Set tcp wrench for impedance control (via topic `impedance_control/set_wrench`)
 - Set stiffness for impedance control (via topic `impedance_control/set_stiffness`)
 - Enable/disable impedance control mode (via service `impedance_control/enable` or `impedance_control/disable`)
+- Move joints (via service `move_joints`)
 
 ## Configuration
 

src/main.rs

@@ -59,6 +59,13 @@ fn main() -> Result<(), RclrsError> {
             move |request_header, request| rsc.disable_impedance_control(request_header, request)
         });
 
+    let ros_node_copy = Arc::clone(&ros_node);
+    let _move_joints =
+        ros_node_copy.create_service::<voraus_interfaces::srv::MoveJoints, _>("~/move_joints", {
+            let rsc = Arc::clone(&ros_services);
+            move |request_header, request| rsc.move_joints(request_header, request)
+        });
+
     let opc_ua_client_copy = Arc::clone(&opc_ua_client);
     let _wrench_subscriber: Arc<Subscription<Wrench>> = ros_node.create_subscription(
         "~/impedance_control/set_wrench",

src/ros_services.rs

@@ -3,6 +3,7 @@ use std::sync::{Arc, Mutex};
 use log::info;
 use opcua::types::{NodeId, Variant};
 use std_srvs::srv::{Empty_Request, Empty_Response};
+use voraus_interfaces::srv::{MoveJoints_Request, MoveJoints_Response};
 
 use crate::opc_ua_client::OPCUAClient;
 
@@ -47,4 +48,33 @@ impl ROSServices {
             structure_needs_at_least_one_member: 0,
         }
     }
+
+    pub fn move_joints(
+        &self,
+        _request_header: &rclrs::rmw_request_id_t,
+        request: MoveJoints_Request,
+    ) -> MoveJoints_Response {
+        info!("ROS service call: Move Joints");
+        let object_id = NodeId::new(1, 100210);
+        let method_id = NodeId::new(1, 100211);
+        self.opc_ua_client.lock().unwrap().call_method(
+            object_id,
+            method_id,
+            Some(vec![
+                request.relative.into(),
+                request.target_reference_cs.into(),
+                request.target_coordinate.into(),
+                request.arriving_cs.into(),
+                request.velocity_scaling.into(),
+                request.with_blending.into(),
+                request.blending_parameter.into(),
+                request.config_vector.into(),
+                request.command_id.into(),
+                request.manual_mode.into(),
+            ]),
+        );
+        MoveJoints_Response {
+            structure_needs_at_least_one_member: 0,
+        }
+    }
 }

tests/helpers/opc_ua_test_server.rs

@@ -6,7 +6,9 @@ use opcua::server::prelude::{
 };
 use opcua::server::session::SessionManager;
 use opcua::sync::RwLock;
-use opcua::types::{CallMethodRequest, CallMethodResult, ObjectId, StatusCode, Variant};
+use opcua::types::{
+    Array, CallMethodRequest, CallMethodResult, ObjectId, StatusCode, Variant, VariantTypeId,
+};
 use std::path::PathBuf;
 use std::sync::mpsc::Sender;
 use std::sync::Arc;
@@ -102,6 +104,33 @@ fn add_methods(server: &mut Server, namespace: u16, assertion_tx: Sender<String>
         "impedance_control/disable".to_string(),
         assertion_tx.clone(),
     );
+    add_method_stub(
+        server,
+        NodeId::new(namespace, 100210),
+        NodeId::new(namespace, 100211),
+        Some(vec![
+            Variant::Boolean(false),
+            Variant::UInt32(0),
+            // The opcua crate creates an array with dimension = Some([]) when decoding an empty array.
+            // The default trait used in the ros service caller generates an empty vector so this is the workaround
+            // to still have a useful assertion.
+            Array::new_multi(VariantTypeId::Double, Vec::new(), Vec::new())
+                .unwrap()
+                .into(),
+            Variant::UInt32(0),
+            Variant::Double(0.0),
+            Variant::Boolean(false),
+            Variant::Double(0.0),
+            Array::new_multi(VariantTypeId::Int32, Vec::new(), Vec::new())
+                .unwrap()
+                .into(),
+            Variant::UInt32(0),
+            Variant::Boolean(false),
+        ]),
+        None,
+        "move_joints".to_string(),
+        assertion_tx.clone(),
+    );
 }
 
 /// Creates an OPC UA variable that is monotonously increasing.

tests/test_bridge_methods.rs

@@ -6,6 +6,7 @@ use std::{
 use common::{wait_for_function_to_pass, ManagedRosBridge};
 use helpers::opc_ua_test_server::OPCUATestServer;
 use std_srvs::srv::Empty;
+use voraus_interfaces::srv::MoveJoints;
 
 pub mod common;
 pub mod helpers;
@@ -63,3 +64,4 @@ make_testcase_method!(
     Empty,
     test_disable_impedance_control
 );
+make_testcase_method!("move_joints", MoveJoints, test_move_joints);

voraus_interfaces/CMakeLists.txt

@@ -20,6 +20,7 @@ set(msg_files
 )
 
 set(srv_files
+  "srv/MoveJoints.srv"
   "srv/Voraus.srv"
 )
 

voraus_interfaces/srv/MoveJoints.srv

@@ -0,0 +1,11 @@
+bool relative
+uint32 target_reference_cs
+float64[] target_coordinate
+uint32 arriving_cs
+float64 velocity_scaling
+bool with_blending
+float64 blending_parameter
+int32[] config_vector
+uint32 command_id
+bool manual_mode
+---