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

Functions

TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory (const Trajectory &traj)
 check if trajectory is invalid or not More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance (const Point &current_pos, const Trajectory &traj)
 calculate distance to stopline from current vehicle position where velocity is 0 More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose (const Quaternion &quaternion)
 calculate pitch angle from estimated current pose More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByTraj (const Trajectory &trajectory, const size_t closest_idx, const float64_t wheel_base)
 calculate pitch angle from trajectory on map NOTE: there is currently no z information so this always returns 0.0 More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcElevationAngle (const TrajectoryPoint &p_from, const TrajectoryPoint &p_to)
 calculate elevation angle More...
 
TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay (const Pose &current_pose, const float64_t delay_time, const float64_t current_vel)
 calculate vehicle pose after time delay by moving the vehicle at current velocity for delayed time More...
 
TRAJECTORY_FOLLOWER_PUBLIC Quaternion lerpOrientation (const Quaternion &o_from, const Quaternion &o_to, const float64_t ratio)
 apply linear interpolation to orientation More...
 
template<class T >
TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint (const T &points, const Point &point)
 apply linear interpolation to trajectory point that is nearest to a certain point More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter (const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val)
 limit variable whose differential is within a certain value More...
 
TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter (const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val, const float64_t min_val)
 limit variable whose differential is within a certain value More...
 
float64_t lerp (const float64_t v_from, const float64_t v_to, const float64_t ratio)
 

Function Documentation

◆ applyDiffLimitFilter() [1/2]

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::applyDiffLimitFilter ( const float64_t  input_val,
const float64_t  prev_val,
const float64_t  dt,
const float64_t  lim_val 
)

limit variable whose differential is within a certain value

Parameters
[in]input_valcurrent value
[in]prev_valprevious value
[in]dttime between current and previous one
[in]lim_vallimitation value for differential

◆ applyDiffLimitFilter() [2/2]

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::applyDiffLimitFilter ( const float64_t  input_val,
const float64_t  prev_val,
const float64_t  dt,
const float64_t  max_val,
const float64_t  min_val 
)

limit variable whose differential is within a certain value

Parameters
[in]input_valcurrent value
[in]prev_valprevious value
[in]dttime between current and previous one
[in]max_valmaximum value for differential
[in]min_valminimum value for differential

◆ calcElevationAngle()

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::calcElevationAngle ( const TrajectoryPoint &  p_from,
const TrajectoryPoint &  p_to 
)

calculate elevation angle

◆ calcPoseAfterTimeDelay()

Pose autoware::motion::control::trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay ( const Pose &  current_pose,
const float64_t  delay_time,
const float64_t  current_vel 
)

calculate vehicle pose after time delay by moving the vehicle at current velocity for delayed time

◆ calcStopDistance()

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::calcStopDistance ( const Point current_pos,
const Trajectory &  traj 
)

calculate distance to stopline from current vehicle position where velocity is 0

◆ getPitchByPose()

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::getPitchByPose ( const Quaternion &  quaternion)

calculate pitch angle from estimated current pose

◆ getPitchByTraj()

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::getPitchByTraj ( const Trajectory &  trajectory,
const size_t  closest_idx,
const float64_t  wheel_base 
)

calculate pitch angle from trajectory on map NOTE: there is currently no z information so this always returns 0.0

Parameters
[in]trajectoryinput trajectory
[in]closest_idxnearest index to current vehicle position
[in]wheel_baselength of wheel base

◆ isValidTrajectory()

bool autoware::motion::control::trajectory_follower::longitudinal_utils::isValidTrajectory ( const Trajectory &  traj)

check if trajectory is invalid or not

◆ lerp()

float64_t autoware::motion::control::trajectory_follower::longitudinal_utils::lerp ( const float64_t  v_from,
const float64_t  v_to,
const float64_t  ratio 
)

◆ lerpOrientation()

Quaternion autoware::motion::control::trajectory_follower::longitudinal_utils::lerpOrientation ( const Quaternion &  o_from,
const Quaternion &  o_to,
const float64_t  ratio 
)

apply linear interpolation to orientation

Parameters
[in]o_fromfirst orientation
[in]o_tosecond orientation
[in]ratioratio between o_from and o_to for interpolation

◆ lerpTrajectoryPoint()

template<class T >
TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint autoware::motion::control::trajectory_follower::longitudinal_utils::lerpTrajectoryPoint ( const T &  points,
const Point point 
)

apply linear interpolation to trajectory point that is nearest to a certain point

Parameters
[in]pointstrajectory points
[in]pointInterpolated point is nearest to this point.