trajectory_optimization_rws.cpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#include <limits>
 
#include <trajectory_optimization/trajectory_optimization_rws.hpp>
 
#include <rclcpp_components/register_node_macro.hpp>
 
RCLCPP_COMPONENTS_REGISTER_NODE(trajectory_optimization::TrajectoryOptimizationRWSNode)
 
namespace trajectory_optimization {
TrajectoryOptimizationRWSNode::TrajectoryOptimizationRWSNode(const rclcpp::NodeOptions& options)
    : TrajectoryOptimizationNode("TrajectoryOptimizationRWSNode", options) {
  this->declareAndLoadParameter("distance_front_axle", distance_front_axle_, "Distance from center of gravity to front axle [m]");
  this->declareAndLoadParameter("distance_rear_axle", distance_rear_axle_, "Distance from center of gravity to rear axle [m]");
}
 
void TrajectoryOptimizationRWSNode::initializeTrajectory(trajectory_planning_msgs::msg::Trajectory& trajectory) {
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(
      trajectory, trajectory_planning_msgs::msg::DRIVABLERWS::TYPE_ID, n_shots_ + 1);
}
 
std::vector<double> TrajectoryOptimizationRWSNode::getBiLevelX0(const perception_msgs::msg::EgoData& ego_data) {
  // 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_FRONT, DELTA_REAR;
  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_FRONT.push_back(trajectory_planning_msgs::trajectory_access::getDeltaFront(tf_trajectory, i));
    DELTA_REAR.push_back(trajectory_planning_msgs::trajectory_access::getDeltaRear(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_front_tgt = 0.0, delta_rear_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, THETA, des_time, theta_tgt, true)) theta_tgt = 0.0;
  if (!linearInterpolation(TIME, V, des_time, v_tgt)) v_tgt = perception_msgs::object_access::getVelocityMagnitude(ego_data);
  if (!linearInterpolation(TIME, A, des_time, a_tgt)) {
    a_tgt = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                              perception_msgs::object_access::getVelocity(ego_data));
  }
  if (!linearInterpolation(TIME, DELTA_FRONT, des_time, delta_front_tgt)) {
    delta_front_tgt = perception_msgs::object_access::getSteeringAngleFront(ego_data);
  }
  if (!linearInterpolation(TIME, DELTA_REAR, des_time, delta_rear_tgt)) {
    delta_rear_tgt = perception_msgs::object_access::getSteeringAngleRear(ego_data);
  }
 
  RCLCPP_DEBUG(this->get_logger(), "y_tgt: %f, v_tgt: %f, a_tgt: %f, theta_tgt: %f, delta_front_tgt: %f, delta_rear_tgt: %f",
               y_tgt, v_tgt, a_tgt, theta_tgt, delta_front_tgt, delta_rear_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::getVelocityMagnitude(ego_data)) > bi_level_dV_) {
    const auto ego_v = perception_msgs::object_access::getVelocityMagnitude(ego_data);
    const auto ego_a = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                                         perception_msgs::object_access::getVelocity(ego_data));
    RCLCPP_WARN(this->get_logger(), "Velocity reinit: v_tgt: %f, a_tgt: %f, ego_v: %f, ego_a: %f", v_tgt, a_tgt, ego_v, ego_a);
    v_tgt = ego_v;
    a_tgt = ego_a;
  }
  // lateral reinits
  if (fabs(y_tgt) > bi_level_dY_ || fabs(theta_tgt) > bi_level_dYaw_ * M_PI / 180.0) {
    const auto ego_delta_front = perception_msgs::object_access::getSteeringAngleFront(ego_data);
    const auto ego_delta_rear = perception_msgs::object_access::getSteeringAngleRear(ego_data);
    RCLCPP_WARN(this->get_logger(),
                "Lateral reinit: y_tgt: %f, theta_tgt: %f, delta_front_tgt: %f, delta_rear_tgt: %f, ego_y: %f, "
                "ego_theta: %f, ego_delta_front: %f, ego_delta_rear: %f",
                y_tgt, theta_tgt, delta_front_tgt, delta_rear_tgt, 0.0, 0.0, ego_delta_front, ego_delta_rear);
    y_tgt = 0.0;
    theta_tgt = 0.0;
    delta_front_tgt = ego_delta_front;
    delta_rear_tgt = ego_delta_rear;
  }
 
  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_front_tgt;
  x_init[7] = delta_rear_tgt;
 
  return x_init;
}
 
std::vector<double> TrajectoryOptimizationRWSNode::getHighLevelX0(const perception_msgs::msg::EgoData& ego_data) {
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
 
  x_init[3] = perception_msgs::object_access::getVelocityMagnitude(ego_data);
  x_init[4] = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                                perception_msgs::object_access::getVelocity(ego_data));
  x_init[6] = perception_msgs::object_access::getSteeringAngleFront(ego_data);
  x_init[7] = perception_msgs::object_access::getSteeringAngleRear(ego_data);
 
  return x_init;
}
 
void TrajectoryOptimizationRWSNode::convertToTrajectoryMsg(trajectory_planning_msgs::msg::Trajectory& trajectory) {
  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::setDeltaFront(trajectory, xtraj_[i * *nlp_dims_->nx + 6], i);
    trajectory_planning_msgs::trajectory_access::setDeltaRear(trajectory, xtraj_[i * *nlp_dims_->nx + 7], i);
    trajectory_planning_msgs::trajectory_access::setBeta(
        trajectory, xtraj_[i * *nlp_dims_->nx + 6], xtraj_[i * *nlp_dims_->nx + 7], distance_front_axle_, distance_rear_axle_, i);
  }
}
 
double TrajectoryOptimizationRWSNode::computeVehicleSlipAngle(const double& delta_front, const double& delta_rear) const {
  double wheel_base = distance_front_axle_ + distance_rear_axle_;
  double slip_angle =
      atan(distance_rear_axle_ / wheel_base * tan(delta_front) + distance_front_axle_ / wheel_base * tan(delta_rear));
  return slip_angle;
}
 
double TrajectoryOptimizationRWSNode::projectVectorAonV(const geometry_msgs::msg::Vector3& a,
                                                        const geometry_msgs::msg::Vector3& v) {
  double v_magnitude_squared = v.x * v.x + v.y * v.y;
  if (v_magnitude_squared < std::numeric_limits<double>::epsilon()) {
    return 0.0;
  }
  double scale = (a.x * v.x + a.y * v.y) / v_magnitude_squared;
  return scale * std::sqrt(v_magnitude_squared);
}
 
}  // namespace trajectory_optimization