From 4f0e84436a31d189e642c15a6a3e7a14ca3e8863 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 08:16:12 +0100 Subject: [PATCH 01/15] Use gmock --- hardware_interface/CMakeLists.txt | 2 +- hardware_interface/package.xml | 2 +- hardware_interface/test/test_macros.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 09e9246676..f7fc18bcbb 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -52,7 +52,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_macros test/test_macros.cpp) + ament_add_gmock(test_macros test/test_macros.cpp) target_include_directories(test_macros PRIVATE include) ament_target_dependencies(test_macros rcpputils) endif() diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c1e218fb37..7bca0d1f37 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -12,7 +12,7 @@ rclcpp rcpputils - ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common 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 { From bfb3b20fc025702d1527dafbf521a21079c9d5fc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 08:45:33 +0100 Subject: [PATCH 02/15] Add Bence as maintainer --- hardware_interface/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 7bca0d1f37..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 From 14771a0eb568f7cd04363c9e91d41dc0d8b209a3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 08:47:12 +0100 Subject: [PATCH 03/15] Test invalid handles --- hardware_interface/CMakeLists.txt | 10 ++- .../test/test_robot_hardware_interface.cpp | 76 +++++++++++++++++++ 2 files changed, 84 insertions(+), 2 deletions(-) create mode 100644 hardware_interface/test/test_robot_hardware_interface.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f7fc18bcbb..5f06a9cdc3 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -49,12 +49,18 @@ install( LIBRARY DESTINATION lib) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + # find_package(ament_lint_auto REQUIRED) + # ament_lint_auto_find_test_dependencies() + 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/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp new file mode 100644 index 0000000000..2b433eae5c --- /dev/null +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -0,0 +1,76 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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" + +using namespace hardware_interface; + +namespace +{ +class MyTestRobotHardware : public RobotHardware +{ + public: + hardware_interface_ret_t init() override + { + return HW_RET_OK; + } + + hardware_interface_ret_t read() override + { + return HW_RET_OK; + } + + hardware_interface_ret_t write() override + { + return HW_RET_OK; + } +}; +} + +namespace testing +{ +class TestRobotHardwareInterface : public Test +{ + protected: + void SetUp() + { + robot.init(); + } + + MyTestRobotHardware robot; +}; + +TEST_F(TestRobotHardwareInterface, initialize) +{ + MyTestRobotHardware local_robot; + EXPECT_EQ(HW_RET_OK, local_robot.init()); +} + +TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles) +{ + JointCommandHandle command_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&command_handle)); + JointStateHandle state_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&state_handle)); + OperationModeHandle op_mode_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&op_mode_handle)); +} + +} // namespace testing \ No newline at end of file From c96b2578d26426fd1d94e4a11c944bac83ac281f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 08:47:32 +0100 Subject: [PATCH 04/15] Add handle validation to RobotHardware --- .../joint_command_handle.hpp | 62 +++-- .../hardware_interface/joint_state_handle.hpp | 67 +++--- .../operation_mode_handle.hpp | 53 +++-- .../src/joint_command_handle.cpp | 39 ++-- hardware_interface/src/joint_state_handle.cpp | 56 ++--- .../src/operation_mode_handle.cpp | 33 ++- hardware_interface/src/robot_hardware.cpp | 218 ++++++++---------- .../test/test_robot_hardware_interface.cpp | 12 +- 8 files changed, 249 insertions(+), 291 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint_command_handle.hpp b/hardware_interface/include/hardware_interface/joint_command_handle.hpp index 57065d2616..769b105c58 100644 --- a/hardware_interface/include/hardware_interface/joint_command_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_command_handle.hpp @@ -26,38 +26,36 @@ namespace hardware_interface /** A handle used to get and set the command of a single joint. */ class JointCommandHandle { -public: - HARDWARE_INTERFACE_PUBLIC - JointCommandHandle(); - - /** - * The commmand handles are passive in terms of ownership. - * That means that the handles are only allowed to read/write - * the storage, however are not allowed to delete or reallocate - * this memory. - * \param name The name of the joint - * \param cmd A pointer to the storage for this joint's command - */ - HARDWARE_INTERFACE_PUBLIC - JointCommandHandle( - const std::string & name, - double * cmd); - - HARDWARE_INTERFACE_PUBLIC - const std::string & - get_name() const; - - HARDWARE_INTERFACE_PUBLIC - double - get_cmd() const; - - HARDWARE_INTERFACE_PUBLIC - void - set_cmd(double cmd); - -private: - std::string name_; - double * cmd_; + public: + HARDWARE_INTERFACE_PUBLIC + JointCommandHandle(); + + /** + * The commmand handles are passive in terms of ownership. + * That means that the handles are only allowed to read/write + * the storage, however are not allowed to delete or reallocate + * this memory. + * \param name The name of the joint + * \param cmd A pointer to the storage for this joint's command + */ + HARDWARE_INTERFACE_PUBLIC + JointCommandHandle(const std::string& name, double* cmd); + + HARDWARE_INTERFACE_PUBLIC + const std::string& get_name() const; + + HARDWARE_INTERFACE_PUBLIC + double get_cmd() const; + + HARDWARE_INTERFACE_PUBLIC + void set_cmd(double cmd); + + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; + + private: + std::string name_; + double* cmd_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/joint_state_handle.hpp b/hardware_interface/include/hardware_interface/joint_state_handle.hpp index 78c55993fa..8a6eb247ae 100644 --- a/hardware_interface/include/hardware_interface/joint_state_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_state_handle.hpp @@ -25,48 +25,43 @@ namespace hardware_interface /** A handle used to read the state of a single joint. */ class JointStateHandle { -public: - HARDWARE_INTERFACE_PUBLIC - JointStateHandle(); + public: + HARDWARE_INTERFACE_PUBLIC + JointStateHandle(); - /** - * The joint handles are passive in terms of ownership. - * That means that the handles are only allowed to read/write - * the storage, however are not allowed to delete or reallocate - * this memory. - * \param name The name of the joint - * \param pos A pointer to the storage for this joint's position - * \param vel A pointer to the storage for this joint's velocity - * \param eff A pointer to the storage for this joint's effort (force or torque) - */ - HARDWARE_INTERFACE_PUBLIC - JointStateHandle( - const std::string & name, - const double * pos, - const double * vel, - const double * eff); + /** + * The joint handles are passive in terms of ownership. + * That means that the handles are only allowed to read/write + * the storage, however are not allowed to delete or reallocate + * this memory. + * \param name The name of the joint + * \param pos A pointer to the storage for this joint's position + * \param vel A pointer to the storage for this joint's velocity + * \param eff A pointer to the storage for this joint's effort (force or torque) + */ + HARDWARE_INTERFACE_PUBLIC + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff); - HARDWARE_INTERFACE_PUBLIC - const std::string & - get_name() const; + HARDWARE_INTERFACE_PUBLIC + const std::string& get_name() const; - HARDWARE_INTERFACE_PUBLIC - double - get_position() const; + HARDWARE_INTERFACE_PUBLIC + double get_position() const; - HARDWARE_INTERFACE_PUBLIC - double - get_velocity() const; + HARDWARE_INTERFACE_PUBLIC + double get_velocity() const; - HARDWARE_INTERFACE_PUBLIC - double - get_effort() const; + HARDWARE_INTERFACE_PUBLIC + double get_effort() const; -private: - std::string name_; - const double * pos_; - const double * vel_; - const double * eff_; + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; + + private: + std::string name_; + const double* pos_; + const double* vel_; + const double* eff_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp index a02a2b698b..e6e3138187 100644 --- a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp +++ b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp @@ -23,44 +23,43 @@ namespace hardware_interface { /// Enum for Operation Mode. /** Operation can either be active or inactive */ -enum class HARDWARE_INTERFACE_PUBLIC_TYPE OperationMode: bool +enum class HARDWARE_INTERFACE_PUBLIC_TYPE OperationMode : bool { - ACTIVE = true, - INACTIVE = false + ACTIVE = true, + INACTIVE = false }; /// A handle for Operation Mode. /** Used to set status to active or inactive for operation such as read/write. */ class OperationModeHandle { -public: - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle(); + public: + HARDWARE_INTERFACE_PUBLIC + OperationModeHandle(); - /// Constructor of Operation Mode. - /** - * The mode handles are passive in terms of ownership for the mode pointer. - * This means the handle is allowed to read and write the respective mode, - * however is not allowed to reallocate or delete the memory storage. - * \param name The name of the joint - * \param mode A pointer to the storage for this hardware's operation mode - */ - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle( - const std::string & name, - OperationMode * mode); + /// Constructor of Operation Mode. + /** + * The mode handles are passive in terms of ownership for the mode pointer. + * This means the handle is allowed to read and write the respective mode, + * however is not allowed to reallocate or delete the memory storage. + * \param name The name of the joint + * \param mode A pointer to the storage for this hardware's operation mode + */ + HARDWARE_INTERFACE_PUBLIC + OperationModeHandle(const std::string& name, OperationMode* mode); - HARDWARE_INTERFACE_PUBLIC - const std::string & - get_name() const; + HARDWARE_INTERFACE_PUBLIC + const std::string& get_name() const; - HARDWARE_INTERFACE_PUBLIC - void - set_mode(OperationMode mode); + HARDWARE_INTERFACE_PUBLIC + void set_mode(OperationMode mode); -private: - std::string name_; - OperationMode * mode_; + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; + + private: + std::string name_; + OperationMode* mode_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/joint_command_handle.cpp b/hardware_interface/src/joint_command_handle.cpp index 6e68bb0d82..a7c08aa21c 100644 --- a/hardware_interface/src/joint_command_handle.cpp +++ b/hardware_interface/src/joint_command_handle.cpp @@ -21,38 +21,37 @@ namespace hardware_interface { +JointCommandHandle::JointCommandHandle() : name_(), cmd_(nullptr) +{ +} -JointCommandHandle::JointCommandHandle() -: name_(), - cmd_(nullptr) -{} - -JointCommandHandle::JointCommandHandle( - const std::string & name, - double * cmd) -: name_(name), cmd_(cmd) +JointCommandHandle::JointCommandHandle(const std::string& name, double* cmd) : name_(name), cmd_(cmd) { - THROW_ON_NULLPTR(cmd) + THROW_ON_NULLPTR(cmd) } -const std::string & -JointCommandHandle::get_name() const +const std::string& JointCommandHandle::get_name() const { - return name_; + return name_; } -double -JointCommandHandle::get_cmd() const +double JointCommandHandle::get_cmd() const { - return *cmd_; + THROW_ON_NULLPTR(cmd_) + + return *cmd_; } -void -JointCommandHandle::set_cmd(double cmd) +void JointCommandHandle::set_cmd(double cmd) { - THROW_ON_NULLPTR(cmd_) + THROW_ON_NULLPTR(cmd_) - * cmd_ = 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..fcb6d2f7d3 100644 --- a/hardware_interface/src/joint_state_handle.cpp +++ b/hardware_interface/src/joint_state_handle.cpp @@ -21,53 +21,47 @@ namespace hardware_interface { +JointStateHandle::JointStateHandle() : name_(), pos_(nullptr), vel_(nullptr), eff_(nullptr) +{ +} -JointStateHandle::JointStateHandle() -: name_(), - pos_(nullptr), - vel_(nullptr), - eff_(nullptr) -{} - -JointStateHandle::JointStateHandle( - const std::string & name, - const double * pos, - const double * vel, - const double * eff) -: name_(name), pos_(pos), vel_(vel), eff_(eff) +JointStateHandle::JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) + : name_(name), pos_(pos), vel_(vel), eff_(eff) { - THROW_ON_NULLPTR(pos) - THROW_ON_NULLPTR(vel) - THROW_ON_NULLPTR(eff) + THROW_ON_NULLPTR(pos) + THROW_ON_NULLPTR(vel) + THROW_ON_NULLPTR(eff) } -const std::string & -JointStateHandle::get_name() const +const std::string& JointStateHandle::get_name() const { - return name_; + return name_; } -double -JointStateHandle::get_position() const +double JointStateHandle::get_position() const { - THROW_ON_NULLPTR(pos_) + THROW_ON_NULLPTR(pos_) - return *pos_; + return *pos_; } -double -JointStateHandle::get_velocity() const +double JointStateHandle::get_velocity() const { - THROW_ON_NULLPTR(vel_) + THROW_ON_NULLPTR(vel_) - return *vel_; + return *vel_; } -double -JointStateHandle::get_effort() const +double JointStateHandle::get_effort() const { - THROW_ON_NULLPTR(eff_) + THROW_ON_NULLPTR(eff_) - return *eff_; + 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..f439efe803 100644 --- a/hardware_interface/src/operation_mode_handle.cpp +++ b/hardware_interface/src/operation_mode_handle.cpp @@ -21,33 +21,30 @@ namespace hardware_interface { +OperationModeHandle::OperationModeHandle() : name_(), mode_(nullptr) +{ +} -OperationModeHandle::OperationModeHandle() -: name_(), - mode_(nullptr) -{} - -OperationModeHandle::OperationModeHandle( - const std::string & name, - OperationMode * mode) -: name_(name), - mode_(mode) +OperationModeHandle::OperationModeHandle(const std::string& name, OperationMode* mode) : name_(name), mode_(mode) { - THROW_ON_NULLPTR(mode) + THROW_ON_NULLPTR(mode) } -const std::string & -OperationModeHandle::get_name() const +const std::string& OperationModeHandle::get_name() const { - return name_; + return name_; } -void -OperationModeHandle::set_mode(OperationMode mode) +void OperationModeHandle::set_mode(OperationMode mode) { - THROW_ON_NULLPTR(mode_) + THROW_ON_NULLPTR(mode_) - * mode_ = 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..628c745b1a 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -22,13 +22,16 @@ #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 auto kActuatorLoggerName = "register actuator"; +} -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. @@ -36,52 +39,51 @@ constexpr char kOperationModeLoggerName[] = "joint operation mode handle"; * \param[in] logger_name The name of the logger. * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ -template -hardware_interface_ret_t -register_handle(std::vector & registered_handles, T * handle, const std::string & logger_name) -{ - auto handle_pos = std::find_if( - registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool { - return handle_ptr->get_name() == handle->get_name(); +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 (not 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 { + return handle_ptr->get_name() == handle->get_name(); }); - // handle exists already - if (handle_pos != registered_handles.end()) { - RCLCPP_ERROR( - rclcpp::get_logger(logger_name), - "cannot register handle! Handle exists already"); - return HW_RET_ERROR; - } - registered_handles.push_back(handle); - return HW_RET_OK; + // handle exists already + if (handle_pos != registered_handles.end()) + { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! Handle exists already"); + return HW_RET_ERROR; + } + registered_handles.push_back(handle); + return HW_RET_OK; } -hardware_interface_ret_t -RobotHardware::register_joint_state_handle(const JointStateHandle * joint_handle) +hardware_interface_ret_t RobotHardware::register_joint_state_handle(const JointStateHandle* joint_handle) { - return register_handle( - registered_joint_state_handles_, - joint_handle, - kJointStateLoggerName); + return register_handle(registered_joint_state_handles_, joint_handle, + kJointStateLoggerName); } -hardware_interface_ret_t -RobotHardware::register_joint_command_handle(JointCommandHandle * joint_handle) +hardware_interface_ret_t RobotHardware::register_joint_command_handle(JointCommandHandle* joint_handle) { - return register_handle( - registered_joint_command_handles_, - joint_handle, - kJointCommandLoggerName); + return register_handle(registered_joint_command_handles_, joint_handle, + kJointCommandLoggerName); } -hardware_interface_ret_t -RobotHardware::register_operation_mode_handle(OperationModeHandle * operation_mode_handle) +hardware_interface_ret_t RobotHardware::register_operation_mode_handle(OperationModeHandle* operation_mode_handle) { - return register_handle( - registered_operation_mode_handles_, - operation_mode_handle, - kOperationModeLoggerName); + return register_handle(registered_operation_mode_handles_, operation_mode_handle, + kOperationModeLoggerName); } /// Find the handle by name from the registered handle list. @@ -92,114 +94,88 @@ RobotHardware::register_operation_mode_handle(OperationModeHandle * operation_mo * \param[out] handle the handle if found. * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ -template -hardware_interface_ret_t -get_handle( - std::vector & registered_handles, - const std::string & name, - const std::string & logger_name, - T ** handle) -{ - if (name.empty()) { - RCLCPP_ERROR( - rclcpp::get_logger(logger_name), - "cannot get handle! No name given"); - return HW_RET_ERROR; - } - - auto handle_pos = std::find_if( - registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool { - return handle_ptr->get_name() == name; - }); - - if (handle_pos == registered_handles.end()) { - RCLCPP_ERROR( - rclcpp::get_logger(logger_name), - "cannot get handle. No joint %s found.\n", name.c_str()); - return HW_RET_ERROR; - } - - *handle = *handle_pos; - return HW_RET_OK; +template +hardware_interface_ret_t get_handle(std::vector& registered_handles, const std::string& name, + const std::string& logger_name, T** handle) +{ + if (name.empty()) + { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle! No name given"); + return HW_RET_ERROR; + } + + auto handle_pos = std::find_if(registered_handles.begin(), registered_handles.end(), + [&](auto handle_ptr) -> bool { return handle_ptr->get_name() == name; }); + + if (handle_pos == registered_handles.end()) + { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle. No joint %s found.\n", name.c_str()); + return HW_RET_ERROR; + } + + *handle = *handle_pos; + return HW_RET_OK; } -hardware_interface_ret_t -RobotHardware::get_joint_state_handle( - const std::string & name, const JointStateHandle ** joint_state_handle) +hardware_interface_ret_t RobotHardware::get_joint_state_handle(const std::string& name, + const JointStateHandle** joint_state_handle) { - THROW_ON_NOT_NULLPTR(*joint_state_handle) - return get_handle( - registered_joint_state_handles_, - name, - kJointStateLoggerName, - joint_state_handle); + THROW_ON_NOT_NULLPTR(*joint_state_handle) + return get_handle(registered_joint_state_handles_, name, kJointStateLoggerName, + joint_state_handle); } -hardware_interface_ret_t -RobotHardware::get_joint_command_handle( - const std::string & name, JointCommandHandle ** joint_command_handle) +hardware_interface_ret_t RobotHardware::get_joint_command_handle(const std::string& name, + JointCommandHandle** joint_command_handle) { - THROW_ON_NOT_NULLPTR(*joint_command_handle) - return get_handle( - registered_joint_command_handles_, - name, - kJointCommandLoggerName, - joint_command_handle); + THROW_ON_NOT_NULLPTR(*joint_command_handle) + return get_handle(registered_joint_command_handles_, name, kJointCommandLoggerName, + joint_command_handle); } -hardware_interface_ret_t -RobotHardware::get_operation_mode_handle( - const std::string & name, OperationModeHandle ** operation_mode_handle) +hardware_interface_ret_t RobotHardware::get_operation_mode_handle(const std::string& name, + OperationModeHandle** operation_mode_handle) { - THROW_ON_NOT_NULLPTR(*operation_mode_handle) - return get_handle( - registered_operation_mode_handles_, - name, - kOperationModeLoggerName, - operation_mode_handle); + THROW_ON_NOT_NULLPTR(*operation_mode_handle) + return get_handle(registered_operation_mode_handles_, name, kOperationModeLoggerName, + operation_mode_handle); } -template -std::vector -get_registered_names(std::vector & registered_handles) +template +std::vector get_registered_names(std::vector& registered_handles) { - std::vector names; - names.reserve(registered_handles.size()); - for (auto handle : registered_handles) { - names.push_back(handle->get_name()); - } - return names; + std::vector names; + names.reserve(registered_handles.size()); + for (auto handle : registered_handles) + { + names.push_back(handle->get_name()); + } + return names; } -std::vector -RobotHardware::get_registered_joint_names() +std::vector RobotHardware::get_registered_joint_names() { - return get_registered_names(registered_joint_state_handles_); + return get_registered_names(registered_joint_state_handles_); } -std::vector -RobotHardware::get_registered_write_op_names() +std::vector RobotHardware::get_registered_write_op_names() { - return get_registered_names(registered_operation_mode_handles_); + return get_registered_names(registered_operation_mode_handles_); } -std::vector -RobotHardware::get_registered_joint_state_handles() +std::vector RobotHardware::get_registered_joint_state_handles() { - return registered_joint_state_handles_; + return registered_joint_state_handles_; } -std::vector -RobotHardware::get_registered_joint_command_handles() +std::vector RobotHardware::get_registered_joint_command_handles() { - return registered_joint_command_handles_; + return registered_joint_command_handles_; } -std::vector -RobotHardware::get_registered_operation_mode_handles() +std::vector RobotHardware::get_registered_operation_mode_handles() { - return registered_operation_mode_handles_; + return registered_operation_mode_handles_; } } // namespace hardware_interface diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 2b433eae5c..f67e4b1d91 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -65,12 +65,12 @@ TEST_F(TestRobotHardwareInterface, initialize) TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles) { - JointCommandHandle command_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&command_handle)); - JointStateHandle state_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&state_handle)); - OperationModeHandle op_mode_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&op_mode_handle)); + JointCommandHandle broken_command_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&broken_command_handle)); + JointStateHandle broken_state_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&broken_state_handle)); + OperationModeHandle broken_op_mode_handle; + EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&broken_op_mode_handle)); } } // namespace testing \ No newline at end of file From 5f78e12ed9e1fe410a5afb3bbcb9c9bac7ccbc5b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 08:57:00 +0100 Subject: [PATCH 05/15] Test proper handles and double registering handles --- .../test/test_robot_hardware_interface.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index f67e4b1d91..78bf794604 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -42,6 +42,8 @@ class MyTestRobotHardware : public RobotHardware return HW_RET_OK; } }; + +const auto JOINT_NAME = "joint_1"; } namespace testing @@ -55,6 +57,13 @@ class TestRobotHardwareInterface : public Test } MyTestRobotHardware robot; + double pos_command_value = 0.0; + JointCommandHandle pos_command_handle{ JOINT_NAME, &pos_command_value }; + double velocity_state_value = 0.0; + double effort_state_value = 0.0; + JointStateHandle state_handle{ JOINT_NAME, &pos_command_value, &velocity_state_value, &effort_state_value }; + OperationMode mode = OperationMode::ACTIVE; + OperationModeHandle op_mode_handle{ JOINT_NAME, &mode }; }; TEST_F(TestRobotHardwareInterface, initialize) @@ -73,4 +82,23 @@ TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles) EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&broken_op_mode_handle)); } +TEST_F(TestRobotHardwareInterface, can_register_proper_handles) +{ + EXPECT_EQ(HW_RET_OK, robot.register_joint_command_handle(&pos_command_handle)); + EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&state_handle)); + EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&op_mode_handle)); +} + +TEST_F(TestRobotHardwareInterface, cannot_double_register_handles) +{ + EXPECT_EQ(HW_RET_OK, robot.register_joint_command_handle(&pos_command_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&pos_command_handle)); + + EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&state_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&state_handle)); + + EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&op_mode_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&op_mode_handle)); +} + } // namespace testing \ No newline at end of file From 5c47656c984e54cca72c9159dd26a411de030b4e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 09:21:42 +0100 Subject: [PATCH 06/15] Test name and blanket handle getters --- .../test/test_robot_hardware_interface.cpp | 69 +++++++++++++++++-- 1 file changed, 64 insertions(+), 5 deletions(-) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 78bf794604..9202f93b2a 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -44,6 +44,8 @@ class MyTestRobotHardware : public RobotHardware }; const auto JOINT_NAME = "joint_1"; +const auto NEW_JOINT_NAME = "joint_2"; +double dummy; } namespace testing @@ -56,6 +58,13 @@ class TestRobotHardwareInterface : public Test 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); + } + MyTestRobotHardware robot; double pos_command_value = 0.0; JointCommandHandle pos_command_handle{ JOINT_NAME, &pos_command_value }; @@ -91,14 +100,64 @@ TEST_F(TestRobotHardwareInterface, can_register_proper_handles) TEST_F(TestRobotHardwareInterface, cannot_double_register_handles) { - EXPECT_EQ(HW_RET_OK, robot.register_joint_command_handle(&pos_command_handle)); - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&pos_command_handle)); + SetUpHandles(); - EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&state_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&pos_command_handle)); EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&state_handle)); - - EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&op_mode_handle)); EXPECT_EQ(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)); + + // let's add a new handle + JointStateHandle new_handle{ NEW_JOINT_NAME, &dummy, &dummy, &dummy }; + EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&new_handle)); + + EXPECT_THAT(robot.get_registered_joint_names(), UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME)); +} + +TEST_F(TestRobotHardwareInterface, can_get_registered_state_handles) +{ + SetUpHandles(); + + EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(1)); + + // let's add a new handle + JointStateHandle new_handle{ NEW_JOINT_NAME, &dummy, &dummy, &dummy }; + EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&new_handle)); + + 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)); + + // let's add a new handle + JointCommandHandle new_handle{ NEW_JOINT_NAME, &dummy }; + EXPECT_EQ(HW_RET_OK, robot.register_joint_command_handle(&new_handle)); + + 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)); + + // let's add a new handle + OperationMode dummy; + OperationModeHandle new_handle{ NEW_JOINT_NAME, &dummy }; + EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&new_handle)); + + EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); +} + } // namespace testing \ No newline at end of file From b09755d7fb53840e08e44ddf4abd81f3bef239df Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 09:29:28 +0100 Subject: [PATCH 07/15] Test named handle getters --- .../test/test_robot_hardware_interface.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 9202f93b2a..3c9f8242be 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -160,4 +160,21 @@ TEST_F(TestRobotHardwareInterface, can_get_registered_op_mode_handles) EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); } +TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) +{ + SetUpHandles(); + + const JointStateHandle* state_handle; + EXPECT_EQ(HW_RET_OK, robot.get_joint_state_handle(JOINT_NAME, &state_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.get_joint_state_handle(JOINT_NAME, &state_handle)); + + JointCommandHandle* cmd_handle; + EXPECT_EQ(HW_RET_OK, robot.get_joint_command_handle(JOINT_NAME, &cmd_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle)); + + OperationModeHandle* op_mode_handle; + EXPECT_EQ(HW_RET_OK, robot.get_operation_mode_handle(JOINT_NAME, &op_mode_handle)); + EXPECT_EQ(HW_RET_ERROR, robot.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); +} + } // namespace testing \ No newline at end of file From fdb6354ab94afcddf96d8c01d1f11d1060e3ad92 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 09:34:03 +0100 Subject: [PATCH 08/15] Refactor registering second handles in tests, add check for getting second name --- .../test/test_robot_hardware_interface.cpp | 56 ++++++++++--------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 3c9f8242be..58142aabbe 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -45,7 +45,6 @@ class MyTestRobotHardware : public RobotHardware const auto JOINT_NAME = "joint_1"; const auto NEW_JOINT_NAME = "joint_2"; -double dummy; } namespace testing @@ -65,6 +64,13 @@ class TestRobotHardwareInterface : public Test 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; JointCommandHandle pos_command_handle{ JOINT_NAME, &pos_command_value }; @@ -73,6 +79,10 @@ class TestRobotHardwareInterface : public Test JointStateHandle state_handle{ JOINT_NAME, &pos_command_value, &velocity_state_value, &effort_state_value }; OperationMode mode = OperationMode::ACTIVE; OperationModeHandle op_mode_handle{ JOINT_NAME, &mode }; + + JointCommandHandle new_pos_command_handle{ NEW_JOINT_NAME, &pos_command_value }; + JointStateHandle new_state_handle{ NEW_JOINT_NAME, &pos_command_value, &velocity_state_value, &effort_state_value }; + OperationModeHandle new_op_mode_handle{ NEW_JOINT_NAME, &mode }; }; TEST_F(TestRobotHardwareInterface, initialize) @@ -110,53 +120,36 @@ TEST_F(TestRobotHardwareInterface, cannot_double_register_handles) TEST_F(TestRobotHardwareInterface, can_get_registered_joint_names) { SetUpHandles(); - EXPECT_THAT(robot.get_registered_joint_names(), ElementsAre(JOINT_NAME)); - // let's add a new handle - JointStateHandle new_handle{ NEW_JOINT_NAME, &dummy, &dummy, &dummy }; - EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&new_handle)); - + SetUpNewHandles(); EXPECT_THAT(robot.get_registered_joint_names(), UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME)); } TEST_F(TestRobotHardwareInterface, can_get_registered_state_handles) { SetUpHandles(); - EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(1)); - // let's add a new handle - JointStateHandle new_handle{ NEW_JOINT_NAME, &dummy, &dummy, &dummy }; - EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&new_handle)); - + 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)); - // let's add a new handle - JointCommandHandle new_handle{ NEW_JOINT_NAME, &dummy }; - EXPECT_EQ(HW_RET_OK, robot.register_joint_command_handle(&new_handle)); - + 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)); - // let's add a new handle - OperationMode dummy; - OperationModeHandle new_handle{ NEW_JOINT_NAME, &dummy }; - EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&new_handle)); - + SetUpNewHandles(); EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); } @@ -164,17 +157,28 @@ TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) { SetUpHandles(); - const JointStateHandle* state_handle; + const JointStateHandle* state_handle = nullptr; EXPECT_EQ(HW_RET_OK, robot.get_joint_state_handle(JOINT_NAME, &state_handle)); - EXPECT_EQ(HW_RET_ERROR, robot.get_joint_state_handle(JOINT_NAME, &state_handle)); + state_handle = nullptr; + EXPECT_EQ(HW_RET_ERROR, robot.get_joint_state_handle(NEW_JOINT_NAME, &state_handle)); - JointCommandHandle* cmd_handle; + JointCommandHandle* cmd_handle = nullptr; EXPECT_EQ(HW_RET_OK, robot.get_joint_command_handle(JOINT_NAME, &cmd_handle)); + cmd_handle = nullptr; EXPECT_EQ(HW_RET_ERROR, robot.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle)); - OperationModeHandle* op_mode_handle; + OperationModeHandle* op_mode_handle = nullptr; EXPECT_EQ(HW_RET_OK, robot.get_operation_mode_handle(JOINT_NAME, &op_mode_handle)); + op_mode_handle = nullptr; EXPECT_EQ(HW_RET_ERROR, robot.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); + + SetUpNewHandles(); + state_handle = nullptr; + EXPECT_EQ(HW_RET_OK, robot.get_joint_state_handle(NEW_JOINT_NAME, &state_handle)); + cmd_handle = nullptr; + EXPECT_EQ(HW_RET_OK, robot.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle)); + op_mode_handle = nullptr; + EXPECT_EQ(HW_RET_OK, robot.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); } } // namespace testing \ No newline at end of file From ca06e6bb97d9ac36907893cac2e6d1583eff26c2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 20 Jun 2020 21:13:26 +0100 Subject: [PATCH 09/15] Make linters happy --- hardware_interface/CMakeLists.txt | 4 +- .../joint_command_handle.hpp | 60 ++--- .../hardware_interface/joint_state_handle.hpp | 62 ++--- .../operation_mode_handle.hpp | 50 ++-- .../src/joint_command_handle.cpp | 22 +- hardware_interface/src/joint_state_handle.cpp | 33 +-- .../src/operation_mode_handle.cpp | 18 +- hardware_interface/src/robot_hardware.cpp | 187 ++++++++------- .../test/test_robot_hardware_interface.cpp | 216 +++++++++--------- 9 files changed, 335 insertions(+), 317 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 5f06a9cdc3..a086993f46 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -49,8 +49,8 @@ install( LIBRARY DESTINATION lib) if(BUILD_TESTING) - # find_package(ament_lint_auto REQUIRED) - # ament_lint_auto_find_test_dependencies() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gmock REQUIRED) ament_add_gmock(test_macros test/test_macros.cpp) diff --git a/hardware_interface/include/hardware_interface/joint_command_handle.hpp b/hardware_interface/include/hardware_interface/joint_command_handle.hpp index 769b105c58..0ab249e450 100644 --- a/hardware_interface/include/hardware_interface/joint_command_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_command_handle.hpp @@ -26,36 +26,36 @@ namespace hardware_interface /** A handle used to get and set the command of a single joint. */ class JointCommandHandle { - public: - HARDWARE_INTERFACE_PUBLIC - JointCommandHandle(); - - /** - * The commmand handles are passive in terms of ownership. - * That means that the handles are only allowed to read/write - * the storage, however are not allowed to delete or reallocate - * this memory. - * \param name The name of the joint - * \param cmd A pointer to the storage for this joint's command - */ - HARDWARE_INTERFACE_PUBLIC - JointCommandHandle(const std::string& name, double* cmd); - - HARDWARE_INTERFACE_PUBLIC - const std::string& get_name() const; - - HARDWARE_INTERFACE_PUBLIC - double get_cmd() const; - - HARDWARE_INTERFACE_PUBLIC - void set_cmd(double cmd); - - HARDWARE_INTERFACE_PUBLIC - bool valid_pointers() const; - - private: - std::string name_; - double* cmd_; +public: + HARDWARE_INTERFACE_PUBLIC + JointCommandHandle(); + + /** + * The commmand handles are passive in terms of ownership. + * That means that the handles are only allowed to read/write + * the storage, however are not allowed to delete or reallocate + * this memory. + * \param name The name of the joint + * \param cmd A pointer to the storage for this joint's command + */ + HARDWARE_INTERFACE_PUBLIC + JointCommandHandle(const std::string & name, double * cmd); + + HARDWARE_INTERFACE_PUBLIC + const std::string & get_name() const; + + HARDWARE_INTERFACE_PUBLIC + double get_cmd() const; + + HARDWARE_INTERFACE_PUBLIC + void set_cmd(double cmd); + + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; + +private: + std::string name_; + double * cmd_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/joint_state_handle.hpp b/hardware_interface/include/hardware_interface/joint_state_handle.hpp index 8a6eb247ae..16dd52a20b 100644 --- a/hardware_interface/include/hardware_interface/joint_state_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_state_handle.hpp @@ -25,43 +25,45 @@ namespace hardware_interface /** A handle used to read the state of a single joint. */ class JointStateHandle { - public: - HARDWARE_INTERFACE_PUBLIC - JointStateHandle(); +public: + HARDWARE_INTERFACE_PUBLIC + JointStateHandle(); - /** - * The joint handles are passive in terms of ownership. - * That means that the handles are only allowed to read/write - * the storage, however are not allowed to delete or reallocate - * this memory. - * \param name The name of the joint - * \param pos A pointer to the storage for this joint's position - * \param vel A pointer to the storage for this joint's velocity - * \param eff A pointer to the storage for this joint's effort (force or torque) - */ - HARDWARE_INTERFACE_PUBLIC - JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff); + /** + * The joint handles are passive in terms of ownership. + * That means that the handles are only allowed to read/write + * the storage, however are not allowed to delete or reallocate + * this memory. + * \param name The name of the joint + * \param pos A pointer to the storage for this joint's position + * \param vel A pointer to the storage for this joint's velocity + * \param eff A pointer to the storage for this joint's effort (force or torque) + */ + HARDWARE_INTERFACE_PUBLIC + JointStateHandle( + const std::string & name, const double * pos, const double * vel, + const double * eff); - HARDWARE_INTERFACE_PUBLIC - const std::string& get_name() const; + HARDWARE_INTERFACE_PUBLIC + const std::string & get_name() const; - HARDWARE_INTERFACE_PUBLIC - double get_position() const; + HARDWARE_INTERFACE_PUBLIC + double get_position() const; - HARDWARE_INTERFACE_PUBLIC - double get_velocity() const; + HARDWARE_INTERFACE_PUBLIC + double get_velocity() const; - HARDWARE_INTERFACE_PUBLIC - double get_effort() const; + HARDWARE_INTERFACE_PUBLIC + double get_effort() const; - HARDWARE_INTERFACE_PUBLIC - bool valid_pointers() const; + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; - private: - std::string name_; - const double* pos_; - const double* vel_; - const double* eff_; +private: + std::string name_; + const double * pos_; + const double * vel_; + const double * eff_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp index e6e3138187..a5b5517222 100644 --- a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp +++ b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp @@ -23,43 +23,43 @@ namespace hardware_interface { /// Enum for Operation Mode. /** Operation can either be active or inactive */ -enum class HARDWARE_INTERFACE_PUBLIC_TYPE OperationMode : bool +enum class HARDWARE_INTERFACE_PUBLIC_TYPE OperationMode: bool { - ACTIVE = true, - INACTIVE = false + ACTIVE = true, + INACTIVE = false }; /// A handle for Operation Mode. /** Used to set status to active or inactive for operation such as read/write. */ class OperationModeHandle { - public: - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle(); +public: + HARDWARE_INTERFACE_PUBLIC + OperationModeHandle(); - /// Constructor of Operation Mode. - /** - * The mode handles are passive in terms of ownership for the mode pointer. - * This means the handle is allowed to read and write the respective mode, - * however is not allowed to reallocate or delete the memory storage. - * \param name The name of the joint - * \param mode A pointer to the storage for this hardware's operation mode - */ - HARDWARE_INTERFACE_PUBLIC - OperationModeHandle(const std::string& name, OperationMode* mode); + /// Constructor of Operation Mode. + /** + * The mode handles are passive in terms of ownership for the mode pointer. + * This means the handle is allowed to read and write the respective mode, + * however is not allowed to reallocate or delete the memory storage. + * \param name The name of the joint + * \param mode A pointer to the storage for this hardware's operation mode + */ + HARDWARE_INTERFACE_PUBLIC + OperationModeHandle(const std::string & name, OperationMode * mode); - HARDWARE_INTERFACE_PUBLIC - const std::string& get_name() const; + HARDWARE_INTERFACE_PUBLIC + const std::string & get_name() const; - HARDWARE_INTERFACE_PUBLIC - void set_mode(OperationMode mode); + HARDWARE_INTERFACE_PUBLIC + void set_mode(OperationMode mode); - HARDWARE_INTERFACE_PUBLIC - bool valid_pointers() const; + HARDWARE_INTERFACE_PUBLIC + bool valid_pointers() const; - private: - std::string name_; - OperationMode* mode_; +private: + std::string name_; + OperationMode * mode_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/joint_command_handle.cpp b/hardware_interface/src/joint_command_handle.cpp index a7c08aa21c..3474f2f569 100644 --- a/hardware_interface/src/joint_command_handle.cpp +++ b/hardware_interface/src/joint_command_handle.cpp @@ -21,37 +21,39 @@ namespace hardware_interface { -JointCommandHandle::JointCommandHandle() : name_(), cmd_(nullptr) +JointCommandHandle::JointCommandHandle() +: name_(), cmd_(nullptr) { } -JointCommandHandle::JointCommandHandle(const std::string& name, double* cmd) : name_(name), cmd_(cmd) +JointCommandHandle::JointCommandHandle(const std::string & name, double * cmd) +: name_(name), cmd_(cmd) { - THROW_ON_NULLPTR(cmd) + THROW_ON_NULLPTR(cmd) } -const std::string& JointCommandHandle::get_name() const +const std::string & JointCommandHandle::get_name() const { - return name_; + return name_; } double JointCommandHandle::get_cmd() const { - THROW_ON_NULLPTR(cmd_) + THROW_ON_NULLPTR(cmd_) - return *cmd_; + return *cmd_; } void JointCommandHandle::set_cmd(double cmd) { - THROW_ON_NULLPTR(cmd_) + THROW_ON_NULLPTR(cmd_) - *cmd_ = cmd; + * cmd_ = cmd; } bool JointCommandHandle::valid_pointers() const { - return cmd_; + return cmd_; } } // namespace hardware_interface diff --git a/hardware_interface/src/joint_state_handle.cpp b/hardware_interface/src/joint_state_handle.cpp index fcb6d2f7d3..8747e4737b 100644 --- a/hardware_interface/src/joint_state_handle.cpp +++ b/hardware_interface/src/joint_state_handle.cpp @@ -21,47 +21,50 @@ namespace hardware_interface { -JointStateHandle::JointStateHandle() : name_(), pos_(nullptr), vel_(nullptr), eff_(nullptr) +JointStateHandle::JointStateHandle() +: name_(), pos_(nullptr), vel_(nullptr), eff_(nullptr) { } -JointStateHandle::JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) +JointStateHandle::JointStateHandle( + const std::string & name, const double * pos, const double * vel, + const double * eff) +: name_(name), pos_(pos), vel_(vel), eff_(eff) { - THROW_ON_NULLPTR(pos) - THROW_ON_NULLPTR(vel) - THROW_ON_NULLPTR(eff) + THROW_ON_NULLPTR(pos) + THROW_ON_NULLPTR(vel) + THROW_ON_NULLPTR(eff) } -const std::string& JointStateHandle::get_name() const +const std::string & JointStateHandle::get_name() const { - return name_; + return name_; } double JointStateHandle::get_position() const { - THROW_ON_NULLPTR(pos_) + THROW_ON_NULLPTR(pos_) - return *pos_; + return *pos_; } double JointStateHandle::get_velocity() const { - THROW_ON_NULLPTR(vel_) + THROW_ON_NULLPTR(vel_) - return *vel_; + return *vel_; } double JointStateHandle::get_effort() const { - THROW_ON_NULLPTR(eff_) + THROW_ON_NULLPTR(eff_) - return *eff_; + return *eff_; } bool JointStateHandle::valid_pointers() const { - return pos_ && vel_ && eff_; + 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 f439efe803..41809f0d36 100644 --- a/hardware_interface/src/operation_mode_handle.cpp +++ b/hardware_interface/src/operation_mode_handle.cpp @@ -21,30 +21,32 @@ namespace hardware_interface { -OperationModeHandle::OperationModeHandle() : name_(), mode_(nullptr) +OperationModeHandle::OperationModeHandle() +: name_(), mode_(nullptr) { } -OperationModeHandle::OperationModeHandle(const std::string& name, OperationMode* mode) : name_(name), mode_(mode) +OperationModeHandle::OperationModeHandle(const std::string & name, OperationMode * mode) +: name_(name), mode_(mode) { - THROW_ON_NULLPTR(mode) + THROW_ON_NULLPTR(mode) } -const std::string& OperationModeHandle::get_name() const +const std::string & OperationModeHandle::get_name() const { - return name_; + return name_; } void OperationModeHandle::set_mode(OperationMode mode) { - THROW_ON_NULLPTR(mode_) + THROW_ON_NULLPTR(mode_) - *mode_ = mode; + * mode_ = mode; } bool OperationModeHandle::valid_pointers() const { - return mode_; + return mode_; } } // namespace hardware_interface diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index 628c745b1a..436ffbfb71 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -39,51 +39,57 @@ namespace hardware_interface * \param[in] logger_name The name of the logger. * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ -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 (not 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 { - return handle_ptr->get_name() == handle->get_name(); +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 { + return handle_ptr->get_name() == handle->get_name(); }); - // handle exists already - if (handle_pos != registered_handles.end()) - { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! Handle exists already"); - return HW_RET_ERROR; - } - registered_handles.push_back(handle); - return HW_RET_OK; + // handle exists already + if (handle_pos != registered_handles.end()) { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! Handle exists already"); + return HW_RET_ERROR; + } + registered_handles.push_back(handle); + return HW_RET_OK; } -hardware_interface_ret_t RobotHardware::register_joint_state_handle(const JointStateHandle* joint_handle) +hardware_interface_ret_t RobotHardware::register_joint_state_handle( + const JointStateHandle * joint_handle) { - return register_handle(registered_joint_state_handles_, joint_handle, - kJointStateLoggerName); + return register_handle( + registered_joint_state_handles_, joint_handle, + kJointStateLoggerName); } -hardware_interface_ret_t RobotHardware::register_joint_command_handle(JointCommandHandle* joint_handle) +hardware_interface_ret_t RobotHardware::register_joint_command_handle( + JointCommandHandle * joint_handle) { - return register_handle(registered_joint_command_handles_, joint_handle, - kJointCommandLoggerName); + return register_handle( + registered_joint_command_handles_, joint_handle, + kJointCommandLoggerName); } -hardware_interface_ret_t RobotHardware::register_operation_mode_handle(OperationModeHandle* operation_mode_handle) +hardware_interface_ret_t RobotHardware::register_operation_mode_handle( + OperationModeHandle * operation_mode_handle) { - return register_handle(registered_operation_mode_handles_, operation_mode_handle, - kOperationModeLoggerName); + return register_handle( + registered_operation_mode_handles_, operation_mode_handle, + kOperationModeLoggerName); } /// Find the handle by name from the registered handle list. @@ -94,88 +100,95 @@ hardware_interface_ret_t RobotHardware::register_operation_mode_handle(Operation * \param[out] handle the handle if found. * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ -template -hardware_interface_ret_t get_handle(std::vector& registered_handles, const std::string& name, - const std::string& logger_name, T** handle) -{ - if (name.empty()) - { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle! No name given"); - return HW_RET_ERROR; - } - - auto handle_pos = std::find_if(registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool { return handle_ptr->get_name() == name; }); - - if (handle_pos == registered_handles.end()) - { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle. No joint %s found.\n", name.c_str()); - return HW_RET_ERROR; - } - - *handle = *handle_pos; - return HW_RET_OK; +template +hardware_interface_ret_t get_handle( + std::vector & registered_handles, const std::string & name, + const std::string & logger_name, T ** handle) +{ + if (name.empty()) { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle! No name given"); + return HW_RET_ERROR; + } + + auto handle_pos = std::find_if( + registered_handles.begin(), registered_handles.end(), + [&](auto handle_ptr) -> bool {return handle_ptr->get_name() == name;}); + + if (handle_pos == registered_handles.end()) { + RCLCPP_ERROR( + rclcpp::get_logger( + logger_name), "cannot get handle. No joint %s found.\n", name.c_str()); + return HW_RET_ERROR; + } + + *handle = *handle_pos; + return HW_RET_OK; } -hardware_interface_ret_t RobotHardware::get_joint_state_handle(const std::string& name, - const JointStateHandle** joint_state_handle) +hardware_interface_ret_t RobotHardware::get_joint_state_handle( + const std::string & name, + const JointStateHandle ** joint_state_handle) { - THROW_ON_NOT_NULLPTR(*joint_state_handle) - return get_handle(registered_joint_state_handles_, name, kJointStateLoggerName, - joint_state_handle); + THROW_ON_NOT_NULLPTR(*joint_state_handle) + return get_handle( + registered_joint_state_handles_, name, kJointStateLoggerName, + joint_state_handle); } -hardware_interface_ret_t RobotHardware::get_joint_command_handle(const std::string& name, - JointCommandHandle** joint_command_handle) +hardware_interface_ret_t RobotHardware::get_joint_command_handle( + const std::string & name, + JointCommandHandle ** joint_command_handle) { - THROW_ON_NOT_NULLPTR(*joint_command_handle) - return get_handle(registered_joint_command_handles_, name, kJointCommandLoggerName, - joint_command_handle); + THROW_ON_NOT_NULLPTR(*joint_command_handle) + return get_handle( + registered_joint_command_handles_, name, kJointCommandLoggerName, + joint_command_handle); } -hardware_interface_ret_t RobotHardware::get_operation_mode_handle(const std::string& name, - OperationModeHandle** operation_mode_handle) +hardware_interface_ret_t RobotHardware::get_operation_mode_handle( + const std::string & name, + OperationModeHandle ** operation_mode_handle) { - THROW_ON_NOT_NULLPTR(*operation_mode_handle) - return get_handle(registered_operation_mode_handles_, name, kOperationModeLoggerName, - operation_mode_handle); + THROW_ON_NOT_NULLPTR(*operation_mode_handle) + return get_handle( + registered_operation_mode_handles_, name, kOperationModeLoggerName, + operation_mode_handle); } -template -std::vector get_registered_names(std::vector& registered_handles) +template +std::vector get_registered_names(std::vector & registered_handles) { - std::vector names; - names.reserve(registered_handles.size()); - for (auto handle : registered_handles) - { - names.push_back(handle->get_name()); - } - return names; + std::vector names; + names.reserve(registered_handles.size()); + for (auto handle : registered_handles) { + names.push_back(handle->get_name()); + } + return names; } std::vector RobotHardware::get_registered_joint_names() { - return get_registered_names(registered_joint_state_handles_); + return get_registered_names(registered_joint_state_handles_); } std::vector RobotHardware::get_registered_write_op_names() { - return get_registered_names(registered_operation_mode_handles_); + return get_registered_names(registered_operation_mode_handles_); } -std::vector RobotHardware::get_registered_joint_state_handles() +std::vector RobotHardware::get_registered_joint_state_handles() { - return registered_joint_state_handles_; + return registered_joint_state_handles_; } -std::vector RobotHardware::get_registered_joint_command_handles() +std::vector RobotHardware::get_registered_joint_command_handles() { - return registered_joint_command_handles_; + return registered_joint_command_handles_; } -std::vector RobotHardware::get_registered_operation_mode_handles() +std::vector RobotHardware::get_registered_operation_mode_handles() { - return registered_operation_mode_handles_; + return registered_operation_mode_handles_; } } // namespace hardware_interface diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 58142aabbe..ca52b5fc0f 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -20,165 +20,161 @@ #include "hardware_interface/robot_hardware.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -using namespace hardware_interface; +namespace hw = hardware_interface; namespace { -class MyTestRobotHardware : public RobotHardware +class MyTestRobotHardware : public hw::RobotHardware { - public: - hardware_interface_ret_t init() override - { - return HW_RET_OK; - } - - hardware_interface_ret_t read() override - { - return HW_RET_OK; - } - - hardware_interface_ret_t write() override - { - return HW_RET_OK; - } +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; + } }; const auto JOINT_NAME = "joint_1"; const auto NEW_JOINT_NAME = "joint_2"; -} +} // namespace namespace testing { class TestRobotHardwareInterface : public 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; - JointCommandHandle pos_command_handle{ JOINT_NAME, &pos_command_value }; - double velocity_state_value = 0.0; - double effort_state_value = 0.0; - JointStateHandle state_handle{ JOINT_NAME, &pos_command_value, &velocity_state_value, &effort_state_value }; - OperationMode mode = OperationMode::ACTIVE; - OperationModeHandle op_mode_handle{ JOINT_NAME, &mode }; - - JointCommandHandle new_pos_command_handle{ NEW_JOINT_NAME, &pos_command_value }; - JointStateHandle new_state_handle{ NEW_JOINT_NAME, &pos_command_value, &velocity_state_value, &effort_state_value }; - OperationModeHandle new_op_mode_handle{ NEW_JOINT_NAME, &mode }; +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, initialize) -{ - MyTestRobotHardware local_robot; - EXPECT_EQ(HW_RET_OK, local_robot.init()); -} - TEST_F(TestRobotHardwareInterface, can_not_register_broken_handles) { - JointCommandHandle broken_command_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&broken_command_handle)); - JointStateHandle broken_state_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&broken_state_handle)); - OperationModeHandle broken_op_mode_handle; - EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&broken_op_mode_handle)); + 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_RET_OK, robot.register_joint_command_handle(&pos_command_handle)); - EXPECT_EQ(HW_RET_OK, robot.register_joint_state_handle(&state_handle)); - EXPECT_EQ(HW_RET_OK, robot.register_operation_mode_handle(&op_mode_handle)); + 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(); + SetUpHandles(); - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_command_handle(&pos_command_handle)); - EXPECT_EQ(HW_RET_ERROR, robot.register_joint_state_handle(&state_handle)); - EXPECT_EQ(HW_RET_ERROR, robot.register_operation_mode_handle(&op_mode_handle)); + 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)); + 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)); + SetUpNewHandles(); + EXPECT_THAT(robot.get_registered_joint_names(), UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME)); } TEST_F(TestRobotHardwareInterface, can_get_registered_state_handles) { - SetUpHandles(); - EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(1)); + SetUpHandles(); + EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(1)); - SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(2)); + 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)); + SetUpHandles(); + EXPECT_THAT(robot.get_registered_joint_command_handles(), SizeIs(1)); - SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_joint_command_handles(), SizeIs(2)); + 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)); + SetUpHandles(); + EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(1)); - SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); + SetUpNewHandles(); + EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); } TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) { - SetUpHandles(); - - const JointStateHandle* state_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_joint_state_handle(JOINT_NAME, &state_handle)); - state_handle = nullptr; - EXPECT_EQ(HW_RET_ERROR, robot.get_joint_state_handle(NEW_JOINT_NAME, &state_handle)); - - JointCommandHandle* cmd_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_joint_command_handle(JOINT_NAME, &cmd_handle)); - cmd_handle = nullptr; - EXPECT_EQ(HW_RET_ERROR, robot.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle)); - - OperationModeHandle* op_mode_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_operation_mode_handle(JOINT_NAME, &op_mode_handle)); - op_mode_handle = nullptr; - EXPECT_EQ(HW_RET_ERROR, robot.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); - - SetUpNewHandles(); - state_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_joint_state_handle(NEW_JOINT_NAME, &state_handle)); - cmd_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_joint_command_handle(NEW_JOINT_NAME, &cmd_handle)); - op_mode_handle = nullptr; - EXPECT_EQ(HW_RET_OK, robot.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); + 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)); } -} // namespace testing \ No newline at end of file +} // namespace testing From 13c6a5325252c7966dbf8e6b0c808113188eea05 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 08:08:33 +0100 Subject: [PATCH 10/15] Check that returned pointers are not null --- .../test/test_robot_hardware_interface.cpp | 130 ++++++++++-------- 1 file changed, 76 insertions(+), 54 deletions(-) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index ca52b5fc0f..743296d53a 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -21,6 +21,11 @@ #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 { @@ -43,110 +48,129 @@ class MyTestRobotHardware : public hw::RobotHardware } }; -const auto JOINT_NAME = "joint_1"; -const auto NEW_JOINT_NAME = "joint_2"; +constexpr auto JOINT_NAME = "joint_1"; +constexpr auto NEW_JOINT_NAME = "joint_2"; } // namespace -namespace testing -{ -class TestRobotHardwareInterface : public Test +class TestRobotHardwareInterface : public testing::Test { protected: void SetUp() { - robot.init(); + 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); + 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); + 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}; + 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + EXPECT_THAT(robot_.get_registered_joint_names(), ElementsAre(JOINT_NAME)); SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_joint_names(), UnorderedElementsAre(JOINT_NAME, NEW_JOINT_NAME)); + 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)); + EXPECT_THAT(robot_.get_registered_joint_state_handles(), SizeIs(1)); SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_joint_state_handles(), SizeIs(2)); + 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)); + EXPECT_THAT(robot_.get_registered_joint_command_handles(), SizeIs(1)); SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_joint_command_handles(), SizeIs(2)); + 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)); + EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(1)); SetUpNewHandles(); - EXPECT_THAT(robot.get_registered_operation_mode_handles(), SizeIs(2)); + EXPECT_THAT(robot_.get_registered_operation_mode_handles(), SizeIs(2)); } TEST_F(TestRobotHardwareInterface, can_get_handles_by_name) @@ -154,27 +178,25 @@ 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + EXPECT_EQ(hw::HW_RET_OK, robot_.get_operation_mode_handle(NEW_JOINT_NAME, &op_mode_handle)); } - -} // namespace testing From 4614ce8950968235de30768753b79f817cab4997 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 08:33:56 +0100 Subject: [PATCH 11/15] Revert unsollicited style changes --- .../joint_command_handle.hpp | 13 ++- .../hardware_interface/joint_state_handle.hpp | 16 ++- .../operation_mode_handle.hpp | 10 +- .../src/joint_command_handle.cpp | 20 ++-- hardware_interface/src/joint_state_handle.cpp | 25 +++-- .../src/operation_mode_handle.cpp | 22 ++-- hardware_interface/src/robot_hardware.cpp | 103 +++++++++++------- 7 files changed, 134 insertions(+), 75 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint_command_handle.hpp b/hardware_interface/include/hardware_interface/joint_command_handle.hpp index 0ab249e450..2c04d36d82 100644 --- a/hardware_interface/include/hardware_interface/joint_command_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_command_handle.hpp @@ -39,16 +39,21 @@ class JointCommandHandle * \param cmd A pointer to the storage for this joint's command */ HARDWARE_INTERFACE_PUBLIC - JointCommandHandle(const std::string & name, double * cmd); + JointCommandHandle( + const std::string & name, + double * cmd); HARDWARE_INTERFACE_PUBLIC - const std::string & get_name() const; + const std::string & + get_name() const; HARDWARE_INTERFACE_PUBLIC - double get_cmd() const; + double + get_cmd() const; HARDWARE_INTERFACE_PUBLIC - void set_cmd(double cmd); + void + set_cmd(double cmd); HARDWARE_INTERFACE_PUBLIC bool valid_pointers() const; diff --git a/hardware_interface/include/hardware_interface/joint_state_handle.hpp b/hardware_interface/include/hardware_interface/joint_state_handle.hpp index 16dd52a20b..ad4a775462 100644 --- a/hardware_interface/include/hardware_interface/joint_state_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_state_handle.hpp @@ -41,20 +41,26 @@ class JointStateHandle */ HARDWARE_INTERFACE_PUBLIC JointStateHandle( - const std::string & name, const double * pos, const double * vel, + const std::string & name, + const double * pos, + const double * vel, const double * eff); HARDWARE_INTERFACE_PUBLIC - const std::string & get_name() const; + const std::string & + get_name() const; HARDWARE_INTERFACE_PUBLIC - double get_position() const; + double + get_position() const; HARDWARE_INTERFACE_PUBLIC - double get_velocity() const; + double + get_velocity() const; HARDWARE_INTERFACE_PUBLIC - double get_effort() const; + double + get_effort() const; HARDWARE_INTERFACE_PUBLIC bool valid_pointers() const; diff --git a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp index a5b5517222..bd9f2053c5 100644 --- a/hardware_interface/include/hardware_interface/operation_mode_handle.hpp +++ b/hardware_interface/include/hardware_interface/operation_mode_handle.hpp @@ -46,13 +46,17 @@ class OperationModeHandle * \param mode A pointer to the storage for this hardware's operation mode */ HARDWARE_INTERFACE_PUBLIC - OperationModeHandle(const std::string & name, OperationMode * mode); + OperationModeHandle( + const std::string & name, + OperationMode * mode); HARDWARE_INTERFACE_PUBLIC - const std::string & get_name() const; + const std::string & + get_name() const; HARDWARE_INTERFACE_PUBLIC - void set_mode(OperationMode mode); + void + set_mode(OperationMode mode); HARDWARE_INTERFACE_PUBLIC bool valid_pointers() const; diff --git a/hardware_interface/src/joint_command_handle.cpp b/hardware_interface/src/joint_command_handle.cpp index 3474f2f569..992e8e0cb9 100644 --- a/hardware_interface/src/joint_command_handle.cpp +++ b/hardware_interface/src/joint_command_handle.cpp @@ -21,30 +21,36 @@ namespace hardware_interface { + JointCommandHandle::JointCommandHandle() -: name_(), cmd_(nullptr) -{ -} +: name_(), + cmd_(nullptr) +{} -JointCommandHandle::JointCommandHandle(const std::string & name, double * cmd) +JointCommandHandle::JointCommandHandle( + const std::string & name, + double * cmd) : name_(name), cmd_(cmd) { THROW_ON_NULLPTR(cmd) } -const std::string & JointCommandHandle::get_name() const +const std::string & +JointCommandHandle::get_name() const { return name_; } -double JointCommandHandle::get_cmd() const +double +JointCommandHandle::get_cmd() const { THROW_ON_NULLPTR(cmd_) return *cmd_; } -void JointCommandHandle::set_cmd(double cmd) +void +JointCommandHandle::set_cmd(double cmd) { THROW_ON_NULLPTR(cmd_) diff --git a/hardware_interface/src/joint_state_handle.cpp b/hardware_interface/src/joint_state_handle.cpp index 8747e4737b..1df95072ad 100644 --- a/hardware_interface/src/joint_state_handle.cpp +++ b/hardware_interface/src/joint_state_handle.cpp @@ -21,13 +21,18 @@ namespace hardware_interface { + JointStateHandle::JointStateHandle() -: name_(), pos_(nullptr), vel_(nullptr), eff_(nullptr) -{ -} +: name_(), + pos_(nullptr), + vel_(nullptr), + eff_(nullptr) +{} JointStateHandle::JointStateHandle( - const std::string & name, const double * pos, const double * vel, + const std::string & name, + const double * pos, + const double * vel, const double * eff) : name_(name), pos_(pos), vel_(vel), eff_(eff) { @@ -36,26 +41,30 @@ JointStateHandle::JointStateHandle( THROW_ON_NULLPTR(eff) } -const std::string & JointStateHandle::get_name() const +const std::string & +JointStateHandle::get_name() const { return name_; } -double JointStateHandle::get_position() const +double +JointStateHandle::get_position() const { THROW_ON_NULLPTR(pos_) return *pos_; } -double JointStateHandle::get_velocity() const +double +JointStateHandle::get_velocity() const { THROW_ON_NULLPTR(vel_) return *vel_; } -double JointStateHandle::get_effort() const +double +JointStateHandle::get_effort() const { THROW_ON_NULLPTR(eff_) diff --git a/hardware_interface/src/operation_mode_handle.cpp b/hardware_interface/src/operation_mode_handle.cpp index 41809f0d36..25ee0b4290 100644 --- a/hardware_interface/src/operation_mode_handle.cpp +++ b/hardware_interface/src/operation_mode_handle.cpp @@ -21,23 +21,29 @@ namespace hardware_interface { -OperationModeHandle::OperationModeHandle() -: name_(), mode_(nullptr) -{ -} -OperationModeHandle::OperationModeHandle(const std::string & name, OperationMode * mode) -: name_(name), mode_(mode) +OperationModeHandle::OperationModeHandle() +: name_(), + mode_(nullptr) +{} + +OperationModeHandle::OperationModeHandle( + const std::string & name, + OperationMode * mode) +: name_(name), + mode_(mode) { THROW_ON_NULLPTR(mode) } -const std::string & OperationModeHandle::get_name() const +const std::string & +OperationModeHandle::get_name() const { return name_; } -void OperationModeHandle::set_mode(OperationMode mode) +void +OperationModeHandle::set_mode(OperationMode mode) { THROW_ON_NULLPTR(mode_) diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index 436ffbfb71..adeeb4e427 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -27,7 +27,6 @@ namespace constexpr auto kJointStateLoggerName = "joint state handle"; constexpr auto kJointCommandLoggerName = "joint cmd handle"; constexpr auto kOperationModeLoggerName = "joint operation mode handle"; -constexpr auto kActuatorLoggerName = "register actuator"; } namespace hardware_interface @@ -40,9 +39,8 @@ namespace hardware_interface * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ template -hardware_interface_ret_t register_handle( - std::vector & registered_handles, T * handle, - const std::string & logger_name) +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"); @@ -55,40 +53,46 @@ hardware_interface_ret_t register_handle( } auto handle_pos = std::find_if( - registered_handles.begin(), registered_handles.end(), [&](auto handle_ptr) -> bool { + registered_handles.begin(), registered_handles.end(), + [&](auto handle_ptr) -> bool { return handle_ptr->get_name() == handle->get_name(); }); // handle exists already if (handle_pos != registered_handles.end()) { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot register handle! Handle exists already"); + RCLCPP_ERROR( + rclcpp::get_logger(logger_name), + "cannot register handle! Handle exists already"); return HW_RET_ERROR; } registered_handles.push_back(handle); return HW_RET_OK; } -hardware_interface_ret_t RobotHardware::register_joint_state_handle( - const JointStateHandle * joint_handle) +hardware_interface_ret_t +RobotHardware::register_joint_state_handle(const JointStateHandle * joint_handle) { return register_handle( - registered_joint_state_handles_, joint_handle, + registered_joint_state_handles_, + joint_handle, kJointStateLoggerName); } -hardware_interface_ret_t RobotHardware::register_joint_command_handle( - JointCommandHandle * joint_handle) +hardware_interface_ret_t +RobotHardware::register_joint_command_handle(JointCommandHandle * joint_handle) { return register_handle( - registered_joint_command_handles_, joint_handle, + registered_joint_command_handles_, + joint_handle, kJointCommandLoggerName); } -hardware_interface_ret_t RobotHardware::register_operation_mode_handle( - OperationModeHandle * operation_mode_handle) +hardware_interface_ret_t +RobotHardware::register_operation_mode_handle(OperationModeHandle * operation_mode_handle) { return register_handle( - registered_operation_mode_handles_, operation_mode_handle, + registered_operation_mode_handles_, + operation_mode_handle, kOperationModeLoggerName); } @@ -101,23 +105,30 @@ hardware_interface_ret_t RobotHardware::register_operation_mode_handle( * \return The return code, one of `HW_RET_OK` or `HW_RET_ERROR`. */ template -hardware_interface_ret_t get_handle( - std::vector & registered_handles, const std::string & name, - const std::string & logger_name, T ** handle) +hardware_interface_ret_t +get_handle( + std::vector & registered_handles, + const std::string & name, + const std::string & logger_name, + T ** handle) { if (name.empty()) { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), "cannot get handle! No name given"); + RCLCPP_ERROR( + rclcpp::get_logger(logger_name), + "cannot get handle! No name given"); return HW_RET_ERROR; } auto handle_pos = std::find_if( registered_handles.begin(), registered_handles.end(), - [&](auto handle_ptr) -> bool {return handle_ptr->get_name() == name;}); + [&](auto handle_ptr) -> bool { + return handle_ptr->get_name() == name; + }); if (handle_pos == registered_handles.end()) { RCLCPP_ERROR( - rclcpp::get_logger( - logger_name), "cannot get handle. No joint %s found.\n", name.c_str()); + rclcpp::get_logger(logger_name), + "cannot get handle. No joint %s found.\n", name.c_str()); return HW_RET_ERROR; } @@ -125,38 +136,45 @@ hardware_interface_ret_t get_handle( return HW_RET_OK; } -hardware_interface_ret_t RobotHardware::get_joint_state_handle( - const std::string & name, - const JointStateHandle ** joint_state_handle) +hardware_interface_ret_t +RobotHardware::get_joint_state_handle( + const std::string & name, const JointStateHandle ** joint_state_handle) { THROW_ON_NOT_NULLPTR(*joint_state_handle) return get_handle( - registered_joint_state_handles_, name, kJointStateLoggerName, + registered_joint_state_handles_, + name, + kJointStateLoggerName, joint_state_handle); } -hardware_interface_ret_t RobotHardware::get_joint_command_handle( - const std::string & name, - JointCommandHandle ** joint_command_handle) +hardware_interface_ret_t +RobotHardware::get_joint_command_handle( + const std::string & name, JointCommandHandle ** joint_command_handle) { THROW_ON_NOT_NULLPTR(*joint_command_handle) return get_handle( - registered_joint_command_handles_, name, kJointCommandLoggerName, + registered_joint_command_handles_, + name, + kJointCommandLoggerName, joint_command_handle); } -hardware_interface_ret_t RobotHardware::get_operation_mode_handle( - const std::string & name, - OperationModeHandle ** operation_mode_handle) +hardware_interface_ret_t +RobotHardware::get_operation_mode_handle( + const std::string & name, OperationModeHandle ** operation_mode_handle) { THROW_ON_NOT_NULLPTR(*operation_mode_handle) return get_handle( - registered_operation_mode_handles_, name, kOperationModeLoggerName, + registered_operation_mode_handles_, + name, + kOperationModeLoggerName, operation_mode_handle); } template -std::vector get_registered_names(std::vector & registered_handles) +std::vector +get_registered_names(std::vector & registered_handles) { std::vector names; names.reserve(registered_handles.size()); @@ -166,27 +184,32 @@ std::vector get_registered_names(std::vector & registered_hand return names; } -std::vector RobotHardware::get_registered_joint_names() +std::vector +RobotHardware::get_registered_joint_names() { return get_registered_names(registered_joint_state_handles_); } -std::vector RobotHardware::get_registered_write_op_names() +std::vector +RobotHardware::get_registered_write_op_names() { return get_registered_names(registered_operation_mode_handles_); } -std::vector RobotHardware::get_registered_joint_state_handles() +std::vector +RobotHardware::get_registered_joint_state_handles() { return registered_joint_state_handles_; } -std::vector RobotHardware::get_registered_joint_command_handles() +std::vector +RobotHardware::get_registered_joint_command_handles() { return registered_joint_command_handles_; } -std::vector RobotHardware::get_registered_operation_mode_handles() +std::vector +RobotHardware::get_registered_operation_mode_handles() { return registered_operation_mode_handles_; } From 32fc92ef955833c1c374bb19530c4e1f3589dd26 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 08:42:22 +0100 Subject: [PATCH 12/15] Fix license in test --- hardware_interface/test/test_robot_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/test/test_robot_hardware_interface.cpp b/hardware_interface/test/test_robot_hardware_interface.cpp index 743296d53a..8d019211f4 100644 --- a/hardware_interface/test/test_robot_hardware_interface.cpp +++ b/hardware_interface/test/test_robot_hardware_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. From c56181539bdc5a61519af82cdb4f0837cf90e393 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 17:10:32 +0100 Subject: [PATCH 13/15] Update hardware_interface/CMakeLists.txt Co-authored-by: Karsten Knese --- hardware_interface/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index a086993f46..d6bb1546f1 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -60,7 +60,6 @@ if(BUILD_TESTING) 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( From 7e4aa2a01f47b40e8a030ca06004f3885912f1d9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 17:15:31 +0100 Subject: [PATCH 14/15] Update ros2ci workflow versions --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 661a966809..d30260b1df 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.19 + - uses: ros-tooling/action-ros-ci@0.0.15 with: package-name: controller_interface controller_manager controller_parameter_server hardware_interface test_robot_hardware colcon-mixin-name: coverage-gcc From 30476af05202ec0df2ab0e8bc299e220649c38e0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 25 Jun 2020 17:30:10 +0100 Subject: [PATCH 15/15] use all latest on workflow --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d30260b1df..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.19 - - uses: ros-tooling/action-ros-ci@0.0.15 + - 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