trajectory_optimization::TrajectoryOptimizationAckermannNode Class Reference

#include <trajectory_optimization_ackermann.hpp>

Inheritance diagram for trajectory_optimization::TrajectoryOptimizationAckermannNode:

Public Member Functions
	TrajectoryOptimizationAckermannNode (const rclcpp::NodeOptions &options)
	Initializes the optimization node for a kinematic bicycle model with Ackermann steering.
Public Member Functions inherited from trajectory_optimization::TrajectoryOptimizationNode
	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.

Private Member Functions
void	initializeTrajectory (trajectory_planning_msgs::msg::Trajectory &trajectory) override
	Initializes a drivable trajectory message for a kinematic bicycle model with Ackermann steering.
std::vector< double >	getBiLevelX0 (const perception_msgs::msg::EgoData &ego_data) override
	Computes the initial optimizer state using bi-level stabilizaion.
std::vector< double >	getHighLevelX0 (const perception_msgs::msg::EgoData &ego_data) override
	Computes the initial optimizer state using high-level stabilization.
void	convertToTrajectoryMsg (trajectory_planning_msgs::msg::Trajectory &trajectory) override
	Maps the optimized state trajectory into a trajectory message for a kinematic bicycle model with Ackermann steering.

Private Attributes
double	bi_level_dDelta_ = 5.0

Additional Inherited Members
Protected Types inherited from trajectory_optimization::TrajectoryOptimizationNode
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 inherited from trajectory_optimization::TrajectoryOptimizationNode
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.
void	logPerformance (const PerformanceMetrics &metrics)
	Emits one machine-readable performance record when performance logging is enabled.
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, const std::vector< double > &x_init)
	Transforms external inputs into the optimizer frame and writes them into the OCP.
void	setOcpGlobalParameters (const std::vector< double > &cost_weights, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
	Writes stage-independent data into the OCP.
void	setOcpParameters (const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list)
	Writes stage-wise obstacle and dynamic weighting parameters into the OCP.
std::vector< std::pair< double, double > >	normalBoundaryDistance (const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
	Computes minimum normal distances from the reference path to the active route boundaries.
std::vector< double >	discretizeBB2Circles (const double x, const double y, const double yaw, const double length, const double width)
	Approximates an oriented bounding box with a set of obstacle circles.
void	vizCircles (const std::vector< double > &obstacles)
	Publishes visualization markers for the obstacle circles currently used by the optimizer.
void	vizEgoCircles (const std::vector< double > &x_trajectory, const std::string &model_name)
	Publishes the ego-vehicle circle approximation used by the selected OCP model.
void	vizBoundaryPoints (const std::vector< Eigen::Vector2d > &left_boundary_points, const std::vector< Eigen::Vector2d > &right_boundary_points)
	Publishes the boundary intersections corresponding to the distances passed to the OCP.
Static Protected Member Functions inherited from trajectory_optimization::TrajectoryOptimizationNode
static double	wrap_angle_rad (double angle_rad, double min_val=-M_PI, double max_val=M_PI)
	Wraps an angle into a configured interval.
static void	keepNClosestObjects (perception_msgs::msg::ObjectList &object_list, const int n_objects)
	Keeps the nearest forward objects and discards the remaining entries.
Protected Attributes inherited from trajectory_optimization::TrajectoryOptimizationNode
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr	ego_data_sub_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr	object_list_sub_
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr	route_sub_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	reference_trajectory_sub_
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	trajectory_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	ego_circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	boundary_pub_
rclcpp::TimerBase::SharedPtr	planning_timer_
std::unique_ptr< tf2_ros::Buffer >	tf2_buffer_
std::shared_ptr< tf2_ros::TransformListener >	tf2_listener_
perception_msgs::msg::EgoData	ego_data_
perception_msgs::msg::ObjectList	object_list_
route_planning_msgs::msg::Route	route_
trajectory_planning_msgs::msg::Trajectory	reference_trajectory_
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
std::string	vehicle_frame_id_ = "base_link"
std::string	trajectory_frame_id_ = "base_link"
std::string	fixed_over_time_frame_id_ = "map"
std::string	model_name_ = "karl"
double	ego_data_timeout_ = 1.0
double	optimization_freq_ = 10.0
int	n_shots_ = 50
double	optimization_horizon_ = 1.0
bool	verbose_ = false
bool	performance_logging_ = false
bool	debug_viz_ = false
double	standstill_threshold_ = 0.45
bool	high_level_stabilization_ = false
bool	add_x_init_to_ref_ = false
uint8_t	consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS
uint8_t	consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE
bool	run_as_callback_ = false
double	bi_level_dV_ = 5.0
double	bi_level_dA_ = 2.0
double	bi_level_dY_ = 0.1
double	bi_level_dYaw_ = 5.0
trajectory_planning_msgs::msg::Trajectory	latest_valid_trajectory_
std::vector< double >	control_guess_
rclcpp::Time	control_guess_stamp_ {0, 0, RCL_ROS_TIME}
std::vector< double >	viz_circles_
std::vector< double >	cost_weights_ = std::vector<double>(12, 1.0)
double	dynamic_weight_ = 1.0
double	thw_ = 2.0
double	d_min_obstacle_long_ = 5.0
double	d_min_obstacle_lat_ = 0.5
double	d_min_boundary_lat_ = 0.0
double	min_prediction_probability_ = 0.0
std::vector< int64_t >	p_cost_weights_shape_ = {12, 1}
std::vector< int64_t >	p_ref_path_shape_ = {51, 6}
std::vector< int64_t >	p_obstacle_circles_shape_ = {30, 3}
ocp_model_capsule_t	ocp_capsule_
ocp_nlp_config *	nlp_config_
ocp_nlp_dims *	nlp_dims_
ocp_nlp_in *	nlp_in_
ocp_nlp_out *	nlp_out_
ocp_nlp_solver *	nlp_solver_
void *	nlp_opts_
std::vector< double >	xtraj_
std::vector< double >	utraj_
uint64_t	logging_cycle_ = 0
std::unique_ptr< PerformanceLogger >	performance_logger_

Detailed Description

Definition at line 10 of file trajectory_optimization_ackermann.hpp.

Constructor & Destructor Documentation

TrajectoryOptimizationAckermannNode()

trajectory_optimization::TrajectoryOptimizationAckermannNode::TrajectoryOptimizationAckermannNode ( const rclcpp::NodeOptions & options )

explicit

Initializes the optimization node for a kinematic bicycle model with Ackermann steering.

Parameters

[in]

options

ROS node options used for construction.

Definition at line 12 of file trajectory_optimization_ackermann.cpp.

    : TrajectoryOptimizationNode("TrajectoryOptimizationAckermannNode", options) {
  this->declareAndLoadParameter("bi_level_dDelta", bi_level_dDelta_,
                                "Threshold for bi-level stabilization: maximum ackermann steering angle difference [degree]");
}

Member Function Documentation

convertToTrajectoryMsg()

void trajectory_optimization::TrajectoryOptimizationAckermannNode::convertToTrajectoryMsg ( trajectory_planning_msgs::msg::Trajectory & trajectory )

overrideprivatevirtual

Maps the optimized state trajectory into a trajectory message for a kinematic bicycle model with Ackermann steering.

Parameters

[in,out]

trajectory

Trajectory message to populate.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 101 of file trajectory_optimization_ackermann.cpp.

                                                                                                                  {
  for (int i = 0; i <= n_shots_; ++i) {
    trajectory_planning_msgs::trajectory_access::setX(trajectory, xtraj_[i * *nlp_dims_->nx + 0], i);
    trajectory_planning_msgs::trajectory_access::setY(trajectory, xtraj_[i * *nlp_dims_->nx + 1], i);
    trajectory_planning_msgs::trajectory_access::setS(trajectory, xtraj_[i * *nlp_dims_->nx + 2], i);
    trajectory_planning_msgs::trajectory_access::setV(trajectory, xtraj_[i * *nlp_dims_->nx + 3], i);
    trajectory_planning_msgs::trajectory_access::setA(trajectory, xtraj_[i * *nlp_dims_->nx + 4], i);
    trajectory_planning_msgs::trajectory_access::setTheta(trajectory, xtraj_[i * *nlp_dims_->nx + 5], i);
    trajectory_planning_msgs::trajectory_access::setDeltaAck(trajectory, xtraj_[i * *nlp_dims_->nx + 6], i);
  }
}

getBiLevelX0()

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

overrideprivatevirtual

Computes the initial optimizer state using bi-level stabilizaion.

Parameters

[in]

ego_data

Current EgoData based on the kinematic bicycle model with Ackermann steering.

Returns

Initial state vector for the Ackermann model.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 23 of file trajectory_optimization_ackermann.cpp.

                                                                                                             {
  // transform latest trajectory to current base_link frame
  trajectory_planning_msgs::msg::Trajectory tf_trajectory;
  try {
    tf_trajectory = tf2_buffer_->transform(latest_valid_trajectory_, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
                                           fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
  } catch (tf2::TransformException& ex) {
    RCLCPP_WARN(this->get_logger(), "Transformation is not available. Init high-level instead. Ex: %s", ex.what());
    return getHighLevelX0(ego_data);
  }
 
  // fill vectors with state values from the transformed trajectory
  std::vector<double> TIME, V, Y, A, THETA, DELTA;
  for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_trajectory); i++) {
    TIME.push_back(trajectory_planning_msgs::trajectory_access::getT(tf_trajectory, i));
    Y.push_back(trajectory_planning_msgs::trajectory_access::getY(tf_trajectory, i));
    V.push_back(trajectory_planning_msgs::trajectory_access::getV(tf_trajectory, i));
    A.push_back(trajectory_planning_msgs::trajectory_access::getA(tf_trajectory, i));
    THETA.push_back(trajectory_planning_msgs::trajectory_access::getTheta(tf_trajectory, i));
    DELTA.push_back(trajectory_planning_msgs::trajectory_access::getDeltaAck(tf_trajectory, i));
  }
 
  // interpolate target states by time from the extracted vectors; if not successful, set to ego state (high-level initialization)
  double v_tgt = 0.0, a_tgt = 0.0, y_tgt = 0.0, theta_tgt = 0.0, delta_tgt = 0.0;
  double des_time = (rclcpp::Time(ego_data.header.stamp) - rclcpp::Time(tf_trajectory.header.stamp)).seconds();
  if (!linearInterpolation(TIME, Y, des_time, y_tgt)) y_tgt = 0.0;
  if (!linearInterpolation(TIME, V, des_time, v_tgt)) v_tgt = perception_msgs::object_access::getVelLon(ego_data);
  if (!linearInterpolation(TIME, A, des_time, a_tgt)) a_tgt = perception_msgs::object_access::getAccLon(ego_data);
  if (!linearInterpolation(TIME, THETA, des_time, theta_tgt, true)) theta_tgt = 0.0;
  if (!linearInterpolation(TIME, DELTA, des_time, delta_tgt)) {
    delta_tgt = perception_msgs::object_access::getSteeringAngleAck(ego_data);
  }
 
  RCLCPP_DEBUG(this->get_logger(), "y_tgt: %f, v_tgt: %f, a_tgt: %f, theta_tgt: %f, delta_tgt: %f", y_tgt, v_tgt, a_tgt,
               theta_tgt, delta_tgt);
 
  // handle thresholds for bi-level stabilization (which means, using ego state as initial state for the optimization)
  // longitudinal reinits
  if (fabs(v_tgt - perception_msgs::object_access::getVelLon(ego_data)) > bi_level_dV_ ||
      fabs(a_tgt - perception_msgs::object_access::getAccLon(ego_data)) > bi_level_dA_) {
    RCLCPP_WARN(this->get_logger(), "Lon reinit: v_tgt: %f, a_tgt: %f, ego_v: %f, ego_a: %f", v_tgt, a_tgt,
                perception_msgs::object_access::getVelLon(ego_data), perception_msgs::object_access::getAccLon(ego_data));
    v_tgt = perception_msgs::object_access::getVelLon(ego_data);
    a_tgt = perception_msgs::object_access::getAccLon(ego_data);
  }
  // lateral reinits
  if (fabs(y_tgt) > bi_level_dY_ || fabs(theta_tgt) > bi_level_dYaw_ * M_PI / 180.0) {
    RCLCPP_WARN(this->get_logger(), "Lat reinit: y_tgt: %f, theta_tgt: %f", y_tgt, theta_tgt);
    y_tgt = 0.0;
    theta_tgt = 0.0;
    delta_tgt = perception_msgs::object_access::getSteeringAngleAck(ego_data);
  } else if (fabs(delta_tgt - perception_msgs::object_access::getSteeringAngleAck(ego_data)) > bi_level_dDelta_ * M_PI / 180.0) {
    RCLCPP_WARN(this->get_logger(), "Delta reinit: delta_tgt: %f, ego_delta: %f", delta_tgt,
                perception_msgs::object_access::getSteeringAngleAck(ego_data));
    delta_tgt = perception_msgs::object_access::getSteeringAngleAck(ego_data);
  }
 
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
  x_init[0] = 0.0;
  x_init[1] = y_tgt;
  x_init[2] = 0.0;
  x_init[3] = v_tgt;
  x_init[4] = a_tgt;
  x_init[5] = theta_tgt;
  x_init[6] = delta_tgt;
 
  return x_init;
}

getHighLevelX0()

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

overrideprivatevirtual

Computes the initial optimizer state using high-level stabilization.

Parameters

[in]

ego_data

Current EgoData based on the kinematic bicycle model with Ackermann steering.

Returns

Initial state vector for the Ackermann model.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 92 of file trajectory_optimization_ackermann.cpp.

                                                                                                               {
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
  x_init[3] = perception_msgs::object_access::getVelLon(ego_data);
  x_init[4] = perception_msgs::object_access::getAccLon(ego_data);
  x_init[6] = perception_msgs::object_access::getSteeringAngleAck(ego_data);
 
  return x_init;
}

initializeTrajectory()

void trajectory_optimization::TrajectoryOptimizationAckermannNode::initializeTrajectory ( trajectory_planning_msgs::msg::Trajectory & trajectory )

overrideprivatevirtual

Initializes a drivable trajectory message for a kinematic bicycle model with Ackermann steering.

Parameters

[out]

trajectory

Trajectory message to initialize.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 18 of file trajectory_optimization_ackermann.cpp.

                                                                                                                {
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(trajectory, trajectory_planning_msgs::msg::DRIVABLE::TYPE_ID,
                                                                    n_shots_ + 1);
}

Member Data Documentation

bi_level_dDelta_

double trajectory_optimization::TrajectoryOptimizationAckermannNode::bi_level_dDelta_ = 5.0

private

Definition at line 52 of file trajectory_optimization_ackermann.hpp.

---

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

• trajectory_optimization/include/trajectory_optimization/trajectory_optimization_ackermann.hpp
• trajectory_optimization/src/trajectory_optimization_ackermann.cpp