From 47b18bc64cfc5e660a614dd10e5425ba2282259d Mon Sep 17 00:00:00 2001 From: rehanshah17 Date: Wed, 19 Mar 2025 22:48:20 -0400 Subject: [PATCH 1/6] fixed the copy and paste #504 issue, added the test check_frame_ids_in_controller_state --- .../admittance_rule_impl.hpp | 2 +- .../test/test_admittance_controller.cpp | 20 +++++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 859c7e3336..2c20c5e844 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 = 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..1493e176e8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/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); + ASSERT_EQ(msg.ref_trans_base_ft.child_frame, controller_->admittance_->parameters_.ft_sensor.frame.id); +} + TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) { SetUpController(); From 46d61e7a471b84e3c4a96a9ce31635e448644d2b Mon Sep 17 00:00:00 2001 From: rehanshah17 Date: Thu, 20 Mar 2025 10:44:57 -0400 Subject: [PATCH 2/6] fixed spacing --- admittance_controller/test/test_admittance_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 1493e176e8..c110e55004 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -320,17 +320,17 @@ TEST_F(AdmittanceControllerTest, check_frame_ids_in_controller_state) 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 + 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(); + 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, controller_->admittance_->parameters_.ft_sensor.frame.id); + ASSERT_EQ( + msg.ref_trans_base_ft.header.frame_id, controller_->admittance_->parameters_.kinematics.base); } TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) From 744fcd5216e4b7e029aeffeb83332d822ecc5692 Mon Sep 17 00:00:00 2001 From: rehanshah17 Date: Sun, 23 Mar 2025 13:09:41 -0400 Subject: [PATCH 3/6] fixing spaces --- .../test/test_admittance_controller.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index c110e55004..21ad4515c0 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -370,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); From 14a9f381ee7cc79823d7a4d7e0ae119c784cc4da Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 23 Mar 2025 18:40:04 +0100 Subject: [PATCH 4/6] Update admittance_controller/include/admittance_controller/admittance_rule_impl.hpp --- .../include/admittance_controller/admittance_rule_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 2c20c5e844..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.child_frame = parameters_.ft_sensor.frame.id; + 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); From 328f0e740e23b2614b6c26e36db18ad9a99388ce Mon Sep 17 00:00:00 2001 From: rehanshah17 Date: Sun, 23 Mar 2025 16:04:52 -0400 Subject: [PATCH 5/6] made friend test --- admittance_controller/test/test_admittance_controller.hpp | 1 + 1 file changed, 1 insertion(+) 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 From ab3f199c65f4f96f609c796bbde3f44295873c60 Mon Sep 17 00:00:00 2001 From: rehanshah17 Date: Sun, 23 Mar 2025 16:21:13 -0400 Subject: [PATCH 6/6] removed buggy copy and paste; fixes the redefinition --- .../test/test_admittance_controller.cpp | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 21ad4515c0..30e4e878f6 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -313,26 +313,6 @@ 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();