Refactor interpolation_method class

joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp

@@ -18,47 +18,134 @@
 #include <string>
 #include <unordered_map>
 
+#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<InterpolationMethod, std::string> 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<std::string, InterpolationMethod> 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
 

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();