diff --git a/forward_state_controller/CMakeLists.txt b/forward_state_controller/CMakeLists.txt new file mode 100644 index 0000000000..a6b0f0be46 --- /dev/null +++ b/forward_state_controller/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.16) +project(forward_state_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + forward_state_controller_parameters + src/forward_state_controller_parameters.yaml +) + +add_library(forward_state_controller SHARED + src/forward_state_controller.cpp +) +target_compile_features(forward_state_controller PUBLIC cxx_std_17) +target_include_directories(forward_state_controller PUBLIC + $ + $ +) +target_link_libraries(forward_state_controller PUBLIC + forward_state_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) +pluginlib_export_plugin_description_file(controller_interface forward_state_plugin.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + + ament_add_gmock(test_load_forward_state_controller + test/test_load_forward_state_controller.cpp + ) + target_link_libraries(test_load_forward_state_controller + forward_state_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_forward_state_controller + test/test_forward_state_controller.cpp + ) + target_link_libraries(test_forward_state_controller + forward_state_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/forward_state_controller +) +install( + TARGETS + forward_state_controller + forward_state_controller_parameters + EXPORT export_forward_state_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_forward_state_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/forward_state_controller/forward_state_plugin.xml b/forward_state_controller/forward_state_plugin.xml new file mode 100644 index 0000000000..37bdb438b2 --- /dev/null +++ b/forward_state_controller/forward_state_plugin.xml @@ -0,0 +1,7 @@ + + + + Forward state controller that forwards state interface values to command interfaces. + + + diff --git a/forward_state_controller/include/forward_state_controller/forward_state_controller.hpp b/forward_state_controller/include/forward_state_controller/forward_state_controller.hpp new file mode 100644 index 0000000000..7523f3c00e --- /dev/null +++ b/forward_state_controller/include/forward_state_controller/forward_state_controller.hpp @@ -0,0 +1,86 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_ +#define FORWARD_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "forward_state_controller/forward_state_controller_parameters.hpp" + +namespace forward_state_controller +{ +/** + * \brief Forward state controller that forwards state interfaces to command interfaces. + * + * + * \param state_interfaces List of state interface names to read from. + * \param forward_state Map from each state interface name to the list of command interfaces + * to forward its value to. + * + * Configuration example: + * \code{.yaml} + * state_interfaces: + * - joint1/position + * - joint1/velocity + * forward_state: + * joint1/position: + * to_command: ["joint2/position", "joint3/position"] + * joint1/velocity: + * to_command: ["joint2/velocity"] + * \endcode + */ +class ForwardStateController : public controller_interface::ControllerInterface +{ +public: + ForwardStateController(); + + ~ForwardStateController() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + Params params_; + + std::vector state_interface_names_; + std::vector command_interface_names_; + + std::unordered_map> state_to_command_map_; +}; + +} // namespace forward_state_controller + +#endif // FORWARD_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_ diff --git a/forward_state_controller/package.xml b/forward_state_controller/package.xml new file mode 100644 index 0000000000..42c0e470ba --- /dev/null +++ b/forward_state_controller/package.xml @@ -0,0 +1,29 @@ + + + forward_state_controller + 0.1.0 + Controller that forwards state interface values to command interfaces. + Nikola Banovic + Apache License 2.0 + https://control.ros.org + + ament_cmake + ros2_control_cmake + + backward_ros + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/forward_state_controller/src/forward_state_controller.cpp b/forward_state_controller/src/forward_state_controller.cpp new file mode 100644 index 0000000000..c577aa62ec --- /dev/null +++ b/forward_state_controller/src/forward_state_controller.cpp @@ -0,0 +1,174 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 "forward_state_controller/forward_state_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/logging.hpp" + +namespace forward_state_controller +{ +ForwardStateController::ForwardStateController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn ForwardStateController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardStateController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (params_.state_interfaces.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'state_interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + state_interface_names_.clear(); + command_interface_names_.clear(); + state_to_command_map_.clear(); + + for (const auto & state_iface_name : params_.state_interfaces) + { + auto it = params_.forward_state.state_interfaces_map.find(state_iface_name); + if (it == params_.forward_state.state_interfaces_map.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "No 'forward_state.%s' mapping found in parameters", state_iface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const auto & to_command = it->second.to_command; + if (to_command.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'forward_state.%s.to_command' is empty", state_iface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const std::size_t state_idx = state_interface_names_.size(); + state_interface_names_.push_back(state_iface_name); + + std::vector cmd_indices; + for (const auto & cmd_iface_name : to_command) + { + // deduplicate command_interface_names_ + auto cmd_it = std::find( + command_interface_names_.begin(), command_interface_names_.end(), cmd_iface_name); + std::size_t cmd_idx; + if (cmd_it == command_interface_names_.end()) + { + cmd_idx = command_interface_names_.size(); + command_interface_names_.push_back(cmd_iface_name); + } + else + { + cmd_idx = static_cast(std::distance(command_interface_names_.begin(), cmd_it)); + } + cmd_indices.push_back(cmd_idx); + } + state_to_command_map_[state_idx] = cmd_indices; + } + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +ForwardStateController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_names_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +ForwardStateController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_names_; + + return state_interfaces_config; +} + +controller_interface::CallbackReturn ForwardStateController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardStateController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type ForwardStateController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (const auto & [state_idx, cmd_indices] : state_to_command_map_) + { + const auto state_value_opt = state_interfaces_[state_idx].get_optional(); + if (!state_value_opt.has_value()) + { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "Unable to get value from state interface '%s'", + state_interfaces_[state_idx].get_name().c_str()); + continue; + } + + const double value = state_value_opt.value(); + for (const auto & cmd_idx : cmd_indices) + { + command_interfaces_[cmd_idx].set_value(value) + } + } + + return controller_interface::return_type::OK; +} + +} // namespace forward_state_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + forward_state_controller::ForwardStateController, controller_interface::ControllerInterface) diff --git a/forward_state_controller/src/forward_state_controller_parameters.yaml b/forward_state_controller/src/forward_state_controller_parameters.yaml new file mode 100644 index 0000000000..17387082ce --- /dev/null +++ b/forward_state_controller/src/forward_state_controller_parameters.yaml @@ -0,0 +1,19 @@ +forward_state_controller: + state_interfaces: { + type: string_array, + description: "List of state interfaces whose values will be forwarded to command interfaces", + validation: { + not_empty<>: [] + }, + read_only: true + } + forward_state: + __map_state_interfaces: + to_command: { + type: string_array, + description: "List of command interfaces to forward this state interface value to", + validation: { + not_empty<>: [] + }, + read_only: true + } diff --git a/forward_state_controller/test/config/test_forward_state_controller.yaml b/forward_state_controller/test/config/test_forward_state_controller.yaml new file mode 100644 index 0000000000..128f7dd3e2 --- /dev/null +++ b/forward_state_controller/test/config/test_forward_state_controller.yaml @@ -0,0 +1,7 @@ +test_forward_state_controller: + ros__parameters: + state_interfaces: + - joint1/position + forward_state: + joint1/position: + to_command: ["joint2/position"] diff --git a/forward_state_controller/test/test_forward_state_controller.cpp b/forward_state_controller/test/test_forward_state_controller.cpp new file mode 100644 index 0000000000..ce18506b4a --- /dev/null +++ b/forward_state_controller/test/test_forward_state_controller.cpp @@ -0,0 +1,192 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 "gmock/gmock.h" + +#include "test_forward_state_controller.hpp" + +#include "forward_state_controller/forward_state_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; + +void ForwardStateControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void ForwardStateControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void ForwardStateControllerTest::SetUp() +{ + controller_ = std::make_unique(); +} + +void ForwardStateControllerTest::TearDown() { controller_.reset(nullptr); } + +void ForwardStateControllerTest::SetUpController( + const std::vector & parameters) +{ + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "forward_state_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(joint2_pos_cmd_, nullptr); + command_ifs.emplace_back(joint3_pos_cmd_, nullptr); + + std::vector state_ifs; + state_ifs.emplace_back(joint1_pos_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(ForwardStateControllerTest, StateInterfacesParameterNotSet) +{ + // state_interfaces is a required parameter - init itself fails when it is not provided + auto node_options = controller_->define_custom_node_options(); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "forward_state_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + ASSERT_EQ(controller_->init(params), controller_interface::return_type::ERROR); +} + +TEST_F(ForwardStateControllerTest, ForwardStateMappingMissing) +{ + // state_interfaces lists joint1/position but forward_state has no entry for it: + // generate_parameter_library enforces the map at init() time, so init() itself fails + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides( + {rclcpp::Parameter("state_interfaces", std::vector{"joint1/position"})}); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "forward_state_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + ASSERT_EQ(controller_->init(params), controller_interface::return_type::ERROR); +} + +TEST_F(ForwardStateControllerTest, ConfigureParamsSuccess) +{ + SetUpController( + {rclcpp::Parameter("state_interfaces", std::vector{"joint1/position"}), + rclcpp::Parameter( + "forward_state.joint1/position.to_command", std::vector{"joint2/position"})}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names[0], "joint2/position"); + + auto state_if_conf = controller_->state_interface_configuration(); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(state_if_conf.names[0], "joint1/position"); +} + +TEST_F(ForwardStateControllerTest, ActivateSuccess) +{ + SetUpController( + {rclcpp::Parameter("state_interfaces", std::vector{"joint1/position"}), + rclcpp::Parameter( + "forward_state.joint1/position.to_command", std::vector{"joint2/position"})}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(ForwardStateControllerTest, UpdateForwardsStateToCommand) +{ + SetUpController( + {rclcpp::Parameter("state_interfaces", std::vector{"joint1/position"}), + rclcpp::Parameter( + "forward_state.joint1/position.to_command", std::vector{"joint2/position"})}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // initial state: joint1 = 5.5, joint2 cmd = 0.0 + ASSERT_DOUBLE_EQ(state_position_value_, 5.5); + ASSERT_DOUBLE_EQ(cmd_position_value_1_, 0.0); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // joint2 cmd should now be forwarded from joint1 state + ASSERT_DOUBLE_EQ(cmd_position_value_1_, 5.5); +} + +TEST_F(ForwardStateControllerTest, UpdateForwardsOneStateToMultipleCommands) +{ + SetUpController( + {rclcpp::Parameter("state_interfaces", std::vector{"joint1/position"}), + rclcpp::Parameter( + "forward_state.joint1/position.to_command", + std::vector{"joint2/position", "joint3/position"})}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_DOUBLE_EQ(state_position_value_, 5.5); + ASSERT_DOUBLE_EQ(cmd_position_value_1_, 0.0); + ASSERT_DOUBLE_EQ(cmd_position_value_2_, 0.0); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // both command interfaces should receive the state value + ASSERT_DOUBLE_EQ(cmd_position_value_1_, 5.5); + ASSERT_DOUBLE_EQ(cmd_position_value_2_, 5.5); +} diff --git a/forward_state_controller/test/test_forward_state_controller.hpp b/forward_state_controller/test/test_forward_state_controller.hpp new file mode 100644 index 0000000000..6bd67cd440 --- /dev/null +++ b/forward_state_controller/test/test_forward_state_controller.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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_STATE_CONTROLLER_HPP_ +#define TEST_FORWARD_STATE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "forward_state_controller/forward_state_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; +using hardware_interface::HW_IF_POSITION; + +// subclassing and friending so we can access member variables +class FriendForwardStateController : public forward_state_controller::ForwardStateController +{ + FRIEND_TEST(ForwardStateControllerTest, StateInterfacesParameterNotSet); + FRIEND_TEST(ForwardStateControllerTest, ForwardStateMappingMissing); + FRIEND_TEST(ForwardStateControllerTest, ConfigureParamsSuccess); + FRIEND_TEST(ForwardStateControllerTest, ActivateSuccess); + FRIEND_TEST(ForwardStateControllerTest, UpdateForwardsStateToCommand); + FRIEND_TEST(ForwardStateControllerTest, UpdateForwardsOneStateToMultipleCommands); +}; + +class ForwardStateControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(const std::vector & parameters = {}); + +protected: + std::unique_ptr controller_; + + const std::string state_joint_name_ = "joint1"; + const std::string cmd_joint_name_1_ = "joint2"; + const std::string cmd_joint_name_2_ = "joint3"; + + double state_position_value_ = 5.5; + double cmd_position_value_1_ = 0.0; + double cmd_position_value_2_ = 0.0; + + StateInterface::SharedPtr joint1_pos_state_ = std::make_shared( + state_joint_name_, HW_IF_POSITION, &state_position_value_); + + CommandInterface::SharedPtr joint2_pos_cmd_ = + std::make_shared(cmd_joint_name_1_, HW_IF_POSITION, &cmd_position_value_1_); + CommandInterface::SharedPtr joint3_pos_cmd_ = + std::make_shared(cmd_joint_name_2_, HW_IF_POSITION, &cmd_position_value_2_); +}; + +#endif // TEST_FORWARD_STATE_CONTROLLER_HPP_ diff --git a/forward_state_controller/test/test_load_forward_state_controller.cpp b/forward_state_controller/test/test_load_forward_state_controller.cpp new file mode 100644 index 0000000000..98b542ecbd --- /dev/null +++ b/forward_state_controller/test/test_load_forward_state_controller.cpp @@ -0,0 +1,45 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadForwardStateController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_forward_state_controller.yaml"; + + cm.set_parameter({"test_forward_state_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_forward_state_controller.type", + "forward_state_controller/ForwardStateController"}); + + ASSERT_NE(cm.load_controller("test_forward_state_controller"), nullptr); + + rclcpp::shutdown(); +}