waypoint types, utils and ros conversions

vortex_utils/CMakeLists.txt

@@ -7,14 +7,20 @@ add_compile_options(-Wall -Wextra -Wpedantic)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
-  src/math.cpp)
+  src/math.cpp
+  src/waypoint_utils.cpp)
 
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  yaml-cpp
+)
+
 ament_target_dependencies(${PROJECT_NAME} PUBLIC
   Eigen3
 )

vortex_utils/include/vortex/utils/types.hpp

@@ -436,6 +436,27 @@ inline std::string mode_to_string(Mode mode) {
     }
 }
 
+/**
+ * @brief Determines which degrees of freedom the reference filter controls.
+ *
+ * The mode affects both the reference goal computation (via
+ * compute_waypoint_goal) and the convergence check (via has_converged).
+ */
+enum class WaypointMode : uint8_t {
+    FULL_POSE = 0,         ///< Control all 6 DOF.
+    ONLY_POSITION = 1,     ///< Control x, y, z; hold current orientation.
+    FORWARD_HEADING = 2,   ///< Control x, y, z with yaw toward target.
+    ONLY_ORIENTATION = 3,  ///< Control roll, pitch, yaw; hold current position.
+};
+
+/**
+ * @brief A target pose with an associated waypoint mode.
+ */
+struct Waypoint {
+    Pose pose{};
+    WaypointMode mode = WaypointMode::FULL_POSE;
+};
+
 struct SonarInfo {
     double meters_per_pixel_x{};
     double meters_per_pixel_y{};

vortex_utils/include/vortex/utils/waypoint_utils.hpp

@@ -0,0 +1,83 @@
+#ifndef VORTEX_UTILS_WAYPOINT_UTILS_HPP
+#define VORTEX_UTILS_WAYPOINT_UTILS_HPP
+
+#include <string>
+#include <vortex/utils/types.hpp>
+
+namespace vortex::utils::waypoints {
+
+using vortex::utils::types::Pose;
+using vortex::utils::types::WaypointMode;
+
+/**
+ * @brief Compute the waypoint goal by applying the mode logic to the incoming
+ * waypoint.
+ *
+ * For modes that don't control all DOFs, the uncontrolled components are
+ * replaced with values from @p current_state.
+ *
+ * @param incoming_waypoint The incoming waypoint to compute goal from.
+ * @param mode The waypoint mode.
+ * @param current_state The current state pose.
+ * @return The adjusted waypoint goal.
+ */
+Pose compute_waypoint_goal(const Pose& incoming_waypoint,
+                           WaypointMode mode,
+                           const Pose& current_state);
+
+/**
+ * @brief Check whether the state has converged to the waypoint goal.
+ *
+ * Only the DOFs relevant to the waypoint mode are included in the error norm.
+ *
+ * @param state The current state pose.
+ * @param waypoint_goal The waypoint goal pose.
+ * @param mode The waypoint mode.
+ * @param convergence_threshold The maximum allowed error norm.
+ * @return True if the error is below the threshold.
+ */
+bool has_converged(const Pose& state,
+                   const Pose& waypoint_goal,
+                   WaypointMode mode,
+                   double convergence_threshold);
+
+/**
+ * @brief Apply a pose offset to a base pose.
+ *
+ * Position: p_target = p_base + p_offset
+ * Orientation: q_target = (q_base * q_offset).normalized()
+ *
+ * @param base The base pose (e.g. a landmark pose).
+ * @param offset The offset to apply.
+ * @return The resulting target pose.
+ */
+Pose apply_pose_offset(const Pose& base, const Pose& offset);
+
+/**
+ * @brief Load a waypoint from a YAML file by identifier.
+ *
+ * Expected YAML format:
+ * @code
+ * waypoint_name:
+ *   position:
+ *     x: 1.0
+ *     y: 0.0
+ *     z: -0.5
+ *   orientation:
+ *     roll: 0.0
+ *     pitch: 0.0
+ *     yaw: 3.14159
+ * @endcode
+ *
+ * @param file_path Path to the YAML file.
+ * @param identifier The waypoint key to look up.
+ * @return Pose with position and orientation (RPY converted to quaternion).
+ * @throws std::runtime_error if the file cannot be opened or the identifier is
+ * not found.
+ */
+Pose load_waypoint_from_yaml(const std::string& file_path,
+                             const std::string& identifier);
+
+}  // namespace vortex::utils::waypoints
+
+#endif  // VORTEX_UTILS_WAYPOINT_UTILS_HPP

vortex_utils/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>eigen</depend>
+  <depend>yaml-cpp</depend>
   <depend>python3-numpy</depend>
   <depend>python3-scipy</depend>
 

vortex_utils/src/waypoint_utils.cpp

@@ -0,0 +1,110 @@
+#include "vortex/utils/waypoint_utils.hpp"
+
+#include <yaml-cpp/yaml.h>
+#include <cmath>
+#include <vortex/utils/math.hpp>
+
+namespace vortex::utils::waypoints {
+
+Pose compute_waypoint_goal(const Pose& incoming_waypoint,
+                           WaypointMode mode,
+                           const Pose& current_state) {
+    Pose waypoint_out = incoming_waypoint;
+
+    switch (mode) {
+        case WaypointMode::FULL_POSE:
+            break;
+
+        case WaypointMode::ONLY_POSITION:
+            waypoint_out.set_ori(current_state.ori_quaternion());
+            break;
+
+        case WaypointMode::FORWARD_HEADING: {
+            double dx = incoming_waypoint.x - current_state.x;
+            double dy = incoming_waypoint.y - current_state.y;
+            double forward_heading = std::atan2(dy, dx);
+
+            waypoint_out.set_ori(Eigen::Quaterniond(
+                Eigen::AngleAxisd(forward_heading, Eigen::Vector3d::UnitZ())));
+            break;
+        }
+
+        case WaypointMode::ONLY_ORIENTATION:
+            waypoint_out.set_pos(current_state.pos_vector());
+            break;
+    }
+
+    return waypoint_out;
+}
+
+bool has_converged(const Pose& state,
+                   const Pose& waypoint_goal,
+                   WaypointMode mode,
+                   double convergence_threshold) {
+    const Eigen::Vector3d ep = state.pos_vector() - waypoint_goal.pos_vector();
+
+    const Eigen::Vector3d ea = vortex::utils::math::quaternion_error(
+        state.ori_quaternion(), waypoint_goal.ori_quaternion());
+
+    const double err = [&] {
+        switch (mode) {
+            case WaypointMode::ONLY_POSITION:
+                return ep.norm();
+            case WaypointMode::ONLY_ORIENTATION:
+                return ea.norm();
+            case WaypointMode::FORWARD_HEADING:
+                return std::sqrt(ep.squaredNorm() + ea(2) * ea(2));
+            case WaypointMode::FULL_POSE:
+            default:
+                return std::sqrt(ep.squaredNorm() + ea.squaredNorm());
+        }
+    }();
+
+    return err < convergence_threshold;
+}
+
+Pose apply_pose_offset(const Pose& base, const Pose& offset) {
+    const Eigen::Vector3d p_base = base.pos_vector();
+    const Eigen::Quaterniond q_base = base.ori_quaternion().normalized();
+
+    const Eigen::Vector3d p_offset = offset.pos_vector();
+    const Eigen::Quaterniond q_offset = offset.ori_quaternion().normalized();
+
+    const Eigen::Vector3d p_target = p_base + p_offset;
+    const Eigen::Quaterniond q_target = (q_base * q_offset).normalized();
+
+    return Pose::from_eigen(p_target, q_target);
+}
+
+Pose load_waypoint_from_yaml(const std::string& file_path,
+                             const std::string& identifier) {
+    YAML::Node root = YAML::LoadFile(file_path);
+
+    if (!root[identifier]) {
+        throw std::runtime_error("Waypoint '" + identifier + "' not found in " +
+                                 file_path);
+    }
+
+    const auto& wp = root[identifier];
+
+    const double x = wp["position"]["x"].as<double>();
+    const double y = wp["position"]["y"].as<double>();
+    const double z = wp["position"]["z"].as<double>();
+
+    const double roll = wp["orientation"]["roll"].as<double>();
+    const double pitch = wp["orientation"]["pitch"].as<double>();
+    const double yaw = wp["orientation"]["yaw"].as<double>();
+
+    const Eigen::Quaterniond q =
+        vortex::utils::math::euler_to_quat(roll, pitch, yaw);
+
+    return Pose{.x = x,
+                .y = y,
+                .z = z,
+                .qw = q.w(),
+                .qx = q.x(),
+                .qy = q.y(),
+                .qz = q.z()};
+}
+
+}  // namespace vortex::utils::waypoints

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

@@ -0,0 +1,47 @@
+#ifndef VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_
+#define VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <vortex/utils/ros/ros_conversions.hpp>
+#include <vortex/utils/types.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
+#include <vortex_msgs/msg/waypoint.hpp>
+#include <vortex_msgs/msg/waypoint_mode.hpp>
+
+namespace vortex::utils::waypoints {
+
+/**
+ * @brief Convert a ROS waypoint mode to a WaypointMode enum.
+ * @throws std::invalid_argument if the mode value is not recognized.
+ */
+inline WaypointMode waypoint_mode_from_ros(
+    const vortex_msgs::msg::WaypointMode& mode_msg) {
+    switch (mode_msg.mode) {
+        case vortex_msgs::msg::WaypointMode::FULL_POSE:
+            return WaypointMode::FULL_POSE;
+        case vortex_msgs::msg::WaypointMode::ONLY_POSITION:
+            return WaypointMode::ONLY_POSITION;
+        case vortex_msgs::msg::WaypointMode::FORWARD_HEADING:
+            return WaypointMode::FORWARD_HEADING;
+        case vortex_msgs::msg::WaypointMode::ONLY_ORIENTATION:
+            return WaypointMode::ONLY_ORIENTATION;
+        default:
+            throw std::invalid_argument("Invalid ROS waypoint mode: " +
+                                        std::to_string(mode_msg.mode));
+    }
+}
+
+/**
+ * @brief Convert a ROS Waypoint message to an internal Waypoint struct.
+ */
+inline vortex::utils::types::Waypoint waypoint_from_ros(
+    const vortex_msgs::msg::Waypoint& ros_wp) {
+    vortex::utils::types::Waypoint wp;
+    wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose(ros_wp.pose);
+    wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode);
+    return wp;
+}
+
+}  // namespace vortex::utils::waypoints
+
+#endif  // VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_