feat: Add configurable frame id prefix

README.md

@@ -26,6 +26,16 @@ Currently, the following voraus.core functionality is exposed to the ROS 2 ecosy
 - 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`)
 
+## Configuration
+
+The voraus ros bridge can be configured via environment variables.
+If your are using the docker image, those environment variables are set in the compose.yml file.
+The following setting can be made:
+
+- VORAUS_CORE_OPC_UA_ENDPOINT (Mandatory): The OPC UA endpoint to reach the voraus.core motion server, defaults to `opc.tcp://127.0.0.1:48401`
+- ROS_NAMESPACE (Optional): Can be used for wrapping the whole node into a namespace (e.g. `/robot1/voraus_ros_bridge/joint_states`) and allows distinction of topics, services, etc.
+- FRAME_ID_PREFIX (Optional): Prefix for the frame ids to be able to distinguish between coordinate frames of different robots e.g. for visualization of multiple robots in rviz. For example for the `base_link` coordinate system and `FRAME_ID_PREFIX=robot1` the resulting frame id is now `robot1_base_link`
+
 ## Development
 
 This repository provides a dev container to streamline the development process (see https://containers.dev/ for details on dev containers).

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,18 +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_run = Arc::clone(&opc_ua_client);
-    let opc_ua_client_copy_services = Arc::clone(&opc_ua_client);
-    let opc_ua_client_copy_wrench = Arc::clone(&opc_ua_client);
-    let opc_ua_client_copy_stiffness = Arc::clone(&opc_ua_client);
-
-    register_opcua_subscriptions(ros_node_copy_register, opc_ua_client);
+    let opc_ua_client_copy = Arc::clone(&opc_ua_client);
+    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);
@@ -57,11 +59,12 @@ fn main() -> Result<(), RclrsError> {
             move |request_header, request| rsc.disable_impedance_control(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",
         QOS_PROFILE_DEFAULT,
         move |msg: Wrench| {
-            opc_ua_client_copy_wrench.lock().unwrap().call_method(
+            opc_ua_client_copy.lock().unwrap().call_method(
                 NodeId::new(1, 100182),
                 NodeId::new(1, 100267),
                 Some(vec![
@@ -75,12 +78,13 @@ fn main() -> Result<(), RclrsError> {
             );
         },
     )?;
+    let opc_ua_client_copy = Arc::clone(&opc_ua_client);
     let _stiffness_subscriber: Arc<Subscription<voraus_interfaces::msg::CartesianStiffness>> =
         ros_node.create_subscription(
             "~/impedance_control/set_stiffness",
             QOS_PROFILE_DEFAULT,
             move |msg: voraus_interfaces::msg::CartesianStiffness| {
-                opc_ua_client_copy_stiffness.lock().unwrap().call_method(
+                opc_ua_client_copy.lock().unwrap().call_method(
                     NodeId::new(1, 100182),
                     NodeId::new(1, 100265),
                     Some(vec![
@@ -96,7 +100,8 @@ fn main() -> Result<(), RclrsError> {
         )?;
 
     info!("Starting OPC UA client");
-    let _session = opc_ua_client_copy_run.lock().unwrap().run_async();
+    let opc_ua_client_copy = Arc::clone(&opc_ua_client);
+    let _session = opc_ua_client_copy.lock().unwrap().run_async();
     info!("Spinning ROS");
     rclrs::spin(ros_node_copy_spin)
 }

src/register_subscriptions.rs

@@ -9,19 +9,34 @@ use crate::{
     ros_publisher::{unpack_data, JointStatesBuffer, RosPublisher, TCPPoseBuffer},
 };
 
-pub fn register_opcua_subscriptions(ros_node: Arc<Node>, opc_ua_client: Arc<Mutex<OPCUAClient>>) {
+/// Register pre-defined ROS functionality on OPC UA subscriptions.
+///
+/// A frame id prefix can be provided which is used for all frame ids of the ROS messages
+/// to be created, e.g. "prefix_base_link".
+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 +123,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 +134,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 +155,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);
     }
 }