trajectory_optimization v1.5.0
Loading...
Searching...
No Matches
trajectory_optimization::TrajectoryOptimizationNode Class Referenceabstract

#include <trajectory_optimization_node.hpp>

Inheritance diagram for trajectory_optimization::TrajectoryOptimizationNode:
trajectory_optimization::TrajectoryOptimizationAckermannNode trajectory_optimization::TrajectoryOptimizationRWSNode

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 object, boundary-activation, and dynamic-weighting parameters into the OCP.
 
bool setSafetyConstraintActivation (bool use_configured_activation)
 Enables the configured object and boundary constraints or temporarily disables all of them.
 
void setSolverTimeout (double timeout_ms)
 
bool solveAndValidate (PerformanceMetrics &metrics, bool &finite_solution)
 Solves the OCP, reads its output and checks primal feasibility.
 
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.
 
void vizObjectBoxes (const std::vector< double > &objects)
 
void vizEgoBoxes (const std::vector< double > &x_trajectory, const std::string &model_name)
 
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, const bool ignore_objects_behind_ego)
 Keeps the nearest objects, optionally excluding those behind the ego vehicle.
 

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 object_marker_pub_
 
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr ego_marker_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
 
bool two_stage_optimization_ = true
 
double relaxed_solve_timeout_ms_ = 100.0
 
double constrained_solve_timeout_ms_ = 100.0
 
double standstill_threshold_ = 0.45
 
bool high_level_stabilization_ = false
 
uint8_t consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS
 
bool ignore_objects_behind_ego_ = true
 
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_object_boxes_
 
std::vector< double > cost_weights_ = std::vector<double>(12, 1.0)
 
double dynamic_weight_ = 1.0
 
double thw_ = 2.0
 
double d_min_object_long_ = 5.0
 
double d_min_object_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_dynamic_weight_shape_ = {1, 1}
 
std::vector< int64_t > p_constraint_activation_shape_ = {2, 1}
 
std::vector< int64_t > p_objects_shape_ = {30, 6}
 
bool object_constraints_active_ = false
 
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

◆ CONSIDER_OBJECTS

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_nameName of the ROS node instance.
[in]optionsROS node options used for construction.

Definition at line 35 of file trajectory_optimization_node.cpp.

36 : rclcpp::Node(node_name, options) {
37 // declare and load node parameters
38 this->declareAndLoadParameter("vehicle_frame_id", vehicle_frame_id_,
39 "Frame ID of local vehicle frame (the ocp is defined in this frame)");
40 this->declareAndLoadParameter("trajectory_frame_id", trajectory_frame_id_, "Frame ID of output trajectory");
41 this->declareAndLoadParameter("fixed_over_time_frame_id", fixed_over_time_frame_id_,
42 "Frame ID of frame that is fixed over time for finding temporal transforms");
43 this->declareAndLoadParameter("ego_data_timeout", ego_data_timeout_,
44 "Time after which a received ego vehicle data is considered invalid [s]. Optimization will not "
45 "be run if ego data is invalid.");
46 this->declareAndLoadParameter("model_name", model_name_,
47 "Name of the model to be used for trajectory optimization [karl, shuttle]");
48 this->declareAndLoadParameter("optimization_frequency", optimization_freq_, "Optimization frequency in Hz");
49 this->declareAndLoadParameter("n_shots", n_shots_, "Number of shooting intervals in optimization horizon");
50 this->declareAndLoadParameter("optimization_horizon", optimization_horizon_, "Optimization Horizon in seconds");
51 this->declareAndLoadParameter("two_stage_optimization", two_stage_optimization_,
52 "Initialize the constrained OCP with a solve without object and boundary constraints");
53 this->declareAndLoadParameter("relaxed_solve_timeout_ms", relaxed_solve_timeout_ms_,
54 "ACADOS timeout for the relaxed initialization solve [ms]; 0 disables the timeout");
55 this->declareAndLoadParameter("constrained_solve_timeout_ms", constrained_solve_timeout_ms_,
56 "ACADOS timeout for the fully constrained solve [ms]; 0 disables the timeout");
57 this->declareAndLoadParameter("verbose", verbose_, "Print solver statistics");
58 this->declareAndLoadParameter("performance_logging", performance_logging_,
59 "Write one CSV record for every completed solver run");
60 this->declareAndLoadParameter("debug_visualization", debug_viz_,
61 "Publish debug visualization markers for ego, objects and boundaries");
62 this->declareAndLoadParameter("run_as_callback", run_as_callback_,
63 "Run OCP once for each received reference trajectory (true) or on a timer (false)");
64 this->declareAndLoadParameter("cost_weights", cost_weights_, "Cost function weights");
65 this->declareAndLoadParameter("dynamic_weight", dynamic_weight_, "Dynamic weight alpha");
66 this->declareAndLoadParameter("thw", thw_, "Desired time headway to front vehicle [s]; soft when object slacks are enabled");
67 this->declareAndLoadParameter("d_min_object_long", d_min_object_long_,
68 "Desired longitudinal object clearance [m]; soft when object slacks are enabled");
69 this->declareAndLoadParameter("d_min_object_lat", d_min_object_lat_,
70 "Desired lateral object clearance [m]; soft when object slacks are enabled");
71 this->declareAndLoadParameter("d_min_boundary_lat", d_min_boundary_lat_,
72 "Minimum distance to keep to boundary in lateral direction [m]");
73 this->declareAndLoadParameter("standstill_threshold", standstill_threshold_,
74 "Threshold for standstill detection [m/s]. If the velocities of all states are below this "
75 "threshold, publish standstill trajectory");
76 this->declareAndLoadParameter("high_level_stabilization", high_level_stabilization_,
77 "Use high-level stabilization strategy for init state (= init with current EgoData)");
79 "consider_objects", consider_objects_,
80 "consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction)");
81 this->declareAndLoadParameter("ignore_objects_behind_ego", ignore_objects_behind_ego_,
82 "Exclude objects with reference-point x <= 0 in vehicle_frame_id");
83 this->declareAndLoadParameter("min_prediction_probability", min_prediction_probability_,
84 "Minimum probability for predicted object states to be considered", true, false, false, 0.0, 1.0);
86 "consider_boundaries", consider_boundaries_,
87 "consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space");
88 this->declareAndLoadParameter("bi_level_dV", bi_level_dV_,
89 "Threshold for bi-level stabilization: maximum velocity difference [m/s]");
90 this->declareAndLoadParameter("bi_level_dY", bi_level_dY_, "Threshold for bi-level stabilization: maximum y-offset [m]");
91 this->declareAndLoadParameter("bi_level_dYaw", bi_level_dYaw_,
92 "Threshold for bi-level stabilization: maximum yaw difference [degree]");
94 try {
95 performance_logger_ = std::make_unique<PerformanceLogger>(get_name());
96 RCLCPP_INFO(get_logger(), "Writing benchmark logs to '%s'.", performance_logger_->path().c_str());
97 } catch (const std::exception& error) {
98 RCLCPP_ERROR(get_logger(), "Could not initialize benchmark logging: %s", error.what());
99 }
100 }
101 this->setup();
102}
void setup()
Creates ROS interfaces, initializes cached messages and prepares the solver.
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.

◆ ~TrajectoryOptimizationNode()

trajectory_optimization::TrajectoryOptimizationNode::~TrajectoryOptimizationNode ( )
override

Releases solver resources owned by the node.

Definition at line 104 of file trajectory_optimization_node.cpp.

104{ freeSolver(); }
void freeSolver()
Frees the acados solver and clears cached optimization results.

◆ 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]trajectoryOutput 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
TParameter value type.
Parameters
[in]nameParameter name.
[out]paramMember variable to store the parameter value.
[in]descriptionHuman-readable parameter description.
[in]add_to_auto_reconfigurable_paramsWhether parameter updates automatically update the member variable.
[in]is_requiredWhether the node should fail if the parameter is not set.
[in]read_onlyWhether the parameter is exposed as read-only.
[in]from_valueOptional lower bound for numeric parameters.
[in]to_valueOptional upper bound for numeric parameters.
[in]step_valueOptional step size for numeric parameters.
[in]additional_constraintsAdditional free-form constraint text for the parameter descriptor.

Definition at line 107 of file trajectory_optimization_node.cpp.

116 {
117 rcl_interfaces::msg::ParameterDescriptor param_desc;
118 param_desc.description = description;
119 param_desc.additional_constraints = additional_constraints;
120 param_desc.read_only = read_only;
121
122 auto type = rclcpp::ParameterValue(param).get_type();
123
124 if (from_value.has_value() && to_value.has_value()) {
125 if constexpr (std::is_integral_v<T>) {
126 rcl_interfaces::msg::IntegerRange range;
127 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
128 if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
129 param_desc.integer_range = {range};
130 } else if constexpr (std::is_floating_point_v<T>) {
131 rcl_interfaces::msg::FloatingPointRange range;
132 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
133 if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
134 param_desc.floating_point_range = {range};
135 } else {
136 RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
137 }
138 }
139
140 this->declare_parameter(name, type, param_desc);
141
142 try {
143 param = this->get_parameter(name).get_value<T>();
144 std::stringstream ss;
145 ss << "Loaded parameter '" << name << "': ";
146 if constexpr (is_vector_v<T>) {
147 ss << "[";
148 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
149 ss << "]";
150 } else {
151 ss << param;
152 }
153 RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
154 } catch (rclcpp::exceptions::ParameterUninitializedException&) {
155 if (is_required) {
156 RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
157 exit(EXIT_FAILURE);
158 } else {
159 std::stringstream ss;
160 ss << "Missing parameter '" << name << "', using default value: ";
161 if constexpr (is_vector_v<T>) {
162 ss << "[";
163 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
164 ss << "]";
165 } else {
166 ss << param;
167 }
168 RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
169 this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
170 }
171 }
172
173 if (add_to_auto_reconfigurable_params) {
174 std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
175 auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
176 }
177}
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > > auto_reconfigurable_params_

◆ 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]msgIncoming ego vehicle message.

Definition at line 7 of file callbacks.cpp.

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

◆ freeSolver()

void trajectory_optimization::TrajectoryOptimizationNode::freeSolver ( )
protected

Frees the acados solver and clears cached optimization results.

Definition at line 316 of file trajectory_optimization_node.cpp.

316 {
317 // free solver
319 if (status != 0) {
320 RCLCPP_ERROR(this->get_logger(), "%s_acados_free() returned status %d.", model_name_.c_str(), status);
321 }
322 // free solver capsule
324 if (status != 0) {
325 RCLCPP_ERROR(this->get_logger(), "%s_acados_free_capsule() returned status %d.", model_name_.c_str(), status);
326 }
327}
int acados_free(ocp_model_capsule_t capsule)
Wrapper around the generated acados solver cleanup function.
int acados_free_capsule(ocp_model_capsule_t capsule)
Wrapper around the generated acados capsule cleanup function.

◆ 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_dataCurrent 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_dataCurrent 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]trajectoryTrajectory 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,
const bool ignore_objects_behind_ego )
staticprotected

Keeps the nearest objects, optionally excluding those behind the ego vehicle.

Parameters
[in,out]object_listObject list to filter.
[in]n_objectsMaximum number of objects to retain.
[in]ignore_objects_behind_egoExclude objects with reference-point x <= 0 in the optimizer frame.

Definition at line 88 of file utils.cpp.

90 {
91 // sort object indices by distance to ego vehicle
92 std::vector<size_t> indices_sorted_by_distance(object_list.objects.size());
93 std::iota(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), 0);
94 std::sort(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), [&](size_t first, size_t second) {
95 const auto& first_object = object_list.objects[first];
96 const auto& second_object = object_list.objects[second];
97 return std::hypot(perception_msgs::object_access::getX(first_object), perception_msgs::object_access::getY(first_object)) <
98 std::hypot(perception_msgs::object_access::getX(second_object), perception_msgs::object_access::getY(second_object));
99 });
100
101 // keep only the closest n_objects, optionally excluding objects behind the ego vehicle
102 std::vector<perception_msgs::msg::Object> closest_objects;
103 closest_objects.reserve(std::min(static_cast<size_t>(n_objects), object_list.objects.size()));
104 for (const size_t index : indices_sorted_by_distance) {
105 if (ignore_objects_behind_ego && perception_msgs::object_access::getX(object_list.objects[index]) <= 0.0) continue;
106 closest_objects.push_back(object_list.objects[index]);
107 if (closest_objects.size() >= static_cast<size_t>(n_objects)) break;
108 }
109 object_list.objects = std::move(closest_objects);
110}

◆ 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]XSample positions.
[in]YSample values.
[in]desired_xQuery position.
[out]output_yInterpolated result.
[in]wrap_angleWhether angular differences should be wrapped to [-pi, pi].
Returns
true if the query lies within the sample range, otherwise false.

Definition at line 23 of file utils.cpp.

24 {
25 if (desired_x == X.front()) {
26 RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Min of the given vector!");
27 output_y = Y.front();
28 return true;
29 } else if (desired_x == X.back()) {
30 RCLCPP_DEBUG(get_logger(), "Desired Time is equal to Time-Max of the given vector!");
31 output_y = Y.back();
32 return true;
33 } else if (desired_x < *min_element(X.begin(), X.end())) {
34 RCLCPP_WARN(get_logger(), "Desired Time is smaller than Time-Min of the given vector! Using first valid value.");
35 RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
36 RCLCPP_DEBUG(get_logger(), "Time-Min: %f s", *min_element(X.begin(), X.end()));
37 output_y = Y.front();
38 return false;
39 } else if (desired_x > *max_element(X.begin(), X.end())) {
40 RCLCPP_WARN(get_logger(), "Desired Time is greater than Time-Max of the given vector! Using last valid value.");
41 RCLCPP_DEBUG(get_logger(), "Desired Time: %f s", desired_x);
42 RCLCPP_DEBUG(get_logger(), "Time-Max: %f s", *max_element(X.begin(), X.end()));
43 output_y = Y.back();
44 return false;
45 } else if (X.size() != Y.size()) {
46 RCLCPP_ERROR(get_logger(), "Input vectors don't have the same length!");
47 return false;
48 }
49
50 //go through array and search for sampling points
51 size_t i = 0;
52 for (i = 0; i < X.size(); i++) {
53 if (X[i] < desired_x) {
54 continue;
55 } else if (X[i] == desired_x) {
56 output_y = Y[i];
57 return true;
58 } else {
59 break;
60 }
61 }
62 double diff = Y[i] - Y[i - 1];
63 if (wrap_angle) {
64 diff = wrap_angle_rad(diff);
65 }
66 output_y = Y[i - 1] + (diff / (X[i] - X[i - 1])) * (desired_x - X[i - 1]);
67 if (wrap_angle) {
68 output_y = wrap_angle_rad(output_y);
69 }
70 return true;
71}
static double wrap_angle_rad(double angle_rad, double min_val=-M_PI, double max_val=M_PI)
Wraps an angle into a configured interval.
Definition utils.cpp:16

◆ 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_trajectoryReference trajectory in optimizer frame.
[in]routeRoute data used to derive active boundaries.
Returns
Left and right boundary distances for each reference sample.

Definition at line 112 of file utils.cpp.

113 {
114 const double NO_BOUNDARY_DISTANCE = 1e6; // finite sentinel avoids NaNs during reference-path interpolation
115 constexpr double MAX_ROUTE_S_DIFFERENCE = 20.0;
116 constexpr double LOOK_BEHIND_REMAINING_ROUTE_S = 2.0;
117
118 struct Boundaries {
119 std::vector<std::pair<double, double>> min_normal_distances;
120 std::vector<Eigen::Vector2d> left_boundary_points;
121 std::vector<Eigen::Vector2d> right_boundary_points;
122 std::vector<double> left_boundary_route_s;
123 std::vector<double> right_boundary_route_s;
124 std::vector<Eigen::Vector2d> left_boundary_intersections;
125 std::vector<Eigen::Vector2d> right_boundary_intersections;
126 };
127
128 Boundaries boundaries;
129 auto route_elements = route_planning_msgs::route_access::getRemainingRouteElements(route, true);
130 const int ref_sample_size = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory);
131
132 if (route_elements.empty()) {
134 RCLCPP_WARN(get_logger(), "Remaining route is empty. Do not constrain boundaries.");
135 }
136 for (int i = 0; i < ref_sample_size; ++i) {
137 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
138 }
139 return boundaries.min_normal_distances;
140 }
141
142 const double current_route_s = route_elements.front().s;
143 const size_t current_route_index = std::min(static_cast<size_t>(route.current_route_element_idx), route.route_elements.size());
144 size_t overlap_begin = current_route_index;
145 while (overlap_begin > 0 && current_route_s - route.route_elements[overlap_begin - 1].s <= LOOK_BEHIND_REMAINING_ROUTE_S) {
146 --overlap_begin;
147 }
148 route_elements.insert(route_elements.begin(), route.route_elements.begin() + static_cast<std::ptrdiff_t>(overlap_begin),
149 route.route_elements.begin() + static_cast<std::ptrdiff_t>(current_route_index));
150
151 boundaries.left_boundary_points.reserve(route_elements.size());
152 boundaries.right_boundary_points.reserve(route_elements.size());
153 boundaries.left_boundary_route_s.reserve(route_elements.size());
154 boundaries.right_boundary_route_s.reserve(route_elements.size());
155 boundaries.min_normal_distances.reserve(ref_sample_size);
156 boundaries.left_boundary_intersections.reserve(ref_sample_size);
157 boundaries.right_boundary_intersections.reserve(ref_sample_size);
158
159 for (const auto& route_element : route_elements) {
160 if (route_element.is_enriched) {
162 route_planning_msgs::msg::LaneElement suggested_lane =
163 route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
164 boundaries.left_boundary_points.emplace_back(suggested_lane.left_boundary.point.x, suggested_lane.left_boundary.point.y);
165 boundaries.right_boundary_points.emplace_back(suggested_lane.right_boundary.point.x,
166 suggested_lane.right_boundary.point.y);
167 boundaries.left_boundary_route_s.push_back(route_element.s);
168 boundaries.right_boundary_route_s.push_back(route_element.s);
170 const auto& lane_elements = route_element.lane_elements;
171 if (!lane_elements.empty()) {
172 boundaries.left_boundary_points.emplace_back(lane_elements.front().left_boundary.point.x,
173 lane_elements.front().left_boundary.point.y);
174 boundaries.right_boundary_points.emplace_back(lane_elements.back().right_boundary.point.x,
175 lane_elements.back().right_boundary.point.y);
176 boundaries.left_boundary_route_s.push_back(route_element.s);
177 boundaries.right_boundary_route_s.push_back(route_element.s);
178 }
180 boundaries.left_boundary_points.emplace_back(route_element.left_boundary.x, route_element.left_boundary.y);
181 boundaries.right_boundary_points.emplace_back(route_element.right_boundary.x, route_element.right_boundary.y);
182 boundaries.left_boundary_route_s.push_back(route_element.s);
183 boundaries.right_boundary_route_s.push_back(route_element.s);
184 }
185 }
186 }
187
188 // Helper lambda to find intersection
189 auto findIntersection = [&](const Eigen::Vector2d& ref_pos, double sin_yaw, double cos_yaw, double expected_route_s,
190 const std::vector<Eigen::Vector2d>& boundary_points, const std::vector<double>& boundary_route_s,
191 bool isLeft) -> std::pair<double, Eigen::Vector2d> {
192 std::pair<double, Eigen::Vector2d> intersection_result = {
193 std::numeric_limits<double>::infinity(),
194 Eigen::Vector2d(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())};
195 double best_route_s_difference = std::numeric_limits<double>::infinity();
196
197 const Eigen::Vector2d normal_dir = isLeft ? Eigen::Vector2d(-sin_yaw, cos_yaw) : Eigen::Vector2d(sin_yaw, -cos_yaw);
198 const auto cross2d = [](const Eigen::Vector2d& u, const Eigen::Vector2d& v) { return u.x() * v.y() - u.y() * v.x(); };
199
200 for (size_t i = 0; i + 1 < boundary_points.size(); ++i) {
201 const Eigen::Vector2d& a = boundary_points[i];
202 const Eigen::Vector2d& b = boundary_points[i + 1];
203 Eigen::Vector2d seg = b - a;
204 Eigen::Vector2d ap = ref_pos - a;
205
206 const double denom = cross2d(seg, normal_dir);
207 if (std::abs(denom) < 1e-9) {
208 continue; // Lines are close to parallel; ignore this segment
209 }
210
211 const double s = cross2d(ap, normal_dir) / denom;
212 const double t = cross2d(ap, seg) / denom;
213
214 if (s >= 0.0 && s <= 1.0 && t >= 0.0) {
215 Eigen::Vector2d intersection = a + s * seg;
216 double euclidean_distance = (ref_pos - intersection).norm();
217 const double intersection_route_s = boundary_route_s[i] + s * (boundary_route_s[i + 1] - boundary_route_s[i]);
218 const double route_s_difference = std::abs(intersection_route_s - expected_route_s);
219
220 if (route_s_difference <= MAX_ROUTE_S_DIFFERENCE &&
221 (route_s_difference < best_route_s_difference ||
222 (std::abs(route_s_difference - best_route_s_difference) < 1e-9 && euclidean_distance < intersection_result.first))) {
223 best_route_s_difference = route_s_difference;
224 intersection_result = {euclidean_distance, intersection};
225 }
226 }
227 }
228 return intersection_result;
229 };
230
231 // Loop over trajectory points and compute intersections
232 double reference_progress = 0.0;
233 Eigen::Vector2d previous_ref_pos = Eigen::Vector2d::Zero();
234 for (int i = 0; i < ref_sample_size; ++i) {
235 Eigen::Vector2d ref_pos(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i),
236 trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
237 reference_progress += (ref_pos - previous_ref_pos).norm();
238 previous_ref_pos = ref_pos;
239 const double expected_route_s = current_route_s + reference_progress;
240
241 double yaw = trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i);
242 const double sin_yaw = std::sin(yaw);
243 const double cos_yaw = std::cos(yaw);
244
245 auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.left_boundary_points,
246 boundaries.left_boundary_route_s, true);
247 auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.right_boundary_points,
248 boundaries.right_boundary_route_s, false);
249 if (left_intersection.first != std::numeric_limits<double>::infinity() &&
250 right_intersection.first != std::numeric_limits<double>::infinity()) {
251 boundaries.min_normal_distances.emplace_back(left_intersection.first, right_intersection.first);
252 boundaries.left_boundary_intersections.emplace_back(left_intersection.second);
253 boundaries.right_boundary_intersections.emplace_back(right_intersection.second);
254 RCLCPP_DEBUG(this->get_logger(), "Minimum left boundary distance: %.2f m", left_intersection.first);
255 RCLCPP_DEBUG(this->get_logger(), "Minimum right boundary distance: %.2f m", right_intersection.first);
256 } else {
257 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
258 RCLCPP_WARN(get_logger(),
259 "No boundary intersection found for trajectory point %d. Do not constrain boundaries at this point.", i);
260 }
261 }
262 if (debug_viz_) {
263 vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections);
264 }
265
266 return boundaries.min_normal_distances;
267}
void vizBoundaryPoints(const std::vector< Eigen::Vector2d > &left_boundary_points, const std::vector< Eigen::Vector2d > &right_boundary_points)
Publishes the boundary intersections corresponding to the distances passed to the OCP.
Definition utils.cpp:269

◆ 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]msgIncoming object list message.

Definition at line 12 of file callbacks.cpp.

12 {
14 RCLCPP_DEBUG(this->get_logger(), "Received object list");
15 object_list_ = *msg;
16 } else {
17 // reset object list to empty
18 object_list_ = perception_msgs::msg::ObjectList();
19 }
20}

◆ 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]parametersParameters requested by the ROS parameter service.
Returns
Result of the parameter update request.

Definition at line 179 of file trajectory_optimization_node.cpp.

180 {
181 for (const auto& param : parameters) {
182 for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
183 if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
184 std::get<1>(auto_reconfigurable_param)(param);
185 RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s' to: %s", param.get_name().c_str(),
186 param.value_to_string().c_str());
187 break;
188 }
189 }
190 // handle special cases
191 if (param.get_name() == "run_as_callback") {
193 planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
195 RCLCPP_WARN(this->get_logger(), "OCP runs now periodically with frequency %f Hz", optimization_freq_);
196 } else if (run_as_callback_ && planning_timer_) {
197 planning_timer_->cancel();
198 planning_timer_.reset();
199 RCLCPP_WARN(this->get_logger(), "OCP runs now on reference trajectory callback");
200 }
201 }
202 }
203 // mark parameter change successful
204 rcl_interfaces::msg::SetParametersResult result;
205 result.successful = true;
206
207 return result;
208}
void planningCycle()
Runs one full planning cycle from input preparation, over solver execution, to trajectory publication...

◆ planningCycle()

void trajectory_optimization::TrajectoryOptimizationNode::planningCycle ( )
protected

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

Definition at line 403 of file trajectory_optimization_node.cpp.

403 {
404 const auto cycle_start = SteadyClock::now();
405 if (debug_viz_) viz_object_boxes_.clear();
406 if (rclcpp::Time(this->now()) - rclcpp::Time(ego_data_.header.stamp) > rclcpp::Duration::from_seconds(ego_data_timeout_)) {
407 RCLCPP_WARN(this->get_logger(), "EgoData outdated. Skipping planning cycle.");
408 return;
409 }
410 // init trajectory message and set header
411 trajectory_planning_msgs::msg::Trajectory::UniquePtr trajectory = std::make_unique<trajectory_planning_msgs::msg::Trajectory>();
412 initializeTrajectory(*trajectory);
413
414 trajectory->header.frame_id = vehicle_frame_id_;
415 trajectory->header.stamp = ego_data_.header.stamp; // use latest ego_data stamp as trajectory stamp
416
417 // init time-steps of trajectory to ensure increasing time-steps even for standstill trajectories
418 double dt = optimization_horizon_ / n_shots_;
419 for (int i = 0; i <= n_shots_; ++i) trajectory_planning_msgs::trajectory_access::setT(*trajectory, i * dt, i);
420
421 // check if the reference trajectory is standstill
422 if (trajectory_planning_msgs::trajectory_access::getStandstill(reference_trajectory_)) {
423 RCLCPP_WARN(this->get_logger(), "Standstill trajectory. Skipping planning cycle. Publish standstill trajectory.");
424 // transform trajectory to output frame
425 if (!trajectory2outputFrame(*trajectory)) {
426 return;
427 }
428 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, true);
429 trajectory_pub_->publish(std::move(trajectory));
430 // Invalidate the warm start and reset the solver once when entering standstill.
431 if (!control_guess_.empty()) {
432 control_guess_.clear();
433 resetSolver();
434 }
435 return;
436 }
437
438 PerformanceMetrics metrics;
439 metrics.cycle = ++logging_cycle_;
440 metrics.ego_stamp_ns = rclcpp::Time(ego_data_.header.stamp).nanoseconds();
441 metrics.reference_stamp_ns = rclcpp::Time(reference_trajectory_.header.stamp).nanoseconds();
442 metrics.route_stamp_ns = rclcpp::Time(route_.header.stamp).nanoseconds();
443 metrics.reference_points = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory_);
444 metrics.objects = static_cast<int>(object_list_.objects.size());
445 auto logCompletedCycle = [&]() {
446 metrics.cycle_ms = elapsedMilliseconds(cycle_start);
447 metrics.postprocessing_ms = metrics.cycle_ms - metrics.preprocessing_ms - metrics.solve_wall_ms;
449 performance_logger_->write(metrics);
450 }
451 };
452
453 // set initial state
454 std::vector<double> x_init(*nlp_dims_->nx, 0.0);
455 if (!trajectory_planning_msgs::trajectory_access::getStandstill(latest_valid_trajectory_)) {
457 } else {
458 RCLCPP_WARN(this->get_logger(),
459 "Latest available trajectory is standstill. Using ego data for initial state (high-level initialization).");
460 x_init = getHighLevelX0(ego_data_);
461 control_guess_.clear(); // rebuild the warm start from zero controls after a standstill trajectory.
462 }
463
464 // project stationary initial states onto forward-only motion.
465 x_init[3] = std::max(x_init[3], 0.0);
466 if (x_init[3] == 0.0) {
467 x_init[4] = std::max(x_init[4], 0.0);
468 }
469
470 // debug print of initial state
471 std::stringstream ss;
472 ss << "Initial state: ";
473 for (size_t i = 0; i < x_init.size(); ++i) ss << "x[" << i << "]: " << x_init[i] << (i != x_init.size() - 1 ? ", " : "");
474 RCLCPP_DEBUG(this->get_logger(), "%s", ss.str().c_str());
475
476 ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "lbx", x_init.data());
477 ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, nlp_out_, 0, "ubx", x_init.data());
478
479 // update inputs to the ocp; skip planning cycle if update fails
481 RCLCPP_WARN(this->get_logger(), "Failed to update inputs. Skipping planning cycle.");
482 return;
483 }
484
485 if (!setInitialGuess(x_init, rclcpp::Time(ego_data_.header.stamp))) {
486 control_guess_.clear();
487 return;
488 }
489
490 // start solving the OCP
491 // first solve the relaxed OCP (if configured and necessary)
492 // then solve the constrained OCP using either the relaxed solution or the regular warm start
493 const auto solve_start = SteadyClock::now();
494 metrics.preprocessing_ms = elapsedMilliseconds(cycle_start, solve_start);
496 bool finite_solution = false;
497 bool primal_feasible = false;
498
499 // solve the relaxed OCP (if configured and necessary)
501 metrics.relaxed_attempted = true;
502 if (!setSafetyConstraintActivation(false)) return;
503
505 const auto relaxed_solve_start = SteadyClock::now();
506 const bool relaxed_primal_feasible = solveAndValidate(metrics, finite_solution);
507 metrics.relaxed_solve_wall_ms = elapsedMilliseconds(relaxed_solve_start);
508 metrics.relaxed_status = metrics.status;
509
510 // evaluate the relaxed residuals before restoration, but retain its nlp_out_
511 if (!setSafetyConstraintActivation(true)) return;
512
513 if (!relaxed_primal_feasible) {
514 metrics.failure_phase = "relaxed";
515 metrics.solve_wall_ms = elapsedMilliseconds(solve_start);
516 printSolution(metrics);
517 RCLCPP_WARN(get_logger(),
518 "Rejecting relaxed initialization: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] "
519 "tolerances=[eq=%e, ineq=%e].",
520 metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
521 resetSolver();
522 logCompletedCycle();
523 return;
524 }
525 }
526
527 // solve the constrained OCP using either the relaxed solution or the regular warm start
529 const auto constrained_solve_start = SteadyClock::now();
530 primal_feasible = solveAndValidate(metrics, finite_solution);
531 metrics.constrained_solve_wall_ms = elapsedMilliseconds(constrained_solve_start);
532 metrics.solve_wall_ms = elapsedMilliseconds(solve_start);
533
534 if (!primal_feasible) {
535 metrics.failure_phase = "constrained";
536 printSolution(metrics);
537 RCLCPP_WARN(this->get_logger(),
538 "Rejecting solver output: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] tolerances=[eq=%e, ineq=%e].",
539 metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
540 if (finite_solution && performance_logger_) {
542 }
543 // keep the last published solution as warm-start source
544 resetSolver();
545 logCompletedCycle();
546 return;
547 }
548
549 // Remove negative roundoff within the accepted feasibility tolerance.
550 for (int stage = 0; stage <= n_shots_; ++stage) {
551 xtraj_[stage * *nlp_dims_->nx + 3] = std::max(xtraj_[stage * *nlp_dims_->nx + 3], 0.0);
552 }
553
555 control_guess_stamp_ = rclcpp::Time(ego_data_.header.stamp);
556
557 printSolution(metrics);
558 if (debug_viz_) {
561 }
562
563 // convert output into trajectory message
564 convertToTrajectoryMsg(*trajectory);
565
566 bool standstill = true;
567 for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(*trajectory); ++i) {
568 if (trajectory_planning_msgs::trajectory_access::getV(*trajectory, i) > standstill_threshold_) standstill = false;
569 }
570 trajectory_planning_msgs::trajectory_access::setStandstill(*trajectory, standstill);
571
572 // transform trajectory to output frame
573 if (!trajectory2outputFrame(*trajectory)) {
574 logCompletedCycle();
575 return;
576 }
577
578 latest_valid_trajectory_ = *trajectory;
579 trajectory_pub_->publish(std::move(trajectory));
580 metrics.published = true;
581 logCompletedCycle();
582 const char* cycle_time_color = metrics.cycle_ms <= 100.0 ? "\x1b[32m" : "\x1b[31m";
583 if (metrics.cycle_ms > 100.0) {
584 if (metrics.relaxed_attempted) {
585 RCLCPP_INFO(this->get_logger(),
586 "Published trajectory (cycle: %s%.2f ms\x1b[0m; relaxed solve: %.2f ms; constrained solve: %.2f ms)",
587 cycle_time_color, metrics.cycle_ms, metrics.relaxed_solve_wall_ms, metrics.constrained_solve_wall_ms);
588 } else {
589 RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m; constrained solve: %.2f ms)",
590 cycle_time_color, metrics.cycle_ms, metrics.constrained_solve_wall_ms);
591 }
592 } else {
593 RCLCPP_INFO(this->get_logger(), "Published trajectory (cycle: %s%.2f ms\x1b[0m)", cycle_time_color, metrics.cycle_ms);
594 }
595}
static void collectConstraintDiagnostics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, const ocp_nlp_dims *dims, int object_boxes)
Collects the largest equality and inequality constraint violations from acados.
void printSolution(const PerformanceMetrics &metrics)
Logs solver status and optional debug statistics for the last optimization run.
Definition utils.cpp:442
trajectory_planning_msgs::msg::Trajectory reference_trajectory_
void vizEgoBoxes(const std::vector< double > &x_trajectory, const std::string &model_name)
Definition utils.cpp:333
bool setSafetyConstraintActivation(bool use_configured_activation)
Enables the configured object and boundary constraints or temporarily disables all of them.
Definition utils.cpp:395
trajectory_planning_msgs::msg::Trajectory latest_valid_trajectory_
virtual void initializeTrajectory(trajectory_planning_msgs::msg::Trajectory &trajectory)=0
Initializes an output trajectory message with the model-specific message type and size.
virtual void convertToTrajectoryMsg(trajectory_planning_msgs::msg::Trajectory &trajectory)=0
Maps the optimized state trajectory into the model-specific output message fields.
bool trajectory2outputFrame(trajectory_planning_msgs::msg::Trajectory &trajectory)
Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required...
Definition utils.cpp:73
bool solveAndValidate(PerformanceMetrics &metrics, bool &finite_solution)
Solves the OCP, reads its output and checks primal feasibility.
Definition utils.cpp:414
bool updateOcpInputs(const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list, const route_planning_msgs::msg::Route &route, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory)
Transforms external inputs into the optimizer frame and writes them into the OCP.
void vizObjectBoxes(const std::vector< double > &objects)
Definition utils.cpp:306
virtual std::vector< double > getBiLevelX0(const perception_msgs::msg::EgoData &ego_data)=0
Computes the initial optimizer state using bi-level stabilizaion based on the model-specific EgoData ...
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr trajectory_pub_
bool setInitialGuess(const std::vector< double > &x_init, const rclcpp::Time &stamp)
Builds and sets a dynamically consistent NLP initial guess from the current state and cached controls...
void resetSolver()
Resets the optimizer memory while retaining the generated solver instance.
virtual std::vector< double > getHighLevelX0(const perception_msgs::msg::EgoData &ego_data)=0
Computes the initial optimizer state using high-level stabilization based on the model-specific EgoDa...
const ocp_nlp_opts * acados_get_common_nlp_opts(ocp_nlp_config *config, void *solver_opts)
Returns the common NLP options stored inside the generated solver-specific options.

◆ printSolution()

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

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

Parameters
[in]metricsPerformance metrics collected for the optimization run.

Definition at line 442 of file utils.cpp.

442 {
443 // Status codes:
444 // 0: Success (ACADOS_SUCCESS)
445 // 1: NaN detected (ACADOS_NAN_DETECTED)
446 // 2: Maximum number of iterations reached (ACADOS_MAXITER)
447 // 3: Minimum step size reached (ACADOS_MINSTEP)
448 // 4: QP solver failed (ACADOS_QP_FAILURE)
449 // 5: Solver created (ACADOS_READY)
450 // 6: Problem unbounded (ACADOS_UNBOUNDED)
451 // 7: Solver timeout (ACADOS_TIMEOUT)
452 if (metrics.status == ACADOS_SUCCESS && verbose_) {
453 RCLCPP_INFO(get_logger(), "\033[1;32mOptimization: SUCCESS!\033[0m");
454 } else if (metrics.status == ACADOS_MAXITER) {
455 RCLCPP_WARN(get_logger(), "Optimization failed with status %d (max iterations).", metrics.status);
456 } else if (metrics.status == ACADOS_TIMEOUT) {
457 RCLCPP_WARN(get_logger(), "\033[38;5;214mOptimization failed with status %d (timeout).\033[0m", metrics.status);
458 } else if (metrics.status != ACADOS_SUCCESS) {
459 RCLCPP_ERROR(get_logger(), "%s_acados_solve() failed with status %d.", model_name_.c_str(), metrics.status);
460 }
461
462 if (verbose_) {
463 RCLCPP_INFO(get_logger(), "Optimization took %.3f ms (SQP iter: %d; QP iter: %d; KKT: %e)", metrics.acados_total_ms,
464 metrics.sqp_iter, metrics.qp_iter, metrics.kkt_norm_inf);
465 RCLCPP_INFO(get_logger(), "cost_value: %f; residuals: stat=%e eq=%e ineq=%e comp=%e", metrics.cost_value, metrics.res_stat,
466 metrics.res_eq, metrics.res_ineq, metrics.res_comp);
467
468 std::fputs("\n--- xtraj ---\n", stdout);
469 d_print_exp_tran_mat(*nlp_dims_->nx, n_shots_ + 1, xtraj_.data(), *nlp_dims_->nx);
470 std::fputs("\n--- utraj ---\n", stdout);
471 d_print_exp_tran_mat(*nlp_dims_->nu, n_shots_, utraj_.data(), *nlp_dims_->nu);
473 }
474}
void acados_print_stats(ocp_model_capsule_t capsule)
Wrapper around the generated acados statistics printer.

◆ 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]msgIncoming reference trajectory message.

Definition at line 22 of file callbacks.cpp.

23 {
24 RCLCPP_DEBUG(this->get_logger(), "Received reference trajectory");
26 if (run_as_callback_) {
28 }
29}

◆ resetSolver()

void trajectory_optimization::TrajectoryOptimizationNode::resetSolver ( )
protected

Resets the optimizer memory while retaining the generated solver instance.

Definition at line 329 of file trajectory_optimization_node.cpp.

329 {
330 const int status = trajectory_optimization::acados_reset(ocp_capsule_, 1, 0, 0, 0);
331 if (status != ACADOS_SUCCESS) {
332 RCLCPP_ERROR(this->get_logger(), "%s_acados_reset() returned status %d. Recreating solver.", model_name_.c_str(), status);
333 freeSolver();
334 setupSolver();
335 }
336}
void setupSolver()
Creates the acados solver instance and initializes its state buffers.
int acados_reset(ocp_model_capsule_t capsule, int reset_qp_solver_mem, int reset_numerical_values, int reset_solver_options, int reset_x_to_x0_bar)
Resets selected solver memory without freeing and recreating the capsule.

◆ 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]msgIncoming route message.

Definition at line 31 of file callbacks.cpp.

31 {
33 RCLCPP_DEBUG(this->get_logger(), "Received route");
34 route_ = *msg;
35 } else {
36 // reset route to empty
37 route_ = route_planning_msgs::msg::Route();
38 }
39}

◆ 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_initHard initial state of the OCP.
[in]stampAbsolute time corresponding to x_init.
Returns
true if the state rollout succeeded.

Definition at line 338 of file trajectory_optimization_node.cpp.

338 {
339 constexpr double MAX_CONTROL_GUESS_AGE_FACTOR = 0.5;
340 const int nx = *nlp_dims_->nx;
341 const int nu = *nlp_dims_->nu;
342 const double time_step = optimization_horizon_ / n_shots_;
343 const size_t expected_control_size = static_cast<size_t>(nu * n_shots_);
344 if (x_init.size() != static_cast<size_t>(nx)) {
345 RCLCPP_ERROR(get_logger(), "Initial state has size %zu, expected %d.", x_init.size(), nx);
346 return false;
347 }
348
349 // Fall back to zero controls if no sufficiently recent solver output is available.
350 std::vector<double> controls(expected_control_size, 0.0);
351 if (control_guess_.size() == expected_control_size) {
352 const double elapsed = (stamp - control_guess_stamp_).seconds();
353 if (elapsed >= 0.0 && elapsed < MAX_CONTROL_GUESS_AGE_FACTOR * optimization_horizon_) {
354 // Shift the previous controls to the current planning time and interpolate between shooting nodes.
355 std::vector<double> lower_controls(nu);
356 std::vector<double> upper_controls(nu);
357 for (int stage = 0; stage < n_shots_; ++stage) {
358 const double previous_stage = (elapsed + stage * time_step) / time_step;
359 if (previous_stage >= n_shots_) break;
360
361 const int lower_stage = static_cast<int>(std::floor(previous_stage));
362 const double interpolation_factor = previous_stage - lower_stage;
363 ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "lbu", lower_controls.data());
364 ocp_nlp_constraints_model_get(nlp_config_, nlp_dims_, nlp_in_, stage, "ubu", upper_controls.data());
365 for (int control = 0; control < nu; ++control) {
366 const double lower_value = control_guess_[lower_stage * nu + control];
367 const double upper_value = lower_stage + 1 < n_shots_ ? control_guess_[(lower_stage + 1) * nu + control] : 0.0;
368 const double interpolated_control = lower_value + interpolation_factor * (upper_value - lower_value);
369 controls[stage * nu + control] = std::clamp(interpolated_control, lower_controls[control], upper_controls[control]);
370 }
371 }
372 }
373 }
374
375 ocp_nlp_out_set_values_to_zero(nlp_config_, nlp_dims_, nlp_out_);
376 std::vector<double> rollout_state = x_init;
377 std::vector<double> intermediate_state(nx);
378 std::vector<double> k1(nx), k2(nx), k3(nx), k4(nx);
379 const double integration_step = time_step / 4.0;
380 for (int stage = 0; stage < n_shots_; ++stage) {
381 double* control = &controls[stage * nu];
382 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "x", rollout_state.data());
383 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, stage, "u", control);
384
385 // Match sim_method_num_stages=4 and sim_method_num_steps=4 from the generated OCP.
386 for (int integration = 0; integration < 4; ++integration) {
387 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, rollout_state.data(), control, k1.data());
388 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k1[i];
389 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k2.data());
390 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + 0.5 * integration_step * k2[i];
391 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k3.data());
392 for (int i = 0; i < nx; ++i) intermediate_state[i] = rollout_state[i] + integration_step * k3[i];
393 trajectory_optimization::acados_evaluate_dynamics(ocp_capsule_, stage, intermediate_state.data(), control, k4.data());
394 for (int i = 0; i < nx; ++i) {
395 rollout_state[i] += integration_step / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
396 }
397 }
398 }
399 ocp_nlp_out_set(nlp_config_, nlp_dims_, nlp_out_, nlp_in_, n_shots_, "x", rollout_state.data());
400 return true;
401}
void acados_evaluate_dynamics(ocp_model_capsule_t capsule, int stage, double *x, double *u, double *x_dot)
Evaluates the explicit model dynamics for a shooting stage.

◆ 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_weightsConfigured cost weights.
[in]reference_trajectoryReference trajectory in optimizer frame.
[in]routeRoute data used for boundary distances.

Definition at line 647 of file trajectory_optimization_node.cpp.

649 {
650 const auto start_time = std::chrono::steady_clock::now();
651 std::vector<double> global_params;
652
653 // cost weights
654 const auto expected_cost_weights_size = static_cast<size_t>(p_cost_weights_shape_[0] * p_cost_weights_shape_[1]);
655 if (cost_weights.size() != expected_cost_weights_size) {
656 RCLCPP_ERROR(this->get_logger(), "Size of cost_weights (%zu) does not match expected size (%zu).", cost_weights.size(),
657 expected_cost_weights_size);
658 throw std::runtime_error("Size of cost_weights does not match expected size.");
659 }
660 global_params.insert(global_params.end(), cost_weights.begin(), cost_weights.end());
661
662 // other cost params
663 global_params.push_back(thw_);
664 global_params.push_back(d_min_object_long_);
665 global_params.push_back(d_min_object_lat_);
666 global_params.push_back(d_min_boundary_lat_);
667
668 // reference path (including boundaries)
669 const size_t n_ref_states = static_cast<size_t>(p_ref_path_shape_[0] * p_ref_path_shape_[1]);
670 std::vector<std::pair<double, double>> boundary_distances = normalBoundaryDistance(reference_trajectory, route);
671 // fill ref vector for ocp -> psi, x, y, v, d_bound_left, d_bound_right
672 std::vector<double> ref;
673 for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory); ++i) {
674 ref.push_back(trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i));
675 ref.push_back(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i));
676 ref.push_back(trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
677 ref.push_back(trajectory_planning_msgs::trajectory_access::getV(reference_trajectory, i));
678 ref.push_back(boundary_distances[i].first); // left boundary distance
679 ref.push_back(boundary_distances[i].second); // right boundary distance
680 }
681 if (ref.size() >= n_ref_states) {
682 global_params.insert(global_params.end(), ref.begin(),
683 ref.begin() + static_cast<std::vector<double>::difference_type>(n_ref_states));
684 } else {
685 // Repeat the final valid state. Infinity padding can propagate NaNs through closest-point calculations.
686 global_params.insert(global_params.end(), ref.begin(), ref.end());
687 const size_t state_width = static_cast<size_t>(p_ref_path_shape_[1]);
688 while (global_params.size() < expected_cost_weights_size + 4 + n_ref_states) {
689 global_params.insert(global_params.end(), ref.end() - static_cast<std::vector<double>::difference_type>(state_width),
690 ref.end());
691 }
692 }
693
694 if (global_params.size() != static_cast<size_t>(nlp_dims_->np_global)) {
695 RCLCPP_ERROR(this->get_logger(), "Size of global parameters (%zu) does not match expected size (%d).", global_params.size(),
696 nlp_dims_->np_global);
697 throw std::runtime_error("Size of global parameters does not match expected size.");
698 }
700 ocp_capsule_, global_params.data(), static_cast<int>(global_params.size()));
701 if (status != ACADOS_SUCCESS) {
702 throw std::runtime_error("acados global parameter update failed with status " + std::to_string(status));
703 }
704 const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
705 RCLCPP_DEBUG(this->get_logger(), "setOcpGlobalParameters duration: %.3f ms", elapsed_ms);
706}
std::vector< std::pair< double, double > > normalBoundaryDistance(const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
Computes minimum normal distances from the reference path to the active route boundaries.
Definition utils.cpp:112
int acados_set_p_global_and_precompute_dependencies(ocp_model_capsule_t capsule, double *data, int data_len)
Wrapper around the generated global-parameter update function.

◆ setOcpParameters()

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

Writes stage-wise object, boundary-activation, and dynamic-weighting parameters into the OCP.

Parameters
[in]ego_dataCurrent ego state.
[in]object_listObject list in optimizer frame.

Definition at line 708 of file trajectory_optimization_node.cpp.

709 {
710 const auto start_time = std::chrono::steady_clock::now();
711
712 // prepare object predictions
713 struct PredictionData {
714 std::vector<double> time;
715 std::vector<double> x;
716 std::vector<double> y;
717 std::vector<double> yaw;
718 };
719
720 std::vector<std::vector<PredictionData>> object_predictions(object_list.objects.size());
721 const double object_stamp = static_cast<double>(rclcpp::Time(object_list.header.stamp).nanoseconds()) / 1e9;
723 for (size_t object_index = 0; object_index < object_list.objects.size(); ++object_index) {
724 const auto& object = object_list.objects[object_index];
725 auto append_prediction = [&](const auto& prediction) {
726 PredictionData prediction_data{{object_stamp},
727 {perception_msgs::object_access::getX(object)},
728 {perception_msgs::object_access::getY(object)},
729 {perception_msgs::object_access::getYaw(object)}};
730 for (const auto& state : prediction.states) {
731 prediction_data.time.push_back(static_cast<double>(rclcpp::Time(state.header.stamp).nanoseconds()) / 1e9);
732 prediction_data.x.push_back(perception_msgs::object_access::getX(state));
733 prediction_data.y.push_back(perception_msgs::object_access::getY(state));
734 prediction_data.yaw.push_back(perception_msgs::object_access::getYaw(state));
735 }
736 object_predictions[object_index].push_back(std::move(prediction_data));
737 };
738
739 // keep all predictions above the configured probability threshold
740 for (const auto& prediction : object.state_predictions) {
741 if (prediction.probability > min_prediction_probability_) append_prediction(prediction);
742 }
743
744 // fall back to the first prediction if none satisfies the threshold
745 if (object_predictions[object_index].empty() && !object.state_predictions.empty()) {
746 append_prediction(object.state_predictions[0]);
747 }
748 }
749 }
750
752 constexpr double MIN_HALF_EXTENT = 0.05;
753
754 // loop over shooting intervals
755 const double dt = optimization_horizon_ / n_shots_;
756 double floating_dynamic_weight = 1.0;
757 for (int stage = 0; stage <= n_shots_; ++stage) {
758 // dynamic weight
759 int idx = 0;
760 int parameter_count = static_cast<int>(p_dynamic_weight_shape_[0] * p_dynamic_weight_shape_[1]);
761 std::vector<int> dynamic_weight_indices{idx};
762 int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, dynamic_weight_indices.data(),
763 &floating_dynamic_weight, parameter_count);
764 if (status != ACADOS_SUCCESS) {
765 throw std::runtime_error("acados dynamic-weight update failed with status " + std::to_string(status));
766 }
767 floating_dynamic_weight *= dynamic_weight_;
768
769 // constraint activation
770 idx += parameter_count;
771 parameter_count = static_cast<int>(p_constraint_activation_shape_[0] * p_constraint_activation_shape_[1]);
772 std::array<int, 2> constraint_activation_indices{idx, idx + 1};
773 std::array<double, 2> constraint_activation{object_constraints_active_ ? 1.0 : 0.0,
775 status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, constraint_activation_indices.data(),
776 constraint_activation.data(), parameter_count);
777 if (status != ACADOS_SUCCESS) {
778 throw std::runtime_error("acados constraint-activation update failed with status " + std::to_string(status));
779 }
780
781 // objects
782 idx += parameter_count;
783 parameter_count = static_cast<int>(p_objects_shape_[0] * p_objects_shape_[1]);
784 std::vector<double> object_boxes;
785 object_boxes.reserve(parameter_count);
786 for (size_t object_index = 0; object_index < object_list.objects.size(); ++object_index) {
787 const auto& object = object_list.objects[object_index];
788 std::vector<std::array<double, 3>> target_states;
789 if (!object_predictions[object_index].empty()) {
790 for (const auto& prediction : object_predictions[object_index]) {
791 const double desired_time = static_cast<double>(rclcpp::Time(ego_data.header.stamp).nanoseconds()) / 1e9 + dt * stage;
792 std::array<double, 3> state{};
793 linearInterpolation(prediction.time, prediction.x, desired_time, state[0]);
794 linearInterpolation(prediction.time, prediction.y, desired_time, state[1]);
795 linearInterpolation(prediction.time, prediction.yaw, desired_time, state[2], true);
796 target_states.push_back(state);
797 }
798 } else {
799 // treat objects without a selected prediction as static
800 target_states.push_back({perception_msgs::object_access::getX(object), perception_msgs::object_access::getY(object),
801 perception_msgs::object_access::getYaw(object)});
802 }
803
804 const double offset_long = object.state.reference_point.translation_to_geometric_center.x;
805 const double offset_lat = object.state.reference_point.translation_to_geometric_center.y;
806 const double half_length = std::max(MIN_HALF_EXTENT, 0.5 * perception_msgs::object_access::getLength(object));
807 const double half_width = std::max(MIN_HALF_EXTENT, 0.5 * perception_msgs::object_access::getWidth(object));
808 for (const auto& state : target_states) {
809 // ensure that the object position represents its geometric center
810 const double cos_yaw = std::cos(state[2]);
811 const double sin_yaw = std::sin(state[2]);
812 const double center_x = state[0] + offset_long * cos_yaw - offset_lat * sin_yaw;
813 const double center_y = state[1] + offset_long * sin_yaw + offset_lat * cos_yaw;
814 object_boxes.insert(object_boxes.end(), {center_x, center_y, state[2], half_length, half_width, 1.0});
815 if (object_boxes.size() >= static_cast<size_t>(parameter_count)) {
816 object_boxes.resize(parameter_count);
817 break;
818 }
819 }
820 if (object_boxes.size() >= static_cast<size_t>(parameter_count)) break;
821 }
822 // fill remaining object slots with inactive boxes
823 while (object_boxes.size() < static_cast<size_t>(parameter_count)) {
824 object_boxes.insert(object_boxes.end(), {0.0, 0.0, 0.0, MIN_HALF_EXTENT, MIN_HALF_EXTENT, 0.0});
825 }
826 if (debug_viz_) viz_object_boxes_.insert(viz_object_boxes_.end(), object_boxes.begin(), object_boxes.end());
827
828 std::vector<int> object_indices(parameter_count);
829 std::iota(object_indices.begin(), object_indices.end(), idx);
830 status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, object_indices.data(), object_boxes.data(),
831 parameter_count);
832 if (status != ACADOS_SUCCESS) {
833 throw std::runtime_error("acados object-box update failed with status " + std::to_string(status));
834 }
835 }
836 const auto elapsed_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time).count();
837 RCLCPP_DEBUG(get_logger(), "setOcpParameters object-box duration: %.3f ms", elapsed_ms);
838}
bool linearInterpolation(const std::vector< double > &X, const std::vector< double > &Y, const double &desired_x, double &output_y, const bool wrap_angle=false)
Interpolates a value from sampled data and optionally handles angle wrap-around.
Definition utils.cpp:23
int acados_update_params_sparse(ocp_model_capsule_t capsule, int stage, int *idx, double *p, int n_update)
Wrapper around the generated sparse parameter update function.

◆ setSafetyConstraintActivation()

bool trajectory_optimization::TrajectoryOptimizationNode::setSafetyConstraintActivation ( bool use_configured_activation)
protected

Enables the configured object and boundary constraints or temporarily disables all of them.

Parameters
[in]use_configured_activationRestore the activation prepared for the current cycle if true; disable all safety constraints if false.
Returns
true if every shooting-stage parameter update succeeded.

Definition at line 395 of file utils.cpp.

395 {
396 const int dynamic_parameter_count = static_cast<int>(p_dynamic_weight_shape_[0] * p_dynamic_weight_shape_[1]);
397
398 std::array<int, 2> indices{dynamic_parameter_count, dynamic_parameter_count + 1};
399 std::array<double, 2> values{use_configured_activation && object_constraints_active_ ? 1.0 : 0.0,
400 use_configured_activation && consider_boundaries_ != CONSIDER_BOUNDARIES::NO_BOUNDS ? 1.0 : 0.0};
401
402 for (int stage = 0; stage <= n_shots_; ++stage) {
403 const int status = trajectory_optimization::acados_update_params_sparse(ocp_capsule_, stage, indices.data(), values.data(),
404 static_cast<int>(indices.size()));
405 if (status != ACADOS_SUCCESS) {
406 RCLCPP_ERROR(get_logger(), "Could not %s safety constraints at stage %d (ACADOS status %d); skipping planning cycle.",
407 use_configured_activation ? "restore" : "disable", stage, status);
408 return false;
409 }
410 }
411 return true;
412}

◆ setSolverTimeout()

void trajectory_optimization::TrajectoryOptimizationNode::setSolverTimeout ( double timeout_ms)
protected

Sets the ACADOS wall-time limit for the next solve.

Definition at line 390 of file utils.cpp.

390 {
391 double timeout_seconds = timeout_ms * 1e-3;
392 ocp_nlp_solver_opts_set(nlp_config_, nlp_opts_, "timeout_max_time", &timeout_seconds);
393}

◆ setup()

void trajectory_optimization::TrajectoryOptimizationNode::setup ( )
protected

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

Definition at line 210 of file trajectory_optimization_node.cpp.

210 {
211 tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
212 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
213
214 // create a callback for dynamic parameter configuration
215 parameters_callback_ = this->add_on_set_parameters_callback(
216 std::bind(&TrajectoryOptimizationNode::parametersCallback, this, std::placeholders::_1));
217
218 // set up subscriber for input topics
219 ego_data_sub_ = this->create_subscription<perception_msgs::msg::EgoData>(
220 "~/ego_data", 1, std::bind(&TrajectoryOptimizationNode::egoDataCallback, this, std::placeholders::_1));
221 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", ego_data_sub_->get_topic_name());
222
223 object_list_sub_ = this->create_subscription<perception_msgs::msg::ObjectList>(
224 "~/object_list", 1, std::bind(&TrajectoryOptimizationNode::objectListCallback, this, std::placeholders::_1));
225 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", object_list_sub_->get_topic_name());
226
227 route_sub_ = this->create_subscription<route_planning_msgs::msg::Route>(
228 "~/route", 1, std::bind(&TrajectoryOptimizationNode::routeCallback, this, std::placeholders::_1));
229 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", route_sub_->get_topic_name());
230
231 reference_trajectory_sub_ = this->create_subscription<trajectory_planning_msgs::msg::Trajectory>(
232 "~/reference_trajectory", 1,
233 std::bind(&TrajectoryOptimizationNode::referenceTrajectoryCallback, this, std::placeholders::_1));
234 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", reference_trajectory_sub_->get_topic_name());
235
236 // set up publisher for output topics
237 trajectory_pub_ = this->create_publisher<trajectory_planning_msgs::msg::Trajectory>("~/trajectory", 1);
238 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", trajectory_pub_->get_topic_name());
239 object_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/objects", 1);
240 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", object_marker_pub_->get_topic_name());
241 ego_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/ego", 1);
242 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", ego_marker_pub_->get_topic_name());
243 boundary_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/visualization/boundaries", 1);
244 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", boundary_pub_->get_topic_name());
245
246 // create timer for planning cycle
247 if (run_as_callback_) {
248 RCLCPP_INFO(this->get_logger(), "OCP runs on reference trajectory callback");
249 } else {
250 RCLCPP_INFO(this->get_logger(), "OCP runs continuously with frequency %f Hz", optimization_freq_);
251 planning_timer_ = this->create_wall_timer(std::chrono::duration<double>(1 / optimization_freq_),
253 }
254
255 // init reference trajectory
256 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
257 reference_trajectory_, trajectory_planning_msgs::msg::REFERENCE::TYPE_ID, n_shots_ + 1);
259
260 // init latest trajectory (doesn't matter which type, only used for standstill detection)
261 trajectory_planning_msgs::trajectory_access::initializeTrajectory(
262 latest_valid_trajectory_, trajectory_planning_msgs::msg::DRIVABLE::TYPE_ID, n_shots_ + 1);
264
265 setupSolver();
266
267 // Annotate message links for tracing: Publish trajectory periodically, which depends an all subscribed topics.
268 std::vector<const void*> link_subs;
269 link_subs.push_back(static_cast<const void*>(ego_data_sub_->get_subscription_handle().get()));
270 link_subs.push_back(static_cast<const void*>(object_list_sub_->get_subscription_handle().get()));
271 link_subs.push_back(static_cast<const void*>(route_sub_->get_subscription_handle().get()));
272 link_subs.push_back(static_cast<const void*>(reference_trajectory_sub_->get_subscription_handle().get()));
273 std::vector<const void*> link_pubs;
274 link_pubs.push_back(static_cast<const void*>(trajectory_pub_->get_publisher_handle().get()));
275 RCLCPP_INFO(get_logger(), "Annotating message links for tracing with %zu subscriptions and %zu publications", link_subs.size(),
276 link_pubs.size());
277 TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
278}
void routeCallback(const route_planning_msgs::msg::Route::ConstSharedPtr msg)
Stores the current route used for boundary constraints when enabled.
Definition callbacks.cpp:31
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr object_marker_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr ego_marker_pub_
void objectListCallback(const perception_msgs::msg::ObjectList::ConstSharedPtr msg)
Stores the current object list when object handling is enabled.
Definition callbacks.cpp:12
void egoDataCallback(const perception_msgs::msg::EgoData::ConstSharedPtr msg)
Stores the latest ego state used by the optimizer.
Definition callbacks.cpp:7
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr route_sub_
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Applies parameter updates that can be reconfigured while the node is running.
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr boundary_pub_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr ego_data_sub_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr object_list_sub_
void referenceTrajectoryCallback(const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
Updates the reference trajectory and optionally triggers optimization immediately.
Definition callbacks.cpp:22
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr reference_trajectory_sub_

◆ setupSolver()

void trajectory_optimization::TrajectoryOptimizationNode::setupSolver ( )
protected

Creates the acados solver instance and initializes its state buffers.

Definition at line 280 of file trajectory_optimization_node.cpp.

280 {
281 if (n_shots_ <= 0) {
282 RCLCPP_FATAL(this->get_logger(), "n_shots must be > 0, got %d", n_shots_);
283 exit(1);
284 }
285
286 // Create exactly once. This also supports a runtime horizon that differs from the generated default.
288 std::vector<double> new_time_steps(n_shots_, optimization_horizon_ / n_shots_);
289 RCLCPP_INFO(this->get_logger(), "Create OCP with: horizon = %f, n_shots = %d, dt = %f", optimization_horizon_, n_shots_,
290 new_time_steps.front());
292
293 if (status != ACADOS_SUCCESS) {
294 RCLCPP_FATAL(this->get_logger(), "%s_acados_create_with_discretization() returned status %d. Exiting.", model_name_.c_str(),
295 status);
296 exit(1);
297 }
298
306 if (nlp_dims_->N != n_shots_) {
307 RCLCPP_FATAL(this->get_logger(), "Created solver has N=%d, expected n_shots=%d. Exiting.", nlp_dims_->N, n_shots_);
308 exit(1);
309 }
310
311 xtraj_.resize(*nlp_dims_->nx * (n_shots_ + 1));
312 utraj_.resize(*nlp_dims_->nu * n_shots_);
313 control_guess_.clear();
314}
ocp_nlp_solver * acados_get_nlp_solver(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP solver instance.
ocp_nlp_out * acados_get_nlp_out(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP output structure.
ocp_nlp_config * acados_get_nlp_config(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP configuration.
int acados_create_with_discretization(ocp_model_capsule_t capsule, int n_time_steps, double *new_time_steps)
Wrapper around the generated acados create function with custom discretization.
void * acados_get_nlp_opts(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP solver options.
ocp_model_capsule_t acados_create_capsule(const std::string &model_name)
Creates the model-specific acados capsule selected by the configured model name.
ocp_nlp_dims * acados_get_nlp_dims(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP dimensions.
ocp_nlp_in * acados_get_nlp_in(ocp_model_capsule_t capsule)
Wrapper around the generated accessor for the OCP input structure.

◆ solveAndValidate()

bool trajectory_optimization::TrajectoryOptimizationNode::solveAndValidate ( PerformanceMetrics & metrics,
bool & finite_solution )
protected

Solves the OCP, reads its output and checks primal feasibility.

Parameters
[out]metricsSolver status, statistics, cost and residuals.
[out]finite_solutionWhether the solver returned finite state, control, cost and residual values.
Returns
true if the solution is finite and satisfies the configured equality and inequality tolerances.

Definition at line 414 of file utils.cpp.

414 {
417 performance_logger_ != nullptr || verbose_);
418
419 const bool usable_status =
420 metrics.status == ACADOS_SUCCESS || metrics.status == ACADOS_MAXITER || metrics.status == ACADOS_TIMEOUT;
421 if (usable_status) {
422 for (int stage = 0; stage <= nlp_dims_->N; ++stage) {
423 ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, stage, "x", &xtraj_[stage * *nlp_dims_->nx]);
424 }
425 for (int stage = 0; stage < nlp_dims_->N; ++stage) {
426 ocp_nlp_out_get(nlp_config_, nlp_dims_, nlp_out_, stage, "u", &utraj_[stage * *nlp_dims_->nu]);
427 }
428 }
429
430 const bool finite_slacks =
432 static_cast<int>(p_objects_shape_[0]),
433 performance_logger_ != nullptr || verbose_);
434 finite_solution = usable_status && finite_slacks && std::isfinite(metrics.cost_value) && std::isfinite(metrics.res_eq) &&
435 std::isfinite(metrics.res_ineq) &&
436 std::all_of(xtraj_.begin(), xtraj_.end(), [](double value) { return std::isfinite(value); }) &&
437 std::all_of(utraj_.begin(), utraj_.end(), [](double value) { return std::isfinite(value); });
439 return finite_solution && metrics.res_eq <= solver_opts->tol_eq && metrics.res_ineq <= solver_opts->tol_ineq;
440}
static bool collectSlackStatistics(PerformanceMetrics &metrics, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, int object_boxes, bool collect_details)
Checks all slacks for finiteness and optionally collects desired-clearance statistics.
static void collectSolverStatistics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, bool collect_details)
Reads timing, iteration, cost, and residual statistics from acados.
int acados_solve(ocp_model_capsule_t capsule)
Wrapper around the generated acados solve function.

◆ 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]trajectoryTrajectory to transform.
Returns
true if the trajectory is already in the target frame or the transformation succeeded.

Definition at line 73 of file utils.cpp.

73 {
75 trajectory_planning_msgs::msg::Trajectory tf_trajectory;
76 try {
77 tf_trajectory = tf2_buffer_->transform(trajectory, trajectory_frame_id_, tf2::durationFromSec(0.01));
78 } catch (tf2::TransformException& ex) {
79 RCLCPP_WARN(this->get_logger(), "Transformation into output frame is not available. Publishing no trajectory. Ex: %s",
80 ex.what());
81 return false;
82 }
83 trajectory = tf_trajectory;
84 }
85 return true;
86}

◆ 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_dataCurrent ego state.
[in]object_listCurrent object list.
[in]routeCurrent route data.
[in]reference_trajectoryCurrent reference trajectory.
Returns
true if all optimizer inputs were updated successfully.

Definition at line 597 of file trajectory_optimization_node.cpp.

600 {
601 // transform inputs to target base_link frame
602 trajectory_planning_msgs::msg::Trajectory tf_reference_trajectory;
603 perception_msgs::msg::ObjectList tf_object_list;
604 route_planning_msgs::msg::Route tf_route;
605 try {
606 // reference trajectory
607 tf_reference_trajectory =
608 tf2_buffer_->transform(reference_trajectory, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
609 fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
610 // object list
611 if (!object_list.objects.empty() && object_list.header.frame_id != vehicle_frame_id_) {
612 tf_object_list = tf2_buffer_->transform(object_list, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
613 fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
614 } else {
615 tf_object_list = object_list;
616 }
617 keepNClosestObjects(tf_object_list, static_cast<int>(p_objects_shape_[0]), ignore_objects_behind_ego_);
618 // route
619 if (!route.route_elements.empty() && route.header.frame_id != vehicle_frame_id_) {
620 tf_route = tf2_buffer_->transform(route, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
621 fixed_over_time_frame_id_, tf2::durationFromSec(0.1));
622 } else {
623 tf_route = route;
624 }
625 } catch (tf2::TransformException& ex) {
626 RCLCPP_WARN(this->get_logger(), "Transformation is not available. Ex: %s", ex.what());
627 return false;
628 }
629
630 if (trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_reference_trajectory) <= 0) {
631 RCLCPP_ERROR(this->get_logger(), "Reference trajectory contains no sample points.");
632 return false;
633 }
634
635 // update ocp parameters
636 try {
637 this->setOcpGlobalParameters(cost_weights_, tf_reference_trajectory, tf_route);
638 this->setOcpParameters(ego_data, tf_object_list);
639 } catch (const std::exception& e) {
640 RCLCPP_ERROR(this->get_logger(), "Exception while setting OCP parameters: %s", e.what());
641 return false;
642 }
643
644 return true;
645}
static void keepNClosestObjects(perception_msgs::msg::ObjectList &object_list, const int n_objects, const bool ignore_objects_behind_ego)
Keeps the nearest objects, optionally excluding those behind the ego vehicle.
Definition utils.cpp:88
void setOcpParameters(const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list)
Writes stage-wise object, boundary-activation, and dynamic-weighting parameters into the OCP.
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.

◆ 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_pointsPoints on the left side.
[in]right_boundary_pointsPoints on the right side.

Definition at line 269 of file utils.cpp.

270 {
271 visualization_msgs::msg::MarkerArray marker_array;
272 int id = 0;
273
274 auto createMarker = [&](int id, const std::string& ns, const Eigen::Vector2d& pos, float r, float g, float b) {
275 visualization_msgs::msg::Marker m;
276 m.header.frame_id = vehicle_frame_id_;
277 m.header.stamp = rclcpp::Time(ego_data_.header.stamp);
278 m.lifetime = rclcpp::Duration::from_seconds(0.5);
279 m.ns = ns;
280 m.id = id;
281 m.type = visualization_msgs::msg::Marker::SPHERE;
282 m.action = visualization_msgs::msg::Marker::ADD;
283 m.pose.position.x = pos.x();
284 m.pose.position.y = pos.y();
285 m.pose.position.z = 0.0;
286 m.scale.x = m.scale.y = m.scale.z = 0.4;
287 m.color.a = 1.0;
288 m.color.r = r;
289 m.color.g = g;
290 m.color.b = b;
291 return m;
292 };
293
294 auto addMarkers = [&](const std::vector<Eigen::Vector2d>& points, const std::string& ns, float r, float g, float b) {
295 for (const auto& point : points) {
296 marker_array.markers.push_back(createMarker(id++, ns, point, r, g, b));
297 }
298 };
299
300 addMarkers(left_boundary_points, "left_boundary_constraint", 0.0F, 1.0F, 0.0F);
301 addMarkers(right_boundary_points, "right_boundary_constraint", 1.0F, 0.0F, 0.0F);
302
303 boundary_pub_->publish(marker_array);
304}

◆ vizEgoBoxes()

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

Publishes the ego box along the optimized trajectory.

Definition at line 333 of file utils.cpp.

333 {
334 visualization_msgs::msg::MarkerArray marker_array;
335 if (x_trajectory.empty()) {
336 visualization_msgs::msg::Marker delete_marker;
337 delete_marker.action = visualization_msgs::msg::Marker::DELETEALL;
338 marker_array.markers.push_back(delete_marker);
339 ego_marker_pub_->publish(marker_array);
340 return;
341 }
342
343 double length = 0.0;
344 double width = 0.0;
345 std::array<double, 2> offset_to_geometric_center{};
346
347 // define vehicle geometry based on model name (should match the OCP definition)
348 if (model_name == "karl") {
349 length = 5.173;
350 width = 1.94;
351 offset_to_geometric_center = {1.4895, 0.0};
352 } else if (model_name == "shuttle") {
353 length = 4.97;
354 width = 2.12;
355 offset_to_geometric_center = {0.0, 0.0};
356 } else {
357 RCLCPP_WARN(get_logger(), "Unknown model '%s'. Could not visualize ego boxes.", model_name.c_str());
358 return;
359 }
360
361 const int state_dim = *nlp_dims_->nx;
362 for (int stage = 0; stage <= n_shots_; ++stage) {
363 const size_t offset = static_cast<size_t>(stage) * static_cast<size_t>(state_dim);
364 const double yaw = x_trajectory[offset + 5];
365 visualization_msgs::msg::Marker marker;
366 marker.header.frame_id = vehicle_frame_id_;
367 marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
368 marker.lifetime = rclcpp::Duration::from_seconds(0.5);
369 marker.ns = "ego-boxes";
370 marker.id = stage;
371 marker.type = visualization_msgs::msg::Marker::CUBE;
372 marker.action = visualization_msgs::msg::Marker::ADD;
373 marker.pose.position.x =
374 x_trajectory[offset] + offset_to_geometric_center[0] * std::cos(yaw) - offset_to_geometric_center[1] * std::sin(yaw);
375 marker.pose.position.y =
376 x_trajectory[offset + 1] + offset_to_geometric_center[0] * std::sin(yaw) + offset_to_geometric_center[1] * std::cos(yaw);
377 marker.pose.orientation.z = std::sin(0.5 * yaw);
378 marker.pose.orientation.w = std::cos(0.5 * yaw);
379 marker.scale.x = length;
380 marker.scale.y = width;
381 marker.scale.z = 0.05;
382 marker.color.a = 0.35F;
383 marker.color.g = 0.6F;
384 marker.color.b = 1.0F;
385 marker_array.markers.push_back(marker);
386 }
387 ego_marker_pub_->publish(marker_array);
388}

◆ vizObjectBoxes()

void trajectory_optimization::TrajectoryOptimizationNode::vizObjectBoxes ( const std::vector< double > & objects)
protected

Publishes the object boxes currently passed to the optimizer.

Definition at line 306 of file utils.cpp.

306 {
307 visualization_msgs::msg::MarkerArray marker_array;
308 const size_t width = static_cast<size_t>(p_objects_shape_[1]);
309 for (size_t index = 0; index < objects.size() / width; ++index) {
310 const size_t offset = index * width;
311 if (objects[offset + 5] < 0.5) continue;
312 visualization_msgs::msg::Marker marker;
313 marker.header.frame_id = vehicle_frame_id_;
314 marker.header.stamp = rclcpp::Time(ego_data_.header.stamp);
315 marker.lifetime = rclcpp::Duration::from_seconds(0.5);
316 marker.ns = "object-boxes";
317 marker.id = static_cast<int>(index);
318 marker.type = visualization_msgs::msg::Marker::CUBE;
319 marker.action = visualization_msgs::msg::Marker::ADD;
320 marker.pose.position.x = objects[offset];
321 marker.pose.position.y = objects[offset + 1];
322 marker.pose.orientation.z = std::sin(0.5 * objects[offset + 2]);
323 marker.pose.orientation.w = std::cos(0.5 * objects[offset + 2]);
324 marker.scale.x = 2.0 * objects[offset + 3];
325 marker.scale.y = 2.0 * objects[offset + 4];
326 marker.scale.z = 0.1;
327 marker.color.a = 0.25F;
328 marker.color.r = 1.0F;
329 marker_array.markers.push_back(marker);
330 }
331 object_marker_pub_->publish(marker_array);
332}

◆ 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_radAngle in radians.
[in]min_valLower bound of the target interval.
[in]max_valUpper bound of the target interval.
Returns
Wrapped angle in radians.

Definition at line 16 of file utils.cpp.

16 {
17 double capped_angle_rad = angle_rad;
18 while (capped_angle_rad > max_val) capped_angle_rad -= 2 * M_PI;
19 while (capped_angle_rad < min_val) capped_angle_rad += 2 * M_PI;
20 return capped_angle_rad;
21}

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 381 of file trajectory_optimization_node.hpp.

◆ bi_level_dV_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dV_ = 2.0
protected

Definition at line 404 of file trajectory_optimization_node.hpp.

◆ bi_level_dY_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dY_ = 0.3
protected

Definition at line 405 of file trajectory_optimization_node.hpp.

◆ bi_level_dYaw_

double trajectory_optimization::TrajectoryOptimizationNode::bi_level_dYaw_ = 89.0
protected

Definition at line 406 of file trajectory_optimization_node.hpp.

◆ boundary_pub_

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

Definition at line 367 of file trajectory_optimization_node.hpp.

◆ consider_boundaries_

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

Definition at line 400 of file trajectory_optimization_node.hpp.

◆ consider_objects_

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

Definition at line 398 of file trajectory_optimization_node.hpp.

◆ constrained_solve_timeout_ms_

double trajectory_optimization::TrajectoryOptimizationNode::constrained_solve_timeout_ms_ = 100.0
protected

Definition at line 395 of file trajectory_optimization_node.hpp.

◆ control_guess_

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

Definition at line 412 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 413 of file trajectory_optimization_node.hpp.

413{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 419 of file trajectory_optimization_node.hpp.

◆ d_min_boundary_lat_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_boundary_lat_ = 0.0
protected

Definition at line 424 of file trajectory_optimization_node.hpp.

◆ d_min_object_lat_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_object_lat_ = 0.5
protected

Definition at line 423 of file trajectory_optimization_node.hpp.

◆ d_min_object_long_

double trajectory_optimization::TrajectoryOptimizationNode::d_min_object_long_ = 5.0
protected

Definition at line 422 of file trajectory_optimization_node.hpp.

◆ debug_viz_

bool trajectory_optimization::TrajectoryOptimizationNode::debug_viz_ = false
protected

Definition at line 392 of file trajectory_optimization_node.hpp.

◆ dynamic_weight_

double trajectory_optimization::TrajectoryOptimizationNode::dynamic_weight_ = 1.0
protected

Definition at line 420 of file trajectory_optimization_node.hpp.

◆ ego_data_

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

Definition at line 375 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 359 of file trajectory_optimization_node.hpp.

◆ ego_data_timeout_

double trajectory_optimization::TrajectoryOptimizationNode::ego_data_timeout_ = 1.0
protected

Definition at line 386 of file trajectory_optimization_node.hpp.

◆ ego_marker_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::ego_marker_pub_
protected

Definition at line 366 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 384 of file trajectory_optimization_node.hpp.

◆ high_level_stabilization_

bool trajectory_optimization::TrajectoryOptimizationNode::high_level_stabilization_ = false
protected

Definition at line 397 of file trajectory_optimization_node.hpp.

◆ ignore_objects_behind_ego_

bool trajectory_optimization::TrajectoryOptimizationNode::ignore_objects_behind_ego_ = true
protected

Definition at line 399 of file trajectory_optimization_node.hpp.

◆ latest_valid_trajectory_

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

Definition at line 409 of file trajectory_optimization_node.hpp.

◆ logging_cycle_

uint64_t trajectory_optimization::TrajectoryOptimizationNode::logging_cycle_ = 0
protected

Definition at line 449 of file trajectory_optimization_node.hpp.

◆ min_prediction_probability_

double trajectory_optimization::TrajectoryOptimizationNode::min_prediction_probability_ = 0.0
protected

Definition at line 425 of file trajectory_optimization_node.hpp.

◆ model_name_

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

Definition at line 385 of file trajectory_optimization_node.hpp.

◆ n_shots_

int trajectory_optimization::TrajectoryOptimizationNode::n_shots_ = 50
protected

Definition at line 388 of file trajectory_optimization_node.hpp.

◆ nlp_config_

ocp_nlp_config* trajectory_optimization::TrajectoryOptimizationNode::nlp_config_
protected

Definition at line 440 of file trajectory_optimization_node.hpp.

◆ nlp_dims_

ocp_nlp_dims* trajectory_optimization::TrajectoryOptimizationNode::nlp_dims_
protected

Definition at line 441 of file trajectory_optimization_node.hpp.

◆ nlp_in_

ocp_nlp_in* trajectory_optimization::TrajectoryOptimizationNode::nlp_in_
protected

Definition at line 442 of file trajectory_optimization_node.hpp.

◆ nlp_opts_

void* trajectory_optimization::TrajectoryOptimizationNode::nlp_opts_
protected

Definition at line 445 of file trajectory_optimization_node.hpp.

◆ nlp_out_

ocp_nlp_out* trajectory_optimization::TrajectoryOptimizationNode::nlp_out_
protected

Definition at line 443 of file trajectory_optimization_node.hpp.

◆ nlp_solver_

ocp_nlp_solver* trajectory_optimization::TrajectoryOptimizationNode::nlp_solver_
protected

Definition at line 444 of file trajectory_optimization_node.hpp.

◆ object_constraints_active_

bool trajectory_optimization::TrajectoryOptimizationNode::object_constraints_active_ = false
protected

Definition at line 436 of file trajectory_optimization_node.hpp.

◆ object_list_

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

Definition at line 376 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 360 of file trajectory_optimization_node.hpp.

◆ object_marker_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr trajectory_optimization::TrajectoryOptimizationNode::object_marker_pub_
protected

Definition at line 365 of file trajectory_optimization_node.hpp.

◆ ocp_capsule_

ocp_model_capsule_t trajectory_optimization::TrajectoryOptimizationNode::ocp_capsule_
protected

Definition at line 439 of file trajectory_optimization_node.hpp.

◆ optimization_freq_

double trajectory_optimization::TrajectoryOptimizationNode::optimization_freq_ = 10.0
protected

Definition at line 387 of file trajectory_optimization_node.hpp.

◆ optimization_horizon_

double trajectory_optimization::TrajectoryOptimizationNode::optimization_horizon_ = 1.0
protected

Definition at line 389 of file trajectory_optimization_node.hpp.

◆ p_constraint_activation_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_constraint_activation_shape_ = {2, 1}
protected

Definition at line 432 of file trajectory_optimization_node.hpp.

432{2, 1}; // [objects, boundaries]

◆ p_cost_weights_shape_

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

Definition at line 429 of file trajectory_optimization_node.hpp.

429{12, 1}; // nWeights x weightDim

◆ p_dynamic_weight_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_dynamic_weight_shape_ = {1, 1}
protected

Definition at line 431 of file trajectory_optimization_node.hpp.

431{1, 1};

◆ p_objects_shape_

std::vector<int64_t> trajectory_optimization::TrajectoryOptimizationNode::p_objects_shape_ = {30, 6}
protected

Definition at line 433 of file trajectory_optimization_node.hpp.

433{30, 6}; // nObjects x [x, y, yaw, half-length, half-width, active]

◆ p_ref_path_shape_

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

Definition at line 430 of file trajectory_optimization_node.hpp.

430{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 357 of file trajectory_optimization_node.hpp.

◆ performance_logger_

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

Definition at line 450 of file trajectory_optimization_node.hpp.

◆ performance_logging_

bool trajectory_optimization::TrajectoryOptimizationNode::performance_logging_ = false
protected

Definition at line 391 of file trajectory_optimization_node.hpp.

◆ planning_timer_

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

Definition at line 369 of file trajectory_optimization_node.hpp.

◆ reference_trajectory_

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

Definition at line 378 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 362 of file trajectory_optimization_node.hpp.

◆ relaxed_solve_timeout_ms_

double trajectory_optimization::TrajectoryOptimizationNode::relaxed_solve_timeout_ms_ = 100.0
protected

Definition at line 394 of file trajectory_optimization_node.hpp.

◆ route_

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

Definition at line 377 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 361 of file trajectory_optimization_node.hpp.

◆ run_as_callback_

bool trajectory_optimization::TrajectoryOptimizationNode::run_as_callback_ = false
protected

Definition at line 401 of file trajectory_optimization_node.hpp.

◆ standstill_threshold_

double trajectory_optimization::TrajectoryOptimizationNode::standstill_threshold_ = 0.45
protected

Definition at line 396 of file trajectory_optimization_node.hpp.

◆ tf2_buffer_

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

Definition at line 371 of file trajectory_optimization_node.hpp.

◆ tf2_listener_

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

Definition at line 372 of file trajectory_optimization_node.hpp.

◆ thw_

double trajectory_optimization::TrajectoryOptimizationNode::thw_ = 2.0
protected

Definition at line 421 of file trajectory_optimization_node.hpp.

◆ trajectory_frame_id_

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

Definition at line 383 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 364 of file trajectory_optimization_node.hpp.

◆ two_stage_optimization_

bool trajectory_optimization::TrajectoryOptimizationNode::two_stage_optimization_ = true
protected

Definition at line 393 of file trajectory_optimization_node.hpp.

◆ utraj_

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

Definition at line 448 of file trajectory_optimization_node.hpp.

◆ vehicle_frame_id_

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

Definition at line 382 of file trajectory_optimization_node.hpp.

◆ verbose_

bool trajectory_optimization::TrajectoryOptimizationNode::verbose_ = false
protected

Definition at line 390 of file trajectory_optimization_node.hpp.

◆ viz_object_boxes_

std::vector<double> trajectory_optimization::TrajectoryOptimizationNode::viz_object_boxes_
protected

Definition at line 416 of file trajectory_optimization_node.hpp.

◆ xtraj_

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

Definition at line 447 of file trajectory_optimization_node.hpp.


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