Skip to content

Commit edb3460

Browse files
jorgenfjjorgenfj
authored
waypoint types, utils and ros conversions (#46)
* waypoint types, utils and ros conversions * update to support separate mode msg
1 parent 8d89529 commit edb3460

6 files changed

Lines changed: 269 additions & 1 deletion

File tree

‎vortex_utils/CMakeLists.txt‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,14 +7,20 @@ add_compile_options(-Wall -Wextra -Wpedantic)
77
find_package(ament_cmake REQUIRED)
88
find_package(ament_cmake_python REQUIRED)
99
find_package(Eigen3 REQUIRED)
10+
find_package(yaml-cpp REQUIRED)
1011

1112
add_library(${PROJECT_NAME} SHARED
12-
src/math.cpp)
13+
src/math.cpp
14+
src/waypoint_utils.cpp)
1315

1416
target_include_directories(${PROJECT_NAME} PUBLIC
1517
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
1618
$<INSTALL_INTERFACE:include>)
1719

20+
target_link_libraries(${PROJECT_NAME} PUBLIC
21+
yaml-cpp
22+
)
23+
1824
ament_target_dependencies(${PROJECT_NAME} PUBLIC
1925
Eigen3
2026
)

‎vortex_utils/include/vortex/utils/types.hpp‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -436,6 +436,27 @@ inline std::string mode_to_string(Mode mode) {
436436
}
437437
}
438438

439+
/**
440+
* @brief Determines which degrees of freedom the reference filter controls.
441+
*
442+
* The mode affects both the reference goal computation (via
443+
* compute_waypoint_goal) and the convergence check (via has_converged).
444+
*/
445+
enum class WaypointMode : uint8_t {
446+
FULL_POSE = 0, ///< Control all 6 DOF.
447+
ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation.
448+
FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target.
449+
ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position.
450+
};
451+
452+
/**
453+
* @brief A target pose with an associated waypoint mode.
454+
*/
455+
struct Waypoint {
456+
Pose pose{};
457+
WaypointMode mode = WaypointMode::FULL_POSE;
458+
};
459+
439460
struct SonarInfo {
440461
double meters_per_pixel_x{};
441462
double meters_per_pixel_y{};

‎vortex_utils/include/vortex/utils/waypoint_utils.hpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
#ifndef VORTEX_UTILS_WAYPOINT_UTILS_HPP
2+
#define VORTEX_UTILS_WAYPOINT_UTILS_HPP
3+
4+
#include <string>
5+
#include <vortex/utils/types.hpp>
6+
7+
namespace vortex::utils::waypoints {
8+
9+
using vortex::utils::types::Pose;
10+
using vortex::utils::types::WaypointMode;
11+
12+
/**
13+
* @brief Compute the waypoint goal by applying the mode logic to the incoming
14+
* waypoint.
15+
*
16+
* For modes that don't control all DOFs, the uncontrolled components are
17+
* replaced with values from @p current_state.
18+
*
19+
* @param incoming_waypoint The incoming waypoint to compute goal from.
20+
* @param mode The waypoint mode.
21+
* @param current_state The current state pose.
22+
* @return The adjusted waypoint goal.
23+
*/
24+
Pose compute_waypoint_goal(const Pose& incoming_waypoint,
25+
WaypointMode mode,
26+
const Pose& current_state);
27+
28+
/**
29+
* @brief Check whether the state has converged to the waypoint goal.
30+
*
31+
* Only the DOFs relevant to the waypoint mode are included in the error norm.
32+
*
33+
* @param state The current state pose.
34+
* @param waypoint_goal The waypoint goal pose.
35+
* @param mode The waypoint mode.
36+
* @param convergence_threshold The maximum allowed error norm.
37+
* @return True if the error is below the threshold.
38+
*/
39+
bool has_converged(const Pose& state,
40+
const Pose& waypoint_goal,
41+
WaypointMode mode,
42+
double convergence_threshold);
43+
44+
/**
45+
* @brief Apply a pose offset to a base pose.
46+
*
47+
* Position: p_target = p_base + p_offset
48+
* Orientation: q_target = (q_base * q_offset).normalized()
49+
*
50+
* @param base The base pose (e.g. a landmark pose).
51+
* @param offset The offset to apply.
52+
* @return The resulting target pose.
53+
*/
54+
Pose apply_pose_offset(const Pose& base, const Pose& offset);
55+
56+
/**
57+
* @brief Load a waypoint from a YAML file by identifier.
58+
*
59+
* Expected YAML format:
60+
* @code
61+
* waypoint_name:
62+
* position:
63+
* x: 1.0
64+
* y: 0.0
65+
* z: -0.5
66+
* orientation:
67+
* roll: 0.0
68+
* pitch: 0.0
69+
* yaw: 3.14159
70+
* @endcode
71+
*
72+
* @param file_path Path to the YAML file.
73+
* @param identifier The waypoint key to look up.
74+
* @return Pose with position and orientation (RPY converted to quaternion).
75+
* @throws std::runtime_error if the file cannot be opened or the identifier is
76+
* not found.
77+
*/
78+
Pose load_waypoint_from_yaml(const std::string& file_path,
79+
const std::string& identifier);
80+
81+
} // namespace vortex::utils::waypoints
82+
83+
#endif // VORTEX_UTILS_WAYPOINT_UTILS_HPP

‎vortex_utils/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
<buildtool_depend>ament_cmake_python</buildtool_depend>
1212

1313
<depend>eigen</depend>
14+
<depend>yaml-cpp</depend>
1415
<depend>python3-numpy</depend>
1516
<depend>python3-scipy</depend>
1617

‎vortex_utils/src/waypoint_utils.cpp‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include "vortex/utils/waypoint_utils.hpp"
2+
3+
#include <yaml-cpp/yaml.h>
4+
#include <cmath>
5+
#include <vortex/utils/math.hpp>
6+
7+
namespace vortex::utils::waypoints {
8+
9+
Pose compute_waypoint_goal(const Pose& incoming_waypoint,
10+
WaypointMode mode,
11+
const Pose& current_state) {
12+
Pose waypoint_out = incoming_waypoint;
13+
14+
switch (mode) {
15+
case WaypointMode::FULL_POSE:
16+
break;
17+
18+
case WaypointMode::ONLY_POSITION:
19+
waypoint_out.set_ori(current_state.ori_quaternion());
20+
break;
21+
22+
case WaypointMode::FORWARD_HEADING: {
23+
double dx = incoming_waypoint.x - current_state.x;
24+
double dy = incoming_waypoint.y - current_state.y;
25+
double forward_heading = std::atan2(dy, dx);
26+
27+
waypoint_out.set_ori(Eigen::Quaterniond(
28+
Eigen::AngleAxisd(forward_heading, Eigen::Vector3d::UnitZ())));
29+
break;
30+
}
31+
32+
case WaypointMode::ONLY_ORIENTATION:
33+
waypoint_out.set_pos(current_state.pos_vector());
34+
break;
35+
}
36+
37+
return waypoint_out;
38+
}
39+
40+
bool has_converged(const Pose& state,
41+
const Pose& waypoint_goal,
42+
WaypointMode mode,
43+
double convergence_threshold) {
44+
const Eigen::Vector3d ep = state.pos_vector() - waypoint_goal.pos_vector();
45+
46+
const Eigen::Vector3d ea = vortex::utils::math::quaternion_error(
47+
state.ori_quaternion(), waypoint_goal.ori_quaternion());
48+
49+
const double err = [&] {
50+
switch (mode) {
51+
case WaypointMode::ONLY_POSITION:
52+
return ep.norm();
53+
case WaypointMode::ONLY_ORIENTATION:
54+
return ea.norm();
55+
case WaypointMode::FORWARD_HEADING:
56+
return std::sqrt(ep.squaredNorm() + ea(2) * ea(2));
57+
case WaypointMode::FULL_POSE:
58+
default:
59+
return std::sqrt(ep.squaredNorm() + ea.squaredNorm());
60+
}
61+
}();
62+
63+
return err < convergence_threshold;
64+
}
65+
66+
Pose apply_pose_offset(const Pose& base, const Pose& offset) {
67+
const Eigen::Vector3d p_base = base.pos_vector();
68+
const Eigen::Quaterniond q_base = base.ori_quaternion().normalized();
69+
70+
const Eigen::Vector3d p_offset = offset.pos_vector();
71+
const Eigen::Quaterniond q_offset = offset.ori_quaternion().normalized();
72+
73+
const Eigen::Vector3d p_target = p_base + p_offset;
74+
const Eigen::Quaterniond q_target = (q_base * q_offset).normalized();
75+
76+
return Pose::from_eigen(p_target, q_target);
77+
}
78+
79+
Pose load_waypoint_from_yaml(const std::string& file_path,
80+
const std::string& identifier) {
81+
YAML::Node root = YAML::LoadFile(file_path);
82+
83+
if (!root[identifier]) {
84+
throw std::runtime_error("Waypoint '" + identifier + "' not found in " +
85+
file_path);
86+
}
87+
88+
const auto& wp = root[identifier];
89+
90+
const double x = wp["position"]["x"].as<double>();
91+
const double y = wp["position"]["y"].as<double>();
92+
const double z = wp["position"]["z"].as<double>();
93+
94+
const double roll = wp["orientation"]["roll"].as<double>();
95+
const double pitch = wp["orientation"]["pitch"].as<double>();
96+
const double yaw = wp["orientation"]["yaw"].as<double>();
97+
98+
const Eigen::Quaterniond q =
99+
vortex::utils::math::euler_to_quat(roll, pitch, yaw);
100+
101+
return Pose{.x = x,
102+
.y = y,
103+
.z = z,
104+
.qw = q.w(),
105+
.qx = q.x(),
106+
.qy = q.y(),
107+
.qz = q.z()};
108+
}
109+
110+
} // namespace vortex::utils::waypoints

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

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
#ifndef VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_
2+
#define VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_
3+
4+
#include <geometry_msgs/msg/pose.hpp>
5+
#include <vortex/utils/ros/ros_conversions.hpp>
6+
#include <vortex/utils/types.hpp>
7+
#include <vortex/utils/waypoint_utils.hpp>
8+
#include <vortex_msgs/msg/waypoint.hpp>
9+
#include <vortex_msgs/msg/waypoint_mode.hpp>
10+
11+
namespace vortex::utils::waypoints {
12+
13+
/**
14+
* @brief Convert a ROS waypoint mode to a WaypointMode enum.
15+
* @throws std::invalid_argument if the mode value is not recognized.
16+
*/
17+
inline WaypointMode waypoint_mode_from_ros(
18+
const vortex_msgs::msg::WaypointMode& mode_msg) {
19+
switch (mode_msg.mode) {
20+
case vortex_msgs::msg::WaypointMode::FULL_POSE:
21+
return WaypointMode::FULL_POSE;
22+
case vortex_msgs::msg::WaypointMode::ONLY_POSITION:
23+
return WaypointMode::ONLY_POSITION;
24+
case vortex_msgs::msg::WaypointMode::FORWARD_HEADING:
25+
return WaypointMode::FORWARD_HEADING;
26+
case vortex_msgs::msg::WaypointMode::ONLY_ORIENTATION:
27+
return WaypointMode::ONLY_ORIENTATION;
28+
default:
29+
throw std::invalid_argument("Invalid ROS waypoint mode: " +
30+
std::to_string(mode_msg.mode));
31+
}
32+
}
33+
34+
/**
35+
* @brief Convert a ROS Waypoint message to an internal Waypoint struct.
36+
*/
37+
inline vortex::utils::types::Waypoint waypoint_from_ros(
38+
const vortex_msgs::msg::Waypoint& ros_wp) {
39+
vortex::utils::types::Waypoint wp;
40+
wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose(ros_wp.pose);
41+
wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode);
42+
return wp;
43+
}
44+
45+
} // namespace vortex::utils::waypoints
46+
47+
#endif // VORTEX_UTILS_ROS__WAYPOINT_ROS_CONVERSIONS_HPP_

0 commit comments

Comments
 (0)