Add unit testing to robot hardware

hardware_interface/CMakeLists.txt

@@ -52,9 +52,15 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(test_macros test/test_macros.cpp)
+  find_package(ament_cmake_gmock REQUIRED)
+  ament_add_gmock(test_macros test/test_macros.cpp)
   target_include_directories(test_macros PRIVATE include)
   ament_target_dependencies(test_macros rcpputils)
+
+  ament_add_gmock(test_robot_hardware_interfaces test/test_robot_hardware_interface.cpp)
+  target_include_directories(test_robot_hardware_interfaces PRIVATE include)
+  target_link_libraries(test_robot_hardware_interfaces hardware_interface)
+
 endif()
 
 ament_export_include_directories(

hardware_interface/include/hardware_interface/joint_command_handle.hpp

@@ -55,6 +55,9 @@ class JointCommandHandle
   void
   set_cmd(double cmd);
 
+  HARDWARE_INTERFACE_PUBLIC
+  bool valid_pointers() const;
+
 private:
   std::string name_;
   double * cmd_;

hardware_interface/include/hardware_interface/joint_state_handle.hpp

@@ -62,6 +62,9 @@ class JointStateHandle
   double
   get_effort() const;
 
+  HARDWARE_INTERFACE_PUBLIC
+  bool valid_pointers() const;
+
 private:
   std::string name_;
   const double * pos_;

hardware_interface/include/hardware_interface/operation_mode_handle.hpp

@@ -58,6 +58,9 @@ class OperationModeHandle
   void
   set_mode(OperationMode mode);
 
+  HARDWARE_INTERFACE_PUBLIC
+  bool valid_pointers() const;
+
 private:
   std::string name_;
   OperationMode * mode_;

hardware_interface/package.xml

@@ -5,14 +5,15 @@
   <description>ROS2 ros_control hardware interface</description>
 
   <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>rcpputils</depend>
 
-  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

hardware_interface/src/joint_command_handle.cpp

@@ -44,6 +44,8 @@ JointCommandHandle::get_name() const
 double
 JointCommandHandle::get_cmd() const
 {
+  THROW_ON_NULLPTR(cmd_)
+
   return *cmd_;
 }
 
@@ -55,4 +57,9 @@ JointCommandHandle::set_cmd(double cmd)
   * cmd_ = cmd;
 }
 
+bool JointCommandHandle::valid_pointers() const
+{
+  return cmd_;
+}
+
 }  // namespace hardware_interface

hardware_interface/src/joint_state_handle.cpp

@@ -70,4 +70,10 @@ JointStateHandle::get_effort() const
 
   return *eff_;
 }
+
+bool JointStateHandle::valid_pointers() const
+{
+  return pos_ && vel_ && eff_;
+}
+
 }  // namespace hardware_interface

hardware_interface/src/operation_mode_handle.cpp

@@ -50,4 +50,9 @@ OperationModeHandle::set_mode(OperationMode mode)
   * mode_ = mode;
 }
 
+bool OperationModeHandle::valid_pointers() const
+{
+  return mode_;
+}
+
 }  // namespace hardware_interface

hardware_interface/src/robot_hardware.cpp

@@ -22,13 +22,15 @@
 #include "hardware_interface/operation_mode_handle.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-namespace hardware_interface
+namespace
 {
+constexpr auto kJointStateLoggerName = "joint state handle";
+constexpr auto kJointCommandLoggerName = "joint cmd handle";
+constexpr auto kOperationModeLoggerName = "joint operation mode handle";
+}
 
-constexpr char kJointStateLoggerName[] = "joint state handle";
-constexpr char kJointCommandLoggerName[] = "joint cmd handle";
-constexpr char kOperationModeLoggerName[] = "joint operation mode handle";
-
+namespace hardware_interface
+{
 /// Register the handle to the handle list if the handle's name is not found.
 /**
  * \param[in] registered_handles The handle list.
@@ -40,6 +42,16 @@ template<typename T>
 hardware_interface_ret_t
 register_handle(std::vector<T *> & registered_handles, T * handle, const std::string & logger_name)
 {
+  if (handle->get_name().empty()) {
+    RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! No name is specified");
+    return HW_RET_ERROR;
+  }
+
+  if (!handle->valid_pointers()) {
+    RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! Points to nullptr!");
+    return HW_RET_ERROR;
+  }
+
   auto handle_pos = std::find_if(
     registered_handles.begin(), registered_handles.end(),
     [&](auto handle_ptr) -> bool {

hardware_interface/test/test_macros.cpp

@@ -16,7 +16,7 @@
 
 #include "hardware_interface/macros.hpp"
 
-#include "gtest/gtest.h"
+#include "gmock/gmock.h"
 
 class TestMacros : public ::testing::Test
 {

hardware_interface/test/test_robot_hardware_interface.cpp

@@ -0,0 +1,202 @@
+// Copyright 2020 ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <vector>
+
+#include "gmock/gmock.h"
+
+#include "hardware_interface/robot_hardware.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+
+namespace hw = hardware_interface;
+using testing::SizeIs;
+using testing::Each;
+using testing::NotNull;
+using testing::UnorderedElementsAre;
+using testing::ElementsAre;
+
+namespace
+{
+class MyTestRobotHardware : public hw::RobotHardware
+{
+public:
+  hw::hardware_interface_ret_t init() override
+  {
+    return hw::HW_RET_OK;
+  }
+
+  hw::hardware_interface_ret_t read() override
+  {
+    return hw::HW_RET_OK;
+  }
+
+  hw::hardware_interface_ret_t write() override
+  {
+    return hw::HW_RET_OK;
+  }
+};
+
+constexpr auto JOINT_NAME = "joint_1";
+constexpr auto NEW_JOINT_NAME = "joint_2";
+}  // namespace
+
+class TestRobotHardwareInterface : public testing::Test
+{
+protected:
+  void SetUp()
+  {
+    robot_.init();
+  }
+
+  void SetUpHandles()
+  {
+    robot_.register_joint_command_handle(&pos_command_handle_);
+    robot_.register_joint_state_handle(&state_handle_);
+    robot_.register_operation_mode_handle(&op_mode_handle_);
+  }
+
+  void SetUpNewHandles()
+  {
+    robot_.register_joint_command_handle(&new_pos_command_handle_);
+    robot_.register_joint_state_handle(&new_state_handle_);
+    robot_.register_operation_mode_handle(&new_op_mode_handle_);
+  }
+
+  MyTestRobotHardware robot_;
+  double pos_command_value_ = 0.0;
+  hw::JointCommandHandle pos_command_handle_{JOINT_NAME, &pos_command_value_};
+  double velocity_state_value_ = 0.0;
+  double effort_state_value_ = 0.0;
+  hw::JointStateHandle state_handle_{JOINT_NAME, &pos_command_value_, &velocity_state_value_,
+    &effort_state_value_};
+  hw::OperationMode mode_ = hw::OperationMode::ACTIVE;
+  hw::OperationModeHandle op_mode_handle_{JOINT_NAME, &mode_};
+
+  hw::JointCommandHandle new_pos_command_handle_{NEW_JOINT_NAME, &pos_command_value_};
+  hw::JointStateHandle new_state_handle_{NEW_JOINT_NAME, &pos_command_value_,
+    &velocity_state_value_,
+    &effort_state_value_};
+  hw::OperationModeHandle new_op_mode_handle_{NEW_JOINT_NAME, &mode_};
+};
+
+TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles)
+{
+  hw::JointCommandHandle broken_command_handle;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_joint_command_handle(&broken_command_handle));
+  hw::JointStateHandle broken_state_handle;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_joint_state_handle(&broken_state_handle));
+  hw::OperationModeHandle broken_op_mode_handle;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_operation_mode_handle(&broken_op_mode_handle));
+}
+
+TEST_F(TestRobotHardwareInterface, can_register_proper_handles)
+{
+  EXPECT_EQ(hw::HW_RET_OK, robot_.register_joint_command_handle(&pos_command_handle_));
+  EXPECT_EQ(hw::HW_RET_OK, robot_.register_joint_state_handle(&state_handle_));
+  EXPECT_EQ(hw::HW_RET_OK, robot_.register_operation_mode_handle(&op_mode_handle_));
+}
+
+TEST_F(TestRobotHardwareInterface, cannot_double_register_handles)
+{
+  SetUpHandles();
+
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_joint_command_handle(&pos_command_handle_));
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_joint_state_handle(&state_handle_));
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.register_operation_mode_handle(&op_mode_handle_));
+}
+
+TEST_F(TestRobotHardwareInterface, can_get_registered_joint_names)
+{
+  SetUpHandles();
+  EXPECT_THAT(robot_.get_registered_joint_names(), ElementsAre(JOINT_NAME));
+
+  SetUpNewHandles();
+  EXPECT_THAT(
+    robot_.get_registered_joint_names(),
+    UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME));
+}
+
+TEST_F(TestRobotHardwareInterface, returned_joint_handles_are_not_null)
+{
+  SetUpHandles();
+  SetUpNewHandles();
+
+  const auto state_handles = robot_.get_registered_joint_state_handles();
+  EXPECT_THAT(state_handles, SizeIs(2));
+  EXPECT_THAT(state_handles, Each(NotNull()));
+
+  const auto command_handles = robot_.get_registered_joint_command_handles();
+  EXPECT_THAT(command_handles, SizeIs(2));
+  EXPECT_THAT(command_handles, Each(NotNull()));
+
+  const auto op_mode_handles = robot_.get_registered_operation_mode_handles();
+  EXPECT_THAT(op_mode_handles, SizeIs(2));
+  EXPECT_THAT(op_mode_handles, Each(NotNull()));
+}
+
+TEST_F(TestRobotHardwareInterface, can_get_registered_state_handles)
+{
+  SetUpHandles();
+  EXPECT_THAT(robot_.get_registered_joint_state_handles(), SizeIs(1));
+
+  SetUpNewHandles();
+  EXPECT_THAT(robot_.get_registered_joint_state_handles(), SizeIs(2));
+}
+
+TEST_F(TestRobotHardwareInterface, can_get_registered_command_handles)
+{
+  SetUpHandles();
+  EXPECT_THAT(robot_.get_registered_joint_command_handles(), SizeIs(1));
+
+  SetUpNewHandles();
+  EXPECT_THAT(robot_.get_registered_joint_command_handles(), SizeIs(2));
+}
+
+TEST_F(TestRobotHardwareInterface, can_get_registered_op_mode_handles)
+{
+  SetUpHandles();
+  EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(1));
+
+  SetUpNewHandles();
+  EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(2));
+}
+
+TEST_F(TestRobotHardwareInterface, can_get_handles_by_name)
+{
+  SetUpHandles();
+
+  const hw::JointStateHandle * state_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_joint_state_handle(JOINT_NAME, &state_handle));
+  state_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.get_joint_state_handle(NEW_JOINT_NAME, &state_handle));
+
+  hw::JointCommandHandle * cmd_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_joint_command_handle(JOINT_NAME, &cmd_handle));
+  cmd_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle));
+
+  hw::OperationModeHandle * op_mode_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_operation_mode_handle(JOINT_NAME, &op_mode_handle));
+  op_mode_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_ERROR, robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle));
+
+  SetUpNewHandles();
+  state_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_joint_state_handle(NEW_JOINT_NAME, &state_handle));
+  cmd_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle));
+  op_mode_handle = nullptr;
+  EXPECT_EQ(hw::HW_RET_OK, robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle));
+}