diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 859c7e3336..4cb711b8cc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -394,7 +394,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 f26cd4df22..30e4e878f6 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -350,6 +350,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 78ddf8d46d..7e6170229f 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