Fix child_frame_id in controller_state_msg (#1601)

Co-authored-by: Christoph Fröhlich <[email protected]>
(cherry picked from commit 1321ae8)

Author: 2 people

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -102,8 +102,6 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   state_message_.wrench_base.header.frame_id = parameters_.kinematics.base;
   state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base;
   state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base;
-  state_message_.admittance_position.header.frame_id = parameters_.kinematics.base;
-  state_message_.admittance_position.child_frame_id = "admittance_offset";
 
   // reset admittance state
   admittance_state_ = AdmittanceState(num_joints);
@@ -392,10 +390,12 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control
     admittance_state_.admittance_acceleration[5];
 
   state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position);
+  state_message_.admittance_position.header.frame_id = parameters_.kinematics.base;
+  state_message_.admittance_position.child_frame_id = "admittance_offset";
 
-  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 = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft);
+  state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base;
+  state_message_.ref_trans_base_ft.child_frame_id = parameters_.ft_sensor.frame.id;
 
   Eigen::Quaterniond quat(admittance_state_.rot_base_control);
   state_message_.rot_base_control.w = quat.w();

admittance_controller/test/test_admittance_controller.cpp

@@ -358,6 +358,31 @@ 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);
+  ASSERT_EQ(
+    msg.admittance_position.header.frame_id, controller_->admittance_->parameters_.kinematics.base);
+  ASSERT_EQ(msg.admittance_position.child_frame_id, "admittance_offset");
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

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