Skip to content

Commit a0c7689

Browse files
jorgenfjjorgenfj
committed
landmark and waypoint goal from yaml
1 parent edb3460 commit a0c7689

2 files changed

Lines changed: 155 additions & 14 deletions

File tree

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

Lines changed: 86 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,28 @@ namespace vortex::utils::waypoints {
99
using vortex::utils::types::Pose;
1010
using vortex::utils::types::WaypointMode;
1111

12+
/**
13+
* @brief Struct to represent a waypoint goal, containing the target pose, the
14+
* waypoint mode, and the convergence threshold.
15+
*/
16+
struct WaypointGoal {
17+
Pose pose;
18+
WaypointMode mode;
19+
double convergence_threshold{0.1};
20+
};
21+
22+
/**
23+
* @brief Struct to represent a landmark convergence goal, containing the
24+
* convergence offset from the landmark pose, the waypoint mode, and the
25+
* convergence threshold.
26+
*/
27+
struct LandmarkConvergenceGoal {
28+
Pose convergence_offset;
29+
WaypointMode mode;
30+
double convergence_threshold{0.1};
31+
double dead_reckoning_threshold{0.5};
32+
};
33+
1234
/**
1335
* @brief Compute the waypoint goal by applying the mode logic to the incoming
1436
* waypoint.
@@ -54,7 +76,32 @@ bool has_converged(const Pose& state,
5476
Pose apply_pose_offset(const Pose& base, const Pose& offset);
5577

5678
/**
57-
* @brief Load a waypoint from a YAML file by identifier.
79+
* @brief Load a pose from a YAML file by identifier.
80+
*
81+
* Expected YAML format:
82+
* @code
83+
* pose_name:
84+
* position:
85+
* x: 1.0
86+
* y: 0.0
87+
* z: -0.5
88+
* orientation:
89+
* roll: 0.0
90+
* pitch: 0.0
91+
* yaw: 3.14159
92+
* @endcode
93+
*
94+
* @param file_path Path to the YAML file.
95+
* @param identifier The pose key to look up.
96+
* @return Pose with position and orientation (RPY converted to quaternion).
97+
* @throws std::runtime_error if the file cannot be opened or the identifier is
98+
* not found.
99+
*/
100+
Pose load_pose_from_yaml(const std::string& file_path,
101+
const std::string& identifier);
102+
103+
/**
104+
* @brief Load a waypoint goal from a YAML file by identifier.
58105
*
59106
* Expected YAML format:
60107
* @code
@@ -67,16 +114,51 @@ Pose apply_pose_offset(const Pose& base, const Pose& offset);
67114
* roll: 0.0
68115
* pitch: 0.0
69116
* yaw: 3.14159
117+
* mode: 0 # WaypointMode as integer (e.g. 0 for FULL_POSE)
118+
* convergence_threshold: 0.1 # Optional, default is 0.1
70119
* @endcode
71120
*
72121
* @param file_path Path to the YAML file.
73122
* @param identifier The waypoint key to look up.
74-
* @return Pose with position and orientation (RPY converted to quaternion).
123+
* @return WaypointGoal with pose, mode, and convergence threshold.
124+
* @throws std::runtime_error if the file cannot be opened or the identifier is
125+
* not found.
126+
*/
127+
WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path,
128+
const std::string& identifier);
129+
130+
/**
131+
* @brief Load a landmark convergence goal from a YAML file by identifier.
132+
*
133+
* The position/orientation fields represent the convergence offset from
134+
* the landmark pose.
135+
*
136+
* Expected YAML format:
137+
* @code
138+
* landmark_goal_name:
139+
* position:
140+
* x: 0.0
141+
* y: 0.0
142+
* z: -0.5
143+
* orientation:
144+
* roll: 0.0
145+
* pitch: 0.0
146+
* yaw: 0.0
147+
* mode: 0 # WaypointMode as integer (e.g. 0 for FULL_POSE)
148+
* convergence_threshold: 0.1 # Optional, default is 0.1
149+
* dead_reckoning_threshold: 0.5 # Optional, default is 0.5
150+
* @endcode
151+
*
152+
* @param file_path Path to the YAML file.
153+
* @param identifier The landmark convergence key to look up.
154+
* @return LandmarkConvergenceGoal with convergence offset, mode,
155+
* convergence threshold, and dead-reckoning threshold.
75156
* @throws std::runtime_error if the file cannot be opened or the identifier is
76157
* not found.
77158
*/
78-
Pose load_waypoint_from_yaml(const std::string& file_path,
79-
const std::string& identifier);
159+
LandmarkConvergenceGoal load_landmark_goal_from_yaml(
160+
const std::string& file_path,
161+
const std::string& identifier);
80162

81163
} // namespace vortex::utils::waypoints
82164

‎vortex_utils/src/waypoint_utils.cpp‎

Lines changed: 69 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -76,24 +76,24 @@ Pose apply_pose_offset(const Pose& base, const Pose& offset) {
7676
return Pose::from_eigen(p_target, q_target);
7777
}
7878

79-
Pose load_waypoint_from_yaml(const std::string& file_path,
80-
const std::string& identifier) {
79+
Pose load_pose_from_yaml(const std::string& file_path,
80+
const std::string& identifier) {
8181
YAML::Node root = YAML::LoadFile(file_path);
8282

8383
if (!root[identifier]) {
84-
throw std::runtime_error("Waypoint '" + identifier + "' not found in " +
84+
throw std::runtime_error("Pose '" + identifier + "' not found in " +
8585
file_path);
8686
}
8787

88-
const auto& wp = root[identifier];
88+
const auto& pose = root[identifier];
8989

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>();
90+
const double x = pose["position"]["x"].as<double>();
91+
const double y = pose["position"]["y"].as<double>();
92+
const double z = pose["position"]["z"].as<double>();
9393

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>();
94+
const double roll = pose["orientation"]["roll"].as<double>();
95+
const double pitch = pose["orientation"]["pitch"].as<double>();
96+
const double yaw = pose["orientation"]["yaw"].as<double>();
9797

9898
const Eigen::Quaterniond q =
9999
vortex::utils::math::euler_to_quat(roll, pitch, yaw);
@@ -107,4 +107,63 @@ Pose load_waypoint_from_yaml(const std::string& file_path,
107107
.qz = q.z()};
108108
}
109109

110+
WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path,
111+
const std::string& identifier) {
112+
YAML::Node root = YAML::LoadFile(file_path);
113+
114+
if (!root[identifier]) {
115+
throw std::runtime_error("Waypoint '" + identifier + "' not found in " +
116+
file_path);
117+
}
118+
119+
const auto& wp = root[identifier];
120+
121+
Pose pose = load_pose_from_yaml(file_path, identifier);
122+
123+
const auto mode = static_cast<WaypointMode>(wp["mode"].as<uint8_t>());
124+
125+
double convergence_threshold = 0.1;
126+
if (wp["convergence_threshold"]) {
127+
convergence_threshold = wp["convergence_threshold"].as<double>();
128+
}
129+
130+
return WaypointGoal{.pose = pose,
131+
.mode = mode,
132+
.convergence_threshold = convergence_threshold};
133+
}
134+
135+
LandmarkConvergenceGoal load_landmark_goal_from_yaml(
136+
const std::string& file_path,
137+
const std::string& identifier) {
138+
YAML::Node root = YAML::LoadFile(file_path);
139+
140+
if (!root[identifier]) {
141+
throw std::runtime_error("Landmark goal '" + identifier +
142+
"' not found in " + file_path);
143+
}
144+
145+
const auto& entry = root[identifier];
146+
147+
Pose convergence_offset = load_pose_from_yaml(file_path, identifier);
148+
149+
const auto mode = static_cast<WaypointMode>(entry["mode"].as<uint8_t>());
150+
151+
double convergence_threshold = 0.1;
152+
if (entry["convergence_threshold"]) {
153+
convergence_threshold = entry["convergence_threshold"].as<double>();
154+
}
155+
156+
double dead_reckoning_threshold = 0.5;
157+
if (entry["dead_reckoning_threshold"]) {
158+
dead_reckoning_threshold =
159+
entry["dead_reckoning_threshold"].as<double>();
160+
}
161+
162+
return LandmarkConvergenceGoal{
163+
.convergence_offset = convergence_offset,
164+
.mode = mode,
165+
.convergence_threshold = convergence_threshold,
166+
.dead_reckoning_threshold = dead_reckoning_threshold};
167+
}
168+
110169
} // namespace vortex::utils::waypoints

0 commit comments

Comments
 (0)