diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index f357a312c5..f4bfb4f592 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -18,47 +18,134 @@ #include #include +#include "hardware_interface/lexical_casts.hpp" #include "rclcpp/rclcpp.hpp" namespace joint_trajectory_controller { + +/// \brief Setup interpolation_methods' rclcpp::Logger instance. static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_trajectory_controller.interpolation_methods"); namespace interpolation_methods { + +/** + * \brief Defines the available interpolation methods used for fitting data curves. + * This enumeration specifies how intermediate values between data points + * should be calculated. + */ enum class InterpolationMethod { + /** + * \brief No interpolation is performed. + * This is typically used when data points are discrete and should not be + * connected by a curve. + * + * It returns the initial point until the time for the first trajectory data + * point is reached. Then, it simply takes the next given datapoint. + */ NONE, + + /** + * \brief Uses a variable-degree spline interpolation. + * The degree of the spline is determined dynamically based on the number of + * available derivatives. This provides a smooth, continuous curve between data points. + * + * Based on available derivatives, it uses the following degree interpolation, + * 1. If only position is available: `Linear Interpolation`. + * 2. If position, and velocity are available: `Cubic Spline Interpolation`. + * 3. If position, velocity, and acceleration are available: `Quintic Spline Interpolation`. + */ VARIABLE_DEGREE_SPLINE }; +/** + * \brief The default interpolation method is set to `InterpolationMethod::VARIABLE_DEGREE_SPLINE`. + * As it provides the most realistic, jerk-free and smooth motion. + */ const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE; -const std::unordered_map InterpolationMethodMap( - {{InterpolationMethod::NONE, "none"}, {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"}}); +/** + * @brief Maps string representations to their corresponding InterpolationMethod enum values. + * + * @deprecated This map is deprecated. Use the direct lookup methods instead. + * (Original use: Converting strings into the InterpolationMethod). + */ +[[deprecated( + "InterpolationMethodMap will be removed in future releases. " + "Instead, use the direct lookup methods.")]] +const std::unordered_map InterpolationMethodMap( + {{"none", InterpolationMethod::NONE}, {"splines", InterpolationMethod::VARIABLE_DEGREE_SPLINE}}); +/** + * \brief Returns corresponding string value for the `InterpolationMethod`. + * This function uses simple switch-case lookup to directly assign string value to + * `InterpolationMethod`. + * + * \param[in] interpolation_method The InterpolationMethod enum value. + * + * \returns The corresponding string. + * + * \note Defaults to return "UNKNOWN". + */ +[[nodiscard]] inline std::string to_string(const InterpolationMethod & interpolation_method) +{ + switch (interpolation_method) + { + case InterpolationMethod::NONE: + return "none"; + case InterpolationMethod::VARIABLE_DEGREE_SPLINE: + return "splines"; + // Default + default: + RCLCPP_WARN( + LOGGER, + "Unknown interpolation method enum value was given. Returning default: " + "UNKNOWN"); + return "UNKNOWN"; + } +} + +/** + * \brief Creates a `InterpolationMethod` enum class value from a string. + * This function directly looks up for corresponding `InterpolationMethod` based + * on interpolation_method string (case-agnostic). + * + * \param[in] interpolation_method The given interpolation method string. + * + * \returns The corresponding InterpolationMethod. + * + * \note If interpolation_method does not have any corresponding InterpolationMethod + * (i.e., "Unknown"), it defaults to `DEFAULT_INTERPOLATION`. + */ [[nodiscard]] inline InterpolationMethod from_string(const std::string & interpolation_method) { - if (interpolation_method.compare(InterpolationMethodMap.at(InterpolationMethod::NONE)) == 0) + // Convert to lowercase, so we have a case-agnostic checking, + // (i.e., "None, NONE, and none" are treated same.) + std::string method = ::hardware_interface::to_lower_case(interpolation_method); + + if (method == "none") { return InterpolationMethod::NONE; } - else if ( - interpolation_method.compare( - InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) + else if (method == "splines") { return InterpolationMethod::VARIABLE_DEGREE_SPLINE; } // Default else { - RCLCPP_INFO( + RCLCPP_WARN( LOGGER, - "No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE."); - return InterpolationMethod::VARIABLE_DEGREE_SPLINE; + "Unknown interpolation method parameter '%s' was given. Using the default: " + "DEFAULT_INTERPOLATION (%s).", + interpolation_method.c_str(), to_string(DEFAULT_INTERPOLATION).c_str()); + return DEFAULT_INTERPOLATION; } } + } // namespace interpolation_methods } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 83d8c05489..765f80fc73 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -940,9 +940,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); - RCLCPP_INFO( - logger, "Using '%s' interpolation method.", - interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + RCLCPP_INFO(logger, "Using '%s' interpolation method.", interpolation_string.c_str()); // prepare hold_position_msg init_hold_position_msg();