diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 6f30c43b64..f370fcfb3b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -18,7 +18,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle realtime_tools rsl - tl_expected trajectory_msgs urdf ) @@ -37,7 +36,6 @@ generate_parameter_library(joint_trajectory_controller_parameters add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp - src/validate_jtc_parameters.cpp ) target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) if(WIN32) @@ -58,7 +56,6 @@ target_link_libraries(joint_trajectory_controller PUBLIC rclcpp_lifecycle::rclcpp_lifecycle realtime_tools::realtime_tools rsl::rsl - tl_expected::tl_expected urdf::urdf angles::angles ${trajectory_msgs_TARGETS} diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index f9dfb7543b..09e8fc7dde 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -20,30 +20,83 @@ #include "rclcpp/parameter.hpp" #include "rsl/algorithm.hpp" -#include "tl_expected/expected.hpp" +#include "tl/expected.hpp" namespace joint_trajectory_controller { - -/** - * \brief Validate command interface type combinations for joint trajectory controller. - * - * \param[in] parameter The rclcpp parameter containing the command interface types. - * \return tl::expected An empty expected on success, or an error message on - * failure. - */ tl::expected command_interface_type_combinations( - rclcpp::Parameter const & parameter); - -/** - * \brief Validate state interface type combinations for joint trajectory controller. - * - * \param[in] parameter The rclcpp parameter containing the state interface types. - * \return tl::expected An empty expected on success, or an error message on - * failure. - */ + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 3. position [velocity, [acceleration]] + // 4. position, effort + + if ( + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' command interface can be used either alone or 'position' " + "command interface has to be present"); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) + { + return tl::make_unexpected( + "'acceleration' command interface can only be used if 'velocity' and " + "'position' command interfaces are present"); + } + + if ( + rsl::contains>(interface_types, "effort") && + !(interface_types.size() == 1 || + (interface_types.size() == 2 && + rsl::contains>(interface_types, "position")))) + { + return tl::make_unexpected( + "'effort' command interface has to be used alone or with a 'position' interface"); + } + + return {}; +} + tl::expected state_interface_type_combinations( - rclcpp::Parameter const & parameter); + rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Valid combinations are + // 1. position [velocity, [acceleration]] + + if ( + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) + { + return tl::make_unexpected( + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + } + + if ( + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) + { + return tl::make_unexpected( + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + } + + return {}; +} } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index e352001307..70b934703c 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -26,13 +26,13 @@ control_toolbox generate_parameter_library hardware_interface + libexpected-dev pluginlib rclcpp rclcpp_action rclcpp_lifecycle realtime_tools rsl - tl_expected trajectory_msgs urdf diff --git a/joint_trajectory_controller/src/validate_jtc_parameters.cpp b/joint_trajectory_controller/src/validate_jtc_parameters.cpp deleted file mode 100644 index 1a186ffc6a..0000000000 --- a/joint_trajectory_controller/src/validate_jtc_parameters.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) 2022 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "joint_trajectory_controller/validate_jtc_parameters.hpp" - -namespace joint_trajectory_controller -{ - -tl::expected command_interface_type_combinations( - rclcpp::Parameter const & parameter) -{ - auto const & interface_types = parameter.as_string_array(); - - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 3. position [velocity, [acceleration]] - // 4. position, effort - - if ( - rsl::contains>(interface_types, "velocity") && - interface_types.size() > 1 && - !rsl::contains>(interface_types, "position")) - { - return tl::make_unexpected( - "'velocity' command interface can be used either alone or 'position' " - "command interface has to be present"); - } - - if ( - rsl::contains>(interface_types, "acceleration") && - (!rsl::contains>(interface_types, "velocity") && - !rsl::contains>(interface_types, "position"))) - { - return tl::make_unexpected( - "'acceleration' command interface can only be used if 'velocity' and " - "'position' command interfaces are present"); - } - - if ( - rsl::contains>(interface_types, "effort") && - !(interface_types.size() == 1 || - (interface_types.size() == 2 && - rsl::contains>(interface_types, "position")))) - { - return tl::make_unexpected( - "'effort' command interface has to be used alone or with a 'position' interface"); - } - - return {}; -} - -tl::expected state_interface_type_combinations( - rclcpp::Parameter const & parameter) -{ - auto const & interface_types = parameter.as_string_array(); - - // Valid combinations are - // 1. position [velocity, [acceleration]] - - if ( - rsl::contains>(interface_types, "velocity") && - !rsl::contains>(interface_types, "position")) - { - return tl::make_unexpected( - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - } - - if ( - rsl::contains>(interface_types, "acceleration") && - (!rsl::contains>(interface_types, "position") || - !rsl::contains>(interface_types, "velocity"))) - { - return tl::make_unexpected( - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - } - - return {}; -} - -} // namespace joint_trajectory_controller