trajectory_optimization_rws.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <trajectory_optimization/trajectory_optimization_node.hpp>
 
namespace trajectory_optimization {
 
class TrajectoryOptimizationRWSNode : public TrajectoryOptimizationNode {
 public:
  explicit TrajectoryOptimizationRWSNode(const rclcpp::NodeOptions& options);
 
 private:
  void initializeTrajectory(trajectory_planning_msgs::msg::Trajectory& trajectory) override;
 
  std::vector<double> getBiLevelX0(const perception_msgs::msg::EgoData& ego_data) override;
 
  std::vector<double> getHighLevelX0(const perception_msgs::msg::EgoData& ego_data) override;
 
  void convertToTrajectoryMsg(trajectory_planning_msgs::msg::Trajectory& trajectory) override;
 
  static double projectVectorAonV(const geometry_msgs::msg::Vector3& a, const geometry_msgs::msg::Vector3& v);
 
  double computeVehicleSlipAngle(const double& delta_front, const double& delta_rear) const;
 
  // parameters
  double distance_front_axle_ = 1.7;
  double distance_rear_axle_ = 1.7;
 
  // model specific bi-level thresholds
  double bi_level_dDelta_front_ = 5.0;
  double bi_level_dDelta_rear_ = 5.0;
};
 
}  // namespace trajectory_optimization