23using SteadyClock = std::chrono::steady_clock;
25double elapsedMilliseconds(
const SteadyClock::time_point& start) {
26 return std::chrono::duration<double, std::milli>(SteadyClock::now() - start).count();
29double elapsedMilliseconds(
const SteadyClock::time_point& start,
const SteadyClock::time_point& end) {
30 return std::chrono::duration<double, std::milli>(end - start).count();
36 : rclcpp::Node(node_name, options) {
39 "Frame ID of local vehicle frame (the ocp is defined in this frame)");
42 "Frame ID of frame that is fixed over time for finding temporal transforms");
44 "Time after which a received ego vehicle data is considered invalid [s]. Optimization will not "
45 "be run if ego data is invalid.");
47 "Name of the model to be used for trajectory optimization [karl, shuttle]");
52 "Initialize the constrained OCP with a solve without object and boundary constraints");
54 "ACADOS timeout for the relaxed initialization solve [ms]; 0 disables the timeout");
56 "ACADOS timeout for the fully constrained solve [ms]; 0 disables the timeout");
59 "Write one CSV record for every completed solver run");
61 "Publish debug visualization markers for ego, objects and boundaries");
63 "Run OCP once for each received reference trajectory (true) or on a timer (false)");
68 "Desired longitudinal object clearance [m]; soft when object slacks are enabled");
70 "Desired lateral object clearance [m]; soft when object slacks are enabled");
72 "Minimum distance to keep to boundary in lateral direction [m]");
74 "Threshold for standstill detection [m/s]. If the velocities of all states are below this "
75 "threshold, publish standstill trajectory");
77 "Use high-level stabilization strategy for init state (= init with current EgoData)");
80 "consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction)");
82 "Exclude objects with reference-point x <= 0 in vehicle_frame_id");
84 "Minimum probability for predicted object states to be considered",
true,
false,
false, 0.0, 1.0);
87 "consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space");
89 "Threshold for bi-level stabilization: maximum velocity difference [m/s]");
92 "Threshold for bi-level stabilization: maximum yaw difference [degree]");
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());
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;
122 auto type = rclcpp::ParameterValue(param).get_type();
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};
136 RCLCPP_WARN(this->get_logger(),
"Parameter type of parameter '%s' does not support specifying a range", name.c_str());
140 this->declare_parameter(name, type, param_desc);
143 param = this->get_parameter(name).get_value<T>();
144 std::stringstream ss;
145 ss <<
"Loaded parameter '" << name <<
"': ";
148 for (
const auto& element : param) ss << element << (&element != ¶m.back() ?
", " :
"");
153 RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
154 }
catch (rclcpp::exceptions::ParameterUninitializedException&) {
156 RCLCPP_FATAL_STREAM(this->get_logger(),
"Missing required parameter '" << name <<
"', exiting");
159 std::stringstream ss;
160 ss <<
"Missing parameter '" << name <<
"', using default value: ";
163 for (
const auto& element : param) ss << element << (&element != ¶m.back() ?
", " :
"");
168 RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
169 this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
173 if (add_to_auto_reconfigurable_params) {
174 std::function<void(
const rclcpp::Parameter&)> setter = [¶m](
const rclcpp::Parameter& p) { param = p.get_value<T>(); };
180 const std::vector<rclcpp::Parameter>& parameters) {
181 for (
const auto& param : parameters) {
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());
191 if (param.get_name() ==
"run_as_callback") {
195 RCLCPP_WARN(this->get_logger(),
"OCP runs now periodically with frequency %f Hz",
optimization_freq_);
199 RCLCPP_WARN(this->get_logger(),
"OCP runs now on reference trajectory callback");
204 rcl_interfaces::msg::SetParametersResult result;
205 result.successful =
true;
211 tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
219 ego_data_sub_ = this->create_subscription<perception_msgs::msg::EgoData>(
221 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
ego_data_sub_->get_topic_name());
223 object_list_sub_ = this->create_subscription<perception_msgs::msg::ObjectList>(
225 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
object_list_sub_->get_topic_name());
227 route_sub_ = this->create_subscription<route_planning_msgs::msg::Route>(
229 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
route_sub_->get_topic_name());
232 "~/reference_trajectory", 1,
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());
248 RCLCPP_INFO(this->get_logger(),
"OCP runs on reference trajectory callback");
250 RCLCPP_INFO(this->get_logger(),
"OCP runs continuously with frequency %f Hz",
optimization_freq_);
256 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
261 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
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()));
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(),
277 TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
282 RCLCPP_FATAL(this->get_logger(),
"n_shots must be > 0, got %d",
n_shots_);
290 new_time_steps.front());
293 if (status != ACADOS_SUCCESS) {
294 RCLCPP_FATAL(this->get_logger(),
"%s_acados_create_with_discretization() returned status %d. Exiting.",
model_name_.c_str(),
307 RCLCPP_FATAL(this->get_logger(),
"Created solver has N=%d, expected n_shots=%d. Exiting.",
nlp_dims_->N,
n_shots_);
320 RCLCPP_ERROR(this->get_logger(),
"%s_acados_free() returned status %d.",
model_name_.c_str(), status);
325 RCLCPP_ERROR(this->get_logger(),
"%s_acados_free_capsule() returned status %d.",
model_name_.c_str(), status);
331 if (status != ACADOS_SUCCESS) {
332 RCLCPP_ERROR(this->get_logger(),
"%s_acados_reset() returned status %d. Recreating solver.",
model_name_.c_str(), status);
339 constexpr double MAX_CONTROL_GUESS_AGE_FACTOR = 0.5;
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);
350 std::vector<double> controls(expected_control_size, 0.0);
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;
361 const int lower_stage =
static_cast<int>(std::floor(previous_stage));
362 const double interpolation_factor = previous_stage - lower_stage;
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]);
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];
386 for (
int integration = 0; integration < 4; ++integration) {
388 for (
int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k1[i];
390 for (
int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k2[i];
392 for (
int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + integration_step * k3[i];
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]);
404 const auto cycle_start = SteadyClock::now();
407 RCLCPP_WARN(this->get_logger(),
"EgoData outdated. Skipping planning cycle.");
411 trajectory_planning_msgs::msg::Trajectory::UniquePtr trajectory = std::make_unique<trajectory_planning_msgs::msg::Trajectory>();
415 trajectory->header.stamp =
ego_data_.header.stamp;
419 for (
int i = 0; i <=
n_shots_; ++i) trajectory_planning_msgs::trajectory_access::setT(*trajectory, i * dt, i);
423 RCLCPP_WARN(this->get_logger(),
"Standstill trajectory. Skipping planning cycle. Publish standstill trajectory.");
428 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory,
true);
445 auto logCompletedCycle = [&]() {
446 metrics.
cycle_ms = elapsedMilliseconds(cycle_start);
454 std::vector<double> x_init(*
nlp_dims_->nx, 0.0);
458 RCLCPP_WARN(this->get_logger(),
459 "Latest available trajectory is standstill. Using ego data for initial state (high-level initialization).");
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);
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());
481 RCLCPP_WARN(this->get_logger(),
"Failed to update inputs. Skipping planning cycle.");
493 const auto solve_start = SteadyClock::now();
496 bool finite_solution =
false;
497 bool primal_feasible =
false;
505 const auto relaxed_solve_start = SteadyClock::now();
506 const bool relaxed_primal_feasible =
solveAndValidate(metrics, finite_solution);
513 if (!relaxed_primal_feasible) {
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);
529 const auto constrained_solve_start = SteadyClock::now();
534 if (!primal_feasible) {
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);
550 for (
int stage = 0; stage <=
n_shots_; ++stage) {
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;
570 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, standstill);
582 const char* cycle_time_color = metrics.
cycle_ms <= 100.0 ?
"\x1b[32m" :
"\x1b[31m";
585 RCLCPP_INFO(this->get_logger(),
586 "Published trajectory (cycle: %s%.2f ms\x1b[0m; relaxed solve: %.2f ms; constrained solve: %.2f ms)",
589 RCLCPP_INFO(this->get_logger(),
"Published trajectory (cycle: %s%.2f ms\x1b[0m; constrained solve: %.2f ms)",
593 RCLCPP_INFO(this->get_logger(),
"Published trajectory (cycle: %s%.2f ms\x1b[0m)", cycle_time_color, metrics.
cycle_ms);
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) {
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;
607 tf_reference_trajectory =
611 if (!object_list.objects.empty() && object_list.header.frame_id !=
vehicle_frame_id_) {
615 tf_object_list = object_list;
619 if (!route.route_elements.empty() && route.header.frame_id !=
vehicle_frame_id_) {
625 }
catch (tf2::TransformException& ex) {
626 RCLCPP_WARN(this->get_logger(),
"Transformation is not available. Ex: %s", ex.what());
630 if (trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) <= 0) {
631 RCLCPP_ERROR(this->get_logger(),
"Reference trajectory contains no sample points.");
639 }
catch (
const std::exception& e) {
640 RCLCPP_ERROR(this->get_logger(),
"Exception while setting OCP parameters: %s", e.what());
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;
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.");
660 global_params.insert(global_params.end(), cost_weights.begin(), cost_weights.end());
663 global_params.push_back(
thw_);
670 std::vector<std::pair<double, double>> boundary_distances =
normalBoundaryDistance(reference_trajectory, route);
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);
679 ref.push_back(boundary_distances[i].second);
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));
686 global_params.insert(global_params.end(), ref.begin(), ref.end());
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),
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(),
697 throw std::runtime_error(
"Size of global parameters does not match expected size.");
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));
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);
709 const perception_msgs::msg::ObjectList& object_list) {
710 const auto start_time = std::chrono::steady_clock::now();
713 struct PredictionData {
714 std::vector<double> time;
715 std::vector<double> x;
716 std::vector<double> y;
717 std::vector<double> yaw;
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));
736 object_predictions[object_index].push_back(std::move(prediction_data));
740 for (
const auto& prediction :
object.state_predictions) {
745 if (object_predictions[object_index].empty() && !
object.state_predictions.empty()) {
746 append_prediction(
object.state_predictions[0]);
752 constexpr double MIN_HALF_EXTENT = 0.05;
756 double floating_dynamic_weight = 1.0;
757 for (
int stage = 0; stage <=
n_shots_; ++stage) {
761 std::vector<int> dynamic_weight_indices{idx};
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));
770 idx += parameter_count;
772 std::array<int, 2> constraint_activation_indices{idx, idx + 1};
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));
782 idx += parameter_count;
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{};
796 target_states.push_back(state);
800 target_states.push_back({perception_msgs::object_access::getX(
object), perception_msgs::object_access::getY(
object),
801 perception_msgs::object_access::getYaw(
object)});
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) {
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);
820 if (object_boxes.size() >=
static_cast<size_t>(parameter_count))
break;
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});
828 std::vector<int> object_indices(parameter_count);
829 std::iota(object_indices.begin(), object_indices.end(), idx);
832 if (status != ACADOS_SUCCESS) {
833 throw std::runtime_error(
"acados object-box update failed with status " + std::to_string(status));
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);
void printSolution(const PerformanceMetrics &metrics)
Logs solver status and optional debug statistics for the last optimization run.
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.
double optimization_horizon_
rclcpp::TimerBase::SharedPtr planning_timer_
std::vector< double > xtraj_
void routeCallback(const route_planning_msgs::msg::Route::ConstSharedPtr msg)
Stores the current route used for boundary constraints when enabled.
double relaxed_solve_timeout_ms_
std::vector< double > cost_weights_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr object_marker_pub_
std::vector< int64_t > p_ref_path_shape_
void vizEgoBoxes(const std::vector< double > &x_trajectory, const std::string &model_name)
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.
trajectory_planning_msgs::msg::Trajectory latest_valid_trajectory_
std::vector< double > control_guess_
uint8_t consider_boundaries_
double standstill_threshold_
perception_msgs::msg::EgoData ego_data_
virtual void initializeTrajectory(trajectory_planning_msgs::msg::Trajectory &trajectory)=0
Initializes an output trajectory message with the model-specific message type and size.
double constrained_solve_timeout_ms_
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.
double d_min_object_long_
double min_prediction_probability_
std::string vehicle_frame_id_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr ego_marker_pub_
uint8_t consider_objects_
bool high_level_stabilization_
std::vector< double > utraj_
ocp_nlp_solver * nlp_solver_
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.
ocp_nlp_config * nlp_config_
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.
std::vector< int64_t > p_dynamic_weight_shape_
void egoDataCallback(const perception_msgs::msg::EgoData::ConstSharedPtr msg)
Stores the latest ego state used by the optimizer.
double optimization_freq_
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...
bool solveAndValidate(PerformanceMetrics &metrics, bool &finite_solution)
Solves the OCP, reads its output and checks primal feasibility.
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.
route_planning_msgs::msg::Route route_
std::unique_ptr< tf2_ros::Buffer > tf2_buffer_
void setSolverTimeout(double timeout_ms)
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr route_sub_
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector< rclcpp::Parameter > ¶meters)
Applies parameter updates that can be reconfigured while the node is running.
OnSetParametersCallbackHandle::SharedPtr parameters_callback_
bool object_constraints_active_
std::vector< int64_t > p_objects_shape_
bool performance_logging_
void vizObjectBoxes(const std::vector< double > &objects)
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.
std::vector< int64_t > p_constraint_activation_shape_
ocp_model_capsule_t ocp_capsule_
perception_msgs::msg::ObjectList object_list_
std::string trajectory_frame_id_
bool ignore_objects_behind_ego_
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 ¶m, 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_
std::string fixed_over_time_frame_id_
rclcpp::Time control_guess_stamp_
std::unique_ptr< PerformanceLogger > performance_logger_
void referenceTrajectoryCallback(const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
Updates the reference trajectory and optionally triggers optimization immediately.
~TrajectoryOptimizationNode() override
Releases solver resources owned by the node.
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr trajectory_pub_
bool two_stage_optimization_
std::vector< int64_t > p_cost_weights_shape_
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
std::vector< double > viz_object_boxes_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr reference_trajectory_sub_
double d_min_boundary_lat_
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.
constexpr bool is_vector_v
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.