Autoware.Auto
autoware::motion::control::trajectory_follower::MPCUtils Namespace Reference

Functions

TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw (const float64_t &yaw)
 convert from yaw to ros-Quaternion More...
 
TRAJECTORY_FOLLOWER_PUBLIC void 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 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 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 convertToAutowareTrajectory (const MPCTrajectory &input, autoware_auto_planning_msgs::msg::Trajectory &output)
 convert the given MPCTrajectory to a Trajectory msg More...
 
TRAJECTORY_FOLLOWER_PUBLIC void 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 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 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 calcMPCTrajectoryTime (MPCTrajectory &traj)
 fill the relative_time field of the given MPCTrajectory More...
 
TRAJECTORY_FOLLOWER_PUBLIC void 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 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 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 > 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 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 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 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 calcStopDistance (const autoware_auto_planning_msgs::msg::Trajectory &current_trajectory, const int64_t origin)
 calculate distance to stopped point More...
 

Function Documentation

◆ calcLateralError()

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

Parameters
[in]ego_posepose to check for error
[in]ref_posereference pose
Returns
lateral distance between the two poses

◆ calcMPCTrajectoryArclength()

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

Parameters
[in]trajectorytrajectory for which to calculate the arc length
[out]arclengththe cummulative arc length at each point of the trajectory

◆ calcMPCTrajectoryTime()

bool8_t autoware::motion::control::trajectory_follower::MPCUtils::calcMPCTrajectoryTime ( MPCTrajectory traj)

fill the relative_time field of the given MPCTrajectory

Parameters
[in]trajMPCTrajectory for which to fill in the relative_time
Returns
true if the calculation was successful

◆ calcNearestIndex() [1/2]

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

Parameters
[in]trajtrajectory to search for the point nearest to the pose
[in]self_posepose for which to search the nearest trajectory point
Returns
index of the input trajectory nearest to the pose

◆ calcNearestIndex() [2/2]

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

Parameters
[in]trajtrajectory to search for the point nearest to the pose
[in]self_posepose for which to search the nearest trajectory point
Returns
index of the input trajectory nearest to the pose

◆ calcNearestPoseInterp()

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

Parameters
[in]trajreference trajectory
[in]self_poseobject pose
[out]nearest_posenearest pose on path
[out]nearest_indexpath index of nearest pose
[out]nearest_timetime of nearest pose on trajectory
[out]loggerto output the reason for failure
[in]clockto throttle log output
Returns
false when nearest pose couldn't find for some reasons

◆ calcStopDistance()

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

◆ calcTrajectoryCurvature() [1/2]

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)

Parameters
[in]curvature_smoothing_numindex distance for 3 points for curvature calculation
[in]trajinput trajectory
Returns
vector of curvatures at each point of the given trajectory

◆ calcTrajectoryCurvature() [2/2]

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)

Parameters
[in]curvature_smoothing_numindex distance for 3 points for curvature calculation
[in,out]trajobject trajectory

◆ calcTrajectoryYawFromXY()

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

Parameters
[in,out]trajobject trajectory
[in]nearest_idxtrajectory index nearest to ego
[in]ego_yawyaw of the ego vehicle

◆ convertEulerAngleToMonotonic()

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

Parameters
[in,out]ainput angle vector

◆ convertToAutowareTrajectory()

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

Parameters
[in]inputMPCTrajectory to convert
[out]outputresulting Trajectory msg
Returns
true if the conversion was successful

◆ convertToMPCTrajectory()

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

Parameters
[in]inputtrajectory to convert
[out]outputresulting MPCTrajectory
Returns
true if the conversion was successful

◆ dynamicSmoothingVelocity()

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

Parameters
[in]start_idxindex of the trajectory point from which to start smoothing
[in]start_velinitial velocity to set at the start_idx
[in]acc_limlimit on the acceleration
[in]tauconstant to control the smoothing (high-value = very smooth)
[in,out]trajMPCTrajectory for which to calculate the smoothed velocity

◆ getQuaternionFromYaw()

geometry_msgs::msg::Quaternion autoware::motion::control::trajectory_follower::MPCUtils::getQuaternionFromYaw ( const float64_t &  yaw)

convert from yaw to ros-Quaternion

Parameters
[in]yawinput yaw angle
Returns
quaternion

◆ linearInterpMPCTrajectory()

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

Parameters
[in]in_indexindexes for each trajectory point
[in]in_trajMPCTrajectory to interpolate
[in]out_indexdesired interpolated indexes
[out]out_trajresulting interpolated MPCTrajectory

◆ resampleMPCTrajectoryByDistance()

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

Parameters
[in]inputtrajectory to resample
[in]resample_interval_distthe desired distance between two successive trajectory points
[out]outputthe resampled trajectory