diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index 6b49e65..8e41c50 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/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"); } } diff --git a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp index 8b7ab4e..eb4782d 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp +++ b/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 diff --git a/vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py b/vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py new file mode 100644 index 0000000..bdcae15 --- /dev/null +++ b/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) diff --git a/vortex_utils_ros/vortex_utils_ros/qos_profiles.py b/vortex_utils_ros/vortex_utils_ros/qos_profiles.py index f3c4f8c..24d38a8 100644 --- a/vortex_utils_ros/vortex_utils_ros/qos_profiles.py +++ b/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, + )