diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt new file mode 100644 index 0000000000..b1dc458c75 --- /dev/null +++ b/forward_command_controller/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.5) +project(forward_command_controller) + +# 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(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(std_msgs REQUIRED) + +add_library(forward_command_controller + SHARED + src/forward_command_controller.cpp +) +target_include_directories(forward_command_controller PRIVATE include) +ament_target_dependencies(forward_command_controller + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + std_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + forward_command_controller + 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_forward_command_controller + test/test_load_forward_command_controller.cpp + ) + target_include_directories(test_load_forward_command_controller PRIVATE include) + ament_target_dependencies(test_load_forward_command_controller + controller_manager + test_robot_hardware + ) + + ament_add_gtest( + test_forward_command_controller + test/test_forward_command_controller.cpp + ) + target_include_directories(test_forward_command_controller PRIVATE include) + target_link_libraries(test_forward_command_controller + forward_command_controller + ) + ament_target_dependencies(test_forward_command_controller + test_robot_hardware + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs +) +ament_export_include_directories( + include +) +ament_export_libraries( + forward_command_controller +) +ament_package() diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml new file mode 100644 index 0000000000..f8b1261d6d --- /dev/null +++ b/forward_command_controller/forward_command_plugin.xml @@ -0,0 +1,7 @@ + + + + The forward command controller commands a group of joints in a given interface + + + diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp new file mode 100644 index 0000000000..bfab7035ba --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -0,0 +1,80 @@ +// 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 FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "forward_command_controller/visibility_control.h" +#include "hardware_interface/joint_handle.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp/subscription.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace forward_command_controller +{ + +/** + * \brief Forward command controller for a set of joints. + * + * This class forwards the command signal down to a set of joints + * on the specified interface. + * + * \param joints Names of the joints to control. + * \param interface_name Name of the interface to command. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class ForwardCommandController : public controller_interface::ControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardCommandController(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type + update() override; + +protected: + using CmdType = std_msgs::msg::Float64MultiArray; + + std::vector joint_cmd_handles_; + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; + + std::string logger_name_; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/visibility_control.h b/forward_command_controller/include/forward_command_controller/visibility_control.h new file mode 100644 index 0000000000..56ae567985 --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/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 FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ +#define FORWARD_COMMAND_CONTROLLER__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 FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) + #define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL + #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT + #else + #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT + #endif + #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC + #define FORWARD_COMMAND_CONTROLLER_LOCAL +#else + #define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define FORWARD_COMMAND_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define FORWARD_COMMAND_CONTROLLER_PUBLIC + #define FORWARD_COMMAND_CONTROLLER_LOCAL + #endif + #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml new file mode 100644 index 0000000000..c076d8c569 --- /dev/null +++ b/forward_command_controller/package.xml @@ -0,0 +1,31 @@ + + + forward_command_controller + 0.0.0 + Generic controller for forwarding commands. + Bence Magyar + Jordan Palacios + + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + + pluginlib + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + controller_manager + test_robot_hardware + + + ament_cmake + + diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp new file mode 100644 index 0000000000..d14b233beb --- /dev/null +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -0,0 +1,154 @@ +// 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 +#include +#include + +#include "forward_command_controller/forward_command_controller.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/logging.hpp" + +namespace forward_command_controller +{ +using CallbackReturn = ForwardCommandController::CallbackReturn; + +ForwardCommandController::ForwardCommandController() +: controller_interface::ControllerInterface(), + joint_cmd_handles_(), + rt_command_ptr_(nullptr), + joints_command_subscriber_(nullptr), + logger_name_("forward command controller") +{} + +CallbackReturn ForwardCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + rclcpp::Parameter joints_param, interface_param; + if (!lifecycle_node_->get_parameter("joints", joints_param)) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name_), "'joints' parameter not set"); + return CallbackReturn::ERROR; + } + if (!lifecycle_node_->get_parameter("interface_name", interface_param)) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name_), "'interface_name' parameter not set"); + return CallbackReturn::ERROR; + } + + auto joint_names = joints_param.as_string_array(); + if (joint_names.empty()) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name_), "'joints' is empty"); + return CallbackReturn::ERROR; + } + + auto interface_name = interface_param.as_string(); + if (interface_name.empty()) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name_), "'interface_name' is empty"); + return CallbackReturn::ERROR; + } + + if (auto rh_ptr = robot_hardware_.lock()) { + const auto registered_joints = rh_ptr->get_registered_joint_names(); + + // check all requested joints are present + for (const auto & joint_name : joint_names) { + if (std::find( + registered_joints.cbegin(), registered_joints.cend(), + joint_name) == registered_joints.cend()) + { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + logger_name_), "joint '" << joint_name << "' not registered"); + return CallbackReturn::ERROR; + } + } + + // get joint handles + for (const auto & joint_name : joint_names) { + hardware_interface::JointHandle joint_handle(joint_name, interface_name); + if (rh_ptr->get_joint_handle(joint_handle) == + hardware_interface::hardware_interface_ret_t::ERROR) + { + // uppon error, clear any previously requested handles + joint_cmd_handles_.clear(); + + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + logger_name_), "could not get handle for joint '" << joint_name << "'"); + return CallbackReturn::ERROR; + } + joint_cmd_handles_.emplace_back(std::move(joint_handle)); + } + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + logger_name_), "could not lock pointer to robot_hardware"); + return CallbackReturn::ERROR; + } + + joints_command_subscriber_ = lifecycle_node_->create_subscription( + "commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) + { + rt_command_ptr_.writeFromNonRT(msg); + }); + + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + logger_name_), "configure successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn ForwardCommandController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +CallbackReturn ForwardCommandController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type ForwardCommandController::update() +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!joint_commands || !(*joint_commands)) { + return controller_interface::return_type::SUCCESS; + } + + const auto joint_num = (*joint_commands)->data.size(); + if (joint_num != joint_cmd_handles_.size()) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger( + logger_name_), + *lifecycle_node_->get_clock(), 1000, "command size does not match number of joints"); + return controller_interface::return_type::ERROR; + } + + for (auto index = 0ul; index < joint_num; ++index) { + joint_cmd_handles_[index].set_value((*joint_commands)->data[index]); + } + + return controller_interface::return_type::SUCCESS; +} + +} // namespace forward_command_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp new file mode 100644 index 0000000000..582dcb746c --- /dev/null +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -0,0 +1,306 @@ +// 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 "forward_command_controller/forward_command_controller.hpp" +#include "hardware_interface/joint_handle.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "test_forward_command_controller.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; + +namespace +{ +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} +} + +void ForwardCommandControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} + +void ForwardCommandControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} + +void ForwardCommandControllerTest::SetUp() +{ + // initialize robot + test_robot_ = std::make_shared(); + test_robot_->init(); + + // initialize controller + controller_ = std::make_unique(); +} + +void ForwardCommandControllerTest::TearDown() +{ + controller_.reset(nullptr); +} + +void ForwardCommandControllerTest::SetUpController() +{ + const auto result = controller_->init(test_robot_, "forward_command_controller"); + ASSERT_EQ(result, controller_interface::return_type::SUCCESS); +} + +void ForwardCommandControllerTest::SetUpHandles() +{ + // get handles from test_robot_hardware + joint1_pos_cmd_handle_ = std::make_shared( + "joint1", + "position_command"); + joint2_pos_cmd_handle_ = std::make_shared( + "joint2", + "position_command"); + joint3_pos_cmd_handle_ = std::make_shared( + "joint3", + "position_command"); + + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint1_pos_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint2_pos_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); + ASSERT_EQ( + test_robot_->get_joint_handle( + *joint3_pos_cmd_handle_), hardware_interface::hardware_interface_ret_t::OK); +} + +TEST_F(ForwardCommandControllerTest, 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, 'interface_name' paremeter not set + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + controller_->lifecycle_node_->declare_parameter("interface_name", ""); + + // 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 failed, 'interface_name' is empty + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + result = controller_->lifecycle_node_->set_parameter( + rclcpp::Parameter( + "interface_name", + rclcpp::ParameterValue("position_command"))); + 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); +} + +TEST_F(ForwardCommandControllerTest, ConfigureJointsChecksTest) +{ + // joint handles not initialized yet + ASSERT_TRUE(controller_->joint_cmd_handles_.empty()); + + SetUpController(); + + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint4"})); + + controller_->lifecycle_node_->declare_parameter("interface_name", "acceleration_command"); + + // configure failed, 'joint4' not in robot_hardware + 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 failed, 'joint1' does not support 'acceleration_command' interface + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + result = controller_->lifecycle_node_->set_parameter( + rclcpp::Parameter( + "interface_name", + rclcpp::ParameterValue("position_command"))); + 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); +} + +TEST_F(ForwardCommandControllerTest, CommandSuccessTest) +{ + SetUpController(); + SetUpHandles(); + + // configure controller + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); + controller_->lifecycle_node_->declare_parameter("interface_name", "position_command"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // update successful though no command has been send yet + ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS); + + // check joint commands are still the default ones + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 1.1); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 2.1); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 3.1); + + // send command + FriendForwardCommandController::CmdType::SharedPtr command_ptr = + std::make_shared(); + command_ptr->data = {10.0, 20.0, 30.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update successful, command received + ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS); + + // check joint commands have been modified + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 10.0); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 20.0); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 30.0); +} + +TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) +{ + SetUpController(); + SetUpHandles(); + + // configure controller + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); + controller_->lifecycle_node_->declare_parameter("interface_name", "position_command"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // send command with wrong numnber of joints + FriendForwardCommandController::CmdType::SharedPtr command_ptr = + std::make_shared(); + command_ptr->data = {10.0, 20.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update failed, command size does not match number of joints + ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + + // check joint commands are still the default ones + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 1.1); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 2.1); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 3.1); +} + +TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) +{ + SetUpController(); + SetUpHandles(); + + // configure controller + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); + controller_->lifecycle_node_->declare_parameter("interface_name", "position_command"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // update successful, no command received yet + ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS); + + // check joint commands are still the default ones + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 1.1); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 2.1); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 3.1); +} + +TEST_F(ForwardCommandControllerTest, CommandCallbackTest) +{ + SetUpController(); + SetUpHandles(); + + controller_->lifecycle_node_->declare_parameter( + "joints", + rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); + controller_->lifecycle_node_->declare_parameter("interface_name", "position_command"); + + // default values + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 1.1); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 2.1); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 3.1); + + auto node_state = controller_->get_lifecycle_node()->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_lifecycle_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto command_pub = test_node.create_publisher( + "commands", rclcpp::SystemDefaultsQoS()); + std_msgs::msg::Float64MultiArray command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + command_pub->publish(command_msg); + + // wait for command message to be passed + ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); + + // process callbacks + rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); + + // update successful + ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS); + + // check command in handle was set + ASSERT_EQ(joint1_pos_cmd_handle_->get_value(), 10.0); + ASSERT_EQ(joint2_pos_cmd_handle_->get_value(), 20.0); + ASSERT_EQ(joint3_pos_cmd_handle_->get_value(), 30.0); +} diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp new file mode 100644 index 0000000000..d75d738742 --- /dev/null +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -0,0 +1,59 @@ +// 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_FORWARD_COMMAND_CONTROLLER_HPP_ +#define TEST_FORWARD_COMMAND_CONTROLLER_HPP_ + +#include +#include + +#include "gtest/gtest.h" + +#include "forward_command_controller/forward_command_controller.hpp" +#include "hardware_interface/joint_handle.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +// subclassing and friending so we can access member varibles +class FriendForwardCommandController : public forward_command_controller::ForwardCommandController +{ + FRIEND_TEST(ForwardCommandControllerTest, ConfigureParamsTest); + FRIEND_TEST(ForwardCommandControllerTest, ConfigureJointsChecksTest); + FRIEND_TEST(ForwardCommandControllerTest, CommandSuccessTest); + FRIEND_TEST(ForwardCommandControllerTest, WrongCommandCheckTest); + FRIEND_TEST(ForwardCommandControllerTest, NoCommandCheckTest); + FRIEND_TEST(ForwardCommandControllerTest, CommandCallbackTest); +}; + +class ForwardCommandControllerTest : 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_pos_cmd_handle_; + std::shared_ptr joint2_pos_cmd_handle_; + std::shared_ptr joint3_pos_cmd_handle_; +}; + +#endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp new file mode 100644 index 0000000000..9d37423fb3 --- /dev/null +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -0,0 +1,42 @@ +// 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 "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +TEST(TestLoadForwardCommandController, 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_forward_command_controller", + "forward_command_controller/ForwardCommandController")); +}