From 77fd3b36b91d543c362d9d8bdc986419b40efb78 Mon Sep 17 00:00:00 2001 From: eirisak Date: Wed, 11 Feb 2026 16:41:03 +0100 Subject: [PATCH 1/4] Added mode conversion and types --- .gitignore | 1 + .vscode/settings.json | 23 +++++++++++++++++++ vortex_utils/include/vortex/utils/types.hpp | 23 +++++++++++++++++++ .../vortex/utils/ros/ros_conversions.hpp | 20 ++++++++++++++++ 4 files changed, 67 insertions(+) create mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 5d05913..20c176c 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ __pycache__/ *.py[cod] *$py.class +vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py # C extensions *.so diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..8e0ee79 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,23 @@ +{ + "ROS2.distro": "humble", + "python.autoComplete.extraPaths": [ + "/home/eirisak/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/keyboard_joy/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/eirisak/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/keyboard_joy/local/lib/python3.10/dist-packages", + "/home/eirisak/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index 7cbbd99..bc8c158 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/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 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 1c57074..b897161 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -176,6 +177,25 @@ inline std::vector 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_ From da797bbd4012d771ba5a750cc2643ec45ffaaaa3 Mon Sep 17 00:00:00 2001 From: eirisak Date: Wed, 11 Feb 2026 16:52:59 +0100 Subject: [PATCH 2/4] Removed .vscode and changed .gitignore --- .gitignore | 1 - .vscode/settings.json | 23 ------------------- .../vortex/utils/ros/ros_conversions.hpp | 4 ++-- 3 files changed, 2 insertions(+), 26 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 20c176c..5d05913 100644 --- a/.gitignore +++ b/.gitignore @@ -13,7 +13,6 @@ __pycache__/ *.py[cod] *$py.class -vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py # C extensions *.so diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 8e0ee79..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "ROS2.distro": "humble", - "python.autoComplete.extraPaths": [ - "/home/eirisak/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/keyboard_joy/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "python.analysis.extraPaths": [ - "/home/eirisak/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/keyboard_joy/local/lib/python3.10/dist-packages", - "/home/eirisak/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ] -} \ No newline at end of file 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 b897161..8b7ab4e 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp @@ -177,14 +177,14 @@ inline std::vector 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) { +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; From d23d6b977c2081fed8603425c704c0c5bb44ffd7 Mon Sep 17 00:00:00 2001 From: eirisak Date: Wed, 11 Feb 2026 19:15:51 +0100 Subject: [PATCH 3/4] Added transient local profile to qos_profiles.py --- vortex_utils_ros/vortex_utils_ros/qos_profiles.py | 9 +++++++++ 1 file changed, 9 insertions(+) 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, + ) From 2675aca148b6c66f12d0f289649868887903ffa6 Mon Sep 17 00:00:00 2001 From: eirisak Date: Thu, 12 Feb 2026 16:52:28 +0100 Subject: [PATCH 4/4] Added conversion to ros for operation mode --- vortex_utils/include/vortex/utils/types.hpp | 5 ++-- .../vortex/utils/ros/ros_conversions.hpp | 27 ++++++++++++++++++- .../vortex_utils_ros/heartbeatpublisher.py | 13 +++++++++ 3 files changed, 42 insertions(+), 3 deletions(-) create mode 100644 vortex_utils_ros/vortex_utils_ros/heartbeatpublisher.py 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)