diff --git a/README.md b/README.md index 61aefa2..9c843a3 100644 --- a/README.md +++ b/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). diff --git a/compose.yaml b/compose.yaml index bf384ad..048b19d 100644 --- a/compose.yaml +++ b/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 diff --git a/src/main.rs b/src/main.rs index cdee6d4..0237470 100644 --- a/src/main.rs +++ b/src/main.rs @@ -33,12 +33,17 @@ 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 opc_ua_client_copy = Arc::clone(&opc_ua_client); let ros_services = Arc::new(ROSServices::new(opc_ua_client_copy)); diff --git a/src/register_subscriptions.rs b/src/register_subscriptions.rs index b297b61..cb2afd2 100644 --- a/src/register_subscriptions.rs +++ b/src/register_subscriptions.rs @@ -9,12 +9,26 @@ use crate::{ ros_publisher::{unpack_data, JointStatesBuffer, RosPublisher, TCPPoseBuffer}, }; -pub fn register_opcua_subscriptions(ros_node: Arc, opc_ua_client: Arc>) { +/// Register pre-defined ROS functionality on OPC UA subscriptions. +/// +/// A frame id prefix can be povided 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, + opc_ua_client: Arc>, + frame_id_prefix: Option, +) { 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); @@ -22,6 +36,7 @@ pub fn register_opcua_subscriptions(ros_node: Arc, opc_ua_client: Arc, opc_ua_client: Arc, opc_ua_client: Arc, opc_ua_client: Arc>>, velocities: &Arc>>, efforts: &Arc>>, + 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,6 +36,7 @@ pub fn create_joint_state_msg( pub fn create_pose_stamped_msg( pose: &Arc>>, quaternion: &Arc>>, + frame_id: &str, ) -> PoseStamped { let pose = pose.lock().unwrap().to_vec(); let quaternion = quaternion.lock().unwrap().to_vec(); @@ -42,7 +44,7 @@ pub fn create_pose_stamped_msg( 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) -> WrenchStamped { +pub fn create_wrench_stamped_msg(tcp_force_torque: Vec, 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) -> 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) -> TwistStamped { +pub fn create_twist_stamped_msg(tcp_velocities: Vec, 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) -> 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); diff --git a/src/ros_publisher.rs b/src/ros_publisher.rs index a61e331..0701706 100644 --- a/src/ros_publisher.rs +++ b/src/ros_publisher.rs @@ -31,10 +31,11 @@ pub struct JointStatesBuffer { current_velocities: Arc>>, current_efforts: Arc>>, publisher: Arc>>, + frame_id: String, } impl JointStatesBuffer { - pub fn new(ros_node: Arc) -> Self { + pub fn new(ros_node: Arc, 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])), @@ -42,6 +43,7 @@ impl JointStatesBuffer { 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>>, current_quaternions: Arc>>, publisher: Arc>>, + frame_id: String, } impl TCPPoseBuffer { - pub fn new(ros_node: Arc) -> Self { + pub fn new(ros_node: Arc, 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 = 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 = 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); } } diff --git a/tests/test_bridge.rs b/tests/test_bridge.rs index 9e892a3..6e31989 100644 --- a/tests/test_bridge.rs +++ b/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 { + fn new(env: Option>) -> subprocess::Result { // 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(("FRAME_ID_PREFIX".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) + ); +}