trajectory_optimization v1.5.0
Loading...
Searching...
No Matches
utils.cpp
Go to the documentation of this file.
1// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
2// SPDX-License-Identifier: Apache-2.0
3
4#include <algorithm>
5#include <array>
6#include <cmath>
7#include <cstdio>
8#include <numeric>
9
11
12#include <blasfeo_d_aux_ext_dep.h> // for printing dense matrices
13
15
16double TrajectoryOptimizationNode::wrap_angle_rad(double angle_rad, double min_val, double max_val) {
17 double capped_angle_rad = angle_rad;
18 while (capped_angle_rad > max_val) capped_angle_rad -= 2 * M_PI;
19 while (capped_angle_rad < min_val) capped_angle_rad += 2 * M_PI;
20 return capped_angle_rad;
21}
22
24 const std::vector<double>& X, const std::vector<double>& Y, const double& desired_x, double& output_y, bool wrap_angle) {
25 if (desired_x == X.front()) {
26 RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Min of the given vector!");
27 output_y = Y.front();
28 return true;
29 } else if (desired_x == X.back()) {
30 RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Max of the given vector!");
31 output_y = Y.back();
32 return true;
33 } else if (desired_x < *min_element(X.begin(), X.end())) {
34 RCLCPP_WARN(get_logger(), "Desired Time is smaller than Time-Min of the given vector! Using first valid value.");
35 RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
36 RCLCPP_DEBUG(get_logger(), "Time-Min: %f s", *min_element(X.begin(), X.end()));
37 output_y = Y.front();
38 return false;
39 } else if (desired_x > *max_element(X.begin(), X.end())) {
40 RCLCPP_WARN(get_logger(), "Desired Time is greater than Time-Max of the given vector! Using last valid value.");
41 RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
42 RCLCPP_DEBUG(get_logger(), "Time-Max: %f s", *max_element(X.begin(), X.end()));
43 output_y = Y.back();
44 return false;
45 } else if (X.size() != Y.size()) {
46 RCLCPP_ERROR(get_logger(), "Input vectors don't have the same length!");
47 return false;
48 }
49
50 //go through array and search for sampling points
51 size_t i = 0;
52 for (i = 0; i < X.size(); i++) {
53 if (X[i] < desired_x) {
54 continue;
55 } else if (X[i] == desired_x) {
56 output_y = Y[i];
57 return true;
58 } else {
59 break;
60 }
61 }
62 double diff = Y[i] - Y[i - 1];
63 if (wrap_angle) {
64 diff = wrap_angle_rad(diff);
65 }
66 output_y = Y[i - 1] + (diff / (X[i] - X[i - 1])) * (desired_x - X[i - 1]);
67 if (wrap_angle) {
68 output_y = wrap_angle_rad(output_y);
69 }
70 return true;
71}
72
73bool TrajectoryOptimizationNode::trajectory2outputFrame(trajectory_planning_msgs::msg::Trajectory& trajectory) {
75 trajectory_planning_msgs::msg::Trajectory tf_trajectory;
76 try {
77 tf_trajectory = tf2_buffer_->transform(trajectory, trajectory_frame_id_, tf2::durationFromSec(0.01));
78 } catch (tf2::TransformException& ex) {
79 RCLCPP_WARN(this->get_logger(), "Transformation into output frame is not available. Publishing no trajectory. Ex: %s",
80 ex.what());
81 return false;
82 }
83 trajectory = tf_trajectory;
84 }
85 return true;
86}
87
88void TrajectoryOptimizationNode::keepNClosestObjects(perception_msgs::msg::ObjectList& object_list,
89 const int n_objects,
90 const bool ignore_objects_behind_ego) {
91 // sort object indices by distance to ego vehicle
92 std::vector<size_t> indices_sorted_by_distance(object_list.objects.size());
93 std::iota(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), 0);
94 std::sort(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), [&](size_t first, size_t second) {
95 const auto& first_object = object_list.objects[first];
96 const auto& second_object = object_list.objects[second];
97 return std::hypot(perception_msgs::object_access::getX(first_object), perception_msgs::object_access::getY(first_object)) <
98 std::hypot(perception_msgs::object_access::getX(second_object), perception_msgs::object_access::getY(second_object));
99 });
100
101 // keep only the closest n_objects, optionally excluding objects behind the ego vehicle
102 std::vector<perception_msgs::msg::Object> closest_objects;
103 closest_objects.reserve(std::min(static_cast<size_t>(n_objects), object_list.objects.size()));
104 for (const size_t index : indices_sorted_by_distance) {
105 if (ignore_objects_behind_ego && perception_msgs::object_access::getX(object_list.objects[index]) <= 0.0) continue;
106 closest_objects.push_back(object_list.objects[index]);
107 if (closest_objects.size() >= static_cast<size_t>(n_objects)) break;
108 }
109 object_list.objects = std::move(closest_objects);
110}
111
112std::vector<std::pair<double, double>> TrajectoryOptimizationNode::normalBoundaryDistance(
113 const trajectory_planning_msgs::msg::Trajectory& reference_trajectory, const route_planning_msgs::msg::Route& route) {
114 const double NO_BOUNDARY_DISTANCE = 1e6; // finite sentinel avoids NaNs during reference-path interpolation
115 constexpr double MAX_ROUTE_S_DIFFERENCE = 20.0;
116 constexpr double LOOK_BEHIND_REMAINING_ROUTE_S = 2.0;
117
118 struct Boundaries {
119 std::vector<std::pair<double, double>> min_normal_distances;
120 std::vector<Eigen::Vector2d> left_boundary_points;
121 std::vector<Eigen::Vector2d> right_boundary_points;
122 std::vector<double> left_boundary_route_s;
123 std::vector<double> right_boundary_route_s;
124 std::vector<Eigen::Vector2d> left_boundary_intersections;
125 std::vector<Eigen::Vector2d> right_boundary_intersections;
126 };
127
128 Boundaries boundaries;
129 auto route_elements = route_planning_msgs::route_access::getRemainingRouteElements(route, true);
130 const int ref_sample_size = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory);
131
132 if (route_elements.empty()) {
134 RCLCPP_WARN(get_logger(), "Remaining route is empty. Do not constrain boundaries.");
135 }
136 for (int i = 0; i < ref_sample_size; ++i) {
137 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
138 }
139 return boundaries.min_normal_distances;
140 }
141
142 const double current_route_s = route_elements.front().s;
143 const size_t current_route_index = std::min(static_cast<size_t>(route.current_route_element_idx), route.route_elements.size());
144 size_t overlap_begin = current_route_index;
145 while (overlap_begin > 0 && current_route_s - route.route_elements[overlap_begin - 1].s <= LOOK_BEHIND_REMAINING_ROUTE_S) {
146 --overlap_begin;
147 }
148 route_elements.insert(route_elements.begin(), route.route_elements.begin() + static_cast<std::ptrdiff_t>(overlap_begin),
149 route.route_elements.begin() + static_cast<std::ptrdiff_t>(current_route_index));
150
151 boundaries.left_boundary_points.reserve(route_elements.size());
152 boundaries.right_boundary_points.reserve(route_elements.size());
153 boundaries.left_boundary_route_s.reserve(route_elements.size());
154 boundaries.right_boundary_route_s.reserve(route_elements.size());
155 boundaries.min_normal_distances.reserve(ref_sample_size);
156 boundaries.left_boundary_intersections.reserve(ref_sample_size);
157 boundaries.right_boundary_intersections.reserve(ref_sample_size);
158
159 for (const auto& route_element : route_elements) {
160 if (route_element.is_enriched) {
162 route_planning_msgs::msg::LaneElement suggested_lane =
163 route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
164 boundaries.left_boundary_points.emplace_back(suggested_lane.left_boundary.point.x, suggested_lane.left_boundary.point.y);
165 boundaries.right_boundary_points.emplace_back(suggested_lane.right_boundary.point.x,
166 suggested_lane.right_boundary.point.y);
167 boundaries.left_boundary_route_s.push_back(route_element.s);
168 boundaries.right_boundary_route_s.push_back(route_element.s);
170 const auto& lane_elements = route_element.lane_elements;
171 if (!lane_elements.empty()) {
172 boundaries.left_boundary_points.emplace_back(lane_elements.front().left_boundary.point.x,
173 lane_elements.front().left_boundary.point.y);
174 boundaries.right_boundary_points.emplace_back(lane_elements.back().right_boundary.point.x,
175 lane_elements.back().right_boundary.point.y);
176 boundaries.left_boundary_route_s.push_back(route_element.s);
177 boundaries.right_boundary_route_s.push_back(route_element.s);
178 }
180 boundaries.left_boundary_points.emplace_back(route_element.left_boundary.x, route_element.left_boundary.y);
181 boundaries.right_boundary_points.emplace_back(route_element.right_boundary.x, route_element.right_boundary.y);
182 boundaries.left_boundary_route_s.push_back(route_element.s);
183 boundaries.right_boundary_route_s.push_back(route_element.s);
184 }
185 }
186 }
187
188 // Helper lambda to find intersection
189 auto findIntersection = [&](const Eigen::Vector2d& ref_pos, double sin_yaw, double cos_yaw, double expected_route_s,
190 const std::vector<Eigen::Vector2d>& boundary_points, const std::vector<double>& boundary_route_s,
191 bool isLeft) -> std::pair<double, Eigen::Vector2d> {
192 std::pair<double, Eigen::Vector2d> intersection_result = {
193 std::numeric_limits<double>::infinity(),
194 Eigen::Vector2d(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())};
195 double best_route_s_difference = std::numeric_limits<double>::infinity();
196
197 const Eigen::Vector2d normal_dir = isLeft ? Eigen::Vector2d(-sin_yaw, cos_yaw) : Eigen::Vector2d(sin_yaw, -cos_yaw);
198 const auto cross2d = [](const Eigen::Vector2d& u, const Eigen::Vector2d& v) { return u.x() * v.y() - u.y() * v.x(); };
199
200 for (size_t i = 0; i + 1 < boundary_points.size(); ++i) {
201 const Eigen::Vector2d& a = boundary_points[i];
202 const Eigen::Vector2d& b = boundary_points[i + 1];
203 Eigen::Vector2d seg = b - a;
204 Eigen::Vector2d ap = ref_pos - a;
205
206 const double denom = cross2d(seg, normal_dir);
207 if (std::abs(denom) < 1e-9) {
208 continue; // Lines are close to parallel; ignore this segment
209 }
210
211 const double s = cross2d(ap, normal_dir) / denom;
212 const double t = cross2d(ap, seg) / denom;
213
214 if (s >= 0.0 && s <= 1.0 && t >= 0.0) {
215 Eigen::Vector2d intersection = a + s * seg;
216 double euclidean_distance = (ref_pos - intersection).norm();
217 const double intersection_route_s = boundary_route_s[i] + s * (boundary_route_s[i + 1] - boundary_route_s[i]);
218 const double route_s_difference = std::abs(intersection_route_s - expected_route_s);
219
220 if (route_s_difference <= MAX_ROUTE_S_DIFFERENCE &&
221 (route_s_difference < best_route_s_difference ||
222 (std::abs(route_s_difference - best_route_s_difference) < 1e-9 && euclidean_distance < intersection_result.first))) {
223 best_route_s_difference = route_s_difference;
224 intersection_result = {euclidean_distance, intersection};
225 }
226 }
227 }
228 return intersection_result;
229 };
230
231 // Loop over trajectory points and compute intersections
232 double reference_progress = 0.0;
233 Eigen::Vector2d previous_ref_pos = Eigen::Vector2d::Zero();
234 for (int i = 0; i < ref_sample_size; ++i) {
235 Eigen::Vector2d ref_pos(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i),
236 trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
237 reference_progress += (ref_pos - previous_ref_pos).norm();
238 previous_ref_pos = ref_pos;
239 const double expected_route_s = current_route_s + reference_progress;
240
241 double yaw = trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i);
242 const double sin_yaw = std::sin(yaw);
243 const double cos_yaw = std::cos(yaw);
244
245 auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.left_boundary_points,
246 boundaries.left_boundary_route_s, true);
247 auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.right_boundary_points,
248 boundaries.right_boundary_route_s, false);
249 if (left_intersection.first != std::numeric_limits<double>::infinity() &&
250 right_intersection.first != std::numeric_limits<double>::infinity()) {
251 boundaries.min_normal_distances.emplace_back(left_intersection.first, right_intersection.first);
252 boundaries.left_boundary_intersections.emplace_back(left_intersection.second);
253 boundaries.right_boundary_intersections.emplace_back(right_intersection.second);
254 RCLCPP_DEBUG(this->get_logger(), "Minimum left boundary distance: %.2f m", left_intersection.first);
255 RCLCPP_DEBUG(this->get_logger(), "Minimum right boundary distance: %.2f m", right_intersection.first);
256 } else {
257 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
258 RCLCPP_WARN(get_logger(),
259 "No boundary intersection found for trajectory point %d. Do not constrain boundaries at this point.", i);
260 }
261 }
262 if (debug_viz_) {
263 vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections);
264 }
265
266 return boundaries.min_normal_distances;
267}
268
269void TrajectoryOptimizationNode::vizBoundaryPoints(const std::vector<Eigen::Vector2d>& left_boundary_points,
270 const std::vector<Eigen::Vector2d>& right_boundary_points) {
271 visualization_msgs::msg::MarkerArray marker_array;
272 int id = 0;
273
274 auto createMarker = [&](int id, const std::string& ns, const Eigen::Vector2d& pos, float r, float g, float b) {
275 visualization_msgs::msg::Marker m;
276 m.header.frame_id = vehicle_frame_id_;
277 m.header.stamp = rclcpp::Time(ego_data_.header.stamp);
278 m.lifetime = rclcpp::Duration::from_seconds(0.5);
279 m.ns = ns;
280 m.id = id;
281 m.type = visualization_msgs::msg::Marker::SPHERE;
282 m.action = visualization_msgs::msg::Marker::ADD;
283 m.pose.position.x = pos.x();
284 m.pose.position.y = pos.y();
285 m.pose.position.z = 0.0;
286 m.scale.x = m.scale.y = m.scale.z = 0.4;
287 m.color.a = 1.0;
288 m.color.r = r;
289 m.color.g = g;
290 m.color.b = b;
291 return m;
292 };
293
294 auto addMarkers = [&](const std::vector<Eigen::Vector2d>& points, const std::string& ns, float r, float g, float b) {
295 for (const auto& point : points) {
296 marker_array.markers.push_back(createMarker(id++, ns, point, r, g, b));
297 }
298 };
299
300 addMarkers(left_boundary_points, "left_boundary_constraint", 0.0F, 1.0F, 0.0F);
301 addMarkers(right_boundary_points, "right_boundary_constraint", 1.0F, 0.0F, 0.0F);
302
303 boundary_pub_->publish(marker_array);
304}
305
306void TrajectoryOptimizationNode::vizObjectBoxes(const std::vector<double>& objects) {
307 visualization_msgs::msg::MarkerArray marker_array;
308 const size_t width = static_cast<size_t>(p_objects_shape_[1]);
309 for (size_t index = 0; index < objects.size() / width; ++index) {
310 const size_t offset = index * width;
311 if (objects[offset + 5] < 0.5) continue;
312 visualization_msgs::msg::Marker marker;
313 marker.header.frame_id = vehicle_frame_id_;
314 marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
315 marker.lifetime = rclcpp::Duration::from_seconds(0.5);
316 marker.ns = "object-boxes";
317 marker.id = static_cast<int>(index);
318 marker.type = visualization_msgs::msg::Marker::CUBE;
319 marker.action = visualization_msgs::msg::Marker::ADD;
320 marker.pose.position.x = objects[offset];
321 marker.pose.position.y = objects[offset + 1];
322 marker.pose.orientation.z = std::sin(0.5 * objects[offset + 2]);
323 marker.pose.orientation.w = std::cos(0.5 * objects[offset + 2]);
324 marker.scale.x = 2.0 * objects[offset + 3];
325 marker.scale.y = 2.0 * objects[offset + 4];
326 marker.scale.z = 0.1;
327 marker.color.a = 0.25F;
328 marker.color.r = 1.0F;
329 marker_array.markers.push_back(marker);
330 }
331 object_marker_pub_->publish(marker_array);
332}
333void TrajectoryOptimizationNode::vizEgoBoxes(const std::vector<double>& x_trajectory, const std::string& model_name) {
334 visualization_msgs::msg::MarkerArray marker_array;
335 if (x_trajectory.empty()) {
336 visualization_msgs::msg::Marker delete_marker;
337 delete_marker.action = visualization_msgs::msg::Marker::DELETEALL;
338 marker_array.markers.push_back(delete_marker);
339 ego_marker_pub_->publish(marker_array);
340 return;
341 }
342
343 double length = 0.0;
344 double width = 0.0;
345 std::array<double, 2> offset_to_geometric_center{};
346
347 // define vehicle geometry based on model name (should match the OCP definition)
348 if (model_name == "karl") {
349 length = 5.173;
350 width = 1.94;
351 offset_to_geometric_center = {1.4895, 0.0};
352 } else if (model_name == "shuttle") {
353 length = 4.97;
354 width = 2.12;
355 offset_to_geometric_center = {0.0, 0.0};
356 } else {
357 RCLCPP_WARN(get_logger(), "Unknown model '%s'. Could not visualize ego boxes.", model_name.c_str());
358 return;
359 }
360
361 const int state_dim = *nlp_dims_->nx;
362 for (int stage = 0; stage <= n_shots_; ++stage) {
363 const size_t offset = static_cast<size_t>(stage) * static_cast<size_t>(state_dim);
364 const double yaw = x_trajectory[offset + 5];
365 visualization_msgs::msg::Marker marker;
366 marker.header.frame_id = vehicle_frame_id_;
367 marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
368 marker.lifetime = rclcpp::Duration::from_seconds(0.5);
369 marker.ns = "ego-boxes";
370 marker.id = stage;
371 marker.type = visualization_msgs::msg::Marker::CUBE;
372 marker.action = visualization_msgs::msg::Marker::ADD;
373 marker.pose.position.x =
374 x_trajectory[offset] + offset_to_geometric_center[0] * std::cos(yaw) - offset_to_geometric_center[1] * std::sin(yaw);
375 marker.pose.position.y =
376 x_trajectory[offset + 1] + offset_to_geometric_center[0] * std::sin(yaw) + offset_to_geometric_center[1] * std::cos(yaw);
377 marker.pose.orientation.z = std::sin(0.5 * yaw);
378 marker.pose.orientation.w = std::cos(0.5 * yaw);
379 marker.scale.x = length;
380 marker.scale.y = width;
381 marker.scale.z = 0.05;
382 marker.color.a = 0.35F;
383 marker.color.g = 0.6F;
384 marker.color.b = 1.0F;
385 marker_array.markers.push_back(marker);
386 }
387 ego_marker_pub_->publish(marker_array);
388}
389
391 double timeout_seconds = timeout_ms * 1e-3;
392 ocp_nlp_solver_opts_set(nlp_config_, nlp_opts_, "timeout_max_time", &timeout_seconds);
393}
394
396 const int dynamic_parameter_count = static_cast<int>(p_dynamic_weight_shape_[0] * p_dynamic_weight_shape_[1]);
397
398 std::array<int, 2> indices{dynamic_parameter_count, dynamic_parameter_count + 1};
399 std::array<double, 2> values{use_configured_activation && object_constraints_active_ ? 1.0 : 0.0,
400 use_configured_activation && consider_boundaries_ != CONSIDER_BOUNDARIES::NO_BOUNDS ? 1.0 : 0.0};
401
402 for (int stage = 0; stage <= n_shots_; ++stage) {
403 const int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, indices.data(), values.data(),
404 static_cast<int>(indices.size()));
405 if (status != ACADOS_SUCCESS) {
406 RCLCPP_ERROR(get_logger(), "Could not %s safety constraints at stage %d (ACADOS status %d); skipping planning cycle.",
407 use_configured_activation ? "restore" : "disable", stage, status);
408 return false;
409 }
410 }
411 return true;
412}
413
417 performance_logger_ != nullptr || verbose_);
418
419 const bool usable_status =
420 metrics.status == ACADOS_SUCCESS || metrics.status == ACADOS_MAXITER || metrics.status == ACADOS_TIMEOUT;
421 if (usable_status) {
422 for (int stage = 0; stage <= nlp_dims_->N; ++stage) {
423 ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, stage, "x", &xtraj_[stage * *nlp_dims_->nx]);
424 }
425 for (int stage = 0; stage < nlp_dims_->N; ++stage) {
426 ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, stage, "u", &utraj_[stage * *nlp_dims_->nu]);
427 }
428 }
429
430 const bool finite_slacks =
432 static_cast<int>(p_objects_shape_[0]),
433 performance_logger_ != nullptr || verbose_);
434 finite_solution = usable_status && finite_slacks && std::isfinite(metrics.cost_value) && std::isfinite(metrics.res_eq) &&
435 std::isfinite(metrics.res_ineq) &&
436 std::all_of(xtraj_.begin(), xtraj_.end(), [](double value) { return std::isfinite(value); }) &&
437 std::all_of(utraj_.begin(), utraj_.end(), [](double value) { return std::isfinite(value); });
439 return finite_solution && metrics.res_eq <= solver_opts->tol_eq && metrics.res_ineq <= solver_opts->tol_ineq;
440}
441
443 // Status codes:
444 // 0: Success (ACADOS_SUCCESS)
445 // 1: NaN detected (ACADOS_NAN_DETECTED)
446 // 2: Maximum number of iterations reached (ACADOS_MAXITER)
447 // 3: Minimum step size reached (ACADOS_MINSTEP)
448 // 4: QP solver failed (ACADOS_QP_FAILURE)
449 // 5: Solver created (ACADOS_READY)
450 // 6: Problem unbounded (ACADOS_UNBOUNDED)
451 // 7: Solver timeout (ACADOS_TIMEOUT)
452 if (metrics.status == ACADOS_SUCCESS && verbose_) {
453 RCLCPP_INFO(get_logger(), "\033[1;32mOptimization: SUCCESS!\033[0m");
454 } else if (metrics.status == ACADOS_MAXITER) {
455 RCLCPP_WARN(get_logger(), "Optimization failed with status %d (max iterations).", metrics.status);
456 } else if (metrics.status == ACADOS_TIMEOUT) {
457 RCLCPP_WARN(get_logger(), "\033[38;5;214mOptimization failed with status %d (timeout).\033[0m", metrics.status);
458 } else if (metrics.status != ACADOS_SUCCESS) {
459 RCLCPP_ERROR(get_logger(), "%s_acados_solve() failed with status %d.", model_name_.c_str(), metrics.status);
460 }
461
462 if (verbose_) {
463 RCLCPP_INFO(get_logger(), "Optimization took %.3f ms (SQP iter: %d; QP iter: %d; KKT: %e)", metrics.acados_total_ms,
464 metrics.sqp_iter, metrics.qp_iter, metrics.kkt_norm_inf);
465 RCLCPP_INFO(get_logger(), "cost_value: %f; residuals: stat=%e eq=%e ineq=%e comp=%e", metrics.cost_value, metrics.res_stat,
466 metrics.res_eq, metrics.res_ineq, metrics.res_comp);
467
468 std::fputs("\n--- xtraj ---\n", stdout);
469 d_print_exp_tran_mat(*nlp_dims_->nx, n_shots_ + 1, xtraj_.data(), *nlp_dims_->nx);
470 std::fputs("\n--- utraj ---\n", stdout);
471 d_print_exp_tran_mat(*nlp_dims_->nu, n_shots_, utraj_.data(), *nlp_dims_->nu);
473 }
474}
475
476} // namespace trajectory_optimization
static bool collectSlackStatistics(PerformanceMetrics &metrics, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, int object_boxes, bool collect_details)
Checks all slacks for finiteness and optionally collects desired-clearance statistics.
static void collectSolverStatistics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, bool collect_details)
Reads timing, iteration, cost, and residual statistics from acados.
void printSolution(const PerformanceMetrics &metrics)
Logs solver status and optional debug statistics for the last optimization run.
Definition utils.cpp:442
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr object_marker_pub_
void vizEgoBoxes(const std::vector< double > &x_trajectory, const std::string &model_name)
Definition utils.cpp:333
bool setSafetyConstraintActivation(bool use_configured_activation)
Enables the configured object and boundary constraints or temporarily disables all of them.
Definition utils.cpp:395
static void keepNClosestObjects(perception_msgs::msg::ObjectList &object_list, const int n_objects, const bool ignore_objects_behind_ego)
Keeps the nearest objects, optionally excluding those behind the ego vehicle.
Definition utils.cpp:88
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr ego_marker_pub_
static double wrap_angle_rad(double angle_rad, double min_val=-M_PI, double max_val=M_PI)
Wraps an angle into a configured interval.
Definition utils.cpp:16
bool linearInterpolation(const std::vector< double > &X, const std::vector< double > &Y, const double &desired_x, double &output_y, const bool wrap_angle=false)
Interpolates a value from sampled data and optionally handles angle wrap-around.
Definition utils.cpp:23
bool trajectory2outputFrame(trajectory_planning_msgs::msg::Trajectory &trajectory)
Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required...
Definition utils.cpp:73
bool solveAndValidate(PerformanceMetrics &metrics, bool &finite_solution)
Solves the OCP, reads its output and checks primal feasibility.
Definition utils.cpp:414
void vizObjectBoxes(const std::vector< double > &objects)
Definition utils.cpp:306
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr boundary_pub_
std::vector< std::pair< double, double > > normalBoundaryDistance(const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
Computes minimum normal distances from the reference path to the active route boundaries.
Definition utils.cpp:112
void vizBoundaryPoints(const std::vector< Eigen::Vector2d > &left_boundary_points, const std::vector< Eigen::Vector2d > &right_boundary_points)
Publishes the boundary intersections corresponding to the distances passed to the OCP.
Definition utils.cpp:269
Namespace for trajectory_optimization package.
int acados_solve(ocp_model_capsule_t capsule)
Wrapper around the generated acados solve function.
void acados_print_stats(ocp_model_capsule_t capsule)
Wrapper around the generated acados statistics printer.
int acados_update_params_sparse(ocp_model_capsule_t capsule, int stage, int *idx, double *p, int n_update)
Wrapper around the generated sparse parameter update function.
const ocp_nlp_opts * acados_get_common_nlp_opts(ocp_nlp_config *config, void *solver_opts)
Returns the common NLP options stored inside the generated solver-specific options.