trajectory_optimization::TrajectoryOptimizationNode Class Reference

#include <trajectory_optimization_node.hpp>

Inheritance diagram for trajectory_optimization::TrajectoryOptimizationNode:

Public Member Functions
	TrajectoryOptimizationNode (const std::string node_name, const rclcpp::NodeOptions &options)
	Initializes the base node and loads the common optimizer configuration.
	~TrajectoryOptimizationNode () override
	Releases solver resources owned by the node.
	TrajectoryOptimizationNode (const TrajectoryOptimizationNode &)=delete
	Copy construction is disabled because the node owns non-copyable runtime resources.
TrajectoryOptimizationNode &	operator= (const TrajectoryOptimizationNode &)=delete
	Copy assignment is disabled because the node owns non-copyable runtime resources.
	TrajectoryOptimizationNode (TrajectoryOptimizationNode &&)=delete
	Move construction is disabled to keep solver and ROS handles bound to a single instance.
TrajectoryOptimizationNode &	operator= (TrajectoryOptimizationNode &&)=delete
	Move assignment is disabled to keep solver and ROS handles bound to a single instance.

Protected Types
enum	CONSIDER_BOUNDARIES { NO_BOUNDS = 0 , SUGGESTED_LANE = 1 , INCLUDING_ADJACENT = 2 , DRIVABLE_SPACE = 3 }
enum	CONSIDER_OBJECTS { NO_OBJECTS = 0 , STATIC_OBJECTS = 1 , PREDICTED_OBJECTS = 2 }

Protected Member Functions
template<typename T >
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.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Applies parameter updates that can be reconfigured while the node is running.
void	setup ()
	Creates ROS interfaces, initializes cached messages and prepares the solver.
void	resetSolver ()
	Resets the optimizer memory while retaining the generated solver instance.
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	setupSolver ()
	Creates the acados solver instance and initializes its state buffers.
void	freeSolver ()
	Frees the acados solver and clears cached optimization results.
void	printSolution (const PerformanceMetrics &metrics)
	Logs solver status and optional debug statistics for the last optimization run.
bool	trajectory2outputFrame (trajectory_planning_msgs::msg::Trajectory &trajectory)
	Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required.
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.
void	egoDataCallback (const perception_msgs::msg::EgoData::ConstSharedPtr msg)
	Stores the latest ego state used by the optimizer.
void	objectListCallback (const perception_msgs::msg::ObjectList::ConstSharedPtr msg)
	Stores the current object list when object handling is enabled.
void	referenceTrajectoryCallback (const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
	Updates the reference trajectory and optionally triggers optimization immediately.
void	routeCallback (const route_planning_msgs::msg::Route::ConstSharedPtr msg)
	Stores the current route used for boundary constraints when enabled.
void	planningCycle ()
	Runs one full planning cycle from input preparation, over solver execution, to trajectory publication.
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.
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.
void	setOcpParameters (const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list)
	Writes stage-wise obstacle and dynamic weighting parameters into the OCP.
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< double >	discretizeBB2Circles (const double x, const double y, const double yaw, const double length, const double width)
	Approximates an oriented bounding box with a set of obstacle circles.
void	vizCircles (const std::vector< double > &obstacles)
	Publishes visualization markers for the obstacle circles currently used by the optimizer.
void	vizEgoCircles (const std::vector< double > &x_trajectory, const std::string &model_name)
	Publishes the ego-vehicle circle approximation used by the selected OCP model.
void	vizBoundaryPoints (const std::vector< Eigen::Vector2d > &left_boundary_points, const std::vector< Eigen::Vector2d > &right_boundary_points)
	Publishes the boundary intersections corresponding to the distances passed to the OCP.
virtual void	initializeTrajectory (trajectory_planning_msgs::msg::Trajectory &trajectory)=0
	Initializes an output trajectory message with the model-specific message type and size.
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 type.
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 EgoData type.
virtual void	convertToTrajectoryMsg (trajectory_planning_msgs::msg::Trajectory &trajectory)=0
	Maps the optimized state trajectory into the model-specific output message fields.

Static Protected Member Functions
static double	wrap_angle_rad (double angle_rad, double min_val=-M_PI, double max_val=M_PI)
	Wraps an angle into a configured interval.
static void	keepNClosestObjects (perception_msgs::msg::ObjectList &object_list, const int n_objects)
	Keeps the nearest forward objects and discards the remaining entries.

Protected Attributes
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr	ego_data_sub_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr	object_list_sub_
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr	route_sub_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	reference_trajectory_sub_
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	trajectory_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	ego_circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	boundary_pub_
rclcpp::TimerBase::SharedPtr	planning_timer_
std::unique_ptr< tf2_ros::Buffer >	tf2_buffer_
std::shared_ptr< tf2_ros::TransformListener >	tf2_listener_
perception_msgs::msg::EgoData	ego_data_
perception_msgs::msg::ObjectList	object_list_
route_planning_msgs::msg::Route	route_
trajectory_planning_msgs::msg::Trajectory	reference_trajectory_
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
std::string	vehicle_frame_id_ = "base_link"
std::string	trajectory_frame_id_ = "base_link"
std::string	fixed_over_time_frame_id_ = "map"
std::string	model_name_ = "karl"
double	ego_data_timeout_ = 1.0
double	optimization_freq_ = 10.0
int	n_shots_ = 50
double	optimization_horizon_ = 1.0
bool	verbose_ = false
bool	performance_logging_ = false
bool	debug_viz_ = false
double	standstill_threshold_ = 0.45
bool	high_level_stabilization_ = false
uint8_t	consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS
uint8_t	consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE
bool	run_as_callback_ = false
double	bi_level_dV_ = 2.0
double	bi_level_dY_ = 0.3
double	bi_level_dYaw_ = 89.0
trajectory_planning_msgs::msg::Trajectory	latest_valid_trajectory_
std::vector< double >	control_guess_
rclcpp::Time	control_guess_stamp_ {0, 0, RCL_ROS_TIME}
std::vector< double >	viz_circles_
std::vector< double >	cost_weights_ = std::vector<double>(12, 1.0)
double	dynamic_weight_ = 1.0
double	thw_ = 2.0
double	d_min_obstacle_long_ = 5.0
double	d_min_obstacle_lat_ = 0.5
double	d_min_boundary_lat_ = 0.0
double	min_prediction_probability_ = 0.0
std::vector< int64_t >	p_cost_weights_shape_ = {12, 1}
std::vector< int64_t >	p_ref_path_shape_ = {51, 6}
std::vector< int64_t >	p_obstacle_circles_shape_ = {30, 3}
ocp_model_capsule_t	ocp_capsule_
ocp_nlp_config *	nlp_config_
ocp_nlp_dims *	nlp_dims_
ocp_nlp_in *	nlp_in_
ocp_nlp_out *	nlp_out_
ocp_nlp_solver *	nlp_solver_
void *	nlp_opts_
std::vector< double >	xtraj_
std::vector< double >	utraj_
uint64_t	logging_cycle_ = 0
std::unique_ptr< PerformanceLogger >	performance_logger_

Detailed Description

Definition at line 45 of file trajectory_optimization_node.hpp.

Member Enumeration Documentation

CONSIDER_BOUNDARIES

enum trajectory_optimization::TrajectoryOptimizationNode::CONSIDER_BOUNDARIES

protected

Enumerator
NO_BOUNDS
SUGGESTED_LANE
INCLUDING_ADJACENT
DRIVABLE_SPACE

Definition at line 85 of file trajectory_optimization_node.hpp.

{ NO_BOUNDS = 0, SUGGESTED_LANE = 1, INCLUDING_ADJACENT = 2, DRIVABLE_SPACE = 3 };

CONSIDER_OBJECTS

enum trajectory_optimization::TrajectoryOptimizationNode::CONSIDER_OBJECTS

protected

Enumerator
NO_OBJECTS
STATIC_OBJECTS
PREDICTED_OBJECTS

Definition at line 87 of file trajectory_optimization_node.hpp.

{ NO_OBJECTS = 0, STATIC_OBJECTS = 1, PREDICTED_OBJECTS = 2 };

Constructor & Destructor Documentation

TrajectoryOptimizationNode() [1/3]

trajectory_optimization::TrajectoryOptimizationNode::TrajectoryOptimizationNode ( const std::string node_name, const rclcpp::NodeOptions & options )

explicit

Initializes the base node and loads the common optimizer configuration.

Parameters

[in]	node_name	Name of the ROS node instance.
[in]	options	ROS node options used for construction.

Definition at line 31 of file trajectory_optimization_node.cpp.

    : 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(
      "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_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()

trajectory_optimization::TrajectoryOptimizationNode::~TrajectoryOptimizationNode ( )

override

Releases solver resources owned by the node.

Definition at line 91 of file trajectory_optimization_node.cpp.

{ freeSolver(); }

TrajectoryOptimizationNode() [2/3]

trajectory_optimization::TrajectoryOptimizationNode::TrajectoryOptimizationNode ( const TrajectoryOptimizationNode & )

delete

Copy construction is disabled because the node owns non-copyable runtime resources.

TrajectoryOptimizationNode() [3/3]

trajectory_optimization::TrajectoryOptimizationNode::TrajectoryOptimizationNode ( TrajectoryOptimizationNode && )

delete

Move construction is disabled to keep solver and ROS handles bound to a single instance.

Member Function Documentation

convertToTrajectoryMsg()

virtual void trajectory_optimization::TrajectoryOptimizationNode::convertToTrajectoryMsg ( trajectory_planning_msgs::msg::Trajectory & trajectory )

protectedpure virtual

Maps the optimized state trajectory into the model-specific output message fields.

Parameters

[in,out]

trajectory

Output trajectory message.

Implemented in trajectory_optimization::TrajectoryOptimizationAckermannNode, and trajectory_optimization::TrajectoryOptimizationRWSNode.

declareAndLoadParameter()

template<typename T >

void trajectory_optimization::TrajectoryOptimizationNode::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 = "" )

protected

Declares a ROS parameter, loads its value and optionally registers it for runtime updates.

Template Parameters

T	Parameter value type.

Parameters

[in]	name	Parameter name.
[out]	param	Member variable to store the parameter value.
[in]	description	Human-readable parameter description.
[in]	add_to_auto_reconfigurable_params	Whether parameter updates automatically update the member variable.
[in]	is_required	Whether the node should fail if the parameter is not set.
[in]	read_only	Whether the parameter is exposed as read-only.
[in]	from_value	Optional lower bound for numeric parameters.
[in]	to_value	Optional upper bound for numeric parameters.
[in]	step_value	Optional step size for numeric parameters.
[in]	additional_constraints	Additional free-form constraint text for the parameter descriptor.

Definition at line 94 of file trajectory_optimization_node.cpp.

                                                                                                  {
  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));
  }
}

discretizeBB2Circles()

std::vector< double > trajectory_optimization::TrajectoryOptimizationNode::discretizeBB2Circles ( const double x, const double y, const double yaw, const double length, const double width )

protected

Approximates an oriented bounding box with a set of obstacle circles.

Parameters

[in]	x	Bounding-box center x-coordinate.
[in]	y	Bounding-box center y-coordinate.
[in]	yaw	Bounding-box heading.
[in]	length	Bounding-box length.
[in]	width	Bounding-box width.

Returns

Flattened circle list in the configured obstacle parameter layout.

Definition at line 116 of file utils.cpp.

                                                                                               {
  uint8_t n_circles = 1;
  if (length <= 0.0 || width <= 0.0) {
    RCLCPP_WARN(get_logger(), "Invalid bounding box dimensions: length = %f, width = %f. Setting n_circles = 1.", length, width);
  } else {
    double aspect_ratio = length / width;
    if (aspect_ratio > 8.0) {
      n_circles = 9;
    } else if (aspect_ratio > 6.0) {
      n_circles = 7;
    } else if (aspect_ratio > 4.0) {
      n_circles = 5;
    } else if (aspect_ratio > 1.8) {
      n_circles = 3;
    } else if (aspect_ratio > 1.3) {
      n_circles = 2;
    } else {
      n_circles = 1;
    }
  }
 
  double radius = std::sqrt(std::pow(length / (2 * n_circles), 2) + std::pow(width / 2.0, 2));
 
  std::vector<double> circles(p_obstacle_circles_shape_[1] * n_circles);
 
  for (int i = 0; i < n_circles; i++) {
    double lon_offset = -length / 2 + (2 * i + 1) * length / (2 * n_circles);
    double x_offset = lon_offset * std::cos(yaw);
    double y_offset = lon_offset * std::sin(yaw);
    circles[p_obstacle_circles_shape_[1] * i + 0] = x + x_offset;
    circles[p_obstacle_circles_shape_[1] * i + 1] = y + y_offset;
    circles[p_obstacle_circles_shape_[1] * i + 2] = radius;
  }
 
  return circles;
}

egoDataCallback()

void trajectory_optimization::TrajectoryOptimizationNode::egoDataCallback ( const perception_msgs::msg::EgoData::ConstSharedPtr msg )

protected

Stores the latest ego state used by the optimizer.

Parameters

[in]

msg

Incoming ego vehicle message.

Definition at line 7 of file callbacks.cpp.

                                                                                                  {
  RCLCPP_DEBUG(this->get_logger(), "Received ego data");
  ego_data_ = *msg;
}

freeSolver()

void trajectory_optimization::TrajectoryOptimizationNode::freeSolver ( )

protected

Frees the acados solver and clears cached optimization results.

Definition at line 302 of file trajectory_optimization_node.cpp.

                                            {
  // 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);
  }
}

getBiLevelX0()

virtual std::vector< double > trajectory_optimization::TrajectoryOptimizationNode::getBiLevelX0 ( const perception_msgs::msg::EgoData & ego_data )

protectedpure virtual

Computes the initial optimizer state using bi-level stabilizaion based on the model-specific EgoData type.

This function uses bi-level stabilization for initializing the optimization problem. In general the initial state is interpolated from the latest trajectory (-> low-level stabilization). But if the difference between the interpolated state and the ego state is too large, the ego state is used instead (-> high-level stabilization). This combination of low- and high-level stabilization is called bi-level stabilization.

Parameters

[in]

ego_data

Current ego state.

Returns

Initial state vector for the OCP.

Implemented in trajectory_optimization::TrajectoryOptimizationAckermannNode, and trajectory_optimization::TrajectoryOptimizationRWSNode.

getHighLevelX0()

virtual std::vector< double > trajectory_optimization::TrajectoryOptimizationNode::getHighLevelX0 ( const perception_msgs::msg::EgoData & ego_data )

protectedpure virtual

Computes the initial optimizer state using high-level stabilization based on the model-specific EgoData type.

This function uses high-level stabilization for initializing the optimization problem. -> initial state = current state of the ego vehicle.

Parameters

[in]

ego_data

Current ego state.

Returns

Initial state vector for the OCP.

Implemented in trajectory_optimization::TrajectoryOptimizationAckermannNode, and trajectory_optimization::TrajectoryOptimizationRWSNode.

initializeTrajectory()

virtual void trajectory_optimization::TrajectoryOptimizationNode::initializeTrajectory ( trajectory_planning_msgs::msg::Trajectory & trajectory )

protectedpure virtual

Initializes an output trajectory message with the model-specific message type and size.

Parameters

[out]

trajectory

Trajectory message to initialize.

Implemented in trajectory_optimization::TrajectoryOptimizationAckermannNode, and trajectory_optimization::TrajectoryOptimizationRWSNode.

keepNClosestObjects()

void trajectory_optimization::TrajectoryOptimizationNode::keepNClosestObjects ( perception_msgs::msg::ObjectList & object_list, const int n_objects )

staticprotected

Keeps the nearest forward objects and discards the remaining entries.

Parameters

[in,out]	object_list	Object list to filter.
[in]	n_objects	Maximum number of objects to retain.

Definition at line 86 of file utils.cpp.

                                                                                                                   {
  // calculate distance to each object
  std::vector<double> distances;
  for (size_t i = 0; i < object_list.objects.size(); ++i) {
    double distance = std::sqrt(std::pow(perception_msgs::object_access::getX(object_list.objects[i]), 2) +
                                std::pow(perception_msgs::object_access::getY(object_list.objects[i]), 2));
    distances.push_back(distance);
  }
 
  // sort objects by distance
  std::vector<size_t> indices_sorted_by_distance(distances.size());
  std::iota(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), 0);
  std::sort(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(),
            [&distances](size_t i1, size_t i2) { return distances[i1] < distances[i2]; });
 
  // keep only the closest objects
  std::vector<perception_msgs::msg::Object> closest_objects;
  const auto n_objects_to_keep = std::min(static_cast<size_t>(n_objects), indices_sorted_by_distance.size());
  int i = 0;
  while (closest_objects.size() < static_cast<size_t>(n_objects_to_keep)) {
    if (static_cast<size_t>(i) >= indices_sorted_by_distance.size()) break;
    // ignore object with negative x-coordinate (behind the ego vehicle)
    if (perception_msgs::object_access::getX(object_list.objects[indices_sorted_by_distance[i]]) > 0.0) {
      closest_objects.push_back(object_list.objects[indices_sorted_by_distance[i]]);
    }
    ++i;
  }
  object_list.objects = closest_objects;
}

linearInterpolation()

bool trajectory_optimization::TrajectoryOptimizationNode::linearInterpolation ( const std::vector< double > & X, const std::vector< double > & Y, const double & desired_x, double & output_y, const bool wrap_angle = false )

protected

Interpolates a value from sampled data and optionally handles angle wrap-around.

Parameters

[in]	X	Sample positions.
[in]	Y	Sample values.
[in]	desired_x	Query position.
[out]	output_y	Interpolated result.
[in]	wrap_angle	Whether angular differences should be wrapped to [-pi, pi].

Returns

true if the query lies within the sample range, otherwise false.

Definition at line 21 of file utils.cpp.

                                                                                                                        {
  if (desired_x == X.front()) {
    RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Min of the given vector!");
    output_y = Y.front();
    return true;
  } else if (desired_x == X.back()) {
    RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Max of the given vector!");
    output_y = Y.back();
    return true;
  } else if (desired_x < *min_element(X.begin(), X.end())) {
    RCLCPP_WARN(get_logger(), "Desired Time is smaller than Time-Min of the given vector! Using first valid value.");
    RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
    RCLCPP_DEBUG(get_logger(), "Time-Min: %f s", *min_element(X.begin(), X.end()));
    output_y = Y.front();
    return false;
  } else if (desired_x > *max_element(X.begin(), X.end())) {
    RCLCPP_WARN(get_logger(), "Desired Time is greater than Time-Max of the given vector! Using last valid value.");
    RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
    RCLCPP_DEBUG(get_logger(), "Time-Max: %f s", *max_element(X.begin(), X.end()));
    output_y = Y.back();
    return false;
  } else if (X.size() != Y.size()) {
    RCLCPP_ERROR(get_logger(), "Input vectors don't have the same length!");
    return false;
  }
 
  //go through array and search for sampling points
  size_t i = 0;
  for (i = 0; i < X.size(); i++) {
    if (X[i] < desired_x) {
      continue;
    } else if (X[i] == desired_x) {
      output_y = Y[i];
      return true;
    } else {
      break;
    }
  }
  double diff = Y[i] - Y[i - 1];
  if (wrap_angle) {
    diff = wrap_angle_rad(diff);
  }
  output_y = Y[i - 1] + (diff / (X[i] - X[i - 1])) * (desired_x - X[i - 1]);
  if (wrap_angle) {
    output_y = wrap_angle_rad(output_y);
  }
  return true;
}

normalBoundaryDistance()

std::vector< std::pair< double, double > > trajectory_optimization::TrajectoryOptimizationNode::normalBoundaryDistance ( const trajectory_planning_msgs::msg::Trajectory & reference_trajectory, const route_planning_msgs::msg::Route & route )

protected

Computes minimum normal distances from the reference path to the active route boundaries.

Parameters

[in]	reference_trajectory	Reference trajectory in optimizer frame.
[in]	route	Route data used to derive active boundaries.

Returns

Left and right boundary distances for each reference sample.

Definition at line 154 of file utils.cpp.

                                                                                                                 {
  const double NO_BOUNDARY_DISTANCE = 1e6;  // finite sentinel avoids NaNs during reference-path interpolation
  constexpr double MAX_ROUTE_S_DIFFERENCE = 20.0;
  constexpr double LOOK_BEHIND_REMAINING_ROUTE_S = 2.0;
 
  struct Boundaries {
    std::vector<std::pair<double, double>> min_normal_distances;
    std::vector<Eigen::Vector2d> left_boundary_points;
    std::vector<Eigen::Vector2d> right_boundary_points;
    std::vector<double> left_boundary_route_s;
    std::vector<double> right_boundary_route_s;
    std::vector<Eigen::Vector2d> left_boundary_intersections;
    std::vector<Eigen::Vector2d> right_boundary_intersections;
  };
 
  Boundaries boundaries;
  auto route_elements = route_planning_msgs::route_access::getRemainingRouteElements(route, true);
  const int ref_sample_size = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory);
 
  if (route_elements.empty()) {
    if (consider_boundaries_ != CONSIDER_BOUNDARIES::NO_BOUNDS) {
      RCLCPP_WARN(get_logger(), "Remaining route is empty. Do not constrain boundaries.");
    }
    for (int i = 0; i < ref_sample_size; ++i) {
      boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
    }
    return boundaries.min_normal_distances;
  }
 
  const double current_route_s = route_elements.front().s;
  const size_t current_route_index = std::min(static_cast<size_t>(route.current_route_element_idx), route.route_elements.size());
  size_t overlap_begin = current_route_index;
  while (overlap_begin > 0 && current_route_s - route.route_elements[overlap_begin - 1].s <= LOOK_BEHIND_REMAINING_ROUTE_S) {
    --overlap_begin;
  }
  route_elements.insert(route_elements.begin(), route.route_elements.begin() + static_cast<std::ptrdiff_t>(overlap_begin),
                        route.route_elements.begin() + static_cast<std::ptrdiff_t>(current_route_index));
 
  boundaries.left_boundary_points.reserve(route_elements.size());
  boundaries.right_boundary_points.reserve(route_elements.size());
  boundaries.left_boundary_route_s.reserve(route_elements.size());
  boundaries.right_boundary_route_s.reserve(route_elements.size());
  boundaries.min_normal_distances.reserve(ref_sample_size);
  boundaries.left_boundary_intersections.reserve(ref_sample_size);
  boundaries.right_boundary_intersections.reserve(ref_sample_size);
 
  for (const auto& route_element : route_elements) {
    if (route_element.is_enriched) {
      if (consider_boundaries_ == CONSIDER_BOUNDARIES::SUGGESTED_LANE) {
        route_planning_msgs::msg::LaneElement suggested_lane =
            route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
        boundaries.left_boundary_points.emplace_back(suggested_lane.left_boundary.point.x, suggested_lane.left_boundary.point.y);
        boundaries.right_boundary_points.emplace_back(suggested_lane.right_boundary.point.x,
                                                      suggested_lane.right_boundary.point.y);
        boundaries.left_boundary_route_s.push_back(route_element.s);
        boundaries.right_boundary_route_s.push_back(route_element.s);
      } else if (consider_boundaries_ == CONSIDER_BOUNDARIES::INCLUDING_ADJACENT) {
        const auto& lane_elements = route_element.lane_elements;
        if (!lane_elements.empty()) {
          boundaries.left_boundary_points.emplace_back(lane_elements.front().left_boundary.point.x,
                                                       lane_elements.front().left_boundary.point.y);
          boundaries.right_boundary_points.emplace_back(lane_elements.back().right_boundary.point.x,
                                                        lane_elements.back().right_boundary.point.y);
          boundaries.left_boundary_route_s.push_back(route_element.s);
          boundaries.right_boundary_route_s.push_back(route_element.s);
        }
      } else if (consider_boundaries_ == CONSIDER_BOUNDARIES::DRIVABLE_SPACE) {
        boundaries.left_boundary_points.emplace_back(route_element.left_boundary.x, route_element.left_boundary.y);
        boundaries.right_boundary_points.emplace_back(route_element.right_boundary.x, route_element.right_boundary.y);
        boundaries.left_boundary_route_s.push_back(route_element.s);
        boundaries.right_boundary_route_s.push_back(route_element.s);
      }
    }
  }
 
  // Helper lambda to find intersection
  auto findIntersection = [&](const Eigen::Vector2d& ref_pos, double sin_yaw, double cos_yaw, double expected_route_s,
                              const std::vector<Eigen::Vector2d>& boundary_points, const std::vector<double>& boundary_route_s,
                              bool isLeft) -> std::pair<double, Eigen::Vector2d> {
    std::pair<double, Eigen::Vector2d> intersection_result = {
        std::numeric_limits<double>::infinity(),
        Eigen::Vector2d(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())};
    double best_route_s_difference = std::numeric_limits<double>::infinity();
 
    const Eigen::Vector2d normal_dir = isLeft ? Eigen::Vector2d(-sin_yaw, cos_yaw) : Eigen::Vector2d(sin_yaw, -cos_yaw);
    const auto cross2d = [](const Eigen::Vector2d& u, const Eigen::Vector2d& v) { return u.x() * v.y() - u.y() * v.x(); };
 
    for (size_t i = 0; i + 1 < boundary_points.size(); ++i) {
      const Eigen::Vector2d& a = boundary_points[i];
      const Eigen::Vector2d& b = boundary_points[i + 1];
      Eigen::Vector2d seg = b - a;
      Eigen::Vector2d ap = ref_pos - a;
 
      const double denom = cross2d(seg, normal_dir);
      if (std::abs(denom) < 1e-9) {
        continue;  // Lines are close to parallel; ignore this segment
      }
 
      const double s = cross2d(ap, normal_dir) / denom;
      const double t = cross2d(ap, seg) / denom;
 
      if (s >= 0.0 && s <= 1.0 && t >= 0.0) {
        Eigen::Vector2d intersection = a + s * seg;
        double euclidean_distance = (ref_pos - intersection).norm();
        const double intersection_route_s = boundary_route_s[i] + s * (boundary_route_s[i + 1] - boundary_route_s[i]);
        const double route_s_difference = std::abs(intersection_route_s - expected_route_s);
 
        if (route_s_difference <= MAX_ROUTE_S_DIFFERENCE &&
            (route_s_difference < best_route_s_difference ||
             (std::abs(route_s_difference - best_route_s_difference) < 1e-9 && euclidean_distance < intersection_result.first))) {
          best_route_s_difference = route_s_difference;
          intersection_result = {euclidean_distance, intersection};
        }
      }
    }
    return intersection_result;
  };
 
  // Loop over trajectory points and compute intersections
  double reference_progress = 0.0;
  Eigen::Vector2d previous_ref_pos = Eigen::Vector2d::Zero();
  for (int i = 0; i < ref_sample_size; ++i) {
    Eigen::Vector2d ref_pos(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i),
                            trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
    reference_progress += (ref_pos - previous_ref_pos).norm();
    previous_ref_pos = ref_pos;
    const double expected_route_s = current_route_s + reference_progress;
 
    double yaw = trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i);
    const double sin_yaw = std::sin(yaw);
    const double cos_yaw = std::cos(yaw);
 
    auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.left_boundary_points,
                                              boundaries.left_boundary_route_s, true);
    auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.right_boundary_points,
                                               boundaries.right_boundary_route_s, false);
    if (left_intersection.first != std::numeric_limits<double>::infinity() &&
        right_intersection.first != std::numeric_limits<double>::infinity()) {
      boundaries.min_normal_distances.emplace_back(left_intersection.first, right_intersection.first);
      boundaries.left_boundary_intersections.emplace_back(left_intersection.second);
      boundaries.right_boundary_intersections.emplace_back(right_intersection.second);
      RCLCPP_DEBUG(this->get_logger(), "Minimum left boundary distance: %.2f m", left_intersection.first);
      RCLCPP_DEBUG(this->get_logger(), "Minimum right boundary distance: %.2f m", right_intersection.first);
    } else {
      boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
      RCLCPP_WARN(get_logger(),
                  "No boundary intersection found for trajectory point %d. Do not constrain boundaries at this point.", i);
    }
  }
  if (debug_viz_) {
    vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections);
  }
 
  return boundaries.min_normal_distances;
}

objectListCallback()

void trajectory_optimization::TrajectoryOptimizationNode::objectListCallback ( const perception_msgs::msg::ObjectList::ConstSharedPtr msg )

protected

Stores the current object list when object handling is enabled.

Parameters

[in]

msg

Incoming object list message.

Definition at line 12 of file callbacks.cpp.

                                                                                                        {
  if (consider_objects_ != CONSIDER_OBJECTS::NO_OBJECTS) {
    RCLCPP_DEBUG(this->get_logger(), "Received object list");
    object_list_ = *msg;
  } else {
    // reset object list to empty
    object_list_ = perception_msgs::msg::ObjectList();
  }
}

operator=() [1/2]

TrajectoryOptimizationNode & trajectory_optimization::TrajectoryOptimizationNode::operator= ( const TrajectoryOptimizationNode & )

delete

Copy assignment is disabled because the node owns non-copyable runtime resources.

Returns

Reference to this node. The operator is deleted.

operator=() [2/2]

TrajectoryOptimizationNode & trajectory_optimization::TrajectoryOptimizationNode::operator= ( TrajectoryOptimizationNode && )

delete

Move assignment is disabled to keep solver and ROS handles bound to a single instance.

Returns

Reference to this node. The operator is deleted.

parametersCallback()

rcl_interfaces::msg::SetParametersResult trajectory_optimization::TrajectoryOptimizationNode::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

Applies parameter updates that can be reconfigured while the node is running.

Parameters

[in]

parameters

Parameters requested by the ROS parameter service.

Returns

Result of the parameter update request.

Definition at line 166 of file trajectory_optimization_node.cpp.

                                                  {
  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;
}

planningCycle()

void trajectory_optimization::TrajectoryOptimizationNode::planningCycle ( )

protected

Runs one full planning cycle from input preparation, over solver execution, to trajectory publication.

Definition at line 389 of file trajectory_optimization_node.cpp.

                                               {
  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;
    if (performance_logger_) {
      performance_logger_->write(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_)) {
    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_,
                                             performance_logger_ != nullptr || verbose_);
 
  const bool usable_status =
      metrics.status == ACADOS_SUCCESS || metrics.status == ACADOS_MAXITER || metrics.status == ACADOS_TIMEOUT;
  if (usable_status) {
    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]);
    }
  }
 
  const auto* solver_opts = trajectory_optimization::acados_get_common_nlp_opts(nlp_config_, nlp_opts_);
  const bool finite_solution = usable_status && std::isfinite(metrics.cost_value) && std::isfinite(metrics.res_eq) &&
                               std::isfinite(metrics.res_ineq) &&
                               std::all_of(xtraj_.begin(), xtraj_.end(), [](double value) { return std::isfinite(value); }) &&
                               std::all_of(utraj_.begin(), utraj_.end(), [](double value) { return std::isfinite(value); });
  const bool primal_feasible =
      finite_solution && metrics.res_eq <= solver_opts->tol_eq && metrics.res_ineq <= solver_opts->tol_ineq;
 
  if (!primal_feasible) {
    printSolution(metrics);
    RCLCPP_WARN(this->get_logger(),
                "Rejecting solver output: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] tolerances=[eq=%e, ineq=%e].",
                metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
    if (finite_solution && performance_logger_) {
      PerformanceLogger::collectConstraintDiagnostics(metrics, nlp_solver_, nlp_dims_,
                                                      static_cast<int>(p_obstacle_circles_shape_[0]));
    }
    if (finite_solution && (metrics.status == ACADOS_MAXITER || metrics.status == ACADOS_TIMEOUT)) {
      // Preserve progress from a recoverable solve; the states are rolled out again from the next x_init.
      control_guess_ = utraj_;
      control_guess_stamp_ = rclcpp::Time(ego_data_.header.stamp);
    } else {
      control_guess_.clear();
    }
    resetSolver();
    logCompletedCycle();
    return;
  }
 
  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);
}

printSolution()

void trajectory_optimization::TrajectoryOptimizationNode::printSolution ( const PerformanceMetrics & metrics )

protected

Logs solver status and optional debug statistics for the last optimization run.

Parameters

[in]

metrics

Performance metrics collected for the optimization run.

Definition at line 451 of file utils.cpp.

                                                                                {
  // Status codes:
  // 0: Success (ACADOS_SUCCESS)
  // 1: NaN detected (ACADOS_NAN_DETECTED)
  // 2: Maximum number of iterations reached (ACADOS_MAXITER)
  // 3: Minimum step size reached (ACADOS_MINSTEP)
  // 4: QP solver failed (ACADOS_QP_FAILURE)
  // 5: Solver created (ACADOS_READY)
  // 6: Problem unbounded (ACADOS_UNBOUNDED)
  // 7: Solver timeout (ACADOS_TIMEOUT)
  if (metrics.status == ACADOS_SUCCESS && verbose_) {
    RCLCPP_INFO(get_logger(), "\033[1;32mOptimization: SUCCESS!\033[0m");
  } else if (metrics.status == ACADOS_MAXITER) {
    RCLCPP_WARN(get_logger(), "Optimization failed with status %d (max iterations).", metrics.status);
  } else if (metrics.status == ACADOS_TIMEOUT) {
    RCLCPP_WARN(get_logger(), "\033[38;5;214mOptimization failed with status %d (timeout).\033[0m", metrics.status);
  } else if (metrics.status != ACADOS_SUCCESS) {
    RCLCPP_ERROR(get_logger(), "%s_acados_solve() failed with status %d.", model_name_.c_str(), metrics.status);
  }
 
  if (verbose_) {
    RCLCPP_INFO(get_logger(), "Optimization took %.3f ms (SQP iter: %d; QP iter: %d; KKT: %e)", metrics.acados_total_ms,
                metrics.sqp_iter, metrics.qp_iter, metrics.kkt_norm_inf);
    RCLCPP_INFO(get_logger(), "cost_value: %f; residuals: stat=%e eq=%e ineq=%e comp=%e", metrics.cost_value, metrics.res_stat,
                metrics.res_eq, metrics.res_ineq, metrics.res_comp);
 
    std::fputs("\n--- xtraj ---\n", stdout);
    d_print_exp_tran_mat(*nlp_dims_->nx, n_shots_ + 1, xtraj_.data(), *nlp_dims_->nx);
    std::fputs("\n--- utraj ---\n", stdout);
    d_print_exp_tran_mat(*nlp_dims_->nu, n_shots_, utraj_.data(), *nlp_dims_->nu);
    trajectory_optimization::acados_print_stats(ocp_capsule_);
  }
}

referenceTrajectoryCallback()

void trajectory_optimization::TrajectoryOptimizationNode::referenceTrajectoryCallback ( const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg )

protected

Updates the reference trajectory and optionally triggers optimization immediately.

Parameters

[in]

msg

Incoming reference trajectory message.

Definition at line 22 of file callbacks.cpp.

                                                                   {
  RCLCPP_DEBUG(this->get_logger(), "Received reference trajectory");
  reference_trajectory_ = *msg;
  if (run_as_callback_) {
    planningCycle();
  }
}

resetSolver()

void trajectory_optimization::TrajectoryOptimizationNode::resetSolver ( )

protected

Resets the optimizer memory while retaining the generated solver instance.

Definition at line 315 of file trajectory_optimization_node.cpp.

                                             {
  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();
  }
}

routeCallback()

void trajectory_optimization::TrajectoryOptimizationNode::routeCallback ( const route_planning_msgs::msg::Route::ConstSharedPtr msg )

protected

Stores the current route used for boundary constraints when enabled.

Parameters

[in]

msg

Incoming route message.

Definition at line 31 of file callbacks.cpp.

                                                                                                  {
  if (consider_boundaries_ != CONSIDER_BOUNDARIES::NO_BOUNDS) {
    RCLCPP_DEBUG(this->get_logger(), "Received route");
    route_ = *msg;
  } else {
    // reset route to empty
    route_ = route_planning_msgs::msg::Route();
  }
}

setInitialGuess()

bool trajectory_optimization::TrajectoryOptimizationNode::setInitialGuess ( const std::vector< double > & x_init, const rclcpp::Time & stamp )

protected

Builds and sets a dynamically consistent NLP initial guess from the current state and cached controls.

Parameters

[in]	x_init	Hard initial state of the OCP.
[in]	stamp	Absolute time corresponding to x_init.

Returns

true if the state rollout succeeded.

Definition at line 324 of file trajectory_optimization_node.cpp.

                                                                                                         {
  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 solver output 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.
      std::vector<double> lower_controls(nu);
      std::vector<double> upper_controls(nu);
      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;
        ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "lbu", lower_controls.data());
        ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "ubu", upper_controls.data());
        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;
          const double interpolated_control = lower_value + interpolation_factor * (upper_value - lower_value);
          controls[stage * nu + control] = std::clamp(interpolated_control, lower_controls[control], upper_controls[control]);
        }
      }
    }
  }
 
  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;
}

setOcpGlobalParameters()

void trajectory_optimization::TrajectoryOptimizationNode::setOcpGlobalParameters ( const std::vector< double > & cost_weights, const trajectory_planning_msgs::msg::Trajectory & reference_trajectory, const route_planning_msgs::msg::Route & route )

protected

Writes stage-independent data into the OCP.

Parameters

[in]	cost_weights	Configured cost weights.
[in]	reference_trajectory	Reference trajectory in optimizer frame.
[in]	route	Route data used for boundary distances.

Definition at line 601 of file trajectory_optimization_node.cpp.

                                                                                                  {
  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);
}

setOcpParameters()

void trajectory_optimization::TrajectoryOptimizationNode::setOcpParameters ( const perception_msgs::msg::EgoData & ego_data, const perception_msgs::msg::ObjectList & object_list )

protected

Writes stage-wise obstacle and dynamic weighting parameters into the OCP.

Parameters

[in]	ego_data	Current ego state.
[in]	object_list	Object list in optimizer frame.

Definition at line 662 of file trajectory_optimization_node.cpp.

                                                                                                   {
  const auto start_time = std::chrono::steady_clock::now();
  struct PredictionData {
    std::vector<double> time;
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> yaw;
    size_t index;
    double probability;
  };
  std::vector<std::vector<PredictionData>> object_predictions(object_list.objects.size());
  const double object_stamp = static_cast<double>(rclcpp::Time(object_list.header.stamp).nanoseconds()) / 1e9;
  for (size_t j = 0; j < object_list.objects.size(); ++j) {
    const auto& object = object_list.objects[j];
    if (consider_objects_ != CONSIDER_OBJECTS::PREDICTED_OBJECTS || object.state_predictions.empty()) continue;
 
    auto append_prediction = [&](const auto& state_prediction, size_t prediction_idx) {
      PredictionData prediction{{object_stamp},
                                {perception_msgs::object_access::getX(object)},
                                {perception_msgs::object_access::getY(object)},
                                {perception_msgs::object_access::getYaw(object)},
                                prediction_idx,
                                state_prediction.probability};
      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));
      }
      object_predictions[j].push_back(std::move(prediction));
    };
 
    for (size_t prediction_idx = 0; prediction_idx < object.state_predictions.size(); ++prediction_idx) {
      const auto& state_prediction = object.state_predictions[prediction_idx];
      if (state_prediction.probability > min_prediction_probability_) {
        append_prediction(state_prediction, prediction_idx);
      }
    }
    if (object_predictions[j].empty()) append_prediction(object.state_predictions[0], 0);
  }
 
  // 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<std::tuple<double, double, double>> target_states;
      if (!object_predictions[j].empty()) {
        for (const auto& prediction : object_predictions[j]) {
          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.index, prediction.probability, relative_des_time, relative_max_time,
                        prediction.time.size() - 1);
            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);
        }
      } else {
        target_states.emplace_back(perception_msgs::object_access::getX(object_list.objects[j]),
                                   perception_msgs::object_access::getY(object_list.objects[j]),
                                   perception_msgs::object_access::getYaw(object_list.objects[j]));
      }
 
      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);
}

setup()

void trajectory_optimization::TrajectoryOptimizationNode::setup ( )

protected

Creates ROS interfaces, initializes cached messages and prepares the solver.

Definition at line 197 of file trajectory_optimization_node.cpp.

                                       {
  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());
}

setupSolver()

void trajectory_optimization::TrajectoryOptimizationNode::setupSolver ( )

protected

Creates the acados solver instance and initializes its state buffers.

Definition at line 267 of file trajectory_optimization_node.cpp.

                                             {
  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();
}

trajectory2outputFrame()

bool trajectory_optimization::TrajectoryOptimizationNode::trajectory2outputFrame ( trajectory_planning_msgs::msg::Trajectory & trajectory )

protected

Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required.

Parameters

[in,out]

trajectory

Trajectory to transform.

Returns

true if the trajectory is already in the target frame or the transformation succeeded.

Definition at line 71 of file utils.cpp.

                                                                                                         {
  if (trajectory_frame_id_ != vehicle_frame_id_) {
    trajectory_planning_msgs::msg::Trajectory tf_trajectory;
    try {
      tf_trajectory = tf2_buffer_->transform(trajectory, trajectory_frame_id_, tf2::durationFromSec(0.01));
    } catch (tf2::TransformException& ex) {
      RCLCPP_WARN(this->get_logger(), "Transformation into output frame is not available. Publishing no trajectory. Ex: %s",
                  ex.what());
      return false;
    }
    trajectory = tf_trajectory;
  }
  return true;
}

updateOcpInputs()

bool trajectory_optimization::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 )

protected

Transforms external inputs into the optimizer frame and writes them into the OCP.

Parameters

[in]	ego_data	Current ego state.
[in]	object_list	Current object list.
[in]	route	Current route data.
[in]	reference_trajectory	Current reference trajectory.

Returns

true if all optimizer inputs were updated successfully.

Definition at line 551 of file trajectory_optimization_node.cpp.

                                                                                                                    {
  // 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));
    // 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;
}

vizBoundaryPoints()

void trajectory_optimization::TrajectoryOptimizationNode::vizBoundaryPoints ( const std::vector< Eigen::Vector2d > & left_boundary_points, const std::vector< Eigen::Vector2d > & right_boundary_points )

protected

Publishes the boundary intersections corresponding to the distances passed to the OCP.

Parameters

[in]	left_boundary_points	Points on the left side.
[in]	right_boundary_points	Points on the right side.

Definition at line 311 of file utils.cpp.

                                                                                                          {
  visualization_msgs::msg::MarkerArray marker_array;
  int id = 0;
 
  auto createMarker = [&](int id, const std::string& ns, const Eigen::Vector2d& pos, float r, float g, float b) {
    visualization_msgs::msg::Marker m;
    m.header.frame_id = vehicle_frame_id_;
    m.header.stamp = rclcpp::Time(ego_data_.header.stamp);
    m.lifetime = rclcpp::Duration::from_seconds(0.5);
    m.ns = ns;
    m.id = id;
    m.type = visualization_msgs::msg::Marker::SPHERE;
    m.action = visualization_msgs::msg::Marker::ADD;
    m.pose.position.x = pos.x();
    m.pose.position.y = pos.y();
    m.pose.position.z = 0.0;
    m.scale.x = m.scale.y = m.scale.z = 0.4;
    m.color.a = 1.0;
    m.color.r = r;
    m.color.g = g;
    m.color.b = b;
    return m;
  };
 
  auto addMarkers = [&](const std::vector<Eigen::Vector2d>& points, const std::string& ns, float r, float g, float b) {
    for (const auto& point : points) {
      marker_array.markers.push_back(createMarker(id++, ns, point, r, g, b));
    }
  };
 
  addMarkers(left_boundary_points, "left_boundary_constraint", 0.0F, 1.0F, 0.0F);
  addMarkers(right_boundary_points, "right_boundary_constraint", 1.0F, 0.0F, 0.0F);
 
  boundary_pub_->publish(marker_array);
}

vizCircles()

void trajectory_optimization::TrajectoryOptimizationNode::vizCircles ( const std::vector< double > & obstacles )

protected

Publishes visualization markers for the obstacle circles currently used by the optimizer.

Parameters

[in]

obstacles

Flattened obstacle circle list.

Definition at line 348 of file utils.cpp.

                                                                              {
  visualization_msgs::msg::MarkerArray marker_array;
  const int n_circles = static_cast<int>(obstacles.size() / static_cast<size_t>(p_obstacle_circles_shape_[1]));
  for (int j = 0; j < n_circles; ++j) {
    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = vehicle_frame_id_;
    marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
    marker.lifetime = rclcpp::Duration::from_seconds(0.5);
    marker.ns = "obstacle-circles";
    marker.id = j;
    marker.type = visualization_msgs::msg::Marker::CYLINDER;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.pose.position.x = obstacles[p_obstacle_circles_shape_[1] * j + 0];
    marker.pose.position.y = obstacles[p_obstacle_circles_shape_[1] * j + 1];
    marker.scale.x = obstacles[p_obstacle_circles_shape_[1] * j + 2] * 2.0;
    marker.scale.y = obstacles[p_obstacle_circles_shape_[1] * j + 2] * 2.0;
    marker.scale.z = 0.1;
    marker.color.a = 0.3;
    marker.color.r = 1.0;
    marker_array.markers.push_back(marker);
  }
  circles_pub_->publish(marker_array);
}

vizEgoCircles()

void trajectory_optimization::TrajectoryOptimizationNode::vizEgoCircles ( const std::vector< double > & x_trajectory, const std::string & model_name )

protected

Publishes the ego-vehicle circle approximation used by the selected OCP model.

Parameters

[in]	x_trajectory	Optimizer state trajectory.
[in]	model_name	Name of the active OCP model.

Definition at line 371 of file utils.cpp.

                                                                                                                 {
  if (!ego_circles_pub_) return;
 
  double ego_length = 0.0, ego_width = 0.0;
  int n_ego_circles = 0;
  std::vector<double> ego_offset2geocenter;
 
  // define vehicle geometry based on model name (should match the OCP definition)
  if (model_name == "karl") {
    ego_length = 5.173;
    ego_width = 1.94;
    ego_offset2geocenter = {1.4895, 0.0};
    n_ego_circles = 5;
  } else if (model_name == "shuttle") {
    ego_length = 4.97;
    ego_width = 2.12;
    ego_offset2geocenter = {0.0, 0.0};
    n_ego_circles = 3;
  } else {
    RCLCPP_WARN(this->get_logger(), "Unknown model '%s'. Could not visualize ego circles.", model_name.c_str());
    return;
  }
 
  visualization_msgs::msg::MarkerArray marker_array;
 
  if (x_trajectory.empty() || n_ego_circles <= 0) {
    visualization_msgs::msg::Marker delete_marker;
    delete_marker.action = visualization_msgs::msg::Marker::DELETEALL;
    marker_array.markers.push_back(delete_marker);
    ego_circles_pub_->publish(marker_array);
    return;
  }
 
  const double offset_x = !ego_offset2geocenter.empty() ? ego_offset2geocenter[0] : 0.0;
  const double offset_y = ego_offset2geocenter.size() > 1 ? ego_offset2geocenter[1] : 0.0;
 
  const double radius = std::sqrt(std::pow(ego_length / (2.0 * n_ego_circles), 2) + std::pow(ego_width / 2.0, 2));
  const int state_dim = *nlp_dims_->nx;
 
  int marker_id = 0;
  for (int stage = 0; stage <= n_shots_; ++stage) {
    const size_t state_offset = static_cast<size_t>(stage) * static_cast<size_t>(state_dim);
    const double base_x = x_trajectory[state_offset + 0];
    const double base_y = x_trajectory[state_offset + 1];
    const double psi = x_trajectory[state_offset + 5];
 
    const double ego_center_x = base_x + offset_x * std::cos(psi) - offset_y * std::sin(psi);
    const double ego_center_y = base_y + offset_x * std::sin(psi) + offset_y * std::cos(psi);
 
    for (int i = 0; i < n_ego_circles; ++i) {
      const double lon_offset = -ego_length / 2.0 + (2 * i + 1) * ego_length / (2.0 * n_ego_circles);
      const double x_offset = lon_offset * std::cos(psi);
      const double y_offset = lon_offset * std::sin(psi);
 
      visualization_msgs::msg::Marker marker;
      marker.header.frame_id = vehicle_frame_id_;
      marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
      marker.lifetime = rclcpp::Duration::from_seconds(0.5);
      marker.ns = "ego-circles";
      marker.id = marker_id++;
      marker.type = visualization_msgs::msg::Marker::CYLINDER;
      marker.action = visualization_msgs::msg::Marker::ADD;
      marker.pose.position.x = ego_center_x + x_offset;
      marker.pose.position.y = ego_center_y + y_offset;
      marker.pose.position.z = 0.0;
      marker.pose.orientation.w = 1.0;
      marker.scale.x = radius * 2.0;
      marker.scale.y = radius * 2.0;
      marker.scale.z = 0.05;
      marker.color.a = 0.4;
      marker.color.r = 0.0F;
      marker.color.g = 0.6F;
      marker.color.b = 1.0F;
      marker_array.markers.push_back(marker);
    }
  }
 
  ego_circles_pub_->publish(marker_array);
}

wrap_angle_rad()

double trajectory_optimization::TrajectoryOptimizationNode::wrap_angle_rad ( double angle_rad, double min_val = -M_PI, double max_val = M_PI )

staticprotected

Wraps an angle into a configured interval.

Parameters

[in]	angle_rad	Angle in radians.
[in]	min_val	Lower bound of the target interval.
[in]	max_val	Upper bound of the target interval.

Returns

Wrapped angle in radians.

Definition at line 14 of file utils.cpp.

                                                                                                  {
  double capped_angle_rad = angle_rad;
  while (capped_angle_rad > max_val) capped_angle_rad -= 2 * M_PI;
  while (capped_angle_rad < min_val) capped_angle_rad += 2 * M_PI;
  return capped_angle_rad;
}

Member Data Documentation

auto_reconfigurable_params_

std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)> > > trajectory_optimization::TrajectoryOptimizationNode::auto_reconfigurable_params_

protected

Definition at line 379 of file trajectory_optimization_node.hpp.

bi_level_dV_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dV_ = 2.0

protected

Definition at line 398 of file trajectory_optimization_node.hpp.

bi_level_dY_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dY_ = 0.3

protected

Definition at line 399 of file trajectory_optimization_node.hpp.

bi_level_dYaw_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dYaw_ = 89.0

protected

Definition at line 400 of file trajectory_optimization_node.hpp.

boundary_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::boundary_pub_

protected

Definition at line 365 of file trajectory_optimization_node.hpp.

circles_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::circles_pub_

protected

Definition at line 363 of file trajectory_optimization_node.hpp.

consider_boundaries_

uint8_t trajectory_optimization::TrajectoryOptimizationNode::consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE

protected

Definition at line 394 of file trajectory_optimization_node.hpp.

consider_objects_

uint8_t trajectory_optimization::TrajectoryOptimizationNode::consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS

protected

Definition at line 393 of file trajectory_optimization_node.hpp.

control_guess_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::control_guess_

protected

Definition at line 406 of file trajectory_optimization_node.hpp.

control_guess_stamp_

rclcpp::Time trajectory_optimization::TrajectoryOptimizationNode::control_guess_stamp_ {0, 0, RCL_ROS_TIME}

protected

Definition at line 407 of file trajectory_optimization_node.hpp.

{0, 0, RCL_ROS_TIME};

cost_weights_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::cost_weights_ = std::vector<double>(12, 1.0)

protected

Definition at line 413 of file trajectory_optimization_node.hpp.

d_min_boundary_lat_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_boundary_lat_ = 0.0

protected

Definition at line 418 of file trajectory_optimization_node.hpp.

d_min_obstacle_lat_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_obstacle_lat_ = 0.5

protected

Definition at line 417 of file trajectory_optimization_node.hpp.

d_min_obstacle_long_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_obstacle_long_ = 5.0

protected

Definition at line 416 of file trajectory_optimization_node.hpp.

debug_viz_

bool trajectory_optimization::TrajectoryOptimizationNode::debug_viz_ = false

protected

Definition at line 390 of file trajectory_optimization_node.hpp.

dynamic_weight_

double trajectory_optimization::TrajectoryOptimizationNode::dynamic_weight_ = 1.0

protected

Definition at line 414 of file trajectory_optimization_node.hpp.

ego_circles_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::ego_circles_pub_

protected

Definition at line 364 of file trajectory_optimization_node.hpp.

ego_data_

perception_msgs::msg::EgoData trajectory_optimization::TrajectoryOptimizationNode::ego_data_

protected

Definition at line 373 of file trajectory_optimization_node.hpp.

ego_data_sub_

rclcpp::Subscription<perception_msgs::msg::EgoData>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::ego_data_sub_

protected

Definition at line 357 of file trajectory_optimization_node.hpp.

ego_data_timeout_

double trajectory_optimization::TrajectoryOptimizationNode::ego_data_timeout_ = 1.0

protected

Definition at line 384 of file trajectory_optimization_node.hpp.

fixed_over_time_frame_id_

std::string trajectory_optimization::TrajectoryOptimizationNode::fixed_over_time_frame_id_ = "map"

protected

Definition at line 382 of file trajectory_optimization_node.hpp.

high_level_stabilization_

bool trajectory_optimization::TrajectoryOptimizationNode::high_level_stabilization_ = false

protected

Definition at line 392 of file trajectory_optimization_node.hpp.

latest_valid_trajectory_

trajectory_planning_msgs::msg::Trajectory trajectory_optimization::TrajectoryOptimizationNode::latest_valid_trajectory_

protected

Definition at line 403 of file trajectory_optimization_node.hpp.

logging_cycle_

uint64_t trajectory_optimization::TrajectoryOptimizationNode::logging_cycle_ = 0

protected

Definition at line 438 of file trajectory_optimization_node.hpp.

min_prediction_probability_

double trajectory_optimization::TrajectoryOptimizationNode::min_prediction_probability_ = 0.0

protected

Definition at line 419 of file trajectory_optimization_node.hpp.

model_name_

std::string trajectory_optimization::TrajectoryOptimizationNode::model_name_ = "karl"

protected

Definition at line 383 of file trajectory_optimization_node.hpp.

n_shots_

int trajectory_optimization::TrajectoryOptimizationNode::n_shots_ = 50

protected

Definition at line 386 of file trajectory_optimization_node.hpp.

nlp_config_

ocp_nlp_config* trajectory_optimization::TrajectoryOptimizationNode::nlp_config_

protected

Definition at line 429 of file trajectory_optimization_node.hpp.

nlp_dims_

ocp_nlp_dims* trajectory_optimization::TrajectoryOptimizationNode::nlp_dims_

protected

Definition at line 430 of file trajectory_optimization_node.hpp.

nlp_in_

ocp_nlp_in* trajectory_optimization::TrajectoryOptimizationNode::nlp_in_

protected

Definition at line 431 of file trajectory_optimization_node.hpp.

nlp_opts_

void* trajectory_optimization::TrajectoryOptimizationNode::nlp_opts_

protected

Definition at line 434 of file trajectory_optimization_node.hpp.

nlp_out_

ocp_nlp_out* trajectory_optimization::TrajectoryOptimizationNode::nlp_out_

protected

Definition at line 432 of file trajectory_optimization_node.hpp.

nlp_solver_

ocp_nlp_solver* trajectory_optimization::TrajectoryOptimizationNode::nlp_solver_

protected

Definition at line 433 of file trajectory_optimization_node.hpp.

object_list_

perception_msgs::msg::ObjectList trajectory_optimization::TrajectoryOptimizationNode::object_list_

protected

Definition at line 374 of file trajectory_optimization_node.hpp.

object_list_sub_

rclcpp::Subscription<perception_msgs::msg::ObjectList>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::object_list_sub_

protected

Definition at line 358 of file trajectory_optimization_node.hpp.

ocp_capsule_

ocp_model_capsule_t trajectory_optimization::TrajectoryOptimizationNode::ocp_capsule_

protected

Definition at line 428 of file trajectory_optimization_node.hpp.

optimization_freq_

double trajectory_optimization::TrajectoryOptimizationNode::optimization_freq_ = 10.0

protected

Definition at line 385 of file trajectory_optimization_node.hpp.

optimization_horizon_

double trajectory_optimization::TrajectoryOptimizationNode::optimization_horizon_ = 1.0

protected

Definition at line 387 of file trajectory_optimization_node.hpp.

p_cost_weights_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_cost_weights_shape_ = {12, 1}

protected

Definition at line 423 of file trajectory_optimization_node.hpp.

{12, 1};      // nWeights x weightDim

p_obstacle_circles_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_obstacle_circles_shape_ = {30, 3}

protected

Definition at line 425 of file trajectory_optimization_node.hpp.

{30, 3};  // nObstacleCircles x [x, y, radius]

p_ref_path_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_ref_path_shape_ = {51, 6}

protected

Definition at line 424 of file trajectory_optimization_node.hpp.

{51, 6};          // nStates x [psi, x, y, v, d_bound_left, d_bound_right]

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::parameters_callback_

protected

Definition at line 355 of file trajectory_optimization_node.hpp.

performance_logger_

std::unique_ptr<PerformanceLogger> trajectory_optimization::TrajectoryOptimizationNode::performance_logger_

protected

Definition at line 439 of file trajectory_optimization_node.hpp.

performance_logging_

bool trajectory_optimization::TrajectoryOptimizationNode::performance_logging_ = false

protected

Definition at line 389 of file trajectory_optimization_node.hpp.

planning_timer_

rclcpp::TimerBase::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::planning_timer_

protected

Definition at line 367 of file trajectory_optimization_node.hpp.

reference_trajectory_

trajectory_planning_msgs::msg::Trajectory trajectory_optimization::TrajectoryOptimizationNode::reference_trajectory_

protected

Definition at line 376 of file trajectory_optimization_node.hpp.

reference_trajectory_sub_

rclcpp::Subscription<trajectory_planning_msgs::msg::Trajectory>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::reference_trajectory_sub_

protected

Definition at line 360 of file trajectory_optimization_node.hpp.

route_

route_planning_msgs::msg::Route trajectory_optimization::TrajectoryOptimizationNode::route_

protected

Definition at line 375 of file trajectory_optimization_node.hpp.

route_sub_

rclcpp::Subscription<route_planning_msgs::msg::Route>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::route_sub_

protected

Definition at line 359 of file trajectory_optimization_node.hpp.

run_as_callback_

bool trajectory_optimization::TrajectoryOptimizationNode::run_as_callback_ = false

protected

Definition at line 395 of file trajectory_optimization_node.hpp.

standstill_threshold_

double trajectory_optimization::TrajectoryOptimizationNode::standstill_threshold_ = 0.45

protected

Definition at line 391 of file trajectory_optimization_node.hpp.

tf2_buffer_

std::unique_ptr<tf2_ros::Buffer> trajectory_optimization::TrajectoryOptimizationNode::tf2_buffer_

protected

Definition at line 369 of file trajectory_optimization_node.hpp.

tf2_listener_

std::shared_ptr<tf2_ros::TransformListener> trajectory_optimization::TrajectoryOptimizationNode::tf2_listener_

protected

Definition at line 370 of file trajectory_optimization_node.hpp.

thw_

double trajectory_optimization::TrajectoryOptimizationNode::thw_ = 2.0

protected

Definition at line 415 of file trajectory_optimization_node.hpp.

trajectory_frame_id_

std::string trajectory_optimization::TrajectoryOptimizationNode::trajectory_frame_id_ = "base_link"

protected

Definition at line 381 of file trajectory_optimization_node.hpp.

trajectory_pub_

rclcpp::Publisher<trajectory_planning_msgs::msg::Trajectory>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::trajectory_pub_

protected

Definition at line 362 of file trajectory_optimization_node.hpp.

utraj_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::utraj_

protected

Definition at line 437 of file trajectory_optimization_node.hpp.

vehicle_frame_id_

std::string trajectory_optimization::TrajectoryOptimizationNode::vehicle_frame_id_ = "base_link"

protected

Definition at line 380 of file trajectory_optimization_node.hpp.

verbose_

bool trajectory_optimization::TrajectoryOptimizationNode::verbose_ = false

protected

Definition at line 388 of file trajectory_optimization_node.hpp.

viz_circles_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::viz_circles_

protected

Definition at line 410 of file trajectory_optimization_node.hpp.

xtraj_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::xtraj_

protected

Definition at line 436 of file trajectory_optimization_node.hpp.

---

The documentation for this class was generated from the following files:

• trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp
• trajectory_optimization/src/callbacks.cpp
• trajectory_optimization/src/trajectory_optimization_node.cpp
• trajectory_optimization/src/utils.cpp