Feat mode types

vortex_utils/include/vortex/utils/types.hpp

@@ -403,7 +403,7 @@ struct LineSegment2D {
 /**
  * @brief Enum class for operation modes.
  */
-enum class Mode : uint8_t { manual, autonomous, reference };
+enum class Mode : int { autonomous, manual, reference };
 
 /**
  * @brief Convert Mode enum to string for logging or display purposes.
@@ -419,7 +419,8 @@ inline std::string mode_to_string(Mode mode) {
         case Mode::reference:
             return "reference mode";
         default:
-            throw std::runtime_error("Invalid operation mode.");
+            throw std::runtime_error(
+                "String conversion failed, invalid mode value");
     }
 }
 

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

@@ -193,7 +193,32 @@ inline vortex::utils::types::Mode convert_from_ros(
         case vortex_msgs::msg::OperationMode::REFERENCE:
             return vortex::utils::types::Mode::reference;
     }
-    throw std::runtime_error("Invalid operation mode.");
+    throw std::runtime_error("Conversion failed, invalid operation mode value");
+}
+
+/**
+ * @brief Converts an internal Mode enum to a ROS
+ * vortex_msgs::msg::OperationMode.
+ * @param mode vortex::utils::types::Mode
+ * @return vortex_msgs::msg::OperationMode ROS mode message
+ */
+inline vortex_msgs::msg::OperationMode convert_to_ros(
+    const vortex::utils::types::Mode& mode) {
+    vortex_msgs::msg::OperationMode mode_msg;
+    switch (mode) {
+        case vortex::utils::types::Mode::manual:
+            mode_msg.operation_mode = vortex_msgs::msg::OperationMode::MANUAL;
+            return mode_msg;
+        case vortex::utils::types::Mode::autonomous:
+            mode_msg.operation_mode =
+                vortex_msgs::msg::OperationMode::AUTONOMOUS;
+            return mode_msg;
+        case vortex::utils::types::Mode::reference:
+            mode_msg.operation_mode =
+                vortex_msgs::msg::OperationMode::REFERENCE;
+            return mode_msg;
+    }
+    throw std::runtime_error("Conversion failed, invalid operation mode value");
 }
 
 }  // namespace vortex::utils::ros_conversions

vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py

@@ -0,0 +1,13 @@
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+def add_heartbeat_publisher(node: Node, node_name: str):
+    publisher = node.create_publisher(String, "heartbeat", 10)
+
+    def publish_heartbeat():
+        msg = String()
+        msg.data = node_name
+        publisher.publish(msg)
+
+    node.create_timer(1.0, publish_heartbeat)

vortex_utils_ros/vortex_utils_ros/qos_profiles.py

@@ -15,3 +15,12 @@ def reliable_profile(depth: int = 10) -> qos.QoSProfile:
         depth=depth,
         reliability=qos.ReliabilityPolicy.RELIABLE,
     )
+
+
+def reliable_transient_local_profile(depth: int = 1) -> qos.QoSProfile:
+    return qos.QoSProfile(
+        history=qos.HistoryPolicy.KEEP_LAST,
+        depth=depth,
+        reliability=qos.ReliabilityPolicy.RELIABLE,
+        durability=qos.DurabilityPolicy.TRANSIENT_LOCAL,
+    )