From 4828acd573d547876fb5b1f23c011fa3382fd773 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Fri, 15 May 2026 01:29:35 +0200 Subject: [PATCH] waypoint xy forward dir mode --- vortex_utils/include/vortex/utils/types.hpp | 3 ++- vortex_utils/src/waypoint_utils.cpp | 19 ++++++++++++++++++- .../utils/ros/waypoint_ros_conversions.hpp | 5 +++++ 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index 92ed4dc..e2f6734 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -448,7 +448,8 @@ enum class WaypointMode : uint8_t { FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. POSITION_AND_YAW = 4, ///< Control x, y, z and yaw; force roll=pitch=0. - XY_AND_YAW = 5, ///< Control x, y and yaw; hold z, force roll=pitch=0. + XY_AND_YAW = 5, ///< Control x, y and yaw; hold z, force roll=pitch=0. + XY_FORWARD_DIR = 6, ///< Control x, y; hold z; yaw auto-computed toward target. }; /** diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp index 73cc999..0a266c3 100644 --- a/vortex_utils/src/waypoint_utils.cpp +++ b/vortex_utils/src/waypoint_utils.cpp @@ -19,6 +19,8 @@ WaypointMode string_to_waypoint_mode(const std::string& str) { return WaypointMode::POSITION_AND_YAW; if (str == "XY_AND_YAW" || str == "xy_and_yaw") return WaypointMode::XY_AND_YAW; + if (str == "XY_FORWARD_DIR" || str == "xy_forward_dir") + return WaypointMode::XY_FORWARD_DIR; throw std::runtime_error("Unknown WaypointMode string: '" + str + "'"); } @@ -38,6 +40,8 @@ WaypointMode int_to_waypoint_mode(int value) { return WaypointMode::POSITION_AND_YAW; case static_cast(WaypointMode::XY_AND_YAW): return WaypointMode::XY_AND_YAW; + case static_cast(WaypointMode::XY_FORWARD_DIR): + return WaypointMode::XY_FORWARD_DIR; default: throw std::runtime_error("Unknown WaypointMode numeric value: " + std::to_string(value)); @@ -63,7 +67,8 @@ Pose load_pose_for_mode(const YAML::Node& node, WaypointMode mode) { // mode == WaypointMode::ONLY_POSITION || mode == WaypointMode::FORWARD_HEADING || mode == WaypointMode::POSITION_AND_YAW || - mode == WaypointMode::XY_AND_YAW); + mode == WaypointMode::XY_AND_YAW || + mode == WaypointMode::XY_FORWARD_DIR); const bool needs_orientation = (mode == WaypointMode::FULL_POSE || mode == WaypointMode::ONLY_ORIENTATION || mode == WaypointMode::POSITION_AND_YAW || @@ -152,6 +157,16 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint, Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()))); break; } + + case WaypointMode::XY_FORWARD_DIR: { + waypoint_out.z = current_state.z; + const double dx = incoming_waypoint.x - current_state.x; + const double dy = incoming_waypoint.y - current_state.y; + const double forward_heading = std::atan2(dy, dx); + waypoint_out.set_ori(Eigen::Quaterniond( + Eigen::AngleAxisd(forward_heading, Eigen::Vector3d::UnitZ()))); + break; + } } return waypoint_out; @@ -178,6 +193,8 @@ bool has_converged(const Pose& state, return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); case WaypointMode::XY_AND_YAW: return std::sqrt(ep.head<2>().squaredNorm() + ea(2) * ea(2)); + case WaypointMode::XY_FORWARD_DIR: + return ep.head<2>().norm(); case WaypointMode::FULL_POSE: default: return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); 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 index 2ac962d..4b8f0a5 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp @@ -29,6 +29,8 @@ inline WaypointMode waypoint_mode_from_ros( return WaypointMode::POSITION_AND_YAW; case vortex_msgs::msg::WaypointMode::XY_AND_YAW: return WaypointMode::XY_AND_YAW; + case vortex_msgs::msg::WaypointMode::XY_FORWARD_DIR: + return WaypointMode::XY_FORWARD_DIR; default: throw std::invalid_argument("Invalid ROS waypoint mode: " + std::to_string(mode_msg.mode)); @@ -60,6 +62,9 @@ inline vortex_msgs::msg::WaypointMode waypoint_mode_to_ros( case WaypointMode::XY_AND_YAW: ros_mode.mode = vortex_msgs::msg::WaypointMode::XY_AND_YAW; break; + case WaypointMode::XY_FORWARD_DIR: + ros_mode.mode = vortex_msgs::msg::WaypointMode::XY_FORWARD_DIR; + break; } return ros_mode; }