Skip to content

Fix child_frame_id in controller_state_msg #1601

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Fix child_frame_id in controller_state_msg #1601

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
47b18bc
fixed the copy and paste #504 issue, added the test check_frame_ids_i…
rehanshah17 Mar 20, 2025
46d61e7
fixed spacing
rehanshah17 Mar 20, 2025
42caee4
Merge branch 'master' into AdmittanceFix
christophfroehlich Mar 20, 2025
744fcd5
fixing spaces
rehanshah17 Mar 23, 2025
14a9f38
Update admittance_controller/include/admittance_controller/admittance…
saikishor Mar 23, 2025
328f0e7
made friend test
rehanshah17 Mar 23, 2025
ab3f199
removed buggy copy and paste; fixes the redefinition
rehanshah17 Mar 23, 2025
d38c51d
Merge branch 'master' into AdmittanceFix
rehanshah17 Mar 26, 2025
74113e7
Merge branch 'master' into AdmittanceFix
bmagyar Mar 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
42 changes: 42 additions & 0 deletions admittance_controller/test/test_admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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);
Expand Down
Loading