diff --git a/dependencies.repos b/dependencies.repos index 4475316..0379f46 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -2,3 +2,4 @@ repositories: vortex-msgs: type: git url: https://github.com/vortexntnu/vortex-msgs.git + version: development diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index baadd2b..cdadaf6 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -467,6 +467,9 @@ struct Waypoint { bool keep_altitude{false}; double desired_altitude{0.0}; bool require_altitude_convergence{false}; + double position_tolerance{0.0}; + double orientation_tolerance{0.0}; + double hold_time_sec{0.0}; }; struct SonarInfo { diff --git a/vortex_utils/include/vortex/utils/waypoint_utils.hpp b/vortex_utils/include/vortex/utils/waypoint_utils.hpp index 0b6ee0c..ebc0649 100644 --- a/vortex_utils/include/vortex/utils/waypoint_utils.hpp +++ b/vortex_utils/include/vortex/utils/waypoint_utils.hpp @@ -11,12 +11,13 @@ using vortex::utils::types::WaypointMode; /** * @brief Struct to represent a waypoint goal, containing the target pose, the - * waypoint mode, and the convergence threshold. + * waypoint mode, and the position [m] and orientation [rad] tolerances. */ struct WaypointGoal { Pose pose; WaypointMode mode; - double convergence_threshold{0.1}; + double position_tolerance{0.0}; + double orientation_tolerance{0.0}; bool keep_altitude{false}; double desired_altitude{0.0}; bool require_altitude_convergence{false}; @@ -70,6 +71,34 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint, WaypointMode mode, const Pose& current_state); +/** + * @brief Position [m] and orientation [rad] error over the DOFs a waypoint + * mode controls. + */ +struct ControlledError { + double position{0.0}; + double orientation{0.0}; +}; + +/** + * @brief Position [m] and orientation [rad] tolerances. A value <= 0 is not + * checked. + */ +struct ConvergenceTolerance { + double position{0.0}; + double orientation{0.0}; +}; + +/** + * @brief Compute the controlled position and orientation error. + * @param state The current state pose. + * @param waypoint_goal The waypoint goal pose. + * @param mode The waypoint mode. + */ +ControlledError controlled_error(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode); + /** * @brief Check whether the state has converged to the waypoint goal. * @@ -86,6 +115,14 @@ bool has_converged(const Pose& state, WaypointMode mode, double convergence_threshold); +/** + * @brief Check convergence with separate position and orientation tolerances. + */ +bool has_converged(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode, + const ConvergenceTolerance& tolerance); + /** * @brief Apply a pose offset to a base pose. * @@ -133,9 +170,10 @@ Pose load_pose_from_yaml(const std::string& file_path, * "only_position") position: # Required for FULL_POSE, * ONLY_POSITION, FORWARD_HEADING x: 1.0 y: 0.0 z: -0.5 orientation: # * Required for FULL_POSE, ONLY_ORIENTATION roll: 0.0 pitch: 0.0 yaw: 3.14159 - * convergence_threshold: 0.1 # Optional, default is 0.1 - * keep_altitude: true # Optional, default false - * desired_altitude: 1.5 # Required when keep_altitude is true + * position_tolerance: 0.1 # Optional [m] + * orientation_tolerance_deg: 5.0 # Optional [deg] + * keep_altitude: true # Optional, default false + * desired_altitude: 1.5 # Required when keep_altitude is true * @endcode * * @param file_path Path to the YAML file. diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp index 318a746..d8510b0 100644 --- a/vortex_utils/src/waypoint_utils.cpp +++ b/vortex_utils/src/waypoint_utils.cpp @@ -236,42 +236,72 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint, return waypoint_out; } -bool has_converged(const Pose& state, - const Pose& waypoint_goal, - WaypointMode mode, - double convergence_threshold) { +namespace { + +struct ControlledErrorSq { + double position{0.0}; + double orientation{0.0}; +}; + +ControlledErrorSq controlled_error_sq(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode) { 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::POSITION_AND_YAW: - 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::LEVEL_ORIENTATION: - return ea.head<2>().norm(); - case WaypointMode::ONLY_Z: - return std::abs(ep(2)); - case WaypointMode::POS_Z_LEVEL_ORIENTATION: - return std::sqrt(ep(2) * ep(2) + ea.head<2>().squaredNorm()); - case WaypointMode::FULL_POSE: - default: - return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); - } - }(); + switch (mode) { + case WaypointMode::ONLY_POSITION: + return {ep.squaredNorm(), 0.0}; + case WaypointMode::ONLY_ORIENTATION: + return {0.0, ea.squaredNorm()}; + case WaypointMode::FORWARD_HEADING: + case WaypointMode::POSITION_AND_YAW: + return {ep.squaredNorm(), ea(2) * ea(2)}; + case WaypointMode::XY_AND_YAW: + return {ep.head<2>().squaredNorm(), ea(2) * ea(2)}; + case WaypointMode::XY_FORWARD_DIR: + return {ep.head<2>().squaredNorm(), 0.0}; + case WaypointMode::LEVEL_ORIENTATION: + return {0.0, ea.head<2>().squaredNorm()}; + case WaypointMode::ONLY_Z: + return {ep(2) * ep(2), 0.0}; + case WaypointMode::POS_Z_LEVEL_ORIENTATION: + return {ep(2) * ep(2), ea.head<2>().squaredNorm()}; + case WaypointMode::FULL_POSE: + default: + return {ep.squaredNorm(), ea.squaredNorm()}; + } +} + +} // namespace - return err < convergence_threshold; +ControlledError controlled_error(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode) { + const auto sq = controlled_error_sq(state, waypoint_goal, mode); + return {std::sqrt(sq.position), std::sqrt(sq.orientation)}; +} + +bool has_converged(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode, + double convergence_threshold) { + const auto sq = controlled_error_sq(state, waypoint_goal, mode); + return std::sqrt(sq.position + sq.orientation) < convergence_threshold; +} + +bool has_converged(const Pose& state, + const Pose& waypoint_goal, + WaypointMode mode, + const ConvergenceTolerance& tolerance) { + const auto err = controlled_error(state, waypoint_goal, mode); + const bool position_ok = + tolerance.position <= 0.0 || err.position < tolerance.position; + const bool orientation_ok = + tolerance.orientation <= 0.0 || err.orientation < tolerance.orientation; + return position_ok && orientation_ok; } Pose apply_pose_offset(const Pose& base, const Pose& offset) { @@ -334,9 +364,14 @@ WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path, const auto mode = load_mode(wp); const Pose pose = load_pose_for_mode(wp, mode); - double convergence_threshold = 0.1; - if (wp["convergence_threshold"]) { - convergence_threshold = wp["convergence_threshold"].as(); + double position_tolerance = 0.0; + if (wp["position_tolerance"]) { + position_tolerance = wp["position_tolerance"].as(); + } + double orientation_tolerance = 0.0; + if (wp["orientation_tolerance_deg"]) { + orientation_tolerance = + wp["orientation_tolerance_deg"].as() * (M_PI / 180.0); } bool keep_altitude = false; @@ -361,7 +396,8 @@ WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path, return WaypointGoal{ .pose = pose, .mode = mode, - .convergence_threshold = convergence_threshold, + .position_tolerance = position_tolerance, + .orientation_tolerance = orientation_tolerance, .keep_altitude = keep_altitude, .desired_altitude = desired_altitude, .require_altitude_convergence = require_altitude_convergence}; diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp new file mode 100644 index 0000000..7e4980f --- /dev/null +++ b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp @@ -0,0 +1,142 @@ +#ifndef VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP +#define VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP + +#include +#include +#include +#include + +#include +#include + +namespace vortex::utils::landmarks { + +/// A LandmarkType constant and its name. +struct LandmarkTypeName { + std::string_view name; + std::uint16_t value; +}; + +/// A LandmarkSubtype constant, its name and the LandmarkType it belongs to. +struct LandmarkSubtypeName { + std::string_view name; + std::uint16_t value; + std::uint16_t type; +}; + +using vortex_msgs::msg::LandmarkSubtype; +using vortex_msgs::msg::LandmarkType; + +// Add new constants from LandmarkType.msg and LandmarkSubtype.msg here. +inline constexpr LandmarkTypeName kLandmarkTypes[] = { + {"ARUCO_MARKER", LandmarkType::ARUCO_MARKER}, + {"ARUCO_BOARD", LandmarkType::ARUCO_BOARD}, + {"PIPELINE_START", LandmarkType::PIPELINE_START}, + {"PIPELINE_END", LandmarkType::PIPELINE_END}, + {"VALVE", LandmarkType::VALVE}, + {"GATE", LandmarkType::GATE}, + {"SLALOM_PIPE", LandmarkType::SLALOM_PIPE}, + {"TORPEDO_BOARD", LandmarkType::TORPEDO_BOARD}, + {"BIN", LandmarkType::BIN}, + {"PATH_MARKER", LandmarkType::PATH_MARKER}, + {"TABLE", LandmarkType::TABLE}, + {"OCTAGON", LandmarkType::OCTAGON}, + {"PINGER", LandmarkType::PINGER}, +}; + +inline constexpr LandmarkSubtypeName kLandmarkSubtypes[] = { + {"ARUCO_BOARD_CAMERA", LandmarkSubtype::ARUCO_BOARD_CAMERA, + LandmarkType::ARUCO_BOARD}, + {"ARUCO_BOARD_SONAR", LandmarkSubtype::ARUCO_BOARD_SONAR, + LandmarkType::ARUCO_BOARD}, + {"ARUCO_BOARD_DETECTION", LandmarkSubtype::ARUCO_BOARD_DETECTION, + LandmarkType::ARUCO_BOARD}, + {"VALVE_VERTICAL", LandmarkSubtype::VALVE_VERTICAL, LandmarkType::VALVE}, + {"VALVE_HORIZONTAL", LandmarkSubtype::VALVE_HORIZONTAL, + LandmarkType::VALVE}, + {"PIPELINE_START_CAMERA", LandmarkSubtype::PIPELINE_START_CAMERA, + LandmarkType::PIPELINE_START}, + {"PIPELINE_START_SONAR", LandmarkSubtype::PIPELINE_START_SONAR, + LandmarkType::PIPELINE_START}, + {"GATE_SEARCH_RESCUE", LandmarkSubtype::GATE_SEARCH_RESCUE, + LandmarkType::GATE}, + {"GATE_SURVEY_REPAIR", LandmarkSubtype::GATE_SURVEY_REPAIR, + LandmarkType::GATE}, + {"SLALOM_PIPE_WHITE", LandmarkSubtype::SLALOM_PIPE_WHITE, + LandmarkType::SLALOM_PIPE}, + {"SLALOM_PIPE_RED", LandmarkSubtype::SLALOM_PIPE_RED, + LandmarkType::SLALOM_PIPE}, + {"TORPEDO_BOARD_WHOLE", LandmarkSubtype::TORPEDO_BOARD_WHOLE, + LandmarkType::TORPEDO_BOARD}, + {"TORPEDO_OPENING_LARGE_LEFT", LandmarkSubtype::TORPEDO_OPENING_LARGE_LEFT, + LandmarkType::TORPEDO_BOARD}, + {"TORPEDO_OPENING_LARGE_RIGHT", + LandmarkSubtype::TORPEDO_OPENING_LARGE_RIGHT, LandmarkType::TORPEDO_BOARD}, + {"TORPEDO_OPENING_SMALL_TOP", LandmarkSubtype::TORPEDO_OPENING_SMALL_TOP, + LandmarkType::TORPEDO_BOARD}, + {"TORPEDO_OPENING_SMALL_BOTTOM", + LandmarkSubtype::TORPEDO_OPENING_SMALL_BOTTOM, + LandmarkType::TORPEDO_BOARD}, + {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE, + LandmarkType::BIN}, + {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR, + LandmarkType::BIN}, + {"BIN_UNCLASSIFIED", LandmarkSubtype::BIN_UNCLASSIFIED, LandmarkType::BIN}, + {"BIN_STRUCTURE", LandmarkSubtype::BIN_STRUCTURE, LandmarkType::BIN}, + {"PATH_MARKER_WHOLE", LandmarkSubtype::PATH_MARKER_WHOLE, + LandmarkType::PATH_MARKER}, + {"TABLE_WHOLE", LandmarkSubtype::TABLE_WHOLE, LandmarkType::TABLE}, + {"TABLE_ITEM_NUTBOLT", LandmarkSubtype::TABLE_ITEM_NUTBOLT, + LandmarkType::TABLE}, + {"TABLE_ITEM_ELECTRIC", LandmarkSubtype::TABLE_ITEM_ELECTRIC, + LandmarkType::TABLE}, + {"TABLE_ITEM_PILL", LandmarkSubtype::TABLE_ITEM_PILL, LandmarkType::TABLE}, + {"TABLE_ITEM_BANDAID", LandmarkSubtype::TABLE_ITEM_BANDAID, + LandmarkType::TABLE}, + {"TABLE_BASKET_SURVEY_REPAIR", LandmarkSubtype::TABLE_BASKET_SURVEY_REPAIR, + LandmarkType::TABLE}, + {"TABLE_BASKET_SEARCH_RESCUE", LandmarkSubtype::TABLE_BASKET_SEARCH_RESCUE, + LandmarkType::TABLE}, + {"OCTAGON_WHOLE", LandmarkSubtype::OCTAGON_WHOLE, LandmarkType::OCTAGON}, + {"OCTAGON_IMAGE_REPAIR", LandmarkSubtype::OCTAGON_IMAGE_REPAIR, + LandmarkType::OCTAGON}, + {"OCTAGON_IMAGE_RESCUE", LandmarkSubtype::OCTAGON_IMAGE_RESCUE, + LandmarkType::OCTAGON}, + {"OCTAGON_IMAGE_SEARCH", LandmarkSubtype::OCTAGON_IMAGE_SEARCH, + LandmarkType::OCTAGON}, + {"OCTAGON_IMAGE_SURVEY", LandmarkSubtype::OCTAGON_IMAGE_SURVEY, + LandmarkType::OCTAGON}, + {"PINGER_DEPLOY", LandmarkSubtype::PINGER_DEPLOY, LandmarkType::PINGER}, + {"PINGER_RESTORE", LandmarkSubtype::PINGER_RESTORE, LandmarkType::PINGER}, +}; + +namespace detail { + +template +constexpr std::optional value_of(const Entry (&table)[N], + std::string_view name) { + for (const auto& entry : table) { + if (entry.name == name) { + return entry.value; + } + } + return std::nullopt; +} + +} // namespace detail + +/// Value of the LandmarkType constant with this name, e.g. "GATE". +constexpr std::optional landmark_type_from_string( + std::string_view name) { + return detail::value_of(kLandmarkTypes, name); +} + +/// Value of the LandmarkSubtype constant with this name. +constexpr std::optional landmark_subtype_from_string( + std::string_view name) { + return detail::value_of(kLandmarkSubtypes, name); +} + +} // namespace vortex::utils::landmarks + +#endif // VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP 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 4ad731a..65e5c0c 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 @@ -96,6 +96,9 @@ inline vortex::utils::types::Waypoint waypoint_from_ros( wp.keep_altitude = ros_wp.keep_altitude; wp.desired_altitude = ros_wp.desired_altitude; wp.require_altitude_convergence = ros_wp.require_altitude_convergence; + wp.position_tolerance = ros_wp.position_tolerance; + wp.orientation_tolerance = ros_wp.orientation_tolerance; + wp.hold_time_sec = ros_wp.hold_time_sec; return wp; }