diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 888b139ca8..5db923fdb4 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -388,7 +388,7 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; - state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; + state_message_.ref_trans_base_ft.child_frame_id = parameters_.ft_sensor.frame.id; state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); Eigen::Quaterniond quat(admittance_state_.rot_base_control); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 80db4be228..8dfeee8889 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -358,6 +358,28 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); } +TEST_F(AdmittanceControllerTest, check_frame_ids_in_controller_state) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); // force torque sensor + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + const auto & msg = controller_->admittance_->get_controller_state(); + + // Ensure correct frame IDs + ASSERT_EQ( + msg.ref_trans_base_ft.header.frame_id, controller_->admittance_->parameters_.kinematics.base); + ASSERT_EQ( + msg.ref_trans_base_ft.child_frame_id, controller_->admittance_->parameters_.ft_sensor.frame.id); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index c2abdecf65..e38bcc82cf 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -67,6 +67,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon FRIEND_TEST(AdmittanceControllerTest, check_interfaces); FRIEND_TEST(AdmittanceControllerTest, activate_success); FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(AdmittanceControllerTest, check_frame_ids_in_controller_state); public: CallbackReturn on_init() override