feat: Properly implement enable/disable impedance control

src/main.rs

@@ -46,11 +46,17 @@ fn main() -> Result<(), RclrsError> {
 
     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", {
+        .create_service::<std_srvs::srv::Empty, _>("~/impedance_control/enable", {
             let rsc = Arc::clone(&ros_services);
             move |request_header, request| rsc.enable_impedance_control(request_header, request)
         });
 
+    let _disable_impedance_control = ros_node_copy_service
+        .create_service::<std_srvs::srv::Empty, _>("~/impedance_control/disable", {
+            let rsc = Arc::clone(&ros_services);
+            move |request_header, request| rsc.disable_impedance_control(request_header, request)
+        });
+
     let _wrench_subscriber: Arc<Subscription<Wrench>> = ros_node.create_subscription(
         "~/impedance_control/set_wrench",
         QOS_PROFILE_DEFAULT,

src/ros_services.rs

@@ -31,4 +31,20 @@ impl ROSServices {
             structure_needs_at_least_one_member: 0,
         }
     }
+
+    pub fn disable_impedance_control(
+        &self,
+        _request_header: &rclrs::rmw_request_id_t,
+        _request: Empty_Request,
+    ) -> Empty_Response {
+        let object_id = NodeId::new(1, 100182);
+        let method_id = NodeId::new(1, 100264);
+        self.opc_ua_client
+            .lock()
+            .unwrap()
+            .call_method(object_id, method_id, None::<Vec<()>>);
+        Empty_Response {
+            structure_needs_at_least_one_member: 0,
+        }
+    }
 }

tests/test_bridge.rs

@@ -126,7 +126,7 @@ async fn e2e_ros_service_to_opc_ua_call() {
 
     let service_caller = Arc::new(
         helpers::ros_service_caller::ServiceCaller::new(
-            "/voraus_bridge_node/enable_impedance_control",
+            "/voraus_bridge_node/impedance_control/enable",
         )
         .unwrap(),
     );