Fix child_frame_id in controller_state_msg

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 = 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);

admittance_controller/test/test_admittance_controller.cpp

@@ -313,6 +313,26 @@ TEST_F(AdmittanceControllerTest, publish_status_success)
   //   }
 }
 
+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);
+}
+
 TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status)
 {
   SetUpController();
@@ -350,6 +370,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);