Skip to content

Commit dcc31af

Browse files
committed
Addition: Added to_string function.
1 parent 1d9fd74 commit dcc31af

File tree

1 file changed

+33
-6
lines changed

1 file changed

+33
-6
lines changed

joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp

Lines changed: 33 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -28,14 +28,16 @@ namespace
2828
{
2929

3030
/// Converts a string to lowercase for case-agnostic checking.
31-
void convert_to_lowercase(std::string& str)
31+
inline std::string convert_to_lowercase(const std::string& str)
3232
{
33-
for (char& c : str)
33+
std::string s = str;
34+
for (char& c : s)
3435
{
3536
// C++ std requires the argument passed to std::tolower must be representable as
3637
// unsigned char or equal to EOF.
3738
c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
3839
}
40+
return s;
3941
}
4042

4143
} // namespace
@@ -94,17 +96,20 @@ const std::unordered_map<std::string, InterpolationMethod> InterpolationMethodMa
9496
* This function looks up `InterpolationMethodMap` for corresponding `InterpolationMethod` based
9597
* on interpolation_method string.
9698
*
97-
* \param[in] `interpolation_method` The given interpolation method `string`.
99+
* \param[in] interpolation_method The given interpolation method string.
98100
*
99101
* \returns The corresponding InterpolationMethod.
100102
*
101-
* \note If interpolation_method do not have any corresponding InterpolationMethod (i.e., Unknown),
103+
* \note If interpolation_method does not have any corresponding InterpolationMethod (i.e., "Unknown"),
102104
* It defaults to `InterpolationMethod::VARIABLE_DEGREE_SPLINE`.
103105
*/
104-
[[nodiscard]] inline InterpolationMethod from_string(const std::string & interpolation_method)
106+
[[nodiscard]] inline InterpolationMethod from_string(const std::string& interpolation_method)
105107
{
108+
// Convert to lowercase, so we have an case-agnostic checking. (i.e., None and none, etc are treated same.)
109+
std::string method = convert_to_lowercase(interpolation_method);
110+
106111
// Iterator to InterpolationMethodMap
107-
const auto iterator = InterpolationMethodMap.find(interpolation_method);
112+
const auto iterator = InterpolationMethodMap.find(method);
108113

109114
// If interpolation_method exists
110115
if (iterator != InterpolationMethodMap.end())
@@ -124,6 +129,28 @@ const std::unordered_map<std::string, InterpolationMethod> InterpolationMethodMa
124129
}
125130
}
126131

132+
/**
133+
* \brief Returns corresponding string value for the `InterpolationMethod`.
134+
* This function uses simple switch-case lookup to directly assign string value to
135+
* `InterpolationMethod`.
136+
*
137+
* \param[in] interpolation_method The InterpolationMethod enum value.
138+
*
139+
* \returns The corresponding string.
140+
*
141+
* \note Defaults to return "UNKNOWN".
142+
*/
143+
[[nodiscard]] inline std::string to_string(const InterpolationMethod& interpolation_method)
144+
{
145+
switch(interpolation_method)
146+
{
147+
case InterpolationMethod::NONE: return "none";
148+
case InterpolationMethod::VARIABLE_DEGREE_SPLINE: return "splines";
149+
// Default
150+
default: return "UNKNOWN";
151+
}
152+
}
153+
127154
} // namespace interpolation_methods
128155
} // namespace joint_trajectory_controller
129156

0 commit comments

Comments
 (0)