diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt new file mode 100644 index 0000000000..344a7691f2 --- /dev/null +++ b/effort_controllers/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.5) +project(effort_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(forward_command_controller REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +add_library(effort_controllers + SHARED + src/joint_group_effort_controller.cpp +) +target_include_directories(effort_controllers PRIVATE include) +ament_target_dependencies(effort_controllers + forward_command_controller + pluginlib + rclcpp +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + effort_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(controller_manager REQUIRED) + find_package(test_robot_hardware REQUIRED) + + ament_lint_auto_find_test_dependencies() + + ament_add_gtest( + test_load_joint_group_effort_controller + test/test_load_joint_group_effort_controller.cpp + ) + target_include_directories(test_load_joint_group_effort_controller PRIVATE include) + ament_target_dependencies(test_load_joint_group_effort_controller + controller_manager + test_robot_hardware + ) + + ament_add_gtest( + test_joint_group_effort_controller + test/test_joint_group_effort_controller.cpp + ) + target_include_directories(test_joint_group_effort_controller PRIVATE include) + target_link_libraries(test_joint_group_effort_controller + effort_controllers + ) + ament_target_dependencies(test_joint_group_effort_controller + test_robot_hardware + ) +endif() + +ament_export_dependencies( + forward_command_controller +) +ament_export_include_directories( + include +) +ament_export_libraries( + effort_controllers +) +ament_package() diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml new file mode 100644 index 0000000000..3f7d5853ae --- /dev/null +++ b/effort_controllers/effort_controllers_plugins.xml @@ -0,0 +1,7 @@ + + + + The joint effort controller commands a group of joints through the effort interface + + + diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp new file mode 100644 index 0000000000..421aac36bf --- /dev/null +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -0,0 +1,51 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +#ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ + +#include "forward_command_controller/forward_command_controller.hpp" +#include "effort_controllers/visibility_control.h" + +namespace effort_controllers +{ + +/** + * \brief Forward command controller for a set of effort controlled joints (linear or angular). + * + * This class forwards the commanded efforts down to a set of joints. + * + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::msg::Float64MultiArray) : The effort commands to apply. + */ +class JointGroupEffortController : public forward_command_controller::ForwardCommandController +{ +public: + EFFORT_CONTROLLERS_PUBLIC + JointGroupEffortController(); + + EFFORT_CONTROLLERS_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; + + EFFORT_CONTROLLERS_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; +}; + +} // namespace effort_controllers + +#endif // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/include/effort_controllers/visibility_control.h b/effort_controllers/include/effort_controllers/visibility_control.h new file mode 100644 index 0000000000..31ea69cfbe --- /dev/null +++ b/effort_controllers/include/effort_controllers/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define EFFORT_CONTROLLERS_EXPORT __attribute__ ((dllexport)) + #define EFFORT_CONTROLLERS_IMPORT __attribute__ ((dllimport)) + #else + #define EFFORT_CONTROLLERS_EXPORT __declspec(dllexport) + #define EFFORT_CONTROLLERS_IMPORT __declspec(dllimport) + #endif + #ifdef EFFORT_CONTROLLERS_BUILDING_DLL + #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT + #else + #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT + #endif + #define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC + #define EFFORT_CONTROLLERS_LOCAL +#else + #define EFFORT_CONTROLLERS_EXPORT __attribute__ ((visibility("default"))) + #define EFFORT_CONTROLLERS_IMPORT + #if __GNUC__ >= 4 + #define EFFORT_CONTROLLERS_PUBLIC __attribute__ ((visibility("default"))) + #define EFFORT_CONTROLLERS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define EFFORT_CONTROLLERS_PUBLIC + #define EFFORT_CONTROLLERS_LOCAL + #endif + #define EFFORT_CONTROLLERS_PUBLIC_TYPE +#endif + +#endif // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml new file mode 100644 index 0000000000..40107bd4ad --- /dev/null +++ b/effort_controllers/package.xml @@ -0,0 +1,27 @@ + + + effort_controllers + 0.0.0 + Generic controller for forwarding commands. + Bence Magyar + Jordan Palacios + + Apache License 2.0 + + ament_cmake + + forward_command_controller + rclcpp + + pluginlib + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + controller_manager + test_robot_hardware + + + ament_cmake + + diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp new file mode 100644 index 0000000000..41af6d4029 --- /dev/null +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -0,0 +1,69 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 "effort_controllers/joint_group_effort_controller.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/parameter.hpp" + +namespace +{ +constexpr auto kJECLoggerName = "joint effort controller"; +} + +namespace effort_controllers +{ +using CallbackReturn = JointGroupEffortController::CallbackReturn; + +JointGroupEffortController::JointGroupEffortController() +: forward_command_controller::ForwardCommandController() +{ + logger_name_ = kJECLoggerName; +} + +CallbackReturn JointGroupEffortController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + rclcpp::Parameter interface_param; + if (!lifecycle_node_->get_parameter("interface_name", interface_param)) { + lifecycle_node_->declare_parameter("interface_name", "effort_command"); + } else { + if (interface_param.as_string() != "effort_command") { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + kJECLoggerName), "'interface_name' already set with an invalid value"); + return CallbackReturn::ERROR; + } + } + return ForwardCommandController::on_configure(previous_state); +} + +CallbackReturn JointGroupEffortController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = ForwardCommandController::on_deactivate(previous_state); + + // stop all joints + for (auto & joint_handle : joint_cmd_handles_) { + joint_handle.set_value(0.0); + } + + return ret; +} + +} // namespace effort_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + effort_controllers::JointGroupEffortController, controller_interface::ControllerInterface) diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp new file mode 100644 index 0000000000..7ef7dbdefa --- /dev/null +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -0,0 +1,173 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 +#include +#include + +#include "gtest/gtest.h" + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/joint_handle.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "test_joint_group_effort_controller.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; + +void JointGroupEffortControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} + +void JointGroupEffortControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} + +void JointGroupEffortControllerTest::SetUp() +{ + // initialize robot + test_robot_ = std::make_shared(); + test_robot_->init(); + + // initialize controller + controller_ = std::make_unique(); +} + +void JointGroupEffortControllerTest::TearDown() +{ + controller_.reset(nullptr); +} + +void JointGroupEffortControllerTest::SetUpController() +{ + const auto result = controller_->init(test_robot_, "forward_command_controller"); + ASSERT_EQ(result, controller_interface::return_type::SUCCESS); +} + +void JointGroupEffortControllerTest::SetUpHandles() +{ + // get handles from test_robot_hardware + joint1_eff_cmd_handle_ = std::make_shared( + "joint1", + "effort_command"); + joint2_eff_cmd_handle_ = std::make_shared( + "joint2", + "effort_command"); + joint3_eff_cmd_handle_ = std::make_shared( + "joint3", + "effort_command"); + + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint1_eff_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint2_eff_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint3_eff_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); +} + +TEST_F(JointGroupEffortControllerTest, ConfigureParamsTest) +{ + // joint handles not initialized yet + ASSERT_TRUE(controller_->joint_cmd_handles_.empty()); + + SetUpController(); + + // configure failed, 'joints' paremeter not set + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector())); + + // configure failed, 'joints' is empty + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + auto result = controller_->lifecycle_node_->set_parameter( + rclcpp::Parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2"}))); + ASSERT_TRUE(result.successful); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // joint handles initialized + ASSERT_EQ(controller_->joint_cmd_handles_.size(), 2ul); + ASSERT_EQ(controller_->joint_cmd_handles_[0].get_interface_name(), "effort_command"); + ASSERT_EQ(controller_->joint_cmd_handles_[1].get_interface_name(), "effort_command"); +} + +TEST_F(JointGroupEffortControllerTest, CheckParamsTest) +{ + // joint handles not initialized yet + ASSERT_TRUE(controller_->joint_cmd_handles_.empty()); + + SetUpController(); + + // configure failed, interface name has already been set, with the wrong interface + controller_->lifecycle_node_->declare_parameter("interface_name", "velocity_command"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + auto result = controller_->lifecycle_node_->set_parameter( + rclcpp::Parameter( + "interface_name", + rclcpp::ParameterValue("effort_command"))); + ASSERT_TRUE(result.successful); + + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2"})); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // joint handles initialized + ASSERT_EQ(controller_->joint_cmd_handles_.size(), 2ul); + ASSERT_EQ(controller_->joint_cmd_handles_[0].get_interface_name(), "effort_command"); + ASSERT_EQ(controller_->joint_cmd_handles_[1].get_interface_name(), "effort_command"); +} + +TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) +{ + SetUpController(); + SetUpHandles(); + + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(test_robot_->joint_names)); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are still the default ones + ASSERT_EQ(joint1_eff_cmd_handle_->get_value(), 1.3); + ASSERT_EQ(joint2_eff_cmd_handle_->get_value(), 2.3); + ASSERT_EQ(joint3_eff_cmd_handle_->get_value(), 3.3); + + // stop the controller + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are now zero + ASSERT_EQ(joint1_eff_cmd_handle_->get_value(), 0.0); + ASSERT_EQ(joint2_eff_cmd_handle_->get_value(), 0.0); + ASSERT_EQ(joint3_eff_cmd_handle_->get_value(), 0.0); +} diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp new file mode 100644 index 0000000000..1c089188be --- /dev/null +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -0,0 +1,56 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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. + +#ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ + +#include +#include + +#include "gtest/gtest.h" + +#include "hardware_interface/joint_handle.hpp" +#include "effort_controllers/joint_group_effort_controller.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +// subclassing and friending so we can access member varibles +class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController +{ + FRIEND_TEST(JointGroupEffortControllerTest, ConfigureParamsTest); + FRIEND_TEST(JointGroupEffortControllerTest, CheckParamsTest); + FRIEND_TEST(JointGroupEffortControllerTest, StopJointsOnDeactivateTest); +}; + +class JointGroupEffortControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::shared_ptr test_robot_; + std::unique_ptr controller_; + + std::shared_ptr joint1_eff_cmd_handle_; + std::shared_ptr joint2_eff_cmd_handle_; + std::shared_ptr joint3_eff_cmd_handle_; +}; + +#endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp new file mode 100644 index 0000000000..44e69b5f94 --- /dev/null +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -0,0 +1,39 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 "controller_manager/controller_manager.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +TEST(TestLoadJointGroupEffortController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr robot = + std::make_shared(); + + robot->init(); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller( + "test_joint_group_effort_controller", + "effort_controllers/JointGroupEffortController")); +}