trajectory_optimization v1.5.0
Loading...
Searching...
No Matches
trajectory_optimization_node.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 <chrono>
7#include <cmath>
8#include <functional>
9#include <numeric>
10#include <stdexcept>
11#include <thread>
12#include <tuple>
13
15
21
22namespace {
23using SteadyClock = std::chrono::steady_clock;
24
25double elapsedMilliseconds(const SteadyClock::time_point& start) {
26 return std::chrono::duration<double, std::milli>(SteadyClock::now() - start).count();
27}
28
29double elapsedMilliseconds(const SteadyClock::time_point& start, const SteadyClock::time_point& end) {
30 return std::chrono::duration<double, std::milli>(end - start).count();
31}
32
33} // namespace
34
35TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_name, const rclcpp::NodeOptions& options)
36 : rclcpp::Node(node_name, options) {
37 // declare and load node parameters
38 this->declareAndLoadParameter("vehicle_frame_id", vehicle_frame_id_,
39 "Frame ID of local vehicle frame (the ocp is defined in this frame)");
40 this->declareAndLoadParameter("trajectory_frame_id", trajectory_frame_id_, "Frame ID of output trajectory");
41 this->declareAndLoadParameter("fixed_over_time_frame_id", fixed_over_time_frame_id_,
42 "Frame ID of frame that is fixed over time for finding temporal transforms");
43 this->declareAndLoadParameter("ego_data_timeout", ego_data_timeout_,
44 "Time after which a received ego vehicle data is considered invalid [s]. Optimization will not "
45 "be run if ego data is invalid.");
46 this->declareAndLoadParameter("model_name", model_name_,
47 "Name of the model to be used for trajectory optimization [karl, shuttle]");
48 this->declareAndLoadParameter("optimization_frequency", optimization_freq_, "Optimization frequency in Hz");
49 this->declareAndLoadParameter("n_shots", n_shots_, "Number of shooting intervals in optimization horizon");
50 this->declareAndLoadParameter("optimization_horizon", optimization_horizon_, "Optimization Horizon in seconds");
51 this->declareAndLoadParameter("two_stage_optimization", two_stage_optimization_,
52 "Initialize the constrained OCP with a solve without object and boundary constraints");
53 this->declareAndLoadParameter("relaxed_solve_timeout_ms", relaxed_solve_timeout_ms_,
54 "ACADOS timeout for the relaxed initialization solve [ms]; 0 disables the timeout");
55 this->declareAndLoadParameter("constrained_solve_timeout_ms", constrained_solve_timeout_ms_,
56 "ACADOS timeout for the fully constrained solve [ms]; 0 disables the timeout");
57 this->declareAndLoadParameter("verbose", verbose_, "Print solver statistics");
58 this->declareAndLoadParameter("performance_logging", performance_logging_,
59 "Write one CSV record for every completed solver run");
60 this->declareAndLoadParameter("debug_visualization", debug_viz_,
61 "Publish debug visualization markers for ego, objects and boundaries");
62 this->declareAndLoadParameter("run_as_callback", run_as_callback_,
63 "Run OCP once for each received reference trajectory (true) or on a timer (false)");
64 this->declareAndLoadParameter("cost_weights", cost_weights_, "Cost function weights");
65 this->declareAndLoadParameter("dynamic_weight", dynamic_weight_, "Dynamic weight alpha");
66 this->declareAndLoadParameter("thw", thw_, "Desired time headway to front vehicle [s]; soft when object slacks are enabled");
67 this->declareAndLoadParameter("d_min_object_long", d_min_object_long_,
68 "Desired longitudinal object clearance [m]; soft when object slacks are enabled");
69 this->declareAndLoadParameter("d_min_object_lat", d_min_object_lat_,
70 "Desired lateral object clearance [m]; soft when object slacks are enabled");
71 this->declareAndLoadParameter("d_min_boundary_lat", d_min_boundary_lat_,
72 "Minimum distance to keep to boundary in lateral direction [m]");
73 this->declareAndLoadParameter("standstill_threshold", standstill_threshold_,
74 "Threshold for standstill detection [m/s]. If the velocities of all states are below this "
75 "threshold, publish standstill trajectory");
76 this->declareAndLoadParameter("high_level_stabilization", high_level_stabilization_,
77 "Use high-level stabilization strategy for init state (= init with current EgoData)");
79 "consider_objects", consider_objects_,
80 "consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction)");
81 this->declareAndLoadParameter("ignore_objects_behind_ego", ignore_objects_behind_ego_,
82 "Exclude objects with reference-point x <= 0 in vehicle_frame_id");
83 this->declareAndLoadParameter("min_prediction_probability", min_prediction_probability_,
84 "Minimum probability for predicted object states to be considered", true, false, false, 0.0, 1.0);
86 "consider_boundaries", consider_boundaries_,
87 "consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space");
88 this->declareAndLoadParameter("bi_level_dV", bi_level_dV_,
89 "Threshold for bi-level stabilization: maximum velocity difference [m/s]");
90 this->declareAndLoadParameter("bi_level_dY", bi_level_dY_, "Threshold for bi-level stabilization: maximum y-offset [m]");
91 this->declareAndLoadParameter("bi_level_dYaw", bi_level_dYaw_,
92 "Threshold for bi-level stabilization: maximum yaw difference [degree]");
94 try {
95 performance_logger_ = std::make_unique<PerformanceLogger>(get_name());
96 RCLCPP_INFO(get_logger(), "Writing benchmark logs to '%s'.", performance_logger_->path().c_str());
97 } catch (const std::exception& error) {
98 RCLCPP_ERROR(get_logger(), "Could not initialize benchmark logging: %s", error.what());
99 }
100 }
101 this->setup();
102}
103
105
106template <typename T>
108 T& param,
109 const std::string& description,
110 const bool add_to_auto_reconfigurable_params,
111 const bool is_required,
112 const bool read_only,
113 const std::optional<double>& from_value,
114 const std::optional<double>& to_value,
115 const std::optional<double>& step_value,
116 const std::string& additional_constraints) {
117 rcl_interfaces::msg::ParameterDescriptor param_desc;
118 param_desc.description = description;
119 param_desc.additional_constraints = additional_constraints;
120 param_desc.read_only = read_only;
121
122 auto type = rclcpp::ParameterValue(param).get_type();
123
124 if (from_value.has_value() && to_value.has_value()) {
125 if constexpr (std::is_integral_v<T>) {
126 rcl_interfaces::msg::IntegerRange range;
127 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
128 if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
129 param_desc.integer_range = {range};
130 } else if constexpr (std::is_floating_point_v<T>) {
131 rcl_interfaces::msg::FloatingPointRange range;
132 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
133 if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
134 param_desc.floating_point_range = {range};
135 } else {
136 RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
137 }
138 }
139
140 this->declare_parameter(name, type, param_desc);
141
142 try {
143 param = this->get_parameter(name).get_value<T>();
144 std::stringstream ss;
145 ss << "Loaded parameter '" << name << "': ";
146 if constexpr (is_vector_v<T>) {
147 ss << "[";
148 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
149 ss << "]";
150 } else {
151 ss << param;
152 }
153 RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
154 } catch (rclcpp::exceptions::ParameterUninitializedException&) {
155 if (is_required) {
156 RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
157 exit(EXIT_FAILURE);
158 } else {
159 std::stringstream ss;
160 ss << "Missing parameter '" << name << "', using default value: ";
161 if constexpr (is_vector_v<T>) {
162 ss << "[";
163 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
164 ss << "]";
165 } else {
166 ss << param;
167 }
168 RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
169 this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
170 }
171 }
172
173 if (add_to_auto_reconfigurable_params) {
174 std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
175 auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
176 }
177}
178
179rcl_interfaces::msg::SetParametersResult TrajectoryOptimizationNode::parametersCallback(
180 const std::vector<rclcpp::Parameter>& parameters) {
181 for (const auto& param : parameters) {
182 for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
183 if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
184 std::get<1>(auto_reconfigurable_param)(param);
185 RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s' to: %s", param.get_name().c_str(),
186 param.value_to_string().c_str());
187 break;
188 }
189 }
190 // handle special cases
191 if (param.get_name() == "run_as_callback") {
193 planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
195 RCLCPP_WARN(this->get_logger(), "OCP runs now periodically with frequency %f Hz", optimization_freq_);
196 } else if (run_as_callback_ && planning_timer_) {
197 planning_timer_->cancel();
198 planning_timer_.reset();
199 RCLCPP_WARN(this->get_logger(), "OCP runs now on reference trajectory callback");
200 }
201 }
202 }
203 // mark parameter change successful
204 rcl_interfaces::msg::SetParametersResult result;
205 result.successful = true;
206
207 return result;
208}
209
211 tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
212 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
213
214 // create a callback for dynamic parameter configuration
215 parameters_callback_ = this->add_on_set_parameters_callback(
216 std::bind(&TrajectoryOptimizationNode::parametersCallback, this, std::placeholders::_1));
217
218 // set up subscriber for input topics
219 ego_data_sub_ = this->create_subscription<perception_msgs::msg::EgoData>(
220 "~/ego_data", 1, std::bind(&TrajectoryOptimizationNode::egoDataCallback, this, std::placeholders::_1));
221 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", ego_data_sub_->get_topic_name());
222
223 object_list_sub_ = this->create_subscription<perception_msgs::msg::ObjectList>(
224 "~/object_list", 1, std::bind(&TrajectoryOptimizationNode::objectListCallback, this, std::placeholders::_1));
225 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", object_list_sub_->get_topic_name());
226
227 route_sub_ = this->create_subscription<route_planning_msgs::msg::Route>(
228 "~/route", 1, std::bind(&TrajectoryOptimizationNode::routeCallback, this, std::placeholders::_1));
229 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", route_sub_->get_topic_name());
230
231 reference_trajectory_sub_ = this->create_subscription<trajectory_planning_msgs::msg::Trajectory>(
232 "~/reference_trajectory", 1,
233 std::bind(&TrajectoryOptimizationNode::referenceTrajectoryCallback, this, std::placeholders::_1));
234 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", reference_trajectory_sub_->get_topic_name());
235
236 // set up publisher for output topics
237 trajectory_pub_ = this->create_publisher<trajectory_planning_msgs::msg::Trajectory>("~/trajectory", 1);
238 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", trajectory_pub_->get_topic_name());
239 object_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/objects", 1);
240 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", object_marker_pub_->get_topic_name());
241 ego_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/ego", 1);
242 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", ego_marker_pub_->get_topic_name());
243 boundary_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/boundaries", 1);
244 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", boundary_pub_->get_topic_name());
245
246 // create timer for planning cycle
247 if (run_as_callback_) {
248 RCLCPP_INFO(this->get_logger(), "OCP runs on reference trajectory callback");
249 } else {
250 RCLCPP_INFO(this->get_logger(), "OCP runs continuously with frequency %f Hz", optimization_freq_);
251 planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
253 }
254
255 // init reference trajectory
256 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
257 reference_trajectory_, trajectory_planning_msgs::msg::REFERENCE::TYPE_ID, n_shots_ + 1);
259
260 // init latest trajectory (doesn't matter which type, only used for standstill detection)
261 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
262 latest_valid_trajectory_, trajectory_planning_msgs::msg::DRIVABLE::TYPE_ID, n_shots_ + 1);
264
265 setupSolver();
266
267 // Annotate message links for tracing: Publish trajectory periodically, which depends an all subscribed topics.
268 std::vector<const void*> link_subs;
269 link_subs.push_back(static_cast<const void*>(ego_data_sub_->get_subscription_handle().get()));
270 link_subs.push_back(static_cast<const void*>(object_list_sub_->get_subscription_handle().get()));
271 link_subs.push_back(static_cast<const void*>(route_sub_->get_subscription_handle().get()));
272 link_subs.push_back(static_cast<const void*>(reference_trajectory_sub_->get_subscription_handle().get()));
273 std::vector<const void*> link_pubs;
274 link_pubs.push_back(static_cast<const void*>(trajectory_pub_->get_publisher_handle().get()));
275 RCLCPP_INFO(get_logger(), "Annotating message links for tracing with %zu subscriptions and %zu publications", link_subs.size(),
276 link_pubs.size());
277 TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
278}
279
281 if (n_shots_ <= 0) {
282 RCLCPP_FATAL(this->get_logger(), "n_shots must be > 0, got %d", n_shots_);
283 exit(1);
284 }
285
286 // Create exactly once. This also supports a runtime horizon that differs from the generated default.
288 std::vector<double> new_time_steps(n_shots_, optimization_horizon_ / n_shots_);
289 RCLCPP_INFO(this->get_logger(), "Create OCP with: horizon = %f, n_shots = %d, dt = %f", optimization_horizon_, n_shots_,
290 new_time_steps.front());
292
293 if (status != ACADOS_SUCCESS) {
294 RCLCPP_FATAL(this->get_logger(), "%s_acados_create_with_discretization() returned status %d. Exiting.", model_name_.c_str(),
295 status);
296 exit(1);
297 }
298
306 if (nlp_dims_->N != n_shots_) {
307 RCLCPP_FATAL(this->get_logger(), "Created solver has N=%d, expected n_shots=%d. Exiting.", nlp_dims_->N, n_shots_);
308 exit(1);
309 }
310
311 xtraj_.resize(*nlp_dims_->nx * (n_shots_ + 1));
312 utraj_.resize(*nlp_dims_->nu * n_shots_);
313 control_guess_.clear();
314}
315
317 // free solver
319 if (status != 0) {
320 RCLCPP_ERROR(this->get_logger(), "%s_acados_free() returned status %d.", model_name_.c_str(), status);
321 }
322 // free solver capsule
324 if (status != 0) {
325 RCLCPP_ERROR(this->get_logger(), "%s_acados_free_capsule() returned status %d.", model_name_.c_str(), status);
326 }
327}
328
330 const int status = trajectory_optimization::acados_reset(ocp_capsule_, 1, 0, 0, 0);
331 if (status != ACADOS_SUCCESS) {
332 RCLCPP_ERROR(this->get_logger(), "%s_acados_reset() returned status %d. Recreating solver.", model_name_.c_str(), status);
333 freeSolver();
334 setupSolver();
335 }
336}
337
338bool TrajectoryOptimizationNode::setInitialGuess(const std::vector<double>& x_init, const rclcpp::Time& stamp) {
339 constexpr double MAX_CONTROL_GUESS_AGE_FACTOR = 0.5;
340 const int nx = *nlp_dims_->nx;
341 const int nu = *nlp_dims_->nu;
342 const double time_step = optimization_horizon_ / n_shots_;
343 const size_t expected_control_size = static_cast<size_t>(nu * n_shots_);
344 if (x_init.size() != static_cast<size_t>(nx)) {
345 RCLCPP_ERROR(get_logger(), "Initial state has size %zu, expected %d.", x_init.size(), nx);
346 return false;
347 }
348
349 // Fall back to zero controls if no sufficiently recent solver output is available.
350 std::vector<double> controls(expected_control_size, 0.0);
351 if (control_guess_.size() == expected_control_size) {
352 const double elapsed = (stamp - control_guess_stamp_).seconds();
353 if (elapsed >= 0.0 && elapsed < MAX_CONTROL_GUESS_AGE_FACTOR * optimization_horizon_) {
354 // Shift the previous controls to the current planning time and interpolate between shooting nodes.
355 std::vector<double> lower_controls(nu);
356 std::vector<double> upper_controls(nu);
357 for (int stage = 0; stage < n_shots_; ++stage) {
358 const double previous_stage = (elapsed + stage * time_step) / time_step;
359 if (previous_stage >= n_shots_) break;
360
361 const int lower_stage = static_cast<int>(std::floor(previous_stage));
362 const double interpolation_factor = previous_stage - lower_stage;
363 ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "lbu", lower_controls.data());
364 ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "ubu", upper_controls.data());
365 for (int control = 0; control < nu; ++control) {
366 const double lower_value = control_guess_[lower_stage * nu + control];
367 const double upper_value = lower_stage + 1 < n_shots_ ? control_guess_[(lower_stage + 1) * nu + control] : 0.0;
368 const double interpolated_control = lower_value + interpolation_factor * (upper_value - lower_value);
369 controls[stage * nu + control] = std::clamp(interpolated_control, lower_controls[control], upper_controls[control]);
370 }
371 }
372 }
373 }
374
375 ocp_nlp_out_set_values_to_zero(nlp_config_, nlp_dims_, nlp_out_);
376 std::vector<double> rollout_state = x_init;
377 std::vector<double> intermediate_state(nx);
378 std::vector<double> k1(nx), k2(nx), k3(nx), k4(nx);
379 const double integration_step = time_step / 4.0;
380 for (int stage = 0; stage < n_shots_; ++stage) {
381 double* control = &controls[stage * nu];
382 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "x", rollout_state.data());
383 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "u", control);
384
385 // Match sim_method_num_stages=4 and sim_method_num_steps=4 from the generated OCP.
386 for (int integration = 0; integration < 4; ++integration) {
387 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, rollout_state.data(), control, k1.data());
388 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k1[i];
389 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k2.data());
390 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k2[i];
391 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k3.data());
392 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + integration_step * k3[i];
393 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k4.data());
394 for (int i = 0; i < nx; ++i) {
395 rollout_state[i] += integration_step / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
396 }
397 }
398 }
399 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, n_shots_, "x", rollout_state.data());
400 return true;
401}
402
404 const auto cycle_start = SteadyClock::now();
405 if (debug_viz_) viz_object_boxes_.clear();
406 if (rclcpp::Time(this->now()) - rclcpp::Time(ego_data_.header.stamp) > rclcpp::Duration::from_seconds(ego_data_timeout_)) {
407 RCLCPP_WARN(this->get_logger(), "EgoData outdated. Skipping planning cycle.");
408 return;
409 }
410 // init trajectory message and set header
411 trajectory_planning_msgs::msg::Trajectory::UniquePtr trajectory = std::make_unique<trajectory_planning_msgs::msg::Trajectory>();
412 initializeTrajectory(*trajectory);
413
414 trajectory->header.frame_id = vehicle_frame_id_;
415 trajectory->header.stamp = ego_data_.header.stamp; // use latest ego_data stamp as trajectory stamp
416
417 // init time-steps of trajectory to ensure increasing time-steps even for standstill trajectories
418 double dt = optimization_horizon_ / n_shots_;
419 for (int i = 0; i <= n_shots_; ++i) trajectory_planning_msgs::trajectory_access::setT(*trajectory, i * dt, i);
420
421 // check if the reference trajectory is standstill
422 if (trajectory_planning_msgs::trajectory_access::getStandstill(reference_trajectory_)) {
423 RCLCPP_WARN(this->get_logger(), "Standstill trajectory. Skipping planning cycle. Publish standstill trajectory.");
424 // transform trajectory to output frame
425 if (!trajectory2outputFrame(*trajectory)) {
426 return;
427 }
428 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, true);
429 trajectory_pub_->publish(std::move(trajectory));
430 // Invalidate the warm start and reset the solver once when entering standstill.
431 if (!control_guess_.empty()) {
432 control_guess_.clear();
433 resetSolver();
434 }
435 return;
436 }
437
438 PerformanceMetrics metrics;
439 metrics.cycle = ++logging_cycle_;
440 metrics.ego_stamp_ns = rclcpp::Time(ego_data_.header.stamp).nanoseconds();
441 metrics.reference_stamp_ns = rclcpp::Time(reference_trajectory_.header.stamp).nanoseconds();
442 metrics.route_stamp_ns = rclcpp::Time(route_.header.stamp).nanoseconds();
443 metrics.reference_points = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory_);
444 metrics.objects = static_cast<int>(object_list_.objects.size());
445 auto logCompletedCycle = [&]() {
446 metrics.cycle_ms = elapsedMilliseconds(cycle_start);
447 metrics.postprocessing_ms = metrics.cycle_ms - metrics.preprocessing_ms - metrics.solve_wall_ms;
449 performance_logger_->write(metrics);
450 }
451 };
452
453 // set initial state
454 std::vector<double> x_init(*nlp_dims_->nx, 0.0);
455 if (!trajectory_planning_msgs::trajectory_access::getStandstill(latest_valid_trajectory_)) {
457 } else {
458 RCLCPP_WARN(this->get_logger(),
459 "Latest available trajectory is standstill. Using ego data for initial state (high-level initialization).");
460 x_init = getHighLevelX0(ego_data_);
461 control_guess_.clear(); // rebuild the warm start from zero controls after a standstill trajectory.
462 }
463
464 // project stationary initial states onto forward-only motion.
465 x_init[3] = std::max(x_init[3], 0.0);
466 if (x_init[3] == 0.0) {
467 x_init[4] = std::max(x_init[4], 0.0);
468 }
469
470 // debug print of initial state
471 std::stringstream ss;
472 ss << "Initial state: ";
473 for (size_t i = 0; i < x_init.size(); ++i) ss << "x[" << i << "]: " << x_init[i] << (i != x_init.size() - 1 ? ", " : "");
474 RCLCPP_DEBUG(this->get_logger(), "%s", ss.str().c_str());
475
476 ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "lbx", x_init.data());
477 ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "ubx", x_init.data());
478
479 // update inputs to the ocp; skip planning cycle if update fails
481 RCLCPP_WARN(this->get_logger(), "Failed to update inputs. Skipping planning cycle.");
482 return;
483 }
484
485 if (!setInitialGuess(x_init, rclcpp::Time(ego_data_.header.stamp))) {
486 control_guess_.clear();
487 return;
488 }
489
490 // start solving the OCP
491 // first solve the relaxed OCP (if configured and necessary)
492 // then solve the constrained OCP using either the relaxed solution or the regular warm start
493 const auto solve_start = SteadyClock::now();
494 metrics.preprocessing_ms = elapsedMilliseconds(cycle_start, solve_start);
496 bool finite_solution = false;
497 bool primal_feasible = false;
498
499 // solve the relaxed OCP (if configured and necessary)
501 metrics.relaxed_attempted = true;
502 if (!setSafetyConstraintActivation(false)) return;
503
505 const auto relaxed_solve_start = SteadyClock::now();
506 const bool relaxed_primal_feasible = solveAndValidate(metrics, finite_solution);
507 metrics.relaxed_solve_wall_ms = elapsedMilliseconds(relaxed_solve_start);
508 metrics.relaxed_status = metrics.status;
509
510 // evaluate the relaxed residuals before restoration, but retain its nlp_out_
511 if (!setSafetyConstraintActivation(true)) return;
512
513 if (!relaxed_primal_feasible) {
514 metrics.failure_phase = "relaxed";
515 metrics.solve_wall_ms = elapsedMilliseconds(solve_start);
516 printSolution(metrics);
517 RCLCPP_WARN(get_logger(),
518 "Rejecting relaxed initialization: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] "
519 "tolerances=[eq=%e, ineq=%e].",
520 metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
521 resetSolver();
522 logCompletedCycle();
523 return;
524 }
525 }
526
527 // solve the constrained OCP using either the relaxed solution or the regular warm start
529 const auto constrained_solve_start = SteadyClock::now();
530 primal_feasible = solveAndValidate(metrics, finite_solution);
531 metrics.constrained_solve_wall_ms = elapsedMilliseconds(constrained_solve_start);
532 metrics.solve_wall_ms = elapsedMilliseconds(solve_start);
533
534 if (!primal_feasible) {
535 metrics.failure_phase = "constrained";
536 printSolution(metrics);
537 RCLCPP_WARN(this->get_logger(),
538 "Rejecting solver output: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] tolerances=[eq=%e, ineq=%e].",
539 metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
540 if (finite_solution && performance_logger_) {
542 }
543 // keep the last published solution as warm-start source
544 resetSolver();
545 logCompletedCycle();
546 return;
547 }
548
549 // Remove negative roundoff within the accepted feasibility tolerance.
550 for (int stage = 0; stage <= n_shots_; ++stage) {
551 xtraj_[stage * *nlp_dims_->nx + 3] = std::max(xtraj_[stage * *nlp_dims_->nx + 3], 0.0);
552 }
553
555 control_guess_stamp_ = rclcpp::Time(ego_data_.header.stamp);
556
557 printSolution(metrics);
558 if (debug_viz_) {
561 }
562
563 // convert output into trajectory message
564 convertToTrajectoryMsg(*trajectory);
565
566 bool standstill = true;
567 for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(*trajectory); ++i) {
568 if (trajectory_planning_msgs::trajectory_access::getV(*trajectory, i) > standstill_threshold_) standstill = false;
569 }
570 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, standstill);
571
572 // transform trajectory to output frame
573 if (!trajectory2outputFrame(*trajectory)) {
574 logCompletedCycle();
575 return;
576 }
577
578 latest_valid_trajectory_ = *trajectory;
579 trajectory_pub_->publish(std::move(trajectory));
580 metrics.published = true;
581 logCompletedCycle();
582 const char* cycle_time_color = metrics.cycle_ms <= 100.0 ? "\x1b[32m" : "\x1b[31m";
583 if (metrics.cycle_ms > 100.0) {
584 if (metrics.relaxed_attempted) {
585 RCLCPP_INFO(this->get_logger(),
586 "Published trajectory (cycle: %s%.2f ms\x1b[0m; relaxed solve: %.2f ms; constrained solve: %.2f ms)",
587 cycle_time_color, metrics.cycle_ms, metrics.relaxed_solve_wall_ms, metrics.constrained_solve_wall_ms);
588 } else {
589 RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m; constrained solve: %.2f ms)",
590 cycle_time_color, metrics.cycle_ms, metrics.constrained_solve_wall_ms);
591 }
592 } else {
593 RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m)", cycle_time_color, metrics.cycle_ms);
594 }
595}
596
597bool TrajectoryOptimizationNode::updateOcpInputs(const perception_msgs::msg::EgoData& ego_data,
598 const perception_msgs::msg::ObjectList& object_list,
599 const route_planning_msgs::msg::Route& route,
600 const trajectory_planning_msgs::msg::Trajectory& reference_trajectory) {
601 // transform inputs to target base_link frame
602 trajectory_planning_msgs::msg::Trajectory tf_reference_trajectory;
603 perception_msgs::msg::ObjectList tf_object_list;
604 route_planning_msgs::msg::Route tf_route;
605 try {
606 // reference trajectory
607 tf_reference_trajectory =
608 tf2_buffer_->transform(reference_trajectory, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
609 fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
610 // object list
611 if (!object_list.objects.empty() && object_list.header.frame_id != vehicle_frame_id_) {
612 tf_object_list = tf2_buffer_->transform(object_list, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
613 fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
614 } else {
615 tf_object_list = object_list;
616 }
617 keepNClosestObjects(tf_object_list, static_cast<int>(p_objects_shape_[0]), ignore_objects_behind_ego_);
618 // route
619 if (!route.route_elements.empty() && route.header.frame_id != vehicle_frame_id_) {
620 tf_route = tf2_buffer_->transform(route, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
621 fixed_over_time_frame_id_, tf2::durationFromSec(0.1));
622 } else {
623 tf_route = route;
624 }
625 } catch (tf2::TransformException& ex) {
626 RCLCPP_WARN(this->get_logger(), "Transformation is not available. Ex: %s", ex.what());
627 return false;
628 }
629
630 if (trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) <= 0) {
631 RCLCPP_ERROR(this->get_logger(), "Reference trajectory contains no sample points.");
632 return false;
633 }
634
635 // update ocp parameters
636 try {
637 this->setOcpGlobalParameters(cost_weights_, tf_reference_trajectory, tf_route);
638 this->setOcpParameters(ego_data, tf_object_list);
639 } catch (const std::exception& e) {
640 RCLCPP_ERROR(this->get_logger(), "Exception while setting OCP parameters: %s", e.what());
641 return false;
642 }
643
644 return true;
645}
646
647void TrajectoryOptimizationNode::setOcpGlobalParameters(const std::vector<double>& cost_weights,
648 const trajectory_planning_msgs::msg::Trajectory& reference_trajectory,
649 const route_planning_msgs::msg::Route& route) {
650 const auto start_time = std::chrono::steady_clock::now();
651 std::vector<double> global_params;
652
653 // cost weights
654 const auto expected_cost_weights_size = static_cast<size_t>(p_cost_weights_shape_[0] * p_cost_weights_shape_[1]);
655 if (cost_weights.size() != expected_cost_weights_size) {
656 RCLCPP_ERROR(this->get_logger(), "Size of cost_weights (%zu) does not match expected size (%zu).", cost_weights.size(),
657 expected_cost_weights_size);
658 throw std::runtime_error("Size of cost_weights does not match expected size.");
659 }
660 global_params.insert(global_params.end(), cost_weights.begin(), cost_weights.end());
661
662 // other cost params
663 global_params.push_back(thw_);
664 global_params.push_back(d_min_object_long_);
665 global_params.push_back(d_min_object_lat_);
666 global_params.push_back(d_min_boundary_lat_);
667
668 // reference path (including boundaries)
669 const size_t n_ref_states = static_cast<size_t>(p_ref_path_shape_[0] * p_ref_path_shape_[1]);
670 std::vector<std::pair<double, double>> boundary_distances = normalBoundaryDistance(reference_trajectory, route);
671 // fill ref vector for ocp -> psi, x, y, v, d_bound_left, d_bound_right
672 std::vector<double> ref;
673 for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory); ++i) {
674 ref.push_back(trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i));
675 ref.push_back(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i));
676 ref.push_back(trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
677 ref.push_back(trajectory_planning_msgs::trajectory_access::getV(reference_trajectory, i));
678 ref.push_back(boundary_distances[i].first); // left boundary distance
679 ref.push_back(boundary_distances[i].second); // right boundary distance
680 }
681 if (ref.size() >= n_ref_states) {
682 global_params.insert(global_params.end(), ref.begin(),
683 ref.begin() + static_cast<std::vector<double>::difference_type>(n_ref_states));
684 } else {
685 // Repeat the final valid state. Infinity padding can propagate NaNs through closest-point calculations.
686 global_params.insert(global_params.end(), ref.begin(), ref.end());
687 const size_t state_width = static_cast<size_t>(p_ref_path_shape_[1]);
688 while (global_params.size() < expected_cost_weights_size + 4 + n_ref_states) {
689 global_params.insert(global_params.end(), ref.end() - static_cast<std::vector<double>::difference_type>(state_width),
690 ref.end());
691 }
692 }
693
694 if (global_params.size() != static_cast<size_t>(nlp_dims_->np_global)) {
695 RCLCPP_ERROR(this->get_logger(), "Size of global parameters (%zu) does not match expected size (%d).", global_params.size(),
696 nlp_dims_->np_global);
697 throw std::runtime_error("Size of global parameters does not match expected size.");
698 }
700 ocp_capsule_, global_params.data(), static_cast<int>(global_params.size()));
701 if (status != ACADOS_SUCCESS) {
702 throw std::runtime_error("acados global parameter update failed with status " + std::to_string(status));
703 }
704 const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
705 RCLCPP_DEBUG(this->get_logger(), "setOcpGlobalParameters duration: %.3f ms", elapsed_ms);
706}
707
708void TrajectoryOptimizationNode::setOcpParameters(const perception_msgs::msg::EgoData& ego_data,
709 const perception_msgs::msg::ObjectList& object_list) {
710 const auto start_time = std::chrono::steady_clock::now();
711
712 // prepare object predictions
713 struct PredictionData {
714 std::vector<double> time;
715 std::vector<double> x;
716 std::vector<double> y;
717 std::vector<double> yaw;
718 };
719
720 std::vector<std::vector<PredictionData>> object_predictions(object_list.objects.size());
721 const double object_stamp = static_cast<double>(rclcpp::Time(object_list.header.stamp).nanoseconds()) / 1e9;
723 for (size_t object_index = 0; object_index < object_list.objects.size(); ++object_index) {
724 const auto& object = object_list.objects[object_index];
725 auto append_prediction = [&](const auto& prediction) {
726 PredictionData prediction_data{{object_stamp},
727 {perception_msgs::object_access::getX(object)},
728 {perception_msgs::object_access::getY(object)},
729 {perception_msgs::object_access::getYaw(object)}};
730 for (const auto& state : prediction.states) {
731 prediction_data.time.push_back(static_cast<double>(rclcpp::Time(state.header.stamp).nanoseconds()) / 1e9);
732 prediction_data.x.push_back(perception_msgs::object_access::getX(state));
733 prediction_data.y.push_back(perception_msgs::object_access::getY(state));
734 prediction_data.yaw.push_back(perception_msgs::object_access::getYaw(state));
735 }
736 object_predictions[object_index].push_back(std::move(prediction_data));
737 };
738
739 // keep all predictions above the configured probability threshold
740 for (const auto& prediction : object.state_predictions) {
741 if (prediction.probability > min_prediction_probability_) append_prediction(prediction);
742 }
743
744 // fall back to the first prediction if none satisfies the threshold
745 if (object_predictions[object_index].empty() && !object.state_predictions.empty()) {
746 append_prediction(object.state_predictions[0]);
747 }
748 }
749 }
750
752 constexpr double MIN_HALF_EXTENT = 0.05;
753
754 // loop over shooting intervals
755 const double dt = optimization_horizon_ / n_shots_;
756 double floating_dynamic_weight = 1.0;
757 for (int stage = 0; stage <= n_shots_; ++stage) {
758 // dynamic weight
759 int idx = 0;
760 int parameter_count = static_cast<int>(p_dynamic_weight_shape_[0] * p_dynamic_weight_shape_[1]);
761 std::vector<int> dynamic_weight_indices{idx};
762 int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, dynamic_weight_indices.data(),
763 &floating_dynamic_weight, parameter_count);
764 if (status != ACADOS_SUCCESS) {
765 throw std::runtime_error("acados dynamic-weight update failed with status " + std::to_string(status));
766 }
767 floating_dynamic_weight *= dynamic_weight_;
768
769 // constraint activation
770 idx += parameter_count;
771 parameter_count = static_cast<int>(p_constraint_activation_shape_[0] * p_constraint_activation_shape_[1]);
772 std::array<int, 2> constraint_activation_indices{idx, idx + 1};
773 std::array<double, 2> constraint_activation{object_constraints_active_ ? 1.0 : 0.0,
775 status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, constraint_activation_indices.data(),
776 constraint_activation.data(), parameter_count);
777 if (status != ACADOS_SUCCESS) {
778 throw std::runtime_error("acados constraint-activation update failed with status " + std::to_string(status));
779 }
780
781 // objects
782 idx += parameter_count;
783 parameter_count = static_cast<int>(p_objects_shape_[0] * p_objects_shape_[1]);
784 std::vector<double> object_boxes;
785 object_boxes.reserve(parameter_count);
786 for (size_t object_index = 0; object_index < object_list.objects.size(); ++object_index) {
787 const auto& object = object_list.objects[object_index];
788 std::vector<std::array<double, 3>> target_states;
789 if (!object_predictions[object_index].empty()) {
790 for (const auto& prediction : object_predictions[object_index]) {
791 const double desired_time = static_cast<double>(rclcpp::Time(ego_data.header.stamp).nanoseconds()) / 1e9 + dt * stage;
792 std::array<double, 3> state{};
793 linearInterpolation(prediction.time, prediction.x, desired_time, state[0]);
794 linearInterpolation(prediction.time, prediction.y, desired_time, state[1]);
795 linearInterpolation(prediction.time, prediction.yaw, desired_time, state[2], true);
796 target_states.push_back(state);
797 }
798 } else {
799 // treat objects without a selected prediction as static
800 target_states.push_back({perception_msgs::object_access::getX(object), perception_msgs::object_access::getY(object),
801 perception_msgs::object_access::getYaw(object)});
802 }
803
804 const double offset_long = object.state.reference_point.translation_to_geometric_center.x;
805 const double offset_lat = object.state.reference_point.translation_to_geometric_center.y;
806 const double half_length = std::max(MIN_HALF_EXTENT, 0.5 * perception_msgs::object_access::getLength(object));
807 const double half_width = std::max(MIN_HALF_EXTENT, 0.5 * perception_msgs::object_access::getWidth(object));
808 for (const auto& state : target_states) {
809 // ensure that the object position represents its geometric center
810 const double cos_yaw = std::cos(state[2]);
811 const double sin_yaw = std::sin(state[2]);
812 const double center_x = state[0] + offset_long * cos_yaw - offset_lat * sin_yaw;
813 const double center_y = state[1] + offset_long * sin_yaw + offset_lat * cos_yaw;
814 object_boxes.insert(object_boxes.end(), {center_x, center_y, state[2], half_length, half_width, 1.0});
815 if (object_boxes.size() >= static_cast<size_t>(parameter_count)) {
816 object_boxes.resize(parameter_count);
817 break;
818 }
819 }
820 if (object_boxes.size() >= static_cast<size_t>(parameter_count)) break;
821 }
822 // fill remaining object slots with inactive boxes
823 while (object_boxes.size() < static_cast<size_t>(parameter_count)) {
824 object_boxes.insert(object_boxes.end(), {0.0, 0.0, 0.0, MIN_HALF_EXTENT, MIN_HALF_EXTENT, 0.0});
825 }
826 if (debug_viz_) viz_object_boxes_.insert(viz_object_boxes_.end(), object_boxes.begin(), object_boxes.end());
827
828 std::vector<int> object_indices(parameter_count);
829 std::iota(object_indices.begin(), object_indices.end(), idx);
830 status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, object_indices.data(), object_boxes.data(),
831 parameter_count);
832 if (status != ACADOS_SUCCESS) {
833 throw std::runtime_error("acados object-box update failed with status " + std::to_string(status));
834 }
835 }
836 const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
837 RCLCPP_DEBUG(get_logger(), "setOcpParameters object-box duration: %.3f ms", elapsed_ms);
838}
839
840} // namespace trajectory_optimization
static void collectConstraintDiagnostics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, const ocp_nlp_dims *dims, int object_boxes)
Collects the largest equality and inequality constraint violations from acados.
void printSolution(const PerformanceMetrics &metrics)
Logs solver status and optional debug statistics for the last optimization run.
Definition utils.cpp:442
void setupSolver()
Creates the acados solver instance and initializes its state buffers.
trajectory_planning_msgs::msg::Trajectory reference_trajectory_
void setup()
Creates ROS interfaces, initializes cached messages and prepares the solver.
void routeCallback(const route_planning_msgs::msg::Route::ConstSharedPtr msg)
Stores the current route used for boundary constraints when enabled.
Definition callbacks.cpp:31
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
void freeSolver()
Frees the acados solver and clears cached optimization results.
bool setSafetyConstraintActivation(bool use_configured_activation)
Enables the configured object and boundary constraints or temporarily disables all of them.
Definition utils.cpp:395
trajectory_planning_msgs::msg::Trajectory latest_valid_trajectory_
virtual void initializeTrajectory(trajectory_planning_msgs::msg::Trajectory &trajectory)=0
Initializes an output trajectory message with the model-specific message type and size.
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_
void setOcpParameters(const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list)
Writes stage-wise object, boundary-activation, and dynamic-weighting parameters into the OCP.
virtual void convertToTrajectoryMsg(trajectory_planning_msgs::msg::Trajectory &trajectory)=0
Maps the optimized state trajectory into the model-specific output message fields.
void objectListCallback(const perception_msgs::msg::ObjectList::ConstSharedPtr msg)
Stores the current object list when object handling is enabled.
Definition callbacks.cpp:12
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
void egoDataCallback(const perception_msgs::msg::EgoData::ConstSharedPtr msg)
Stores the latest ego state used by the optimizer.
Definition callbacks.cpp:7
void planningCycle()
Runs one full planning cycle from input preparation, over solver execution, to trajectory publication...
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > > auto_reconfigurable_params_
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
bool updateOcpInputs(const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list, const route_planning_msgs::msg::Route &route, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory)
Transforms external inputs into the optimizer frame and writes them into the OCP.
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr route_sub_
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Applies parameter updates that can be reconfigured while the node is running.
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
virtual std::vector< double > getBiLevelX0(const perception_msgs::msg::EgoData &ego_data)=0
Computes the initial optimizer state using bi-level stabilizaion based on the model-specific EgoData ...
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr ego_data_sub_
TrajectoryOptimizationNode(const std::string node_name, const rclcpp::NodeOptions &options)
Initializes the base node and loads the common optimizer configuration.
void declareAndLoadParameter(const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
Declares a ROS parameter, loads its value and optionally registers it for runtime updates.
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr object_list_sub_
void referenceTrajectoryCallback(const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
Updates the reference trajectory and optionally triggers optimization immediately.
Definition callbacks.cpp:22
~TrajectoryOptimizationNode() override
Releases solver resources owned by the node.
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr trajectory_pub_
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr reference_trajectory_sub_
void setOcpGlobalParameters(const std::vector< double > &cost_weights, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
Writes stage-independent data into the OCP.
bool setInitialGuess(const std::vector< double > &x_init, const rclcpp::Time &stamp)
Builds and sets a dynamically consistent NLP initial guess from the current state and cached controls...
void resetSolver()
Resets the optimizer memory while retaining the generated solver instance.
virtual std::vector< double > getHighLevelX0(const perception_msgs::msg::EgoData &ego_data)=0
Computes the initial optimizer state using high-level stabilization based on the model-specific EgoDa...
Namespace for trajectory_optimization package.
int acados_free(ocp_model_capsule_t capsule)
Wrapper around the generated acados solver cleanup function.
ocp_nlp_solver * acados_get_nlp_solver(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP solver instance.
ocp_nlp_out * acados_get_nlp_out(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP output structure.
ocp_nlp_config * acados_get_nlp_config(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP configuration.
int acados_create_with_discretization(ocp_model_capsule_t capsule, int n_time_steps, double *new_time_steps)
Wrapper around the generated acados create function with custom discretization.
void * acados_get_nlp_opts(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP solver options.
int acados_free_capsule(ocp_model_capsule_t capsule)
Wrapper around the generated acados capsule cleanup function.
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.
ocp_model_capsule_t acados_create_capsule(const std::string &model_name)
Creates the model-specific acados capsule selected by the configured model name.
ocp_nlp_dims * acados_get_nlp_dims(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP dimensions.
int acados_reset(ocp_model_capsule_t capsule, int reset_qp_solver_mem, int reset_numerical_values, int reset_solver_options, int reset_x_to_x0_bar)
Resets selected solver memory without freeing and recreating the capsule.
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.
ocp_nlp_in * acados_get_nlp_in(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP input structure.
void acados_evaluate_dynamics(ocp_model_capsule_t capsule, int stage, double *x, double *u, double *x_dot)
Evaluates the explicit model dynamics for a shooting stage.
int acados_set_p_global_and_precompute_dependencies(ocp_model_capsule_t capsule, double *data, int data_len)
Wrapper around the generated global-parameter update function.