Autoware.Auto
mpc_utils.hpp File Reference
#include <cmath>
#include <string>
#include <vector>
#include "trajectory_follower/interpolate.hpp"
#include "trajectory_follower/mpc_trajectory.hpp"
#include "trajectory_follower/visibility_control.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "geometry/common_2d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "helper_functions/angle_utils.hpp"
#include "motion_common/motion_common.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
Include dependency graph for mpc_utils.hpp:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 autoware
 This file defines the lanelet2_map_provider_node class.
 
 autoware::motion
 
 autoware::motion::control
 
 autoware::motion::control::trajectory_follower
 
 autoware::motion::control::trajectory_follower::MPCUtils
 

Functions

TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion autoware::motion::control::trajectory_follower::MPCUtils::getQuaternionFromYaw (const float64_t &yaw)
 convert from yaw to ros-Quaternion More...
 
TRAJECTORY_FOLLOWER_PUBLIC void autoware::motion::control::trajectory_follower::MPCUtils::convertEulerAngleToMonotonic (std::vector< float64_t > *a)
 convert Euler angle vector including +-2pi to 0 jump to continuous series data More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t autoware::motion::control::trajectory_follower::MPCUtils::calcLateralError (const geometry_msgs::msg::Pose &ego_pose, const geometry_msgs::msg::Pose &ref_pose)
 calculate the lateral error of the given pose relative to the given reference pose More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::convertToMPCTrajectory (const autoware_auto_planning_msgs::msg::Trajectory &input, MPCTrajectory &output)
 convert the given Trajectory msg to a MPCTrajectory object More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory (const MPCTrajectory &input, autoware_auto_planning_msgs::msg::Trajectory &output)
 convert the given MPCTrajectory to a Trajectory msg More...
 
TRAJECTORY_FOLLOWER_PUBLIC void autoware::motion::control::trajectory_follower::MPCUtils::calcMPCTrajectoryArclength (const MPCTrajectory &trajectory, std::vector< float64_t > *arclength)
 calculate the arc length at each point of the given trajectory More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance (const MPCTrajectory &input, const float64_t resample_interval_dist, MPCTrajectory *output)
 resample the given trajectory with the given fixed interval More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::linearInterpMPCTrajectory (const std::vector< float64_t > &in_index, const MPCTrajectory &in_traj, const std::vector< float64_t > &out_index, MPCTrajectory *out_traj)
 linearly interpolate the given trajectory assuming a base indexing and a new desired indexing More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::calcMPCTrajectoryTime (MPCTrajectory &traj)
 fill the relative_time field of the given MPCTrajectory More...
 
TRAJECTORY_FOLLOWER_PUBLIC void autoware::motion::control::trajectory_follower::MPCUtils::dynamicSmoothingVelocity (const size_t start_idx, const float64_t start_vel, const float64_t acc_lim, const float64_t tau, MPCTrajectory &traj)
 recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing More...
 
TRAJECTORY_FOLLOWER_PUBLIC void autoware::motion::control::trajectory_follower::MPCUtils::calcTrajectoryYawFromXY (MPCTrajectory *traj, const int64_t nearest_idx, const float64_t ego_yaw)
 calculate yaw angle in MPCTrajectory from xy vector More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::calcTrajectoryCurvature (const size_t curvature_smoothing_num, MPCTrajectory *traj)
 Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1) More...
 
TRAJECTORY_FOLLOWER_PUBLIC std::vector< float64_t > autoware::motion::control::trajectory_follower::MPCUtils::calcTrajectoryCurvature (const size_t curvature_smoothing_num, const MPCTrajectory &traj)
 Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1) More...
 
TRAJECTORY_FOLLOWER_PUBLIC bool8_t autoware::motion::control::trajectory_follower::MPCUtils::calcNearestPoseInterp (const MPCTrajectory &traj, const geometry_msgs::msg::Pose &self_pose, geometry_msgs::msg::Pose *nearest_pose, size_t *nearest_index, float64_t *nearest_time, const rclcpp::Logger &logger, rclcpp::Clock &clock)
 calculate nearest pose on MPCTrajectory with linear interpolation More...
 
TRAJECTORY_FOLLOWER_PUBLIC int64_t autoware::motion::control::trajectory_follower::MPCUtils::calcNearestIndex (const MPCTrajectory &traj, const geometry_msgs::msg::Pose &self_pose)
 calculate the index of the trajectory point nearest to the given pose More...
 
TRAJECTORY_FOLLOWER_PUBLIC int64_t autoware::motion::control::trajectory_follower::MPCUtils::calcNearestIndex (const autoware_auto_planning_msgs::msg::Trajectory &traj, const geometry_msgs::msg::Pose &self_pose)
 calculate the index of the trajectory point nearest to the given pose More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t autoware::motion::control::trajectory_follower::MPCUtils::calcStopDistance (const autoware_auto_planning_msgs::msg::Trajectory &current_trajectory, const int64_t origin)
 calculate distance to stopped point More...