get command interface values by reference for new 'hardware_interface'

boschresearch · Dec 5, 2024 · e705c3d · e705c3d
1 parent 8e9376a
commit e705c3d
Show file tree

Hide file tree

Showing 5 changed files with 65 additions and 19 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<double*>(&[...])'
 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
@@ -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<double*>(&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<double*>(&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<double*>(&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<double*>(&robot_state_address),
               k_EPS);  // testing that the casted robot state ptr
                        // is correctly pushed to state interface
 }