diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 661a966809..1028a5a2c5 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -14,8 +14,8 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.0.13 - - uses: ros-tooling/action-ros-ci@0.0.13 + - uses: ros-tooling/setup-ros@0.0.23 + - uses: ros-tooling/action-ros-ci@0.0.17 with: package-name: controller_interface controller_manager controller_parameter_server hardware_interface test_robot_hardware colcon-mixin-name: coverage-gcc diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 09e9246676..d6bb1546f1 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -52,9 +52,14 @@ 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( diff --git a/hardware_interface/include/hardware_interface/joint_command_handle.hpp b/hardware_interface/include/hardware_interface/joint_command_handle.hpp index 57065d2616..2c04d36d82 100644 --- a/hardware_interface/include/hardware_interface/joint_command_handle.hpp +++ b/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_; diff --git a/hardware_interface/include/hardware_interface/joint_state_handle.hpp b/hardware_interface/include/hardware_interface/joint_state_handle.hpp index 78c55993fa..ad4a775462 100644 --- a/hardware_interface/include/hardware_interface/joint_state_handle.hpp +++ b/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_; diff --git a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp index a02a2b698b..bd9f2053c5 100644 --- a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp +++ b/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_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c1e218fb37..7040620623 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -5,6 +5,7 @@ ROS2 ros_control hardware interface Karsten Knese + Bence Magyar Apache License 2.0 ament_cmake @@ -12,7 +13,7 @@ rclcpp rcpputils - ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common diff --git a/hardware_interface/src/joint_command_handle.cpp b/hardware_interface/src/joint_command_handle.cpp index 6e68bb0d82..992e8e0cb9 100644 --- a/hardware_interface/src/joint_command_handle.cpp +++ b/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 diff --git a/hardware_interface/src/joint_state_handle.cpp b/hardware_interface/src/joint_state_handle.cpp index 486fb9187f..1df95072ad 100644 --- a/hardware_interface/src/joint_state_handle.cpp +++ b/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 diff --git a/hardware_interface/src/operation_mode_handle.cpp b/hardware_interface/src/operation_mode_handle.cpp index e77a0f7bf4..25ee0b4290 100644 --- a/hardware_interface/src/operation_mode_handle.cpp +++ b/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 diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index 224c3c164c..adeeb4e427 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/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 hardware_interface_ret_t register_handle(std::vector & 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 { diff --git a/hardware_interface/test/test_macros.cpp b/hardware_interface/test/test_macros.cpp index d03ffb4c63..b18ad8ab0e 100644 --- a/hardware_interface/test/test_macros.cpp +++ b/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 { diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp new file mode 100644 index 0000000000..8d019211f4 --- /dev/null +++ b/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 +#include + +#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)); +}