diff --git a/vortex_utils/CMakeLists.txt b/vortex_utils/CMakeLists.txt index 62423ce..01564ab 100644 --- a/vortex_utils/CMakeLists.txt +++ b/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 $ $) +target_link_libraries(${PROJECT_NAME} PUBLIC + yaml-cpp +) + ament_target_dependencies(${PROJECT_NAME} PUBLIC Eigen3 ) diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index 40f47ee..c8eaf09 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/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{}; diff --git a/vortex_utils/include/vortex/utils/waypoint_utils.hpp b/vortex_utils/include/vortex/utils/waypoint_utils.hpp new file mode 100644 index 0000000..0669e5b --- /dev/null +++ b/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 +#include + +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 diff --git a/vortex_utils/package.xml b/vortex_utils/package.xml index 22d8452..1944995 100644 --- a/vortex_utils/package.xml +++ b/vortex_utils/package.xml @@ -11,6 +11,7 @@ ament_cmake_python eigen + yaml-cpp python3-numpy python3-scipy diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp new file mode 100644 index 0000000..6394ef7 --- /dev/null +++ b/vortex_utils/src/waypoint_utils.cpp @@ -0,0 +1,110 @@ +#include "vortex/utils/waypoint_utils.hpp" + +#include +#include +#include + +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(); + const double y = wp["position"]["y"].as(); + const double z = wp["position"]["z"].as(); + + const double roll = wp["orientation"]["roll"].as(); + const double pitch = wp["orientation"]["pitch"].as(); + const double yaw = wp["orientation"]["yaw"].as(); + + 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 diff --git a/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp new file mode 100644 index 0000000..8381099 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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_