From e705c3dd2f4b877f98c65964711e62e84b631f4a Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 5 Dec 2024 09:19:58 +0100 Subject: [PATCH] get command interface values by reference for new 'hardware_interface' --- franka_example_controllers/CMakeLists.txt | 6 +++++ .../test_gravity_compensation_example.cpp | 24 ++++++++++++----- .../test_move_to_start_example_controller.cpp | 24 ++++++++++++----- franka_hardware/test/CMakeLists.txt | 4 +++ .../test/franka_hardware_interface_test.cpp | 26 +++++++++++++++---- 5 files changed, 65 insertions(+), 19 deletions(-) diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index ba0f86c8..e76767c5 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -109,6 +109,9 @@ if(BUILD_TESTING) ${PROJECT_NAME}_gravity_test PUBLIC controller_interface_VERSION_MAJOR=${controller_interface_VERSION_MAJOR} ) + if(${hardware_interface_VERSION} VERSION_GREATER_EQUAL "4.20") + target_compile_definitions(${PROJECT_NAME}_gravity_test PRIVATE HW_HAS_GET_BY_REF) + endif() ament_add_gmock(${PROJECT_NAME}_move_to_start_test test/test_move_to_start_example_controller.cpp) target_include_directories(${PROJECT_NAME}_move_to_start_test PRIVATE include) @@ -118,6 +121,9 @@ if(BUILD_TESTING) ${PROJECT_NAME}_move_to_start_test PUBLIC controller_interface_VERSION_MAJOR=${controller_interface_VERSION_MAJOR} ) + if(${hardware_interface_VERSION} VERSION_GREATER_EQUAL "4.20") + target_compile_definitions(${PROJECT_NAME}_move_to_start_test PRIVATE HW_HAS_GET_BY_REF) + endif() if(CHECK_TIDY) find_package(ament_cmake_clang_tidy REQUIRED) diff --git a/franka_example_controllers/test/test_gravity_compensation_example.cpp b/franka_example_controllers/test/test_gravity_compensation_example.cpp index cf21ba3e..9a8e1e25 100644 --- a/franka_example_controllers/test/test_gravity_compensation_example.cpp +++ b/franka_example_controllers/test/test_gravity_compensation_example.cpp @@ -29,6 +29,16 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; using hardware_interface::LoanedCommandInterface; +void assert_val_eq(const CommandInterface& cmdintf) { +#ifdef HW_HAS_GET_BY_REF + double val = 0; + EXPECT_TRUE(cmdintf.get_value(val)); +#else + const double val = cmdintf.get_value(); +#endif + ASSERT_EQ(val, 0.0); +} + class TestGravityCompensationExample : public ::testing::Test { public: static void SetUpTestSuite(); @@ -137,11 +147,11 @@ TEST_F(TestGravityCompensationExample, given_joints_and_interface_when_update_ex controller_interface::return_type::OK); // check joint commands are updated to zero torque value - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_4_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_5_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_6_pos_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_7_pos_cmd_.get_value(), 0.0); + assert_val_eq(joint_1_pos_cmd_); + assert_val_eq(joint_2_pos_cmd_); + assert_val_eq(joint_3_pos_cmd_); + assert_val_eq(joint_4_pos_cmd_); + assert_val_eq(joint_5_pos_cmd_); + assert_val_eq(joint_6_pos_cmd_); + assert_val_eq(joint_7_pos_cmd_); } diff --git a/franka_example_controllers/test/test_move_to_start_example_controller.cpp b/franka_example_controllers/test/test_move_to_start_example_controller.cpp index 3e84eb94..5b0806b5 100644 --- a/franka_example_controllers/test/test_move_to_start_example_controller.cpp +++ b/franka_example_controllers/test/test_move_to_start_example_controller.cpp @@ -26,6 +26,16 @@ const double k_EPS = 1e-5; +void exp_val_near(const CommandInterface& cmdintf) { +#ifdef HW_HAS_GET_BY_REF + double val = 0; + EXPECT_TRUE(cmdintf.get_value(val)); +#else + const double val = cmdintf.get_value(); +#endif + EXPECT_NEAR(val, 0.0, k_EPS); +} + void MoveToStartExampleControllerTest::SetUpTestSuite() { rclcpp::init(0, nullptr); } @@ -136,11 +146,11 @@ TEST_F(MoveToStartExampleControllerTest, correct_setup_on_update_expect_ok) { ASSERT_EQ(controller_->update(time, duration), controller_interface::return_type::OK); - EXPECT_NEAR(joint_1_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_2_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_3_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_4_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_5_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_6_pos_cmd_.get_value(), 0.0, k_EPS); - EXPECT_NEAR(joint_7_pos_cmd_.get_value(), 0.0, k_EPS); + exp_val_near(joint_1_pos_cmd_); + exp_val_near(joint_2_pos_cmd_); + exp_val_near(joint_3_pos_cmd_); + exp_val_near(joint_4_pos_cmd_); + exp_val_near(joint_5_pos_cmd_); + exp_val_near(joint_6_pos_cmd_); + exp_val_near(joint_7_pos_cmd_); } diff --git a/franka_hardware/test/CMakeLists.txt b/franka_hardware/test/CMakeLists.txt index 2cf1ac40..994b0ea6 100644 --- a/franka_hardware/test/CMakeLists.txt +++ b/franka_hardware/test/CMakeLists.txt @@ -4,5 +4,9 @@ ament_add_gmock(${PROJECT_NAME}_test franka_hardware_interface_test.cpp) target_include_directories(${PROJECT_NAME}_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) +if(${hardware_interface_VERSION} VERSION_GREATER_EQUAL "4.20") + target_compile_definitions(${PROJECT_NAME}_test PRIVATE HW_HAS_GET_BY_REF) +endif() + # ignore type punning via 'reinterpret_cast(&[...])' target_compile_options(${PROJECT_NAME}_test PRIVATE -Wno-error=strict-aliasing) diff --git a/franka_hardware/test/franka_hardware_interface_test.cpp b/franka_hardware/test/franka_hardware_interface_test.cpp index a94a51d3..3f2e311c 100644 --- a/franka_hardware/test/franka_hardware_interface_test.cpp +++ b/franka_hardware/test/franka_hardware_interface_test.cpp @@ -137,7 +137,13 @@ TEST( } else { EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller); } - EXPECT_EQ(states[i].get_value(), 0.0); +#ifdef HW_HAS_GET_BY_REF + double val = 0; + EXPECT_TRUE(states[i].get_value(val)); +#else + const double val = states[i].get_value(); +#endif + EXPECT_EQ(val, 0.0); } EXPECT_EQ(states.size(), state_interface_size); @@ -167,8 +173,13 @@ TEST( auto states = franka_hardware_interface.export_state_interfaces(); EXPECT_EQ(states[state_interface_size - 1].get_name(), "panda/robot_model"); // Last state interface is the robot model state - EXPECT_NEAR(states[state_interface_size - 1].get_value(), - *reinterpret_cast(&model_address), +#ifdef HW_HAS_GET_BY_REF + double val = 0; + EXPECT_TRUE(states[state_interface_size - 1].get_value(val)); +#else + const double val = states[state_interface_size - 1].get_value(); +#endif + EXPECT_NEAR(val, *reinterpret_cast(&model_address), k_EPS); // testing that the casted mock_model ptr // is correctly pushed to state interface } @@ -199,8 +210,13 @@ TEST( auto states = franka_hardware_interface.export_state_interfaces(); EXPECT_EQ(states[state_interface_size - 2].get_name(), "panda/robot_state"); // Last state interface is the robot model state - EXPECT_NEAR(states[state_interface_size - 2].get_value(), - *reinterpret_cast(&robot_state_address), +#ifdef HW_HAS_GET_BY_REF + double val = 0; + EXPECT_TRUE(states[state_interface_size - 2].get_value(val)); +#else + const double val = states[state_interface_size - 2].get_value(); +#endif + EXPECT_NEAR(val, *reinterpret_cast(&robot_state_address), k_EPS); // testing that the casted robot state ptr // is correctly pushed to state interface }