trajectory_optimization_node.cpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#include <chrono>
#include <cmath>
#include <functional>
#include <thread>
 
#include <trajectory_optimization/trajectory_optimization_node.hpp>
 
namespace trajectory_optimization {
 
namespace {
using SteadyClock = std::chrono::steady_clock;
 
double elapsedMilliseconds(const SteadyClock::time_point& start) {
  return std::chrono::duration<double, std::milli>(SteadyClock::now() - start).count();
}
 
double elapsedMilliseconds(const SteadyClock::time_point& start, const SteadyClock::time_point& end) {
  return std::chrono::duration<double, std::milli>(end - start).count();
}
 
}  // namespace
 
TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_name, const rclcpp::NodeOptions& options)
    : rclcpp::Node(node_name, options) {
  // declare and load node parameters
  this->declareAndLoadParameter("vehicle_frame_id", vehicle_frame_id_,
                                "Frame ID of local vehicle frame (the ocp is defined in this frame)");
  this->declareAndLoadParameter("trajectory_frame_id", trajectory_frame_id_, "Frame ID of output trajectory");
  this->declareAndLoadParameter("fixed_over_time_frame_id", fixed_over_time_frame_id_,
                                "Frame ID of frame that is fixed over time for finding temporal transforms");
  this->declareAndLoadParameter("ego_data_timeout", ego_data_timeout_,
                                "Time after which a received ego vehicle data is considered invalid [s]. Optimization will not "
                                "be run if ego data is invalid.");
  this->declareAndLoadParameter("model_name", model_name_,
                                "Name of the model to be used for trajectory optimization [karl, shuttle]");
  this->declareAndLoadParameter("optimization_frequency", optimization_freq_, "Optimization frequency in Hz");
  this->declareAndLoadParameter("n_shots", n_shots_, "Number of shooting intervals in optimization horizon");
  this->declareAndLoadParameter("optimization_horizon", optimization_horizon_, "Optimization Horizon in seconds");
  this->declareAndLoadParameter("verbose", verbose_, "Print solver statistics");
  this->declareAndLoadParameter("performance_logging", performance_logging_,
                                "Write one CSV record for every completed solver run", false, false, true);
  this->declareAndLoadParameter("debug_visualization", debug_viz_, "Publish debug visualization markers (e.g. obstacle circles)");
  this->declareAndLoadParameter("run_as_callback", run_as_callback_,
                                "Run OCP once for each received reference trajectory (true) or on a timer (false)");
  this->declareAndLoadParameter("cost_weights", cost_weights_, "Cost function weights");
  this->declareAndLoadParameter("dynamic_weight", dynamic_weight_, "Dynamic weight alpha");
  this->declareAndLoadParameter("thw", thw_, "Time headway to front vehicle");
  this->declareAndLoadParameter("d_min_obstacle_long", d_min_obstacle_long_,
                                "Minimum distance to keep to obstacle in longitudinal direction [m]");
  this->declareAndLoadParameter("d_min_obstacle_lat", d_min_obstacle_lat_,
                                "Minimum distance to keep to obstacle in lateral direction [m]");
  this->declareAndLoadParameter("d_min_boundary_lat", d_min_boundary_lat_,
                                "Minimum distance to keep to boundary in lateral direction [m]");
  this->declareAndLoadParameter("standstill_threshold", standstill_threshold_,
                                "Threshold for standstill detection [m/s]. If the velocities of all states are below this "
                                "threshold, publish standstill trajectory");
  this->declareAndLoadParameter("high_level_stabilization", high_level_stabilization_,
                                "Use high-level stabilization strategy for init state (= init with current EgoData)");
  this->declareAndLoadParameter(
      "add_x_init_to_ref", add_x_init_to_ref_,
      "add initial state of OCP to beginning of reference trajectory if this starts in front of ego vehicle");
  this->declareAndLoadParameter(
      "consider_objects", consider_objects_,
      "consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction)");
  this->declareAndLoadParameter("min_prediction_probability", min_prediction_probability_,
                                "Minimum probability for predicted object states to be considered", true, false, false, 0.0, 1.0);
  this->declareAndLoadParameter(
      "consider_boundaries", consider_boundaries_,
      "consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space");
  this->declareAndLoadParameter("bi_level_dV", bi_level_dV_,
                                "Threshold for bi-level stabilization: maximum velocity difference [m/s]");
  this->declareAndLoadParameter("bi_level_dA", bi_level_dA_,
                                "Threshold for bi-level stabilization: maximum acceleration difference [m/s^2]");
  this->declareAndLoadParameter("bi_level_dY", bi_level_dY_, "Threshold for bi-level stabilization: maximum y-offset [m]");
  this->declareAndLoadParameter("bi_level_dYaw", bi_level_dYaw_,
                                "Threshold for bi-level stabilization: maximum yaw difference [degree]");
  if (performance_logging_) {
    try {
      performance_logger_ = std::make_unique<PerformanceLogger>(get_name());
      RCLCPP_INFO(get_logger(), "Writing benchmark logs to '%s'.", performance_logger_->path().c_str());
    } catch (const std::exception& error) {
      RCLCPP_ERROR(get_logger(), "Could not initialize benchmark logging: %s", error.what());
    }
  }
  this->setup();
}
 
TrajectoryOptimizationNode::~TrajectoryOptimizationNode() { freeSolver(); }
 
template <typename T>
void TrajectoryOptimizationNode::declareAndLoadParameter(const std::string& name,
                                                         T& param,
                                                         const std::string& description,
                                                         const bool add_to_auto_reconfigurable_params,
                                                         const bool is_required,
                                                         const bool read_only,
                                                         const std::optional<double>& from_value,
                                                         const std::optional<double>& to_value,
                                                         const std::optional<double>& step_value,
                                                         const std::string& additional_constraints) {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = description;
  param_desc.additional_constraints = additional_constraints;
  param_desc.read_only = read_only;
 
  auto type = rclcpp::ParameterValue(param).get_type();
 
  if (from_value.has_value() && to_value.has_value()) {
    if constexpr (std::is_integral_v<T>) {
      rcl_interfaces::msg::IntegerRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.floating_point_range = {range};
    } else {
      RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
    }
  }
 
  this->declare_parameter(name, type, param_desc);
 
  try {
    param = this->get_parameter(name).get_value<T>();
    std::stringstream ss;
    ss << "Loaded parameter '" << name << "': ";
    if constexpr (is_vector_v<T>) {
      ss << "[";
      for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
      ss << "]";
    } else {
      ss << param;
    }
    RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
  } catch (rclcpp::exceptions::ParameterUninitializedException&) {
    if (is_required) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
      exit(EXIT_FAILURE);
    } else {
      std::stringstream ss;
      ss << "Missing parameter '" << name << "', using default value: ";
      if constexpr (is_vector_v<T>) {
        ss << "[";
        for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
        ss << "]";
      } else {
        ss << param;
      }
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
    }
  }
 
  if (add_to_auto_reconfigurable_params) {
    std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
    auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
  }
}
 
rcl_interfaces::msg::SetParametersResult TrajectoryOptimizationNode::parametersCallback(
    const std::vector<rclcpp::Parameter>& parameters) {
  for (const auto& param : parameters) {
    for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
      if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
        std::get<1>(auto_reconfigurable_param)(param);
        RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s' to: %s", param.get_name().c_str(),
                    param.value_to_string().c_str());
        break;
      }
    }
    // handle special cases
    if (param.get_name() == "run_as_callback") {
      if (!run_as_callback_ && !planning_timer_) {
        planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
                                                  std::bind(&TrajectoryOptimizationNode::planningCycle, this));
        RCLCPP_WARN(this->get_logger(), "OCP runs now periodically with frequency %f Hz", optimization_freq_);
      } else if (run_as_callback_ && planning_timer_) {
        planning_timer_->cancel();
        planning_timer_.reset();
        RCLCPP_WARN(this->get_logger(), "OCP runs now on reference trajectory callback");
      }
    }
  }
  // mark parameter change successful
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  return result;
}
 
void TrajectoryOptimizationNode::setup() {
  tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
 
  // create a callback for dynamic parameter configuration
  parameters_callback_ = this->add_on_set_parameters_callback(
      std::bind(&TrajectoryOptimizationNode::parametersCallback, this, std::placeholders::_1));
 
  // set up subscriber for input topics
  ego_data_sub_ = this->create_subscription<perception_msgs::msg::EgoData>(
      "~/ego_data", 1, std::bind(&TrajectoryOptimizationNode::egoDataCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", ego_data_sub_->get_topic_name());
 
  object_list_sub_ = this->create_subscription<perception_msgs::msg::ObjectList>(
      "~/object_list", 1, std::bind(&TrajectoryOptimizationNode::objectListCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", object_list_sub_->get_topic_name());
 
  route_sub_ = this->create_subscription<route_planning_msgs::msg::Route>(
      "~/route", 1, std::bind(&TrajectoryOptimizationNode::routeCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", route_sub_->get_topic_name());
 
  reference_trajectory_sub_ = this->create_subscription<trajectory_planning_msgs::msg::Trajectory>(
      "~/reference_trajectory", 1,
      std::bind(&TrajectoryOptimizationNode::referenceTrajectoryCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", reference_trajectory_sub_->get_topic_name());
 
  // set up publisher for output topics
  trajectory_pub_ = this->create_publisher<trajectory_planning_msgs::msg::Trajectory>("~/trajectory", 1);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", trajectory_pub_->get_topic_name());
  circles_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/object_circles", 1);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", circles_pub_->get_topic_name());
  ego_circles_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/ego_circles", 1);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", ego_circles_pub_->get_topic_name());
  boundary_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/boundaries", 1);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", boundary_pub_->get_topic_name());
 
  // create timer for planning cycle
  if (run_as_callback_) {
    RCLCPP_INFO(this->get_logger(), "OCP runs on reference trajectory callback");
  } else {
    RCLCPP_INFO(this->get_logger(), "OCP runs continuously with frequency %f Hz", optimization_freq_);
    planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
                                              std::bind(&TrajectoryOptimizationNode::planningCycle, this));
  }
 
  // init reference trajectory
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(
      reference_trajectory_, trajectory_planning_msgs::msg::REFERENCE::TYPE_ID, n_shots_ + 1);
  reference_trajectory_.header.frame_id = vehicle_frame_id_;
 
  // init latest trajectory (doesn't matter which type, only used for standstill detection)
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(
      latest_valid_trajectory_, trajectory_planning_msgs::msg::DRIVABLE::TYPE_ID, n_shots_ + 1);
  latest_valid_trajectory_.header.frame_id = trajectory_frame_id_;
 
  setupSolver();
 
  // Annotate message links for tracing: Publish trajectory periodically, which depends an all subscribed topics.
  std::vector<const void*> link_subs;
  link_subs.push_back(static_cast<const void*>(ego_data_sub_->get_subscription_handle().get()));
  link_subs.push_back(static_cast<const void*>(object_list_sub_->get_subscription_handle().get()));
  link_subs.push_back(static_cast<const void*>(route_sub_->get_subscription_handle().get()));
  link_subs.push_back(static_cast<const void*>(reference_trajectory_sub_->get_subscription_handle().get()));
  std::vector<const void*> link_pubs;
  link_pubs.push_back(static_cast<const void*>(trajectory_pub_->get_publisher_handle().get()));
  RCLCPP_INFO(get_logger(), "Annotating message links for tracing with %zu subscriptions and %zu publications", link_subs.size(),
              link_pubs.size());
  TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
}
 
void TrajectoryOptimizationNode::setupSolver() {
  if (n_shots_ <= 0) {
    RCLCPP_FATAL(this->get_logger(), "n_shots must be > 0, got %d", n_shots_);
    exit(1);
  }
 
  // Create exactly once. This also supports a runtime horizon that differs from the generated default.
  ocp_capsule_ = trajectory_optimization::acados_create_capsule(model_name_);
  std::vector<double> new_time_steps(n_shots_, optimization_horizon_ / n_shots_);
  RCLCPP_INFO(this->get_logger(), "Create OCP with: horizon = %f, n_shots = %d, dt = %f", optimization_horizon_, n_shots_,
              new_time_steps.front());
  int status = trajectory_optimization::acados_create_with_discretization(ocp_capsule_, n_shots_, new_time_steps.data());
 
  if (status != ACADOS_SUCCESS) {
    RCLCPP_FATAL(this->get_logger(), "%s_acados_create_with_discretization() returned status %d. Exiting.", model_name_.c_str(),
                 status);
    exit(1);
  }
 
  nlp_config_ = trajectory_optimization::acados_get_nlp_config(ocp_capsule_);
  nlp_dims_ = trajectory_optimization::acados_get_nlp_dims(ocp_capsule_);
  nlp_in_ = trajectory_optimization::acados_get_nlp_in(ocp_capsule_);
  nlp_out_ = trajectory_optimization::acados_get_nlp_out(ocp_capsule_);
  nlp_solver_ = trajectory_optimization::acados_get_nlp_solver(ocp_capsule_);
  nlp_opts_ = trajectory_optimization::acados_get_nlp_opts(ocp_capsule_);
  if (nlp_dims_->N != n_shots_) {
    RCLCPP_FATAL(this->get_logger(), "Created solver has N=%d, expected n_shots=%d. Exiting.", nlp_dims_->N, n_shots_);
    exit(1);
  }
 
  xtraj_.resize(*nlp_dims_->nx * (n_shots_ + 1));
  utraj_.resize(*nlp_dims_->nu * n_shots_);
  control_guess_.clear();
}
 
void TrajectoryOptimizationNode::freeSolver() {
  // free solver
  int status = trajectory_optimization::acados_free(ocp_capsule_);
  if (status != 0) {
    RCLCPP_ERROR(this->get_logger(), "%s_acados_free() returned status %d.", model_name_.c_str(), status);
  }
  // free solver capsule
  status = trajectory_optimization::acados_free_capsule(ocp_capsule_);
  if (status != 0) {
    RCLCPP_ERROR(this->get_logger(), "%s_acados_free_capsule() returned status %d.", model_name_.c_str(), status);
  }
}
 
void TrajectoryOptimizationNode::resetSolver() {
  const int status = trajectory_optimization::acados_reset(ocp_capsule_, 1, 0, 0, 0);
  if (status != ACADOS_SUCCESS) {
    RCLCPP_ERROR(this->get_logger(), "%s_acados_reset() returned status %d. Recreating solver.", model_name_.c_str(), status);
    freeSolver();
    setupSolver();
  }
}
 
bool TrajectoryOptimizationNode::setInitialGuess(const std::vector<double>& x_init, const rclcpp::Time& stamp) {
  constexpr double MAX_CONTROL_GUESS_AGE_FACTOR = 0.5;
  const int nx = *nlp_dims_->nx;
  const int nu = *nlp_dims_->nu;
  const double time_step = optimization_horizon_ / n_shots_;
  const size_t expected_control_size = static_cast<size_t>(nu * n_shots_);
  if (x_init.size() != static_cast<size_t>(nx)) {
    RCLCPP_ERROR(get_logger(), "Initial state has size %zu, expected %d.", x_init.size(), nx);
    return false;
  }
 
  // Fall back to zero controls if no sufficiently recent accepted solution is available.
  std::vector<double> controls(expected_control_size, 0.0);
  if (control_guess_.size() == expected_control_size) {
    const double elapsed = (stamp - control_guess_stamp_).seconds();
    if (elapsed >= 0.0 && elapsed < MAX_CONTROL_GUESS_AGE_FACTOR * optimization_horizon_) {
      // Shift the previous controls to the current planning time and interpolate between shooting nodes.
      for (int stage = 0; stage < n_shots_; ++stage) {
        const double previous_stage = (elapsed + stage * time_step) / time_step;
        if (previous_stage >= n_shots_) break;
 
        const int lower_stage = static_cast<int>(std::floor(previous_stage));
        const double interpolation_factor = previous_stage - lower_stage;
        for (int control = 0; control < nu; ++control) {
          const double lower_value = control_guess_[lower_stage * nu + control];
          const double upper_value = lower_stage + 1 < n_shots_ ? control_guess_[(lower_stage + 1) * nu + control] : 0.0;
          controls[stage * nu + control] = lower_value + interpolation_factor * (upper_value - lower_value);
        }
      }
    }
  }
 
  ocp_nlp_out_set_values_to_zero(nlp_config_, nlp_dims_, nlp_out_);
  std::vector<double> rollout_state = x_init;
  std::vector<double> intermediate_state(nx);
  std::vector<double> k1(nx), k2(nx), k3(nx), k4(nx);
  const double integration_step = time_step / 2.0;
  for (int stage = 0; stage < n_shots_; ++stage) {
    double* control = &controls[stage * nu];
    ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "x", rollout_state.data());
    ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "u", control);
 
    // Match sim_method_num_stages=4 and sim_method_num_steps=2 from the generated OCP.
    for (int integration = 0; integration < 2; ++integration) {
      trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, rollout_state.data(), control, k1.data());
      for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k1[i];
      trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k2.data());
      for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k2[i];
      trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k3.data());
      for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + integration_step * k3[i];
      trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k4.data());
      for (int i = 0; i < nx; ++i) {
        rollout_state[i] += integration_step / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
      }
    }
  }
  ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, n_shots_, "x", rollout_state.data());
  return true;
}
 
void TrajectoryOptimizationNode::planningCycle() {
  const auto cycle_start = SteadyClock::now();
  if (debug_viz_) viz_circles_.clear();
  if (rclcpp::Time(this->now()) - rclcpp::Time(ego_data_.header.stamp) > rclcpp::Duration::from_seconds(ego_data_timeout_)) {
    RCLCPP_WARN(this->get_logger(), "EgoData outdated. Skipping planning cycle.");
    return;
  }
  // init trajectory message and set header
  trajectory_planning_msgs::msg::Trajectory::UniquePtr trajectory = std::make_unique<trajectory_planning_msgs::msg::Trajectory>();
  initializeTrajectory(*trajectory);
 
  trajectory->header.frame_id = vehicle_frame_id_;
  trajectory->header.stamp = ego_data_.header.stamp;  // use latest ego_data stamp as trajectory stamp
 
  // init time-steps of trajectory to ensure increasing time-steps even for standstill trajectories
  double dt = optimization_horizon_ / n_shots_;
  for (int i = 0; i <= n_shots_; ++i) trajectory_planning_msgs::trajectory_access::setT(*trajectory, i * dt, i);
 
  // check if the reference trajectory is standstill
  if (trajectory_planning_msgs::trajectory_access::getStandstill(reference_trajectory_)) {
    RCLCPP_WARN(this->get_logger(), "Standstill trajectory. Skipping planning cycle. Publish standstill trajectory.");
    // transform trajectory to output frame
    if (!trajectory2outputFrame(*trajectory)) {
      return;
    }
    trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, true);
    trajectory_pub_->publish(std::move(trajectory));
    // Invalidate the warm start and reset the solver once when entering standstill.
    if (!control_guess_.empty()) {
      control_guess_.clear();
      resetSolver();
    }
    return;
  }
 
  PerformanceMetrics metrics;
  metrics.cycle = ++logging_cycle_;
  metrics.ego_stamp_ns = rclcpp::Time(ego_data_.header.stamp).nanoseconds();
  metrics.reference_stamp_ns = rclcpp::Time(reference_trajectory_.header.stamp).nanoseconds();
  metrics.route_stamp_ns = rclcpp::Time(route_.header.stamp).nanoseconds();
  metrics.reference_points = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory_);
  metrics.objects = static_cast<int>(object_list_.objects.size());
  auto logCompletedCycle = [&]() {
    metrics.cycle_ms = elapsedMilliseconds(cycle_start);
    metrics.postprocessing_ms = metrics.cycle_ms - metrics.preprocessing_ms - metrics.solve_wall_ms;
    logPerformance(metrics);
  };
 
  // set initial state
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
  if (!trajectory_planning_msgs::trajectory_access::getStandstill(latest_valid_trajectory_)) {
    x_init = high_level_stabilization_ ? getHighLevelX0(ego_data_) : getBiLevelX0(ego_data_);
  } else {
    RCLCPP_WARN(this->get_logger(),
                "Latest available trajectory is standstill. Using ego data for initial state (high-level initialization).");
    x_init = getHighLevelX0(ego_data_);
  }
 
  // debug print of initial state
  std::stringstream ss;
  ss << "Initial state: ";
  for (size_t i = 0; i < x_init.size(); ++i) ss << "x[" << i << "]: " << x_init[i] << (i != x_init.size() - 1 ? ", " : "");
  RCLCPP_DEBUG(this->get_logger(), "%s", ss.str().c_str());
 
  ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "lbx", x_init.data());
  ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "ubx", x_init.data());
 
  // update inputs to the ocp; skip planning cycle if update fails
  if (!updateOcpInputs(ego_data_, object_list_, route_, reference_trajectory_, x_init)) {
    RCLCPP_WARN(this->get_logger(), "Failed to update inputs. Skipping planning cycle.");
    return;
  }
 
  if (!setInitialGuess(x_init, rclcpp::Time(ego_data_.header.stamp))) {
    control_guess_.clear();
    return;
  }
 
  // solve the optimization problem
  const auto solve_start = SteadyClock::now();
  metrics.preprocessing_ms = elapsedMilliseconds(cycle_start, solve_start);
  metrics.status = trajectory_optimization::acados_solve(ocp_capsule_);
  const auto solve_end = SteadyClock::now();
  metrics.solve_wall_ms = elapsedMilliseconds(solve_start, solve_end);
 
  PerformanceLogger::collectSolverStatistics(metrics, nlp_solver_, nlp_config_, nlp_dims_, nlp_in_, nlp_out_);
 
  if (metrics.status == ACADOS_NAN_DETECTED || metrics.status == ACADOS_MINSTEP || metrics.status == ACADOS_QP_FAILURE) {
    printSolution(metrics);
    // Keep the last accepted controls; the failed solver output is not added to the cache.
    resetSolver();
    logCompletedCycle();
    return;
  }
 
  // get solution
  for (int ii = 0; ii <= nlp_dims_->N; ++ii) {
    ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, ii, "x", &xtraj_[ii * *nlp_dims_->nx]);
  }
  for (int ii = 0; ii < nlp_dims_->N; ++ii) {
    ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, ii, "u", &utraj_[ii * *nlp_dims_->nu]);
  }
  control_guess_ = utraj_;
  control_guess_stamp_ = rclcpp::Time(ego_data_.header.stamp);
 
  printSolution(metrics);
  if (debug_viz_) {
    vizCircles(viz_circles_);
    vizEgoCircles(xtraj_, model_name_);
  }
 
  // convert output into trajectory message
  convertToTrajectoryMsg(*trajectory);
 
  bool standstill = true;
  for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(*trajectory); ++i) {
    if (trajectory_planning_msgs::trajectory_access::getV(*trajectory, i) > standstill_threshold_) standstill = false;
  }
  trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, standstill);
 
  // transform trajectory to output frame
  if (!trajectory2outputFrame(*trajectory)) {
    logCompletedCycle();
    return;
  }
 
  latest_valid_trajectory_ = *trajectory;
  trajectory_pub_->publish(std::move(trajectory));
  metrics.published = true;
  logCompletedCycle();
  const char* cycle_time_color = metrics.cycle_ms <= 100.0 ? "\x1b[32m" : "\x1b[31m";
  RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m)", cycle_time_color, metrics.cycle_ms);
}
 
bool TrajectoryOptimizationNode::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,
                                                 const std::vector<double>& x_init) {
  // transform inputs to target base_link frame
  trajectory_planning_msgs::msg::Trajectory tf_reference_trajectory;
  perception_msgs::msg::ObjectList tf_object_list;
  route_planning_msgs::msg::Route tf_route;
  try {
    // reference trajectory
    tf_reference_trajectory =
        tf2_buffer_->transform(reference_trajectory, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
                               fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
    if (add_x_init_to_ref_ && trajectory_planning_msgs::trajectory_access::getX(tf_reference_trajectory, 0) > 0.0) {
      RCLCPP_INFO(this->get_logger(), "Adding x_init to beginning of reference trajectory");
      std::vector<double> x_0_ref = {0.0, x_init[0], x_init[1], x_init[3]};
      tf_reference_trajectory.states.insert(tf_reference_trajectory.states.begin(), x_0_ref.begin(), x_0_ref.end());
    }
    // object list
    if (!object_list.objects.empty() && object_list.header.frame_id != vehicle_frame_id_) {
      tf_object_list = tf2_buffer_->transform(object_list, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
                                              fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
    } else {
      tf_object_list = object_list;
    }
    keepNClosestObjects(tf_object_list, static_cast<int>(p_obstacle_circles_shape_[0]));
    // route
    if (!route.route_elements.empty() && route.header.frame_id != vehicle_frame_id_) {
      tf_route = tf2_buffer_->transform(route, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
                                        fixed_over_time_frame_id_, tf2::durationFromSec(0.1));
    } else {
      tf_route = route;
    }
  } catch (tf2::TransformException& ex) {
    RCLCPP_WARN(this->get_logger(), "Transformation is not available. Ex: %s", ex.what());
    return false;
  }
 
  if (trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) <= 0) {
    RCLCPP_ERROR(this->get_logger(), "Reference trajectory contains no sample points.");
    return false;
  }
 
  // update ocp parameters
  try {
    this->setOcpGlobalParameters(cost_weights_, tf_reference_trajectory, tf_route);
    this->setOcpParameters(ego_data, tf_object_list);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(this->get_logger(), "Exception while setting OCP parameters: %s", e.what());
    return false;
  }
 
  return true;
}
 
void TrajectoryOptimizationNode::setOcpGlobalParameters(const std::vector<double>& cost_weights,
                                                        const trajectory_planning_msgs::msg::Trajectory& reference_trajectory,
                                                        const route_planning_msgs::msg::Route& route) {
  const auto start_time = std::chrono::steady_clock::now();
  std::vector<double> global_params;
 
  // cost weights
  const auto expected_cost_weights_size = static_cast<size_t>(p_cost_weights_shape_[0] * p_cost_weights_shape_[1]);
  if (cost_weights.size() != expected_cost_weights_size) {
    RCLCPP_ERROR(this->get_logger(), "Size of cost_weights (%zu) does not match expected size (%zu).", cost_weights.size(),
                 expected_cost_weights_size);
    throw std::runtime_error("Size of cost_weights does not match expected size.");
  }
  global_params.insert(global_params.end(), cost_weights.begin(), cost_weights.end());
 
  // other cost params
  global_params.push_back(thw_);
  global_params.push_back(d_min_obstacle_long_);
  global_params.push_back(d_min_obstacle_lat_);
  global_params.push_back(d_min_boundary_lat_);
 
  // reference path (including boundaries)
  const size_t n_ref_states = static_cast<size_t>(p_ref_path_shape_[0] * p_ref_path_shape_[1]);
  std::vector<std::pair<double, double>> boundary_distances = normalBoundaryDistance(reference_trajectory, route);
  // fill ref vector for ocp -> psi, x, y, v, d_bound_left, d_bound_right
  std::vector<double> ref;
  for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory); ++i) {
    ref.push_back(trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i));
    ref.push_back(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i));
    ref.push_back(trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
    ref.push_back(trajectory_planning_msgs::trajectory_access::getV(reference_trajectory, i));
    ref.push_back(boundary_distances[i].first);   // left boundary distance
    ref.push_back(boundary_distances[i].second);  // right boundary distance
  }
  if (ref.size() >= n_ref_states) {
    global_params.insert(global_params.end(), ref.begin(),
                         ref.begin() + static_cast<std::vector<double>::difference_type>(n_ref_states));
  } else {
    // Repeat the final valid state. Infinity padding can propagate NaNs through closest-point calculations.
    global_params.insert(global_params.end(), ref.begin(), ref.end());
    const size_t state_width = static_cast<size_t>(p_ref_path_shape_[1]);
    while (global_params.size() < expected_cost_weights_size + 4 + n_ref_states) {
      global_params.insert(global_params.end(), ref.end() - static_cast<std::vector<double>::difference_type>(state_width),
                           ref.end());
    }
  }
 
  if (global_params.size() != static_cast<size_t>(nlp_dims_->np_global)) {
    RCLCPP_ERROR(this->get_logger(), "Size of global parameters (%zu) does not match expected size (%d).", global_params.size(),
                 nlp_dims_->np_global);
    throw std::runtime_error("Size of global parameters does not match expected size.");
  }
  const int status = trajectory_optimization::acados_set_p_global_and_precompute_dependencies(
      ocp_capsule_, global_params.data(), static_cast<int>(global_params.size()));
  if (status != ACADOS_SUCCESS) {
    throw std::runtime_error("acados global parameter update failed with status " + std::to_string(status));
  }
  const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
  RCLCPP_DEBUG(this->get_logger(), "setOcpGlobalParameters duration: %.3f ms", elapsed_ms);
}
 
void TrajectoryOptimizationNode::setOcpParameters(const perception_msgs::msg::EgoData& ego_data,
                                                  const perception_msgs::msg::ObjectList& object_list) {
  const auto start_time = std::chrono::steady_clock::now();
  // loop over shooting intervals
  double floating_dynamic_weight = 1.0;
  double dt = optimization_horizon_ / n_shots_;
  for (int i = 0; i <= n_shots_; ++i) {
    // dynamic weight
    int idx = 0;
    int n = 1;
    std::vector<int> idx_dynamic_weight(n);
    // fill vector with values from idx to idx + n
    std::iota(idx_dynamic_weight.begin(), idx_dynamic_weight.end(), idx);
    int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_dynamic_weight.data(),
                                                                      &floating_dynamic_weight, n);
    if (status != ACADOS_SUCCESS) {
      throw std::runtime_error("acados dynamic-weight update failed with status " + std::to_string(status));
    }
    floating_dynamic_weight *= dynamic_weight_;
 
    // obstacles
    idx += n;
    n = static_cast<int>(p_obstacle_circles_shape_[0] * p_obstacle_circles_shape_[1]);
    std::vector<double> circles;  // [x1, y1, r1, x2, y2, r2, ...]
 
    for (size_t j = 0; j < object_list.objects.size(); ++j) {
      std::vector<double> TIME, X, Y, YAW;
      std::vector<std::tuple<double, double, double>> target_states;
      // TODO(ika): Build prediction arrays once per object outside the shooting-interval loop.
      TIME.push_back(static_cast<double>(rclcpp::Time(object_list.header.stamp).nanoseconds()) / 1e9);
      X.push_back(perception_msgs::object_access::getX(object_list.objects[j]));
      Y.push_back(perception_msgs::object_access::getY(object_list.objects[j]));
      YAW.push_back(perception_msgs::object_access::getYaw(object_list.objects[j]));
      if (consider_objects_ == CONSIDER_OBJECTS::PREDICTED_OBJECTS && !object_list.objects[j].state_predictions.empty()) {
        // build one target state for a single prediction hypothesis at the current shooting interval
        auto appendPredictionTargetState = [&](const auto& state_prediction, size_t prediction_idx) {
          std::vector<double> prediction_time = TIME;
          std::vector<double> prediction_x = X;
          std::vector<double> prediction_y = Y;
          std::vector<double> prediction_yaw = YAW;
          for (const auto& predicted_state : state_prediction.states) {
            prediction_time.push_back(static_cast<double>(rclcpp::Time(predicted_state.header.stamp).nanoseconds()) / 1e9);
            prediction_x.push_back(perception_msgs::object_access::getX(predicted_state));
            prediction_y.push_back(perception_msgs::object_access::getY(predicted_state));
            prediction_yaw.push_back(perception_msgs::object_access::getYaw(predicted_state));
          }
          double x_tgt = 0.0, y_tgt = 0.0, yaw_tgt = 0.0;
          double des_time = static_cast<double>(rclcpp::Time(ego_data.header.stamp).nanoseconds()) / 1e9 + dt * i;
          if (des_time > prediction_time.back()) {
            const double relative_des_time = des_time - prediction_time.front();
            const double relative_max_time = prediction_time.back() - prediction_time.front();
            RCLCPP_WARN(this->get_logger(),
                        "Prediction horizon shorter than requested interpolation time. "
                        "object=%zu prediction=%zu probability=%.3f desired_rel=%.3f s max_rel=%.3f s n_states=%zu. "
                        "Using last prediction state.",
                        j, prediction_idx, state_prediction.probability, relative_des_time, relative_max_time,
                        state_prediction.states.size());
            x_tgt = prediction_x.back();
            y_tgt = prediction_y.back();
            yaw_tgt = prediction_yaw.back();
          } else {
            linearInterpolation(prediction_time, prediction_x, des_time, x_tgt);
            linearInterpolation(prediction_time, prediction_y, des_time, y_tgt);
            linearInterpolation(prediction_time, prediction_yaw, des_time, yaw_tgt, true);
          }
          target_states.emplace_back(x_tgt, y_tgt, yaw_tgt);
        };
 
        // consider all prediction hypotheses whose probability exceeds the configured threshold
        bool has_prediction_above_threshold = false;
        for (size_t prediction_idx = 0; prediction_idx < object_list.objects[j].state_predictions.size(); ++prediction_idx) {
          const auto& state_prediction = object_list.objects[j].state_predictions[prediction_idx];
          if (state_prediction.probability <= min_prediction_probability_) {
            continue;
          }
          has_prediction_above_threshold = true;
          appendPredictionTargetState(state_prediction, prediction_idx);
        }
        if (!has_prediction_above_threshold) {
          // always keep at least one prediction hypothesis for predicted objects
          appendPredictionTargetState(object_list.objects[j].state_predictions[0], 0);
        }
      } else {
        // static object handling or missing predictions: use the current object state
        target_states.emplace_back(X.front(), Y.front(), YAW.front());
      }
 
      double alpha = std::atan2(object_list.objects[j].state.reference_point.translation_to_geometric_center.y,
                                object_list.objects[j].state.reference_point.translation_to_geometric_center.x);
      double a = std::sqrt(std::pow(object_list.objects[j].state.reference_point.translation_to_geometric_center.x, 2) +
                           std::pow(object_list.objects[j].state.reference_point.translation_to_geometric_center.y, 2));
      for (auto& [x_tgt, y_tgt, yaw_tgt] : target_states) {
        // ensure that x_tgt and y_tgt represent the geometric center of the object
        double beta = wrap_angle_rad(yaw_tgt - alpha);
        x_tgt += a * std::cos(beta);
        y_tgt += a * std::sin(beta);
 
        std::vector<double> obj_circles =
            discretizeBB2Circles(x_tgt, y_tgt, yaw_tgt, perception_msgs::object_access::getLength(object_list.objects[j]),
                                 perception_msgs::object_access::getWidth(object_list.objects[j]));
 
        circles.insert(circles.end(), obj_circles.begin(), obj_circles.end());
        if (circles.size() >= static_cast<size_t>(n)) {
          circles.resize(n);
          break;
        }
      }
      if (circles.size() >= static_cast<size_t>(n)) {
        break;
      }
    }
    // fill up with dummy "ghost" obstacle circles at (10000, 10000) to avoid NaNs in the optimization problem
    // TODO: improve this  // NOLINT(google-readability-todo)
    while (circles.size() < static_cast<size_t>(n)) {
      std::vector<double> dummy_circle = {10000.0, 10000.0, 1.0};
      circles.insert(circles.end(), dummy_circle.begin(), dummy_circle.end());
    }
    if ((circles.size() % p_obstacle_circles_shape_[1]) != 0) {
      RCLCPP_WARN(this->get_logger(), "Circles vector size is not a multiple of the circle shape. Resizing.");
      circles.resize(n);
    }
    if (debug_viz_) viz_circles_.insert(viz_circles_.end(), circles.begin(), circles.end());
 
    std::vector<int> idx_obstacles(n);
    // fill vector with values from idx to idx + n
    std::iota(idx_obstacles.begin(), idx_obstacles.end(), idx);
    status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, i, idx_obstacles.data(), circles.data(), n);
    if (status != ACADOS_SUCCESS) {
      throw std::runtime_error("acados obstacle update failed with status " + std::to_string(status));
    }
  }
  const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
  RCLCPP_DEBUG(this->get_logger(), "setOcpParameters duration: %.3f ms", elapsed_ms);
}
 
}  // namespace trajectory_optimization