Added mode conversion and types

vortex_utils/include/vortex/utils/types.hpp

@@ -395,6 +395,29 @@ struct LineSegment2D {
     }
 };
 
+/**
+ * @brief Enum class for operation modes.
+ */
+enum class Mode : uint8_t { manual, autonomous, reference };
+
+/**
+ * @brief Convert Mode enum to string for logging or display purposes.
+ * @param mode Mode enum value
+ * @return std::string representation of the mode
+ */
+inline std::string mode_to_string(Mode mode) {
+    switch (mode) {
+        case Mode::manual:
+            return "manual mode";
+        case Mode::autonomous:
+            return "autonomous mode";
+        case Mode::reference:
+            return "reference mode";
+        default:
+            throw std::runtime_error("Invalid operation mode.");
+    }
+}
+
 }  // namespace vortex::utils::types
 
 #endif  // VORTEX_UTILS_TYPES_HPP

vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp

@@ -14,6 +14,7 @@
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <vortex_msgs/msg/landmark_array.hpp>
+#include <vortex_msgs/msg/operation_mode.hpp>
 
 #include <vortex/utils/concepts.hpp>
 #include <vortex/utils/math.hpp>
@@ -176,6 +177,25 @@ inline std::vector<vortex::utils::types::Pose> ros_to_pose_vec(
     return poses;
 }
 
+/**
+ * @brief Converts a ROS vortex_msgs::msg::OperationMode to an internal Mode
+ * enum.
+ * @param mode_msg vortex_msgs::msg::OperationMode
+ * @return vortex::utils::types::Mode Internal mode representation
+ */
+inline vortex::utils::types::Mode convert_from_ros(
+    const vortex_msgs::msg::OperationMode& mode_msg) {
+    switch (mode_msg.operation_mode) {
+        case vortex_msgs::msg::OperationMode::MANUAL:
+            return vortex::utils::types::Mode::manual;
+        case vortex_msgs::msg::OperationMode::AUTONOMOUS:
+            return vortex::utils::types::Mode::autonomous;
+        case vortex_msgs::msg::OperationMode::REFERENCE:
+            return vortex::utils::types::Mode::reference;
+    }
+    throw std::runtime_error("Invalid operation mode.");
+}
+
 }  // namespace vortex::utils::ros_conversions
 
 #endif  // VORTEX_UTILS__ROS_CONVERSIONS_HPP_