feat: Add configurable frame id prefix

compose.yaml

@@ -7,4 +7,5 @@ services:
         environment:
             - AMENT_PREFIX_PATH=/root/voraus-ros-bridge/install/voraus-ros-bridge:/opt/ros/humble
             - ROS_NAMESPACE=robot1
+            - FRAME_ID_PREFIX=robot1
             - VORAUS_CORE_OPC_UA_ENDPOINT=opc.tcp://127.0.0.1:48401

src/main.rs

@@ -33,14 +33,20 @@ fn main() -> Result<(), RclrsError> {
         Ok(val) => val,
         Err(_) => "opc.tcp://127.0.0.1:48401".to_string(),
     };
+    let env_var_name = "FRAME_ID_PREFIX";
+    let frame_id_prefix = match env::var(env_var_name) {
+        Ok(val) => Some(val),
+        Err(_) => None,
+    };
     let opc_ua_client = Arc::new(Mutex::new(OPCUAClient::new(voraus_core_opc_ua_endpoint)));
     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 = Arc::clone(&opc_ua_client);
-    register_opcua_subscriptions(ros_node_copy_register, opc_ua_client_copy);
+    register_opcua_subscriptions(ros_node_copy_register, opc_ua_client_copy, frame_id_prefix);
 
-    let ros_services = Arc::new(ROSServices::new(opc_ua_client_copy_services));
+    let opc_ua_client_copy = Arc::clone(&opc_ua_client);
+    let ros_services = Arc::new(ROSServices::new(opc_ua_client_copy));
     let _enable_impedance_control = ros_node_copy_service
         .create_service::<std_srvs::srv::Empty, _>("~/impedance_control/enable", {
             let rsc = Arc::clone(&ros_services);

src/register_subscriptions.rs

@@ -9,19 +9,30 @@ use crate::{
     ros_publisher::{unpack_data, JointStatesBuffer, RosPublisher, TCPPoseBuffer},
 };
 
-pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mutex<OPCUAClient>>) {
+pub fn register_opcua_subscriptions(
+    ros_node: Arc<Node>,
+    opc_ua_client: Arc<Mutex<OPCUAClient>>,
+    frame_id_prefix: Option<String>,
+) {
     let ros_node_copy_joint_states_buffer = Arc::clone(&ros_node);
     let ros_node_copy_tcp_pose_buffer = Arc::clone(&ros_node);
 
+    let base_link_frame_id = match frame_id_prefix {
+        Some(prefix) => format!("{}_{}", prefix.clone(), "base_link"),
+        None => "base_link".to_string(),
+    };
+
     let joint_states_buffer = Arc::new(Mutex::new(JointStatesBuffer::new(
         ros_node_copy_joint_states_buffer,
+        base_link_frame_id.clone(),
     )));
     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,
+        base_link_frame_id.clone(),
     )));
     let tcp_pose_buffer_copy_pose = Arc::clone(&tcp_pose_buffer);
     let tcp_pose_buffer_copy_quaternion = Arc::clone(&tcp_pose_buffer);
@@ -108,6 +119,7 @@ pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mute
         )
         .expect("ERROR: Got an error while subscribing to variable");
 
+    let base_link_frame_id_with_prefix_clone = base_link_frame_id.clone();
     opc_ua_client
         .lock()
         .unwrap()
@@ -118,13 +130,17 @@ pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mute
                 tcp_twist_publisher
                     .lock()
                     .unwrap()
-                    .publish_data(&create_twist_stamped_msg(unpack_data(x)))
+                    .publish_data(&create_twist_stamped_msg(
+                        unpack_data(x),
+                        &base_link_frame_id_with_prefix_clone,
+                    ))
                     .unwrap()
             },
             0.0,
         )
         .expect("ERROR: Got an error while subscribing to variable");
 
+    let base_link_frame_id_with_prefix_clone = base_link_frame_id.clone();
     opc_ua_client
         .lock()
         .unwrap()
@@ -135,7 +151,10 @@ pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mute
                 tcp_wrench_publisher
                     .lock()
                     .unwrap()
-                    .publish_data(&create_wrench_stamped_msg(unpack_data(x)))
+                    .publish_data(&create_wrench_stamped_msg(
+                        unpack_data(x),
+                        &base_link_frame_id_with_prefix_clone,
+                    ))
                     .unwrap()
             },
             0.0,

src/ros_message_creation.rs

@@ -12,11 +12,12 @@ pub fn create_joint_state_msg(
     positions: &Arc<Mutex<Vec<f64>>>,
     velocities: &Arc<Mutex<Vec<f64>>>,
     efforts: &Arc<Mutex<Vec<f64>>>,
+    frame_id: &str,
 ) -> JointState {
     JointState {
         header: Header {
             stamp: create_time_msg(),
-            frame_id: "base_link".to_string(),
+            frame_id: frame_id.to_owned(),
         },
         name: vec![
             "joint1".to_string(),
@@ -35,14 +36,15 @@ pub fn create_joint_state_msg(
 pub fn create_pose_stamped_msg(
     pose: &Arc<Mutex<Vec<f64>>>,
     quaternion: &Arc<Mutex<Vec<f64>>>,
+    frame_id: &str,
 ) -> PoseStamped {
     let pose = pose.lock().unwrap().to_vec();
     let quaternion = quaternion.lock().unwrap().to_vec();
 
     PoseStamped {
         header: Header {
             stamp: create_time_msg(),
-            frame_id: "base_link".to_string(),
+            frame_id: frame_id.to_owned(),
         },
         pose: Pose {
             position: geometry_msgs::msg::Point {
@@ -60,7 +62,7 @@ pub fn create_pose_stamped_msg(
     }
 }
 
-pub fn create_wrench_stamped_msg(tcp_force_torque: Vec<f64>) -> WrenchStamped {
+pub fn create_wrench_stamped_msg(tcp_force_torque: Vec<f64>, frame_id: &str) -> WrenchStamped {
     let force = Vector3 {
         x: tcp_force_torque[0],
         y: tcp_force_torque[1],
@@ -74,13 +76,13 @@ pub fn create_wrench_stamped_msg(tcp_force_torque: Vec<f64>) -> WrenchStamped {
     WrenchStamped {
         header: Header {
             stamp: create_time_msg(),
-            frame_id: "base_link".to_string(),
+            frame_id: frame_id.to_owned(),
         },
         wrench: Wrench { force, torque },
     }
 }
 
-pub fn create_twist_stamped_msg(tcp_velocities: Vec<f64>) -> TwistStamped {
+pub fn create_twist_stamped_msg(tcp_velocities: Vec<f64>, frame_id: &str) -> TwistStamped {
     let linear = Vector3 {
         x: tcp_velocities[0],
         y: tcp_velocities[1],
@@ -94,7 +96,7 @@ pub fn create_twist_stamped_msg(tcp_velocities: Vec<f64>) -> TwistStamped {
     TwistStamped {
         header: Header {
             stamp: create_time_msg(),
-            frame_id: "base_link".to_string(),
+            frame_id: frame_id.to_owned(),
         },
         twist: Twist { linear, angular },
     }
@@ -140,7 +142,7 @@ mod tests {
                 },
             },
         };
-        let mut pose_stamped_msg = create_pose_stamped_msg(&pose, &quaternions);
+        let mut pose_stamped_msg = create_pose_stamped_msg(&pose, &quaternions, "base_link");
         pose_stamped_msg.header.stamp.sec = 1;
         pose_stamped_msg.header.stamp.nanosec = 2;
         assert_eq!(expected_pose_stamped_msg, pose_stamped_msg);
@@ -173,7 +175,8 @@ mod tests {
             velocity: vec![7.0, 8.0, 9.0, 10.0, 11.0, 12.0],
             effort: vec![13.0, 14.0, 15.0, 16.0, 17.0, 18.0],
         };
-        let mut joint_states_msg = create_joint_state_msg(&positions, &velocities, &efforts);
+        let mut joint_states_msg =
+            create_joint_state_msg(&positions, &velocities, &efforts, "base_link");
         joint_states_msg.header.stamp.sec = 1;
         joint_states_msg.header.stamp.nanosec = 2;
         assert_eq!(expected_joint_states_msg, joint_states_msg);
@@ -199,7 +202,7 @@ mod tests {
                 },
             },
         };
-        let mut twist_stamped_msg = create_twist_stamped_msg(tcp_velocities);
+        let mut twist_stamped_msg = create_twist_stamped_msg(tcp_velocities, "base_link");
         twist_stamped_msg.header.stamp.sec = 1;
         twist_stamped_msg.header.stamp.nanosec = 2;
         assert_eq!(expected_twist_stamped_msg, twist_stamped_msg);
@@ -225,7 +228,7 @@ mod tests {
                 },
             },
         };
-        let mut wrench_stamped_msg = create_wrench_stamped_msg(tcp_force_torque);
+        let mut wrench_stamped_msg = create_wrench_stamped_msg(tcp_force_torque, "base_link");
         wrench_stamped_msg.header.stamp.sec = 1;
         wrench_stamped_msg.header.stamp.nanosec = 2;
         assert_eq!(expected_wrench_stamped_msg, wrench_stamped_msg);

src/ros_publisher.rs

@@ -31,17 +31,19 @@ pub struct JointStatesBuffer {
     current_velocities: Arc<Mutex<Vec<f64>>>,
     current_efforts: Arc<Mutex<Vec<f64>>>,
     publisher: Arc<Mutex<RosPublisher<JointStateMsg>>>,
+    frame_id: String,
 }
 
 impl JointStatesBuffer {
-    pub fn new(ros_node: Arc<Node>) -> Self {
+    pub fn new(ros_node: Arc<Node>, frame_id: String) -> Self {
         Self {
             current_positions: Arc::new(Mutex::new(vec![0.0; 6])),
             current_velocities: Arc::new(Mutex::new(vec![0.0; 6])),
             current_efforts: Arc::new(Mutex::new(vec![0.0; 6])),
             publisher: Arc::new(Mutex::new(
                 RosPublisher::new(&ros_node, "~/joint_states").unwrap(),
             )),
+            frame_id,
         }
     }
 
@@ -60,6 +62,7 @@ impl JointStatesBuffer {
             &self.current_positions,
             &self.current_velocities,
             &self.current_efforts,
+            &self.frame_id,
         );
         self.publish_new_joint_state_message(joint_state_msg);
     }
@@ -71,6 +74,7 @@ impl JointStatesBuffer {
             &self.current_positions,
             &self.current_velocities,
             &self.current_efforts,
+            &self.frame_id,
         );
         self.publish_new_joint_state_message(joint_state_msg);
     }
@@ -82,6 +86,7 @@ impl JointStatesBuffer {
             &self.current_positions,
             &self.current_velocities,
             &self.current_efforts,
+            &self.frame_id,
         );
         self.publish_new_joint_state_message(joint_state_msg);
     }
@@ -108,16 +113,18 @@ pub struct TCPPoseBuffer {
     current_pose: Arc<Mutex<Vec<f64>>>,
     current_quaternions: Arc<Mutex<Vec<f64>>>,
     publisher: Arc<Mutex<RosPublisher<PoseStampedMsg>>>,
+    frame_id: String,
 }
 
 impl TCPPoseBuffer {
-    pub fn new(ros_node: Arc<Node>) -> Self {
+    pub fn new(ros_node: Arc<Node>, frame_id: String) -> Self {
         Self {
             current_pose: Arc::new(Mutex::new(vec![0.0; 6])),
             current_quaternions: Arc::new(Mutex::new(vec![0.0; 4])),
             publisher: Arc::new(Mutex::new(
                 RosPublisher::new(&ros_node, "~/tcp_pose").unwrap(),
             )),
+            frame_id,
         }
     }
 
@@ -132,14 +139,22 @@ impl TCPPoseBuffer {
     pub fn on_pose_change(&mut self, input: Variant) {
         let tcp_pose: Vec<f64> = unpack_data(input);
         *self.current_pose.lock().unwrap() = tcp_pose;
-        let tcp_pose_msg = create_pose_stamped_msg(&self.current_pose, &self.current_quaternions);
+        let tcp_pose_msg = create_pose_stamped_msg(
+            &self.current_pose,
+            &self.current_quaternions,
+            &self.frame_id,
+        );
         self.publish_new_tcp_pose_message(tcp_pose_msg);
     }
 
     pub fn on_quaternion_change(&mut self, input: Variant) {
         let tcp_quaternions: Vec<f64> = unpack_data(input);
         *self.current_quaternions.lock().unwrap() = tcp_quaternions;
-        let tcp_pose_msg = create_pose_stamped_msg(&self.current_pose, &self.current_quaternions);
+        let tcp_pose_msg = create_pose_stamped_msg(
+            &self.current_pose,
+            &self.current_quaternions,
+            &self.frame_id,
+        );
         self.publish_new_tcp_pose_message(tcp_pose_msg);
     }
 }

tests/test_bridge.rs

@@ -1,5 +1,6 @@
 use std::{
     env,
+    ffi::OsString,
     path::PathBuf,
     sync::{atomic::Ordering, Arc},
     time::Duration,
@@ -16,7 +17,7 @@ struct ManagedRosBridge {
 }
 
 impl ManagedRosBridge {
-    fn new() -> subprocess::Result<Self> {
+    fn new(env: Option<Vec<(OsString, OsString)>>) -> 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");
@@ -30,6 +31,7 @@ impl ManagedRosBridge {
                 stdout: Redirection::Pipe,
                 stderr: Redirection::Pipe,
                 detached: false,
+                env,
                 ..Default::default()
             },
         )?;
@@ -68,7 +70,7 @@ 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;
 
-    let mut _bridge_process = ManagedRosBridge::new().expect("Failed to start subprocess");
+    let mut _bridge_process = ManagedRosBridge::new(None).expect("Failed to start subprocess");
 
     // Spawn ROS Subscriber against a sample topic
     let subscription = Arc::new(
@@ -122,7 +124,7 @@ 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 mut bridge_process = ManagedRosBridge::new(None).expect("Failed to start subprocess");
 
     let service_caller = Arc::new(
         helpers::ros_service_caller::ServiceCaller::new(
@@ -147,3 +149,43 @@ async fn e2e_ros_service_to_opc_ua_call() {
         .unwrap()
         .contains("ROS service called"));
 }
+
+#[tokio::test]
+async fn e2e_frame_id_prefix() {
+    // Start the OPC UA server in the background
+    helpers::opc_ua_publisher_single_linear::run_rapid_clock().await;
+
+    let expected_frame_id_prefix = "robot42";
+    let mut current_env: Vec<(OsString, OsString)> = env::vars_os().collect();
+    current_env.push(("ROBOT_INDEX".into(), expected_frame_id_prefix.into()));
+    let mut _bridge_process =
+        ManagedRosBridge::new(Some(current_env)).expect("Failed to start subprocess");
+
+    // Spawn ROS Subscriber against a sample topic
+    let subscription = Arc::new(
+        helpers::ros_subscriber::Subscriber::new(
+            "joint_states_subscriber",
+            "/voraus_bridge_node/joint_states",
+        )
+        .unwrap(),
+    );
+    let clone = Arc::clone(&subscription.node);
+    // TODO: Figure out why this takes almost 10 s [RR-836]
+    rclrs::spin_once(clone, Some(Duration::from_secs(25))).expect("Could not spin");
+
+    let number_of_messages_received = subscription.num_messages.load(Ordering::SeqCst);
+    let received_frame_id = &subscription
+        .data
+        .lock()
+        .unwrap()
+        .as_ref()
+        .unwrap()
+        .header
+        .frame_id
+        .clone();
+    assert_eq!(number_of_messages_received, 1);
+    assert_eq!(
+        *received_frame_id,
+        format!("{}_base_link", expected_frame_id_prefix)
+    );
+}