diff --git a/examples/opc_ua/rapid-clock.rs b/examples/opc_ua/rapid-clock.rs index 90a2cc4..d54c0c8 100644 --- a/examples/opc_ua/rapid-clock.rs +++ b/examples/opc_ua/rapid-clock.rs @@ -24,7 +24,7 @@ fn rapid_clock() { fn add_timed_variable(server: &mut Server, namespace: u16) { // These will be the node ids of the new variables - let ticks_since_launch_node_id = NodeId::new(namespace, "ticks_since_launch"); + let ticks_since_launch_node_id = NodeId::new(namespace, "100111"); let address_space = server.address_space(); @@ -39,9 +39,9 @@ fn add_timed_variable(server: &mut Server, namespace: u16) { let _ = address_space.add_variables( vec![Variable::new( &ticks_since_launch_node_id, - "ticks_since_launch", - "ticks_since_launch", - 0i32, + "joint_positions", + "joint_positions", + vec![0.0f64; 6], )], &rapid_folder_id, ); @@ -54,7 +54,7 @@ fn add_timed_variable(server: &mut Server, namespace: u16) { let ticks_in_100_ns = now.ticks(); let _ = address_space.set_variable_value( ticks_since_launch_node_id.clone(), - ticks_in_100_ns as i32, + vec![ticks_in_100_ns as f64; 6], &now, &now, ); diff --git a/src/main.rs b/src/main.rs index 442408b..7d81c8c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -31,9 +31,18 @@ fn main() -> Result<(), RclrsError> { let provider = Arc::clone(&joint_state_publisher); move |x: Variant| { println!("Value = {:?}", &x); - let data_value: f64 = x - .try_into() - .expect("This should have been encapsulated f64 but wasn't."); + let mut data_value: Vec = vec![]; + match x { + Variant::Array(unwrapped) => { + unwrapped.values.into_iter().for_each(|value| { + let unpacked_value = value + .as_f64() + .expect("This should have been encapsulated f64 but wasn't."); + data_value.push(unpacked_value) + }); + } + _ => panic!("Expected an array"), + }; let j_msg = create_joint_state_msg(data_value); provider .publish_data(&j_msg) diff --git a/src/ros_publisher.rs b/src/ros_publisher.rs index 746f0de..dbfe846 100644 --- a/src/ros_publisher.rs +++ b/src/ros_publisher.rs @@ -25,7 +25,7 @@ impl RosPublisher { } } -pub fn create_joint_state_msg(data: f64) -> JointStateMsg { +pub fn create_joint_state_msg(data: Vec) -> JointStateMsg { let system_timestamp = SystemTime::now().duration_since(UNIX_EPOCH).unwrap(); // Workaround for https://github.com/ros2-rust/ros2_rust/issues/385 let time_msgs = TimeMsg { @@ -39,9 +39,9 @@ pub fn create_joint_state_msg(data: f64) -> JointStateMsg { frame_id: "0".to_string(), }, name: vec!["Test Joint States".to_string()], - position: vec![data, 3.0], - velocity: vec![2.0, 3.3], - effort: vec![], + position: data, + velocity: vec![0.0; 6], + effort: vec![0.0; 6], }; joint_state_msg } diff --git a/tests/helpers/opc_ua_publisher_single_linear.rs b/tests/helpers/opc_ua_publisher_single_linear.rs index 15e0762..0d6fa80 100644 --- a/tests/helpers/opc_ua_publisher_single_linear.rs +++ b/tests/helpers/opc_ua_publisher_single_linear.rs @@ -38,9 +38,9 @@ fn add_timed_variable(server: &mut Server, namespace: u16) { let _ = address_space.add_variables( vec![Variable::new( &ticks_since_launch_node_id, - "ticks_since_launch", - "ticks_since_launch", - 0i32, + "joint_positions", + "joint_positions", + vec![0.0f64; 6], )], &rapid_folder_id, ); @@ -53,7 +53,7 @@ fn add_timed_variable(server: &mut Server, namespace: u16) { let ticks_in_100_ns = now.ticks(); let _ = address_space.set_variable_value( ticks_since_launch_node_id.clone(), - ticks_in_100_ns as i32, + vec![ticks_in_100_ns as f64; 6], &now, &now, ); diff --git a/tests/test_simple_opc_ua_subscriber.rs b/tests/test_simple_opc_ua_subscriber.rs index 24e278d..c316489 100644 --- a/tests/test_simple_opc_ua_subscriber.rs +++ b/tests/test_simple_opc_ua_subscriber.rs @@ -40,7 +40,7 @@ async fn test_simple_subscriber_receives_data_changes() { .expect("Failed to read line from client") { println!("Subscriber stdout: {}", line); - if line.contains("Value = Int32(") { + if line.contains("Value = Array(Array { value_type: Double") { found_ticks_since_launch_changed_times += 1; if found_ticks_since_launch_changed_times == expected_changed_times { return found_ticks_since_launch_changed_times;